diff --git a/wpimath/src/main/java/edu/wpi/first/math/DARE.java b/wpimath/src/main/java/edu/wpi/first/math/DARE.java
index 698c3f7731..d9d0b55877 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/DARE.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/DARE.java
@@ -12,33 +12,122 @@ public final class DARE {
}
/**
- * Solves the discrete algebraic Riccati equation.
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
*
+ * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ *
This internal function skips expensive precondition checks for increased performance. The
+ * solver may hang if any of the following occur:
+ *
+ *
+ * - Q isn't symmetric positive semidefinite
+ *
- R isn't symmetric positive definite
+ *
- The (A, B) pair isn't stabilizable
+ *
- The (A, C) pair where Q = CᵀC isn't detectable
+ *
+ *
+ * Only use this function if you're sure the preconditions are met.
+ *
+ * @param Number of states.
+ * @param Number of inputs.
* @param A System matrix.
* @param B Input matrix.
* @param Q State cost matrix.
* @param R Input cost matrix.
* @return Solution of DARE.
- * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
- * @throws IllegalArgumentException if R isn't symmetric positive definite.
- * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
- * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
*/
- public static SimpleMatrix dare(SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
- var S = new SimpleMatrix(A.getNumRows(), A.getNumCols());
- WPIMathJNI.dareABQR(
- A.getDDRM().getData(),
- B.getDDRM().getData(),
- Q.getDDRM().getData(),
- R.getDDRM().getData(),
+ public static Matrix dareDetail(
+ Matrix A,
+ Matrix B,
+ Matrix Q,
+ Matrix R) {
+ var S = new Matrix(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+ WPIMathJNI.dareDetailABQR(
+ A.getStorage().getDDRM().getData(),
+ B.getStorage().getDDRM().getData(),
+ Q.getStorage().getDDRM().getData(),
+ R.getStorage().getDDRM().getData(),
A.getNumCols(),
B.getNumCols(),
- S.getDDRM().getData());
+ S.getStorage().getDDRM().getData());
return S;
}
/**
- * Solves the discrete algebraic Riccati equation.
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ * AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+ *
+ *
This overload of the DARE is useful for finding the control law uₖ that minimizes the
+ * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+ *
+ *
+ * ∞ [xₖ]ᵀ[Q N][xₖ]
+ * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+ * k=0
+ *
+ *
+ * This is a more general form of the following. The linear-quadratic regulator is the feedback
+ * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+ *
+ *
+ * ∞
+ * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+ * k=0
+ *
+ *
+ * This can be refactored as:
+ *
+ *
+ * ∞ [xₖ]ᵀ[Q 0][xₖ]
+ * J = Σ [uₖ] [0 R][uₖ] ΔT
+ * k=0
+ *
+ *
+ * This internal function skips expensive precondition checks for increased performance. The
+ * solver may hang if any of the following occur:
+ *
+ *
+ * - Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
+ *
- R isn't symmetric positive definite
+ *
- The (A, B) pair isn't stabilizable
+ *
- The (A, C) pair where Q = CᵀC isn't detectable
+ *
+ *
+ * Only use this function if you're sure the preconditions are met.
+ *
+ * @param Number of states.
+ * @param Number of inputs.
+ * @param A System matrix.
+ * @param B Input matrix.
+ * @param Q State cost matrix.
+ * @param R Input cost matrix.
+ * @param N State-input cross-term cost matrix.
+ * @return Solution of DARE.
+ */
+ public static Matrix dareDetail(
+ Matrix A,
+ Matrix B,
+ Matrix Q,
+ Matrix R,
+ Matrix N) {
+ var S = new Matrix(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+ WPIMathJNI.dareDetailABQRN(
+ A.getStorage().getDDRM().getData(),
+ B.getStorage().getDDRM().getData(),
+ Q.getStorage().getDDRM().getData(),
+ R.getStorage().getDDRM().getData(),
+ N.getStorage().getDDRM().getData(),
+ A.getNumCols(),
+ B.getNumCols(),
+ S.getStorage().getDDRM().getData());
+ return S;
+ }
+
+ /**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
*
* @param Number of states.
* @param Number of inputs.
@@ -57,40 +146,48 @@ public final class DARE {
Matrix B,
Matrix Q,
Matrix R) {
- return new Matrix<>(dare(A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage()));
- }
-
- /**
- * Solves the discrete algebraic Riccati equation.
- *
- * @param A System matrix.
- * @param B Input matrix.
- * @param Q State cost matrix.
- * @param R Input cost matrix.
- * @param N State-input cross-term cost matrix.
- * @return Solution of DARE.
- * @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
- * @throws IllegalArgumentException if R isn't symmetric positive definite.
- * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
- * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
- */
- public static SimpleMatrix dare(
- SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix N) {
- var S = new SimpleMatrix(A.getNumRows(), A.getNumCols());
- WPIMathJNI.dareABQRN(
- A.getDDRM().getData(),
- B.getDDRM().getData(),
- Q.getDDRM().getData(),
- R.getDDRM().getData(),
- N.getDDRM().getData(),
+ var S = new Matrix(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+ WPIMathJNI.dareABQR(
+ A.getStorage().getDDRM().getData(),
+ B.getStorage().getDDRM().getData(),
+ Q.getStorage().getDDRM().getData(),
+ R.getStorage().getDDRM().getData(),
A.getNumCols(),
B.getNumCols(),
- S.getDDRM().getData());
+ S.getStorage().getDDRM().getData());
return S;
}
/**
- * Solves the discrete algebraic Riccati equation.
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ * AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+ *
+ *
This overload of the DARE is useful for finding the control law uₖ that minimizes the
+ * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+ *
+ *
+ * ∞ [xₖ]ᵀ[Q N][xₖ]
+ * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+ * k=0
+ *
+ *
+ * This is a more general form of the following. The linear-quadratic regulator is the feedback
+ * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+ *
+ *
+ * ∞
+ * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+ * k=0
+ *
+ *
+ * This can be refactored as:
+ *
+ *
+ * ∞ [xₖ]ᵀ[Q 0][xₖ]
+ * J = Σ [uₖ] [0 R][uₖ] ΔT
+ * k=0
+ *
*
* @param Number of states.
* @param Number of inputs.
@@ -111,7 +208,16 @@ public final class DARE {
Matrix Q,
Matrix R,
Matrix N) {
- return new Matrix<>(
- dare(A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage(), N.getStorage()));
+ var S = new Matrix(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+ WPIMathJNI.dareABQRN(
+ A.getStorage().getDDRM().getData(),
+ B.getStorage().getDDRM().getData(),
+ Q.getStorage().getDDRM().getData(),
+ R.getStorage().getDDRM().getData(),
+ N.getStorage().getDDRM().getData(),
+ A.getNumCols(),
+ B.getNumCols(),
+ S.getStorage().getDDRM().getData());
+ return S;
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
index 390d356771..b7a86fa7d0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
@@ -38,6 +38,18 @@ public class Matrix {
Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(columns).getNum());
}
+ /**
+ * Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
+ * provided generic bounds match the shape of the provided {@link Matrix}.
+ *
+ * @param rows The number of rows of the matrix.
+ * @param columns The number of columns of the matrix.
+ * @param storage The double array to back this value.
+ */
+ public Matrix(Nat rows, Nat columns, double[] storage) {
+ this.m_storage = new SimpleMatrix(rows.getNum(), columns.getNum(), true, storage);
+ }
+
/**
* Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
* provided generic bounds match the shape of the provided {@link Matrix}.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
index 37a85ffa61..a13726ffb7 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -44,7 +44,100 @@ public final class WPIMathJNI {
}
/**
- * Solves the discrete alegebraic Riccati equation.
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ *
This internal function skips expensive precondition checks for increased performance. The
+ * solver may hang if any of the following occur:
+ *
+ *
+ * - Q isn't symmetric positive semidefinite
+ *
- R isn't symmetric positive definite
+ *
- The (A, B) pair isn't stabilizable
+ *
- The (A, C) pair where Q = CᵀC isn't detectable
+ *
+ *
+ * Only use this function if you're sure the preconditions are met. Solves the discrete
+ * alegebraic Riccati equation.
+ *
+ * @param A Array containing elements of A in row-major order.
+ * @param B Array containing elements of B in row-major order.
+ * @param Q Array containing elements of Q in row-major order.
+ * @param R Array containing elements of R in row-major order.
+ * @param states Number of states in A matrix.
+ * @param inputs Number of inputs in B matrix.
+ * @param S Array storage for DARE solution.
+ */
+ public static native void dareDetailABQR(
+ double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
+
+ /**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ *
AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+ *
+ *
This overload of the DARE is useful for finding the control law uₖ that minimizes the
+ * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+ *
+ *
+ * ∞ [xₖ]ᵀ[Q N][xₖ]
+ * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+ * k=0
+ *
+ *
+ * This is a more general form of the following. The linear-quadratic regulator is the feedback
+ * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+ *
+ *
+ * ∞
+ * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+ * k=0
+ *
+ *
+ * This can be refactored as:
+ *
+ *
+ * ∞ [xₖ]ᵀ[Q 0][xₖ]
+ * J = Σ [uₖ] [0 R][uₖ] ΔT
+ * k=0
+ *
+ *
+ * This internal function skips expensive precondition checks for increased performance. The
+ * solver may hang if any of the following occur:
+ *
+ *
+ * - Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
+ *
- R isn't symmetric positive definite
+ *
- The (A, B) pair isn't stabilizable
+ *
- The (A, C) pair where Q = CᵀC isn't detectable
+ *
+ *
+ * Only use this function if you're sure the preconditions are met.
+ *
+ * @param A Array containing elements of A in row-major order.
+ * @param B Array containing elements of B in row-major order.
+ * @param Q Array containing elements of Q in row-major order.
+ * @param R Array containing elements of R in row-major order.
+ * @param N Array containing elements of N in row-major order.
+ * @param states Number of states in A matrix.
+ * @param inputs Number of inputs in B matrix.
+ * @param S Array storage for DARE solution.
+ */
+ public static native void dareDetailABQRN(
+ double[] A,
+ double[] B,
+ double[] Q,
+ double[] R,
+ double[] N,
+ int states,
+ int inputs,
+ double[] S);
+
+ /**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ *
AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
*
* @param A Array containing elements of A in row-major order.
* @param B Array containing elements of B in row-major order.
@@ -62,7 +155,35 @@ public final class WPIMathJNI {
double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
/**
- * Solves the discrete alegebraic Riccati equation.
+ * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+ *
+ *
AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+ *
+ *
This overload of the DARE is useful for finding the control law uₖ that minimizes the
+ * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+ *
+ *
+ * ∞ [xₖ]ᵀ[Q N][xₖ]
+ * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+ * k=0
+ *
+ *
+ * This is a more general form of the following. The linear-quadratic regulator is the feedback
+ * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+ *
+ *
+ * ∞
+ * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+ * k=0
+ *
+ *
+ * This can be refactored as:
+ *
+ *
+ * ∞ [xₖ]ᵀ[Q 0][xₖ]
+ * J = Σ [uₖ] [0 R][uₖ] ΔT
+ * k=0
+ *
*
* @param A Array containing elements of A in row-major order.
* @param B Array containing elements of B in row-major order.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
index c30b200792..dd0459ff38 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
@@ -4,6 +4,7 @@
package edu.wpi.first.math.controller;
+import edu.wpi.first.math.DARE;
import edu.wpi.first.math.InterpolatingMatrixTreeMap;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
@@ -15,6 +16,7 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.trajectory.Trajectory;
@@ -146,11 +148,26 @@ public class LTVDifferentialDriveController {
// The DARE is ill-conditioned if the velocity is close to zero, so don't
// let the system stop.
if (Math.abs(velocity) < 1e-4) {
- m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N5()));
+ A.set(State.kY.value, State.kHeading.value, 1e-4);
} else {
A.set(State.kY.value, State.kHeading.value, velocity);
- m_table.put(velocity, new LinearQuadraticRegulator(A, B, Q, R, dt).getK());
}
+
+ var discABPair = Discretization.discretizeAB(A, B, dt);
+ var discA = discABPair.getFirst();
+ var discB = discABPair.getSecond();
+
+ var S = DARE.dareDetail(discA, discB, Q, R);
+
+ // K = (BᵀSB + R)⁻¹BᵀSA
+ m_table.put(
+ velocity,
+ discB
+ .transpose()
+ .times(S)
+ .times(discB)
+ .plus(R)
+ .solve(discB.transpose().times(S).times(discA)));
}
}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
index d2ac72b7c3..ad3bd94f30 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
@@ -4,6 +4,7 @@
package edu.wpi.first.math.controller;
+import edu.wpi.first.math.DARE;
import edu.wpi.first.math.InterpolatingMatrixTreeMap;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
@@ -15,6 +16,7 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.trajectory.Trajectory;
/**
@@ -152,11 +154,26 @@ public class LTVUnicycleController {
// The DARE is ill-conditioned if the velocity is close to zero, so don't
// let the system stop.
if (Math.abs(velocity) < 1e-4) {
- m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N3()));
+ A.set(State.kY.value, State.kHeading.value, 1e-4);
} else {
A.set(State.kY.value, State.kHeading.value, velocity);
- m_table.put(velocity, new LinearQuadraticRegulator(A, B, Q, R, dt).getK());
}
+
+ var discABPair = Discretization.discretizeAB(A, B, dt);
+ var discA = discABPair.getFirst();
+ var discB = discABPair.getSecond();
+
+ var S = DARE.dareDetail(discA, discB, Q, R);
+
+ // K = (BᵀSB + R)⁻¹BᵀSA
+ m_table.put(
+ velocity,
+ discB
+ .transpose()
+ .times(S)
+ .times(discB)
+ .plus(R)
+ .solve(discB.transpose().times(S).times(discA)));
}
}
diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
index 7d32bfdebd..13c5832008 100644
--- a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
+++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
@@ -7,9 +7,11 @@
#include
#include
+#include "Eigen/Cholesky"
+#include "frc/DARE.h"
#include "frc/MathUtil.h"
#include "frc/StateSpaceUtil.h"
-#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
using namespace frc;
@@ -64,6 +66,7 @@ LTVDifferentialDriveController::LTVDifferentialDriveController(
// Ax = -Bu
// x = -A⁻¹Bu
units::meters_per_second_t maxV{
+ // NOLINTNEXTLINE(clang-analyzer-unix.Malloc)
-plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)};
if (maxV <= 0_mps) {
@@ -75,16 +78,27 @@ LTVDifferentialDriveController::LTVDifferentialDriveController(
"Max velocity of plant with 12 V input must be less than 15 m/s.");
}
+ auto R_llt = R.llt();
+
for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) {
// The DARE is ill-conditioned if the velocity is close to zero, so don't
// let the system stop.
if (units::math::abs(velocity) < 1e-4_mps) {
- m_table.insert(velocity, Matrixd<2, 5>::Zero());
+ A(State::kY, State::kHeading) = 1e-4;
} else {
A(State::kY, State::kHeading) = velocity.value();
- m_table.insert(velocity,
- frc::LinearQuadraticRegulator<5, 2>{A, B, Q, R, dt}.K());
}
+
+ Matrixd<5, 5> discA;
+ Matrixd<5, 2> discB;
+ DiscretizeAB(A, B, dt, &discA, &discB);
+
+ Matrixd<5, 5> S = detail::DARE<5, 2>(discA, discB, Q, R_llt);
+
+ // K = (BᵀSB + R)⁻¹BᵀSA
+ m_table.insert(velocity, (discB.transpose() * S * discB + R)
+ .llt()
+ .solve(discB.transpose() * S * discA));
}
}
diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
index 75188cb7a8..4f482471c0 100644
--- a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
+++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
@@ -6,8 +6,10 @@
#include
+#include "Eigen/Cholesky"
+#include "frc/DARE.h"
#include "frc/StateSpaceUtil.h"
-#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
#include "units/math.h"
using namespace frc;
@@ -83,17 +85,28 @@ LTVUnicycleController::LTVUnicycleController(
Matrixd<3, 3> Q = frc::MakeCostMatrix(Qelems);
Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
+ auto R_llt = R.llt();
+
for (auto velocity = -maxVelocity; velocity < maxVelocity;
velocity += 0.01_mps) {
// The DARE is ill-conditioned if the velocity is close to zero, so don't
// let the system stop.
if (units::math::abs(velocity) < 1e-4_mps) {
- m_table.insert(velocity, Matrixd<2, 3>::Zero());
+ A(State::kY, State::kHeading) = 1e-4;
} else {
A(State::kY, State::kHeading) = velocity.value();
- m_table.insert(velocity,
- frc::LinearQuadraticRegulator<3, 2>{A, B, Q, R, dt}.K());
}
+
+ Matrixd<3, 3> discA;
+ Matrixd<3, 2> discB;
+ DiscretizeAB(A, B, dt, &discA, &discB);
+
+ Matrixd<3, 3> S = detail::DARE<3, 2>(discA, discB, Q, R_llt);
+
+ // K = (BᵀSB + R)⁻¹BᵀSA
+ m_table.insert(velocity, (discB.transpose() * S * discB + R)
+ .llt()
+ .solve(discB.transpose() * S * discA));
}
}
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
index 43d1491dcc..66471f7097 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
@@ -62,6 +62,84 @@ frc::Trajectory CreateTrajectoryFromElements(std::span elements) {
extern "C" {
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: dareDetailABQR
+ * Signature: ([D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
+ (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+ jdoubleArray R, jint states, jint inputs, jdoubleArray S)
+{
+ JDoubleArrayRef nativeA{env, A};
+ JDoubleArrayRef nativeB{env, B};
+ JDoubleArrayRef nativeQ{env, Q};
+ JDoubleArrayRef nativeR{env, R};
+
+ Eigen::Map>
+ Amat{nativeA.array().data(), states, states};
+ Eigen::Map>
+ Bmat{nativeB.array().data(), states, inputs};
+ Eigen::Map>
+ Qmat{nativeQ.array().data(), states, states};
+ Eigen::Map>
+ Rmat{nativeR.array().data(), inputs, inputs};
+
+ Eigen::MatrixXd RmatCopy{Rmat};
+ auto R_llt = RmatCopy.llt();
+
+ Eigen::MatrixXd result = frc::detail::DARE(
+ Amat, Bmat, Qmat, R_llt);
+
+ env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
+/*
+ * Class: edu_wpi_first_math_WPIMathJNI
+ * Method: dareDetailABQRN
+ * Signature: ([D[D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
+ (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+ jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
+{
+ JDoubleArrayRef nativeA{env, A};
+ JDoubleArrayRef nativeB{env, B};
+ JDoubleArrayRef nativeQ{env, Q};
+ JDoubleArrayRef nativeR{env, R};
+ JDoubleArrayRef nativeN{env, N};
+
+ Eigen::Map>
+ Amat{nativeA.array().data(), states, states};
+ Eigen::Map>
+ Bmat{nativeB.array().data(), states, inputs};
+ Eigen::Map>
+ Qmat{nativeQ.array().data(), states, states};
+ Eigen::Map>
+ Rmat{nativeR.array().data(), inputs, inputs};
+ Eigen::Map>
+ Nmat{nativeN.array().data(), states, inputs};
+
+ Eigen::MatrixXd Rcopy{Rmat};
+ auto R_llt = Rcopy.llt();
+
+ Eigen::MatrixXd result = frc::detail::DARE(
+ Amat, Bmat, Qmat, R_llt, Nmat);
+
+ env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: dareABQR
@@ -72,33 +150,28 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQR
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jint states, jint inputs, jdoubleArray S)
{
- jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
- jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
- jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
- jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);
+ JDoubleArrayRef nativeA{env, A};
+ JDoubleArrayRef nativeB{env, B};
+ JDoubleArrayRef nativeQ{env, Q};
+ JDoubleArrayRef nativeR{env, R};
- Eigen::Map<
- Eigen::Matrix>
- Amat{nativeA, states, states};
- Eigen::Map<
- Eigen::Matrix>
- Bmat{nativeB, states, inputs};
- Eigen::Map<
- Eigen::Matrix>
- Qmat{nativeQ, states, states};
- Eigen::Map<
- Eigen::Matrix>
- Rmat{nativeR, inputs, inputs};
+ Eigen::Map>
+ Amat{nativeA.array().data(), states, states};
+ Eigen::Map>
+ Bmat{nativeB.array().data(), states, inputs};
+ Eigen::Map>
+ Qmat{nativeQ.array().data(), states, states};
+ Eigen::Map>
+ Rmat{nativeR.array().data(), inputs, inputs};
try {
Eigen::MatrixXd result =
frc::DARE(Amat, Bmat, Qmat, Rmat);
- env->ReleaseDoubleArrayElements(A, nativeA, 0);
- env->ReleaseDoubleArrayElements(B, nativeB, 0);
- env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
- env->ReleaseDoubleArrayElements(R, nativeR, 0);
-
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
} catch (const std::invalid_argument& e) {
jclass cls = env->FindClass("java/lang/IllegalArgumentException");
@@ -118,38 +191,32 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
{
- jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
- jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
- jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
- jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);
- jdouble* nativeN = env->GetDoubleArrayElements(N, nullptr);
+ JDoubleArrayRef nativeA{env, A};
+ JDoubleArrayRef nativeB{env, B};
+ JDoubleArrayRef nativeQ{env, Q};
+ JDoubleArrayRef nativeR{env, R};
+ JDoubleArrayRef nativeN{env, N};
- Eigen::Map<
- Eigen::Matrix>
- Amat{nativeA, states, states};
- Eigen::Map<
- Eigen::Matrix>
- Bmat{nativeB, states, inputs};
- Eigen::Map<
- Eigen::Matrix>
- Qmat{nativeQ, states, states};
- Eigen::Map<
- Eigen::Matrix>
- Rmat{nativeR, inputs, inputs};
- Eigen::Map<
- Eigen::Matrix>
- Nmat{nativeN, states, inputs};
+ Eigen::Map>
+ Amat{nativeA.array().data(), states, states};
+ Eigen::Map>
+ Bmat{nativeB.array().data(), states, inputs};
+ Eigen::Map>
+ Qmat{nativeQ.array().data(), states, states};
+ Eigen::Map>
+ Rmat{nativeR.array().data(), inputs, inputs};
+ Eigen::Map>
+ Nmat{nativeN.array().data(), states, inputs};
try {
Eigen::MatrixXd result =
frc::DARE(Amat, Bmat, Qmat, Rmat, Nmat);
- env->ReleaseDoubleArrayElements(A, nativeA, 0);
- env->ReleaseDoubleArrayElements(B, nativeB, 0);
- env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
- env->ReleaseDoubleArrayElements(R, nativeR, 0);
- env->ReleaseDoubleArrayElements(N, nativeN, 0);
-
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
} catch (const std::invalid_argument& e) {
jclass cls = env->FindClass("java/lang/IllegalArgumentException");
diff --git a/wpimath/src/test/java/edu/wpi/first/math/DARETest.java b/wpimath/src/test/java/edu/wpi/first/math/DARETest.java
index 6dddaefef9..19ba39253e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/DARETest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/DARETest.java
@@ -16,7 +16,8 @@ class DARETest extends UtilityClassTest {
super(DARE.class);
}
- public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
+ public static void assertMatrixEqual(
+ Matrix A, Matrix B) {
for (int i = 0; i < A.getNumRows(); i++) {
for (int j = 0; j < A.getNumCols(); j++) {
assertEquals(A.get(i, j), B.get(i, j), 1e-4);
@@ -24,39 +25,45 @@ class DARETest extends UtilityClassTest {
}
}
- void assertDARESolution(
- SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix X) {
+ void assertDARESolution(
+ Matrix A,
+ Matrix B,
+ Matrix Q,
+ Matrix R,
+ Matrix X) {
// Check that X is the solution to the DARE
// Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
var Y =
- (A.transpose().mult(X).mult(A))
+ (A.transpose().times(X).times(A))
.minus(X)
.minus(
- (A.transpose().mult(X).mult(B))
- .mult((B.transpose().mult(X).mult(B).plus(R)).invert())
- .mult(B.transpose().mult(X).mult(A)))
+ (A.transpose().times(X).times(B))
+ .times((B.transpose().times(X).times(B).plus(R)).inv())
+ .times(B.transpose().times(X).times(A)))
.plus(Q);
- assertMatrixEqual(new SimpleMatrix(Y.getNumRows(), Y.getNumCols()), Y);
+ assertMatrixEqual(
+ new Matrix(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
}
- void assertDARESolution(
- SimpleMatrix A,
- SimpleMatrix B,
- SimpleMatrix Q,
- SimpleMatrix R,
- SimpleMatrix N,
- SimpleMatrix X) {
+ void assertDARESolution(
+ Matrix A,
+ Matrix B,
+ Matrix Q,
+ Matrix R,
+ Matrix N,
+ Matrix X) {
// Check that X is the solution to the DARE
// Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
var Y =
- (A.transpose().mult(X).mult(A))
+ (A.transpose().times(X).times(A))
.minus(X)
.minus(
- (A.transpose().mult(X).mult(B).plus(N))
- .mult((B.transpose().mult(X).mult(B).plus(R)).invert())
- .mult(B.transpose().mult(X).mult(A).plus(N.transpose())))
+ (A.transpose().times(X).times(B).plus(N))
+ .times((B.transpose().times(X).times(B).plus(R)).inv())
+ .times(B.transpose().times(X).times(A).plus(N.transpose())))
.plus(Q);
- assertMatrixEqual(new SimpleMatrix(Y.getNumRows(), Y.getNumCols()), Y);
+ assertMatrixEqual(
+ new Matrix(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
}
@Test
@@ -65,12 +72,13 @@ class DARETest extends UtilityClassTest {
// Riccati Equation"
var A =
- new SimpleMatrix(
- 4, 4, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
- var B = new SimpleMatrix(4, 1, true, new double[] {0, 0, 0, 1});
+ new Matrix<>(
+ Nat.N4(), Nat.N4(), new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
+ var B = new Matrix<>(Nat.N4(), Nat.N1(), new double[] {0, 0, 0, 1});
var Q =
- new SimpleMatrix(4, 4, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
- var R = new SimpleMatrix(1, 1, true, new double[] {0.25});
+ new Matrix<>(
+ Nat.N4(), Nat.N4(), new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
+ var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.25});
var X = DARE.dare(A, B, Q, R);
assertMatrixEqual(X, X.transpose());
@@ -83,19 +91,20 @@ class DARETest extends UtilityClassTest {
// Riccati Equation"
var A =
- new SimpleMatrix(
- 4, 4, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
- var B = new SimpleMatrix(4, 1, true, new double[] {0, 0, 0, 1});
+ new Matrix<>(
+ Nat.N4(), Nat.N4(), new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
+ var B = new Matrix<>(Nat.N4(), Nat.N1(), new double[] {0, 0, 0, 1});
var Q =
- new SimpleMatrix(4, 4, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
- var R = new SimpleMatrix(1, 1, true, new double[] {0.25});
+ new Matrix<>(
+ Nat.N4(), Nat.N4(), new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
+ var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.25});
var Aref =
- new SimpleMatrix(
- 4, 4, true, new double[] {0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
- Q = A.minus(Aref).transpose().mult(Q).mult(A.minus(Aref));
- R = B.transpose().mult(Q).mult(B).plus(R);
- var N = A.minus(Aref).transpose().mult(Q).mult(B);
+ new Matrix<>(
+ Nat.N4(), Nat.N4(), new double[] {0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
+ Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
+ R = B.transpose().times(Q).times(B).plus(R);
+ var N = A.minus(Aref).transpose().times(Q).times(B);
var X = DARE.dare(A, B, Q, R, N);
assertMatrixEqual(X, X.transpose());
@@ -104,10 +113,10 @@ class DARETest extends UtilityClassTest {
@Test
void testInvertibleA_ABQR() {
- var A = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1});
- var B = new SimpleMatrix(2, 1, true, new double[] {0, 1});
- var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0});
- var R = new SimpleMatrix(1, 1, true, new double[] {0.3});
+ var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 1, 0, 1});
+ var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+ var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 0});
+ var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.3});
var X = DARE.dare(A, B, Q, R);
assertMatrixEqual(X, X.transpose());
@@ -116,15 +125,15 @@ class DARETest extends UtilityClassTest {
@Test
void testInvertibleA_ABQRN() {
- var A = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1});
- var B = new SimpleMatrix(2, 1, true, new double[] {0, 1});
- var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0});
- var R = new SimpleMatrix(1, 1, true, new double[] {0.3});
+ var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 1, 0, 1});
+ var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+ var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 0});
+ var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.3});
- var Aref = new SimpleMatrix(2, 2, true, new double[] {0.5, 1, 0, 1});
- Q = A.minus(Aref).transpose().mult(Q).mult(A.minus(Aref));
- R = B.transpose().mult(Q).mult(B).plus(R);
- var N = A.minus(Aref).transpose().mult(Q).mult(B);
+ var Aref = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.5, 1, 0, 1});
+ Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
+ R = B.transpose().times(Q).times(B).plus(R);
+ var N = A.minus(Aref).transpose().times(Q).times(B);
var X = DARE.dare(A, B, Q, R, N);
assertMatrixEqual(X, X.transpose());
@@ -135,10 +144,10 @@ class DARETest extends UtilityClassTest {
void testFirstGeneralizedEigenvalueOfSTIsStable_ABQR() {
// The first generalized eigenvalue of (S, T) is stable
- var A = new SimpleMatrix(2, 2, true, new double[] {0, 1, 0, 0});
- var B = new SimpleMatrix(2, 1, true, new double[] {0, 1});
- var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 1});
- var R = new SimpleMatrix(1, 1, true, new double[] {1});
+ var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 1, 0, 0});
+ var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+ var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 1});
+ var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {1});
var X = DARE.dare(A, B, Q, R);
assertMatrixEqual(X, X.transpose());
@@ -149,15 +158,15 @@ class DARETest extends UtilityClassTest {
void testFirstGeneralizedEigenvalueOfSTIsStable_ABQRN() {
// The first generalized eigenvalue of (S, T) is stable
- var A = new SimpleMatrix(2, 2, true, new double[] {0, 1, 0, 0});
- var B = new SimpleMatrix(2, 1, true, new double[] {0, 1});
- var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 1});
- var R = new SimpleMatrix(1, 1, true, new double[] {1});
+ var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 1, 0, 0});
+ var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+ var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 1});
+ var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {1});
- var Aref = new SimpleMatrix(2, 2, true, new double[] {0, 0.5, 0, 0});
- Q = A.minus(Aref).transpose().mult(Q).mult(A.minus(Aref));
- R = B.transpose().mult(Q).mult(B).plus(R);
- var N = A.minus(Aref).transpose().mult(Q).mult(B);
+ var Aref = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 0.5, 0, 0});
+ Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
+ R = B.transpose().times(Q).times(B).plus(R);
+ var N = A.minus(Aref).transpose().times(Q).times(B);
var X = DARE.dare(A, B, Q, R, N);
assertMatrixEqual(X, X.transpose());
@@ -166,10 +175,10 @@ class DARETest extends UtilityClassTest {
@Test
void testIdentitySystem_ABQR() {
- var A = SimpleMatrix.identity(2);
- var B = SimpleMatrix.identity(2);
- var Q = SimpleMatrix.identity(2);
- var R = SimpleMatrix.identity(2);
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N2());
var X = DARE.dare(A, B, Q, R);
assertMatrixEqual(X, X.transpose());
@@ -178,11 +187,11 @@ class DARETest extends UtilityClassTest {
@Test
void testIdentitySystem_ABQRN() {
- var A = SimpleMatrix.identity(2);
- var B = SimpleMatrix.identity(2);
- var Q = SimpleMatrix.identity(2);
- var R = SimpleMatrix.identity(2);
- var N = SimpleMatrix.identity(2);
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N2());
+ var N = Matrix.eye(Nat.N2());
var X = DARE.dare(A, B, Q, R, N);
assertMatrixEqual(X, X.transpose());
@@ -191,10 +200,10 @@ class DARETest extends UtilityClassTest {
@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 A = Matrix.eye(Nat.N2());
+ var B = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 0.5, 0.3});
+ var Q = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N3());
var X = DARE.dare(A, B, Q, R);
assertMatrixEqual(X, X.transpose());
@@ -203,11 +212,11 @@ class DARETest extends UtilityClassTest {
@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 A = Matrix.eye(Nat.N2());
+ var B = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 0.5, 0.3});
+ var Q = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N3());
+ var N = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 1, 0});
var X = DARE.dare(A, B, Q, R, N);
assertMatrixEqual(X, X.transpose());
@@ -216,90 +225,90 @@ class DARETest extends UtilityClassTest {
@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);
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+ var R = Matrix.eye(Nat.N2());
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);
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+ var R = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+ var N = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {2.0, 0.0, 0.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 A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
- var R1 = new SimpleMatrix(2, 2);
+ var R1 = new Matrix<>(Nat.N2(), Nat.N2());
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1));
- var R2 = SimpleMatrix.diag(-1.0, -1.0);
+ var R2 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.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 A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+ var N = Matrix.eye(Nat.N2());
- var R1 = new SimpleMatrix(2, 2);
+ var R1 = new Matrix<>(Nat.N2(), Nat.N2());
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1, N));
- var R2 = SimpleMatrix.diag(-1.0, -1.0);
+ var R2 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.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);
+ var A = Matrix.eye(Nat.N2());
+ var B = new Matrix<>(Nat.N2(), Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N2());
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);
+ var A = Matrix.eye(Nat.N2());
+ var B = new Matrix<>(Nat.N2(), Nat.N2());
+ var Q = Matrix.eye(Nat.N2());
+ var R = Matrix.eye(Nat.N2());
+ var N = Matrix.eye(Nat.N2());
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);
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = new Matrix<>(Nat.N2(), Nat.N2());
+ var R = Matrix.eye(Nat.N2());
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);
+ var A = Matrix.eye(Nat.N2());
+ var B = Matrix.eye(Nat.N2());
+ var Q = new Matrix<>(Nat.N2(), Nat.N2());
+ var R = Matrix.eye(Nat.N2());
+ var N = new Matrix<>(Nat.N2(), Nat.N2());
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
}