Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Use SDA algorithm instead of SSCA for DARE solver (#5526)

Browse Source 
Both seem to work, but the SDA algorithm is specifically recommended for
solving DAREs as opposed to P-DAREs.

The QR decomposition was replaced with a partial pivoting LU
decomposition at the recommendation of section 2.4 of the paper.

More tests and a separate JNI function for each DARE solver variant were
added.
This commit is contained in:
Tyler Veness
2023-08-12 19:45:45 -07:00
committed by GitHub
parent a4b7fde767
commit 394cfeadbd
7 changed files with 549 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files

116
wpimath/src/test/java/edu/wpi/first/math/DARETest.java
View File

@@ -5,6 +5,7 @@
package edu.wpi.first.math;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
import edu.wpi.first.wpilibj.UtilityClassTest;
import org.ejml.simple.SimpleMatrix;
@@ -187,4 +188,119 @@ class DARETest extends UtilityClassTest<DARE> {
assertMatrixEqual(X, X.transpose());
assertDARESolution(A, B, Q, R, N, X);
}
@Test
void testMoreInputsThanStates_ABQR() {
var A = SimpleMatrix.identity(2);
var B = new SimpleMatrix(2, 3, true, new double[] {1, 0, 0, 0, 0.5, 0.3});
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(3);
var X = DARE.dare(A, B, Q, R);
assertMatrixEqual(X, X.transpose());
assertDARESolution(A, B, Q, R, X);
}
@Test
void testMoreInputsThanStates_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = new SimpleMatrix(2, 3, true, new double[] {1, 0, 0, 0, 0.5, 0.3});
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(3);
var N = new SimpleMatrix(2, 3, true, new double[] {1, 0, 0, 0, 1, 0});
var X = DARE.dare(A, B, Q, R, N);
assertMatrixEqual(X, X.transpose());
assertDARESolution(A, B, Q, R, N, X);
}
@Test
void testQNotSymmetricPositiveSemidefinite_ABQR() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.diag(-1.0, -1.0);
var R = SimpleMatrix.identity(2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
}
@Test
void testQNotSymmetricPositiveSemidefinite_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.diag(-1.0, -1.0);
var N = SimpleMatrix.diag(2.0, 2.0);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
}
@Test
void testRNotSymmetricPositiveDefinite_ABQR() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.identity(2);
var R1 = new SimpleMatrix(2, 2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1));
var R2 = SimpleMatrix.diag(-1.0, -1.0);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R2));
}
@Test
void testRNotSymmetricPositiveDefinite_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.identity(2);
var N = SimpleMatrix.identity(2);
var R1 = new SimpleMatrix(2, 2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1, N));
var R2 = SimpleMatrix.diag(-1.0, -1.0);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R2, N));
}
@Test
void testABNotStabilizable_ABQR() {
var A = SimpleMatrix.identity(2);
var B = new SimpleMatrix(2, 2);
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
}
@Test
void testABNotStabilizable_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = new SimpleMatrix(2, 2);
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(2);
var N = SimpleMatrix.identity(2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
}
@Test
void testACNotDetectable_ABQR() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = new SimpleMatrix(2, 2);
var R = SimpleMatrix.identity(2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
}
@Test
void testACNotDetectable_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = new SimpleMatrix(2, 2);
var R = SimpleMatrix.identity(2);
var N = new SimpleMatrix(2, 2);
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
}
}

117
wpimath/src/test/native/cpp/DARETest.cpp
View File

@@ -2,6 +2,8 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <stdexcept>
#include <fmt/core.h>
#include "Eigen/Core"
@@ -71,7 +73,6 @@ void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
}
// NOLINTNEXTLINE(google-readability-avoid-underscore-in-googletest-name)
TEST(DARETest, NonInvertibleA_ABQR) {
// Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
// Riccati Equation"
@@ -91,7 +92,6 @@ TEST(DARETest, NonInvertibleA_ABQR) {
ExpectDARESolution(A, B, Q, R, X);
}
// NOLINTNEXTLINE(google-readability-avoid-underscore-in-googletest-name)
TEST(DARETest, NonInvertibleA_ABQRN) {
// Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
// Riccati Equation"
@@ -117,7 +117,6 @@ TEST(DARETest, NonInvertibleA_ABQRN) {
ExpectDARESolution(A, B, Q, R, N, X);
}
// NOLINTNEXTLINE(google-readability-avoid-underscore-in-googletest-name)
TEST(DARETest, InvertibleA_ABQR) {
Eigen::MatrixXd A{2, 2};
A << 1, 1, 0, 1;
@@ -134,7 +133,6 @@ TEST(DARETest, InvertibleA_ABQR) {
ExpectDARESolution(A, B, Q, R, X);
}
// NOLINTNEXTLINE(google-readability-avoid-underscore-in-googletest-name)
TEST(DARETest, InvertibleA_ABQRN) {
Eigen::MatrixXd A{2, 2};
A << 1, 1, 0, 1;
@@ -157,7 +155,6 @@ TEST(DARETest, InvertibleA_ABQRN) {
ExpectDARESolution(A, B, Q, R, N, X);
}
// NOLINTNEXTLINE(google-readability-avoid-underscore-in-googletest-name)
TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) {
// The first generalized eigenvalue of (S, T) is stable
@@ -176,7 +173,6 @@ TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) {
ExpectDARESolution(A, B, Q, R, X);
}
// NOLINTNEXTLINE(google-readability-avoid-underscore-in-googletest-name)
TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQRN) {
// The first generalized eigenvalue of (S, T) is stable
@@ -201,7 +197,6 @@ TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQRN) {
ExpectDARESolution(A, B, Q, R, N, X);
}
// NOLINTNEXTLINE(google-readability-avoid-underscore-in-googletest-name)
TEST(DARETest, IdentitySystem_ABQR) {
const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()};
@@ -214,7 +209,6 @@ TEST(DARETest, IdentitySystem_ABQR) {
ExpectDARESolution(A, B, Q, R, X);
}
// NOLINTNEXTLINE(google-readability-avoid-underscore-in-googletest-name)
TEST(DARETest, IdentitySystem_ABQRN) {
const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()};
@@ -227,3 +221,110 @@ TEST(DARETest, IdentitySystem_ABQRN) {
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, N, X);
}
TEST(DARETest, MoreInputsThanStates_ABQR) {
const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}};
const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R{Eigen::Matrix3d::Identity()};
Eigen::MatrixXd X = frc::DARE(A, B, Q, R);
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, X);
}
TEST(DARETest, MoreInputsThanStates_ABQRN) {
const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}};
const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R{Eigen::Matrix3d::Identity()};
const Eigen::MatrixXd N{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}};
Eigen::MatrixXd X = frc::DARE(A, B, Q, R, N);
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, N, X);
}
TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQR) {
const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd Q{-Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R), std::invalid_argument);
}
TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQRN) {
const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd N{2.0 * Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R, N), std::invalid_argument);
}
TEST(DARETest, RNotSymmetricPositiveDefinite_ABQR) {
const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R1{Eigen::Matrix2d::Zero()};
EXPECT_THROW(frc::DARE(A, B, Q, R1), std::invalid_argument);
const Eigen::MatrixXd R2{-Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R2), std::invalid_argument);
}
TEST(DARETest, RNotSymmetricPositiveDefinite_ABQRN) {
const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd N{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R1{Eigen::Matrix2d::Zero()};
EXPECT_THROW(frc::DARE(A, B, Q, R1, N), std::invalid_argument);
const Eigen::MatrixXd R2{-Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R2, N), std::invalid_argument);
}
TEST(DARETest, ABNotStabilizable_ABQR) {
const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd B{Eigen::Matrix2d::Zero()};
const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R), std::invalid_argument);
}
TEST(DARETest, ABNotStabilizable_ABQRN) {
const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd B{Eigen::Matrix2d::Zero()};
const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd N{Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R, N), std::invalid_argument);
}
TEST(DARETest, ACNotDetectable_ABQR) {
const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd Q{Eigen::Matrix2d::Zero()};
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
EXPECT_THROW(frc::DARE(A, B, Q, R), std::invalid_argument);
}
TEST(DARETest, ACNotDetectable_ABQRN) {
const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd Q{Eigen::Matrix2d::Zero()};
const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()};
const Eigen::MatrixXd N{Eigen::Matrix2d::Zero()};
EXPECT_THROW(frc::DARE(A, B, Q, R, N), std::invalid_argument);
}