mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-29 02:21:44 +00:00
[wpimath] Make LTV controller constructors use faster DARE solver (#5543)
Made JNI modifications to expose the faster function, made the API use the typesafe Matrix API, and synchronized the documentation with C++. Sped up C++ LTV diff drive test from 20 ms to 15 ms. Sped up C++ LTV unicycle test from 15 ms to 10 ms.
This commit is contained in:
Tyler Veness
@@ -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.
|
||||
*
|
||||
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
*
|
||||
* <p>This internal function skips expensive precondition checks for increased performance. The
|
||||
* solver may hang if any of the following occur:
|
||||
*
|
||||
* <ul>
|
||||
* <li>Q isn't symmetric positive semidefinite
|
||||
* <li>R isn't symmetric positive definite
|
||||
* <li>The (A, B) pair isn't stabilizable
|
||||
* <li>The (A, C) pair where Q = CᵀC isn't detectable
|
||||
* </ul>
|
||||
*
|
||||
* <p>Only use this function if you're sure the preconditions are met.
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> 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 <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
|
||||
Matrix<States, States> A,
|
||||
Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R) {
|
||||
var S = new Matrix<States, States>(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.
|
||||
*
|
||||
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
*
|
||||
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
|
||||
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>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ₖ:
|
||||
*
|
||||
* <pre>
|
||||
* ∞
|
||||
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This can be refactored as:
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
* J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This internal function skips expensive precondition checks for increased performance. The
|
||||
* solver may hang if any of the following occur:
|
||||
*
|
||||
* <ul>
|
||||
* <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
|
||||
* <li>R isn't symmetric positive definite
|
||||
* <li>The (A, B) pair isn't stabilizable
|
||||
* <li>The (A, C) pair where Q = CᵀC isn't detectable
|
||||
* </ul>
|
||||
*
|
||||
* <p>Only use this function if you're sure the preconditions are met.
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> 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 <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
|
||||
Matrix<States, States> A,
|
||||
Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R,
|
||||
Matrix<States, Inputs> N) {
|
||||
var S = new Matrix<States, States>(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.
|
||||
*
|
||||
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> Number of inputs.
|
||||
@@ -57,40 +146,48 @@ public final class DARE {
|
||||
Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> 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<States, States>(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.
|
||||
*
|
||||
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
*
|
||||
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
|
||||
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>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ₖ:
|
||||
*
|
||||
* <pre>
|
||||
* ∞
|
||||
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This can be refactored as:
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
* J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> Number of inputs.
|
||||
@@ -111,7 +208,16 @@ public final class DARE {
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R,
|
||||
Matrix<States, Inputs> N) {
|
||||
return new Matrix<>(
|
||||
dare(A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage(), N.getStorage()));
|
||||
var S = new Matrix<States, States>(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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -38,6 +38,18 @@ public class Matrix<R extends Num, C extends Num> {
|
||||
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<R> rows, Nat<C> 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}.
|
||||
|
||||
@@ -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.
|
||||
*
|
||||
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
|
||||
*
|
||||
* <p>This internal function skips expensive precondition checks for increased performance. The
|
||||
* solver may hang if any of the following occur:
|
||||
*
|
||||
* <ul>
|
||||
* <li>Q isn't symmetric positive semidefinite
|
||||
* <li>R isn't symmetric positive definite
|
||||
* <li>The (A, B) pair isn't stabilizable
|
||||
* <li>The (A, C) pair where Q = CᵀC isn't detectable
|
||||
* </ul>
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
*
|
||||
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
|
||||
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>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ₖ:
|
||||
*
|
||||
* <pre>
|
||||
* ∞
|
||||
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This can be refactored as:
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
* J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This internal function skips expensive precondition checks for increased performance. The
|
||||
* solver may hang if any of the following occur:
|
||||
*
|
||||
* <ul>
|
||||
* <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
|
||||
* <li>R isn't symmetric positive definite
|
||||
* <li>The (A, B) pair isn't stabilizable
|
||||
* <li>The (A, C) pair where Q = CᵀC isn't detectable
|
||||
* </ul>
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
|
||||
*
|
||||
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
|
||||
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q N][xₖ]
|
||||
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>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ₖ:
|
||||
*
|
||||
* <pre>
|
||||
* ∞
|
||||
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* <p>This can be refactored as:
|
||||
*
|
||||
* <pre>
|
||||
* ∞ [xₖ]ᵀ[Q 0][xₖ]
|
||||
* J = Σ [uₖ] [0 R][uₖ] ΔT
|
||||
* k=0
|
||||
* </pre>
|
||||
*
|
||||
* @param A Array containing elements of A in row-major order.
|
||||
* @param B Array containing elements of B in row-major order.
|
||||
|
||||
@@ -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<N5, N2, N5>(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)));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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<N3, N2, N3>(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)));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -7,9 +7,11 @@
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
|
||||
#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));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -6,8 +6,10 @@
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
#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));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -62,6 +62,84 @@ frc::Trajectory CreateTrajectoryFromElements(std::span<const double> 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<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Amat{nativeA.array().data(), states, states};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Bmat{nativeB.array().data(), states, inputs};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Qmat{nativeQ.array().data(), states, states};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Rmat{nativeR.array().data(), inputs, inputs};
|
||||
|
||||
Eigen::MatrixXd RmatCopy{Rmat};
|
||||
auto R_llt = RmatCopy.llt();
|
||||
|
||||
Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
|
||||
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<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Amat{nativeA.array().data(), states, states};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Bmat{nativeB.array().data(), states, inputs};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Qmat{nativeQ.array().data(), states, states};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Rmat{nativeR.array().data(), inputs, inputs};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Nmat{nativeN.array().data(), states, inputs};
|
||||
|
||||
Eigen::MatrixXd Rcopy{Rmat};
|
||||
auto R_llt = Rcopy.llt();
|
||||
|
||||
Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
|
||||
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<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Amat{nativeA, states, states};
|
||||
Eigen::Map<
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Bmat{nativeB, states, inputs};
|
||||
Eigen::Map<
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Qmat{nativeQ, states, states};
|
||||
Eigen::Map<
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Rmat{nativeR, inputs, inputs};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Amat{nativeA.array().data(), states, states};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Bmat{nativeB.array().data(), states, inputs};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Qmat{nativeQ.array().data(), states, states};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Rmat{nativeR.array().data(), inputs, inputs};
|
||||
|
||||
try {
|
||||
Eigen::MatrixXd result =
|
||||
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(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<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Amat{nativeA, states, states};
|
||||
Eigen::Map<
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Bmat{nativeB, states, inputs};
|
||||
Eigen::Map<
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Qmat{nativeQ, states, states};
|
||||
Eigen::Map<
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Rmat{nativeR, inputs, inputs};
|
||||
Eigen::Map<
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
|
||||
Nmat{nativeN, states, inputs};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Amat{nativeA.array().data(), states, states};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Bmat{nativeB.array().data(), states, inputs};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Qmat{nativeQ.array().data(), states, states};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Rmat{nativeR.array().data(), inputs, inputs};
|
||||
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
|
||||
Eigen::RowMajor>>
|
||||
Nmat{nativeN.array().data(), states, inputs};
|
||||
|
||||
try {
|
||||
Eigen::MatrixXd result =
|
||||
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(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");
|
||||
|
||||
@@ -16,7 +16,8 @@ class DARETest extends UtilityClassTest<DARE> {
|
||||
super(DARE.class);
|
||||
}
|
||||
|
||||
public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
|
||||
public static <R extends Num, C extends Num> void assertMatrixEqual(
|
||||
Matrix<R, C> A, Matrix<R, C> 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<DARE> {
|
||||
}
|
||||
}
|
||||
|
||||
void assertDARESolution(
|
||||
SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix X) {
|
||||
<States extends Num, Inputs extends Num> void assertDARESolution(
|
||||
Matrix<States, States> A,
|
||||
Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R,
|
||||
Matrix<States, States> 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<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
|
||||
}
|
||||
|
||||
void assertDARESolution(
|
||||
SimpleMatrix A,
|
||||
SimpleMatrix B,
|
||||
SimpleMatrix Q,
|
||||
SimpleMatrix R,
|
||||
SimpleMatrix N,
|
||||
SimpleMatrix X) {
|
||||
<States extends Num, Inputs extends Num> void assertDARESolution(
|
||||
Matrix<States, States> A,
|
||||
Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q,
|
||||
Matrix<Inputs, Inputs> R,
|
||||
Matrix<States, Inputs> N,
|
||||
Matrix<States, States> 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<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
|
||||
}
|
||||
|
||||
@Test
|
||||
@@ -65,12 +72,13 @@ class DARETest extends UtilityClassTest<DARE> {
|
||||
// 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<DARE> {
|
||||
// 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<DARE> {
|
||||
|
||||
@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<DARE> {
|
||||
|
||||
@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<DARE> {
|
||||
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<DARE> {
|
||||
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<DARE> {
|
||||
|
||||
@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<DARE> {
|
||||
|
||||
@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<DARE> {
|
||||
|
||||
@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<DARE> {
|
||||
|
||||
@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<DARE> {
|
||||
|
||||
@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));
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user