Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] DARE: Use wpi::expected instead of exceptions (#7312)

Browse Source
This commit is contained in:
Tyler Veness
2024-10-31 20:37:57 -07:00
committed by GitHub
parent 21980c7447
commit 9f6f267f5c
19 changed files with 717 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
View File

@@ -5,7 +5,6 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.DARE;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.StateSpaceUtil;
@@ -110,17 +109,6 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
if (!StateSpaceUtil.isStabilizable(discA, discB)) {
var msg =
"The system passed to the LQR is unstabilizable!\n\nA =\n"
+ discA.getStorage().toString()
+ "\nB =\n"
+ discB.getStorage().toString()
+ '\n';
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}
var S = DARE.dare(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA

12
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
View File

@@ -5,7 +5,6 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.DARE;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
@@ -85,17 +84,6 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
var C = plant.getC();
if (!StateSpaceUtil.isDetectable(discA, C)) {
var msg =
"The system passed to the Kalman filter is undetectable!\n\nA =\n"
+ discA.getStorage().toString()
+ "\nC =\n"
+ C.getStorage().toString()
+ '\n';
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}
m_initP = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
reset();

12
wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java
View File

@@ -5,7 +5,6 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.DARE;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
@@ -84,17 +83,6 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
var C = plant.getC();
if (!StateSpaceUtil.isDetectable(discA, C)) {
var msg =
"The system passed to the Kalman filter is undetectable!\n\nA =\n"
+ discA.getStorage().toString()
+ "\nC =\n"
+ C.getStorage().toString()
+ '\n';
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}
var P = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
// S = CPCᵀ + R

2
wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
View File

@@ -97,7 +97,7 @@ LTVDifferentialDriveController::LTVDifferentialDriveController(
Matrixd<5, 2> discB;
DiscretizeAB(A, B, dt, &discA, &discB);
Matrixd<5, 5> S = detail::DARE<5, 2>(discA, discB, Q, R_llt);
auto 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)

2
wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
View File

@@ -103,7 +103,7 @@ LTVUnicycleController::LTVUnicycleController(
Matrixd<3, 2> discB;
DiscretizeAB(A, B, dt, &discA, &discB);
Matrixd<3, 3> S = detail::DARE<3, 2>(discA, discB, Q, R_llt);
auto 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)

62
wpimath/src/main/native/cpp/jni/DAREJNI.cpp
View File

@@ -5,12 +5,14 @@
#include <jni.h>
#include <stdexcept>
#include <string>
#include <wpi/jni_util.h>
#include "Exceptions.h"
#include "edu_wpi_first_math_jni_DAREJNI.h"
#include "frc/DARE.h"
#include "frc/fmt/Eigen.h"
using namespace wpi::java;
@@ -47,8 +49,8 @@ Java_edu_wpi_first_math_jni_DAREJNI_dareDetailABQR
Eigen::MatrixXd RmatCopy{Rmat};
auto R_llt = RmatCopy.llt();
Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
Amat, Bmat, Qmat, R_llt);
auto result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat,
Qmat, R_llt);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
}
@@ -88,7 +90,7 @@ Java_edu_wpi_first_math_jni_DAREJNI_dareDetailABQRN
Eigen::MatrixXd Rcopy{Rmat};
auto R_llt = Rcopy.llt();
Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
auto result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
Amat, Bmat, Qmat, R_llt, Nmat);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
@@ -122,13 +124,24 @@ Java_edu_wpi_first_math_jni_DAREJNI_dareABQR
Eigen::RowMajor>>
Rmat{nativeR.data(), inputs, inputs};
try {
Eigen::MatrixXd result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
} catch (const std::invalid_argument& e) {
illegalArgEx.Throw(env, e.what());
if (auto result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat)) {
env->SetDoubleArrayRegion(S, 0, states * states, result.value().data());
// K = (BᵀSB + R)⁻¹BᵀSA
} else if (result.error() == frc::DAREError::QNotSymmetric ||
result.error() == frc::DAREError::QNotPositiveSemidefinite) {
illegalArgEx.Throw(
env, fmt::format("{}\n\nQ =\n{}\n", to_string(result.error()), Qmat));
} else if (result.error() == frc::DAREError::RNotSymmetric ||
result.error() == frc::DAREError::RNotPositiveDefinite) {
illegalArgEx.Throw(
env, fmt::format("{}\n\nR =\n{}\n", to_string(result.error()), Rmat));
} else if (result.error() == frc::DAREError::ABNotStabilizable) {
illegalArgEx.Throw(env, fmt::format("{}\n\nA =\n{}\nB =\n{}\n",
to_string(result.error()), Amat, Bmat));
} else if (result.error() == frc::DAREError::ACNotDetectable) {
illegalArgEx.Throw(env, fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
to_string(result.error()), Amat, Qmat));
}
}
@@ -164,13 +177,28 @@ Java_edu_wpi_first_math_jni_DAREJNI_dareABQRN
Eigen::RowMajor>>
Nmat{nativeN.data(), states, inputs};
try {
Eigen::MatrixXd result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, Nmat);
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
} catch (const std::invalid_argument& e) {
illegalArgEx.Throw(env, e.what());
if (auto result = frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat,
Rmat, Nmat)) {
env->SetDoubleArrayRegion(S, 0, states * states, result.value().data());
} else if (result.error() == frc::DAREError::QNotSymmetric ||
result.error() == frc::DAREError::QNotPositiveSemidefinite) {
illegalArgEx.Throw(
env, fmt::format("{}\n\nQ =\n{}\n", to_string(result.error()), Qmat));
} else if (result.error() == frc::DAREError::RNotSymmetric ||
result.error() == frc::DAREError::RNotPositiveDefinite) {
illegalArgEx.Throw(
env, fmt::format("{}\n\nR =\n{}\n", to_string(result.error()), Rmat));
} else if (result.error() == frc::DAREError::ABNotStabilizable) {
illegalArgEx.Throw(
env,
fmt::format("{}\n\nA =\n{}\nB =\n{}\n", to_string(result.error()),
Amat - Bmat * Rmat.llt().solve(Nmat.transpose()), Bmat));
} else if (result.error() == frc::DAREError::ACNotDetectable) {
auto R_llt = Rmat.llt();
illegalArgEx.Throw(
env, fmt::format("{}\n\nA =\n{}\nQ =\n{}\n", to_string(result.error()),
Amat - Bmat * R_llt.solve(Nmat.transpose()),
Qmat - Nmat * R_llt.solve(Nmat.transpose())));
}
}

264
wpimath/src/main/native/include/frc/DARE.h
View File

@@ -4,94 +4,59 @@
#pragma once
#include <stdexcept>
#include <string>
#include <string_view>
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/LU>
#include <wpi/expected>
#include "frc/StateSpaceUtil.h"
#include "frc/fmt/Eigen.h"
// Works cited:
//
// [1] E. K.-W. Chu, H.-Y. Fan, W.-W. Lin & C.-S. Wang "Structure-Preserving
// Algorithms for Periodic Discrete-Time Algebraic Riccati Equations",
// International Journal of Control, 77:8, 767-788, 2004.
// DOI: 10.1080/00207170410001714988
namespace frc {
namespace detail {
/**
* Errors the DARE solver can encounter.
*/
enum class DAREError {
/// Q was not symmetric.
QNotSymmetric,
/// Q was not positive semidefinite.
QNotPositiveSemidefinite,
/// R was not symmetric.
RNotSymmetric,
/// R was not positive definite.
RNotPositiveDefinite,
/// (A, B) pair was not stabilizable.
ABNotStabilizable,
/// (A, C) pair where Q = CᵀC was not detectable.
ACNotDetectable,
};
/**
* Checks the preconditions of A, B, and Q for the DARE solver.
*
* @tparam States Number of states.
* @tparam Inputs Number of inputs.
* @param A The system matrix.
* @param B The input matrix.
* @param Q The state cost matrix.
* @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
* @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
* @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't
* detectable.
* Converts the given DAREError enum to a string.
*/
template <int States, int Inputs>
void CheckDARE_ABQ(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q) {
// Require Q be symmetric
if ((Q - Q.transpose()).norm() > 1e-10) {
std::string msg = fmt::format("Q isn't symmetric!\n\nQ =\n{}\n", Q);
throw std::invalid_argument(msg);
constexpr std::string_view to_string(const DAREError& error) {
switch (error) {
case DAREError::QNotSymmetric:
return "Q was not symmetric.";
case DAREError::QNotPositiveSemidefinite:
return "Q was not positive semidefinite.";
case DAREError::RNotSymmetric:
return "R was not symmetric.";
case DAREError::RNotPositiveDefinite:
return "R was not positive definite.";
case DAREError::ABNotStabilizable:
return "(A, B) pair was not stabilizable.";
case DAREError::ACNotDetectable:
return "(A, C) pair where Q = CᵀC was not detectable.";
}
// Require Q be positive semidefinite
//
// If Q is a symmetric matrix with a decomposition LDLᵀ, the number of
// positive, negative, and zero diagonal entries in D equals the number of
// positive, negative, and zero eigenvalues respectively in Q (see
// https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
//
// Therefore, D having no negative diagonal entries is sufficient to prove Q
// is positive semidefinite.
auto Q_ldlt = Q.ldlt();
if (Q_ldlt.info() != Eigen::Success ||
(Q_ldlt.vectorD().array() < 0.0).any()) {
std::string msg =
fmt::format("Q isn't positive semidefinite!\n\nQ =\n{}\n", Q);
throw std::invalid_argument(msg);
}
// Require (A, B) pair be stabilizable
if (!IsStabilizable<States, Inputs>(A, B)) {
std::string msg = fmt::format(
"The (A, B) pair isn't stabilizable!\n\nA =\n{}\nB =\n{}\n", A, B);
throw std::invalid_argument(msg);
}
// Require (A, C) pair be detectable where Q = CᵀC
//
// Q = CᵀC = PᵀLDLᵀP
// C = √(D)LᵀP
{
Eigen::Matrix<double, States, States> C =
Q_ldlt.vectorD().cwiseSqrt().asDiagonal() *
Eigen::Matrix<double, States, States>{Q_ldlt.matrixL().transpose()} *
Q_ldlt.transpositionsP();
if (!IsDetectable<States, States>(A, C)) {
std::string msg = fmt::format(
"The (A, C) pair where Q = CᵀC isn't detectable!\n\nA =\n{}\nQ "
"=\n{}\n",
A, Q);
throw std::invalid_argument(msg);
}
}
return "";
}
namespace detail {
/**
* Computes the unique stabilizing solution X to the discrete-time algebraic
* Riccati equation:
@@ -114,6 +79,7 @@ void CheckDARE_ABQ(const Eigen::Matrix<double, States, States>& A,
* @param B The input matrix.
* @param Q The state cost matrix.
* @param R_llt The LLT decomposition of the input cost matrix.
* @return Solution to the DARE.
*/
template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
@@ -123,19 +89,18 @@ Eigen::Matrix<double, States, States> DARE(
const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt) {
using StateMatrix = Eigen::Matrix<double, States, States>;
// Implements the SDA algorithm on page 5 of [1].
// Implements SDA algorithm on p. 5 of [1] (initial A, G, H are from (4)).
//
// [1] E. K.-W. Chu, H.-Y. Fan, W.-W. Lin & C.-S. Wang "Structure-Preserving
// Algorithms for Periodic Discrete-Time Algebraic Riccati Equations",
// International Journal of Control, 77:8, 767-788, 2004.
// DOI: 10.1080/00207170410001714988
// A₀ = A
StateMatrix A_k = A;
// G₀ = BR⁻¹Bᵀ
//
// See equation (4) of [1].
StateMatrix G_k = B * R_llt.solve(B.transpose());
// H₀ = Q
//
// See equation (4) of [1].
StateMatrix A_k = A;
StateMatrix G_k = B * R_llt.solve(B.transpose());
StateMatrix H_k;
StateMatrix H_k1 = Q;
@@ -166,12 +131,10 @@ Eigen::Matrix<double, States, States> DARE(
StateMatrix V_2 = W_solver.solve(G_k.transpose()).transpose();
// Gₖ₊₁ = Gₖ + AₖV₂Aₖᵀ
G_k += A_k * V_2 * A_k.transpose();
// Hₖ₊₁ = Hₖ + V₁ᵀHₖAₖ
H_k1 = H_k + V_1.transpose() * H_k * A_k;
// Aₖ₊₁ = AₖV₁
G_k += A_k * V_2 * A_k.transpose();
H_k1 = H_k + V_1.transpose() * H_k * A_k;
A_k *= V_1;
// while |Hₖ₊₁ − Hₖ| > ε |Hₖ₊₁|
@@ -238,6 +201,7 @@ Only use this function if you're sure the preconditions are met.
@param Q The state cost matrix.
@param R_llt The LLT decomposition of the input cost matrix.
@param N The state-input cross cost matrix.
@return Solution to the DARE.
*/
template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
@@ -276,32 +240,69 @@ Eigen::Matrix<double, States, States> DARE(
* @param B The input matrix.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
* @throws std::invalid_argument if R isn't symmetric positive definite.
* @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
* @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't
* detectable.
* @param checkPreconditions Whether to check preconditions (30% less time if
* user is sure precondtions are already met).
* @return Solution to the DARE on success, or DAREError on failure.
*/
template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
wpi::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R) {
// Require R be symmetric
if ((R - R.transpose()).norm() > 1e-10) {
std::string msg = fmt::format("R isn't symmetric!\n\nR =\n{}\n", R);
throw std::invalid_argument(msg);
const Eigen::Matrix<double, Inputs, Inputs>& R,
bool checkPreconditions = true) {
if (checkPreconditions) {
// Require R be symmetric
if ((R - R.transpose()).norm() > 1e-10) {
return wpi::unexpected{DAREError::RNotSymmetric};
}
}
// Require R be positive definite
auto R_llt = R.llt();
if (R_llt.info() != Eigen::Success) {
std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n", R);
throw std::invalid_argument(msg);
return wpi::unexpected{DAREError::RNotPositiveDefinite};
}
detail::CheckDARE_ABQ<States, Inputs>(A, B, Q);
if (checkPreconditions) {
// Require Q be symmetric
if ((Q - Q.transpose()).norm() > 1e-10) {
return wpi::unexpected{DAREError::QNotSymmetric};
}
// Require Q be positive semidefinite
//
// If Q is a symmetric matrix with a decomposition LDLᵀ, the number of
// positive, negative, and zero diagonal entries in D equals the number of
// positive, negative, and zero eigenvalues respectively in Q (see
// https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
//
// Therefore, D having no negative diagonal entries is sufficient to prove Q
// is positive semidefinite.
auto Q_ldlt = Q.ldlt();
if (Q_ldlt.info() != Eigen::Success ||
(Q_ldlt.vectorD().array() < 0.0).any()) {
return wpi::unexpected{DAREError::QNotPositiveSemidefinite};
}
// Require (A, B) pair be stabilizable
if (!IsStabilizable<States, Inputs>(A, B)) {
return wpi::unexpected{DAREError::ABNotStabilizable};
}
// Require (A, C) pair be detectable where Q = CᵀC
//
// Q = CᵀC = PᵀLDLᵀP
// C = √(D)LᵀP
Eigen::Matrix<double, States, States> C =
Q_ldlt.vectorD().cwiseSqrt().asDiagonal() *
Eigen::Matrix<double, States, States>{Q_ldlt.matrixL().transpose()} *
Q_ldlt.transpositionsP();
if (!IsDetectable<States, States>(A, C)) {
return wpi::unexpected{DAREError::ACNotDetectable};
}
}
return detail::DARE<States, Inputs>(A, B, Q, R_llt);
}
@@ -354,30 +355,29 @@ J = Σ [uₖ] [0 R][uₖ] ΔT
@param Q The state cost matrix.
@param R The input cost matrix.
@param N The state-input cross cost matrix.
@throws std::invalid_argument if Q₂ isn't symmetric positive semidefinite.
@throws std::invalid_argument if R isn't symmetric positive definite.
@throws std::invalid_argument if the (A₂, B) pair isn't stabilizable.
@throws std::invalid_argument if the (A₂, C) pair where Q₂ = CᵀC isn't
detectable.
@param checkPreconditions Whether to check preconditions (30% less time if user
is sure precondtions are already met).
@return Solution to the DARE on success, or DAREError on failure.
*/
template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
wpi::expected<Eigen::Matrix<double, States, States>, DAREError> DARE(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
const Eigen::Matrix<double, States, Inputs>& N) {
// Require R be symmetric
if ((R - R.transpose()).norm() > 1e-10) {
std::string msg = fmt::format("R isn't symmetric!\n\nR =\n{}\n", R);
throw std::invalid_argument(msg);
const Eigen::Matrix<double, States, Inputs>& N,
bool checkPreconditions = true) {
if (checkPreconditions) {
// Require R be symmetric
if ((R - R.transpose()).norm() > 1e-10) {
return wpi::unexpected{DAREError::RNotSymmetric};
}
}
// Require R be positive definite
auto R_llt = R.llt();
if (R_llt.info() != Eigen::Success) {
std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n", R);
throw std::invalid_argument(msg);
return wpi::unexpected{DAREError::RNotPositiveDefinite};
}
// This is a change of variables to make the DARE that includes Q, R, and N
@@ -396,7 +396,45 @@ Eigen::Matrix<double, States, States> DARE(
Eigen::Matrix<double, States, States> Q_2 =
Q - N * R_llt.solve(N.transpose());
detail::CheckDARE_ABQ<States, Inputs>(A_2, B, Q_2);
if (checkPreconditions) {
// Require Q be symmetric
if ((Q_2 - Q_2.transpose()).norm() > 1e-10) {
return wpi::unexpected{DAREError::QNotSymmetric};
}
// Require Q be positive semidefinite
//
// If Q is a symmetric matrix with a decomposition LDLᵀ, the number of
// positive, negative, and zero diagonal entries in D equals the number of
// positive, negative, and zero eigenvalues respectively in Q (see
// https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
//
// Therefore, D having no negative diagonal entries is sufficient to prove Q
// is positive semidefinite.
auto Q_ldlt = Q_2.ldlt();
if (Q_ldlt.info() != Eigen::Success ||
(Q_ldlt.vectorD().array() < 0.0).any()) {
return wpi::unexpected{DAREError::QNotPositiveSemidefinite};
}
// Require (A, B) pair be stabilizable
if (!IsStabilizable<States, Inputs>(A_2, B)) {
return wpi::unexpected{DAREError::ABNotStabilizable};
}
// Require (A, C) pair be detectable where Q = CᵀC
//
// Q = CᵀC = PᵀLDLᵀP
// C = √(D)LᵀP
Eigen::Matrix<double, States, States> C =
Q_ldlt.vectorD().cwiseSqrt().asDiagonal() *
Eigen::Matrix<double, States, States>{Q_ldlt.matrixL().transpose()} *
Q_ldlt.transpositionsP();
if (!IsDetectable<States, States>(A_2, C)) {
return wpi::unexpected{DAREError::ACNotDetectable};
}
}
return detail::DARE<States, Inputs>(A_2, B, Q_2, R_llt);
}

76
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
View File

@@ -103,23 +103,37 @@ class LinearQuadraticRegulator {
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
if (!IsStabilizable<States, Inputs>(discA, discB)) {
std::string msg = fmt::format(
"The system passed to the LQR is unstabilizable!\n\nA =\n{}\nB "
"=\n{}\n",
discA, discB);
if (auto S = DARE<States, Inputs>(discA, discB, Q, R)) {
// K = (BᵀSB + R)⁻¹BᵀSA
m_K = (discB.transpose() * S.value() * discB + R)
.llt()
.solve(discB.transpose() * S.value() * discA);
} else if (S.error() == DAREError::QNotSymmetric ||
S.error() == DAREError::QNotPositiveSemidefinite) {
std::string msg = fmt::format("{}\n\nQ =\n{}\n", to_string(S.error()), Q);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (S.error() == DAREError::RNotSymmetric ||
S.error() == DAREError::RNotPositiveDefinite) {
std::string msg = fmt::format("{}\n\nR =\n{}\n", to_string(S.error()), R);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (S.error() == DAREError::ABNotStabilizable) {
std::string msg = fmt::format("{}\n\nA =\n{}\nB =\n{}\n",
to_string(S.error()), discA, discB);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (S.error() == DAREError::ACNotDetectable) {
std::string msg = fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
to_string(S.error()), discA, Q);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
m_K = (discB.transpose() * S * discB + R)
.llt()
.solve(discB.transpose() * S * discA);
Reset();
}
@@ -144,12 +158,40 @@ class LinearQuadraticRegulator {
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R, N);
if (auto S = DARE<States, Inputs>(discA, discB, Q, R, N)) {
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
m_K = (discB.transpose() * S.value() * discB + R)
.llt()
.solve(discB.transpose() * S.value() * discA + N.transpose());
} else if (S.error() == DAREError::QNotSymmetric ||
S.error() == DAREError::QNotPositiveSemidefinite) {
std::string msg = fmt::format("{}\n\nQ =\n{}\n", to_string(S.error()), Q);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
m_K = (discB.transpose() * S * discB + R)
.llt()
.solve(discB.transpose() * S * discA + N.transpose());
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (S.error() == DAREError::RNotSymmetric ||
S.error() == DAREError::RNotPositiveDefinite) {
std::string msg = fmt::format("{}\n\nR =\n{}\n", to_string(S.error()), R);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (S.error() == DAREError::ABNotStabilizable) {
std::string msg =
fmt::format("{}\n\nA =\n{}\nB =\n{}\n", to_string(S.error()),
discA - discB * R.llt().solve(N.transpose()), discB);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (S.error() == DAREError::ACNotDetectable) {
auto R_llt = R.llt();
std::string msg =
fmt::format("{}\n\nA =\n{}\nQ =\n{}\n", to_string(S.error()),
discA - discB * R_llt.solve(N.transpose()),
Q - N * R_llt.solve(N.transpose()));
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
Reset();
}

68
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -5,6 +5,7 @@
#pragma once
#include <functional>
#include <string>
#include <utility>
#include <Eigen/Cholesky>
@@ -13,6 +14,7 @@
#include "frc/DARE.h"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/fmt/Eigen.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
@@ -100,8 +102,37 @@ class ExtendedKalmanFilter {
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
if (auto P = DARE<States, Outputs>(discA.transpose(), C.transpose(),
discQ, discR)) {
m_initP = P.value();
} else if (P.error() == DAREError::QNotSymmetric ||
P.error() == DAREError::QNotPositiveSemidefinite) {
std::string msg =
fmt::format("{}\n\nQ =\n{}\n", to_string(P.error()), discQ);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (P.error() == DAREError::RNotSymmetric ||
P.error() == DAREError::RNotPositiveDefinite) {
std::string msg =
fmt::format("{}\n\nR =\n{}\n", to_string(P.error()), discR);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (P.error() == DAREError::ABNotStabilizable) {
std::string msg = fmt::format(
"The (A, C) pair is not detectable.\n\nA =\n{}\nC =\n{}\n",
to_string(P.error()), discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (P.error() == DAREError::ACNotDetectable) {
std::string msg = fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
to_string(P.error()), discA, discQ);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
} else {
m_initP = StateMatrix::Zero();
}
@@ -155,8 +186,37 @@ class ExtendedKalmanFilter {
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
if (auto P = DARE<States, Outputs>(discA.transpose(), C.transpose(),
discQ, discR)) {
m_initP = P.value();
} else if (P.error() == DAREError::QNotSymmetric ||
P.error() == DAREError::QNotPositiveSemidefinite) {
std::string msg =
fmt::format("{}\n\nQ =\n{}\n", to_string(P.error()), discQ);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (P.error() == DAREError::RNotSymmetric ||
P.error() == DAREError::RNotPositiveDefinite) {
std::string msg =
fmt::format("{}\n\nR =\n{}\n", to_string(P.error()), discR);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (P.error() == DAREError::ABNotStabilizable) {
std::string msg = fmt::format(
"The (A, C) pair is not detectable.\n\nA =\n{}\nC =\n{}\n",
to_string(P.error()), discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (P.error() == DAREError::ACNotDetectable) {
std::string msg = fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
to_string(P.error()), discA, discQ);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
} else {
m_initP = StateMatrix::Zero();
}

33
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
View File

@@ -85,19 +85,38 @@ class KalmanFilter {
const auto& C = plant.C();
if (!IsDetectable<States, Outputs>(discA, C)) {
if (auto P = DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ,
discR)) {
m_initP = P.value();
} else if (P.error() == DAREError::QNotSymmetric ||
P.error() == DAREError::QNotPositiveSemidefinite) {
std::string msg =
fmt::format("{}\n\nQ =\n{}\n", to_string(P.error()), discQ);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (P.error() == DAREError::RNotSymmetric ||
P.error() == DAREError::RNotPositiveDefinite) {
std::string msg =
fmt::format("{}\n\nR =\n{}\n", to_string(P.error()), discR);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (P.error() == DAREError::ABNotStabilizable) {
std::string msg = fmt::format(
"The system passed to the Kalman filter is undetectable!\n\n"
"A =\n{}\nC =\n{}\n",
discA, C);
"The (A, C) pair is not detectable.\n\nA =\n{}\nC =\n{}\n",
to_string(P.error()), discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (P.error() == DAREError::ACNotDetectable) {
std::string msg = fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
to_string(P.error()), discA, discQ);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
Reset();
}

61
wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h
View File

@@ -97,25 +97,52 @@ class SteadyStateKalmanFilter {
throw std::invalid_argument(msg);
}
Matrixd<States, States> P =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
if (auto P = DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ,
discR)) {
// S = CPCᵀ + R
Matrixd<Outputs, Outputs> S = C * P.value() * C.transpose() + discR;
// S = CPCᵀ + R
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K = S.transpose().ldlt().solve(C * P.value().transpose()).transpose();
} else if (P.error() == DAREError::QNotSymmetric ||
P.error() == DAREError::QNotPositiveSemidefinite) {
std::string msg =
fmt::format("{}\n\nQ =\n{}\n", to_string(P.error()), discQ);
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (P.error() == DAREError::RNotSymmetric ||
P.error() == DAREError::RNotPositiveDefinite) {
std::string msg =
fmt::format("{}\n\nR =\n{}\n", to_string(P.error()), discR);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (P.error() == DAREError::ABNotStabilizable) {
std::string msg = fmt::format(
"The (A, C) pair is not detectable.\n\nA =\n{}\nC =\n{}\n",
to_string(P.error()), discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
} else if (P.error() == DAREError::ACNotDetectable) {
std::string msg = fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
to_string(P.error()), discA, discQ);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
Reset();
}

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

@@ -223,7 +223,17 @@ class DARETest extends UtilityClassTest<DARE> {
}
@Test
void testQNotSymmetricPositiveSemidefinite_ABQR() {
void testQNotSymmetric_ABQR() {
var A = Matrix.eye(Nat.N2());
var B = Matrix.eye(Nat.N2());
var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1.0, 1.0, 0.0, 1.0});
var R = Matrix.eye(Nat.N2());
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
}
@Test
void testQNotPositiveSemidefinite_ABQR() {
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});
@@ -233,18 +243,39 @@ class DARETest extends UtilityClassTest<DARE> {
}
@Test
void testQNotSymmetricPositiveSemidefinite_ABQRN() {
void testQNotSymmetric_ABQRN() {
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 Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1.0, 1.0, 0.0, 1.0});
var R = Matrix.eye(Nat.N2());
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() {
void testQNotPositiveSemidefinite_ABQRN() {
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 = 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 testRNotSymmetric_ABQR() {
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, 1.0, 0.0, 1.0});
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
}
@Test
void testRNotPositiveDefinite_ABQR() {
var A = Matrix.eye(Nat.N2());
var B = Matrix.eye(Nat.N2());
var Q = Matrix.eye(Nat.N2());
@@ -257,7 +288,18 @@ class DARETest extends UtilityClassTest<DARE> {
}
@Test
void testRNotSymmetricPositiveDefinite_ABQRN() {
void testRNotSymmetric_ABQRN() {
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 R = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1.0, 1.0, 0.0, 1.0});
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
}
@Test
void testRNotPositiveDefinite_ABQRN() {
var A = Matrix.eye(Nat.N2());
var B = Matrix.eye(Nat.N2());
var Q = Matrix.eye(Nat.N2());

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

@@ -5,48 +5,127 @@
#include <stdexcept>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <gtest/gtest.h>
#include <wpi/expected>
#include <wpi/print.h>
#include "DARETestUtil.h"
#include "frc/DARE.h"
#include "frc/EigenCore.h"
#include "frc/fmt/Eigen.h"
// 2x1
extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R);
extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 2, 1>& N);
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R, bool checkPreconditions);
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 2, 1>& N, bool checkPreconditions);
// 4x1
extern template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R);
extern template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 4, 1>& N);
extern template wpi::expected<Eigen::Matrix<double, 4, 4>, frc::DAREError>
frc::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q,
const Eigen::Matrix<double, 1, 1>& R, bool checkPreconditions);
extern template wpi::expected<Eigen::Matrix<double, 4, 4>, frc::DAREError>
frc::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q,
const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 4, 1>& N, bool checkPreconditions);
// 2x2
extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R);
extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
const Eigen::Matrix<double, 2, 2>& N);
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R, bool checkPreconditions);
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R,
const Eigen::Matrix<double, 2, 2>& N, bool checkPreconditions);
// 2x3
extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R);
extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R,
const Eigen::Matrix<double, 2, 3>& N);
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 3, 3>& R, bool checkPreconditions);
extern template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 3, 3>& R,
const Eigen::Matrix<double, 2, 3>& N, bool checkPreconditions);
void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
double tolerance) {
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
EXPECT_NEAR(lhs(row, col), rhs(row, col), tolerance)
<< fmt::format("row = {}, col = {}", row, col);
}
}
if (::testing::Test::HasFailure()) {
wpi::print("lhs =\n{}\n", lhs);
wpi::print("rhs =\n{}\n", rhs);
wpi::print("delta =\n{}\n", Eigen::MatrixXd{lhs - rhs});
}
}
void ExpectPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& X) {
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigX{X,
Eigen::EigenvaluesOnly};
for (int i = 0; i < X.rows(); ++i) {
EXPECT_GE(eigX.eigenvalues()[i], 0.0);
}
}
void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& X) {
// Check that X is the solution to the DARE
// Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
// clang-format off
Eigen::MatrixXd Y =
A.transpose() * X * A
- X
- (A.transpose() * X * B * (B.transpose() * X * B + R).inverse()
* B.transpose() * X * A)
+ Q;
// clang-format on
ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
}
void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& N,
const Eigen::Ref<const Eigen::MatrixXd>& 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
// clang-format off
Eigen::MatrixXd Y =
A.transpose() * X * A
- X
- ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse()
* (B.transpose() * X * A + N.transpose()))
+ Q;
// clang-format on
ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
}
TEST(DARETest, NonInvertibleA_ABQR) {
// Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
@@ -58,7 +137,10 @@ TEST(DARETest, NonInvertibleA_ABQR) {
frc::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
frc::Matrixd<1, 1> R{0.25};
frc::Matrixd<4, 4> X = frc::DARE<4, 1>(A, B, Q, R);
auto ret = frc::DARE<4, 1>(A, B, Q, R);
EXPECT_TRUE(ret);
auto X = ret.value();
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, X);
@@ -80,7 +162,10 @@ TEST(DARETest, NonInvertibleA_ABQRN) {
R = B.transpose() * Q * B + R;
frc::Matrixd<4, 1> N = (A - Aref).transpose() * Q * B;
frc::Matrixd<4, 4> X = frc::DARE<4, 1>(A, B, Q, R, N);
auto ret = frc::DARE<4, 1>(A, B, Q, R, N);
EXPECT_TRUE(ret);
auto X = ret.value();
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, N, X);
@@ -92,7 +177,10 @@ TEST(DARETest, InvertibleA_ABQR) {
frc::Matrixd<2, 2> Q{{1, 0}, {0, 0}};
frc::Matrixd<1, 1> R{{0.3}};
frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R);
auto ret = frc::DARE<2, 1>(A, B, Q, R);
EXPECT_TRUE(ret);
auto X = ret.value();
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, X);
@@ -109,7 +197,10 @@ TEST(DARETest, InvertibleA_ABQRN) {
R = B.transpose() * Q * B + R;
frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R, N);
auto ret = frc::DARE<2, 1>(A, B, Q, R, N);
EXPECT_TRUE(ret);
auto X = ret.value();
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, N, X);
@@ -123,7 +214,10 @@ TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) {
frc::Matrixd<2, 2> Q{{1, 0}, {0, 1}};
frc::Matrixd<1, 1> R{1};
frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R);
auto ret = frc::DARE<2, 1>(A, B, Q, R);
EXPECT_TRUE(ret);
auto X = ret.value();
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, X);
@@ -142,7 +236,10 @@ TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQRN) {
R = B.transpose() * Q * B + R;
frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R, N);
auto ret = frc::DARE<2, 1>(A, B, Q, R, N);
EXPECT_TRUE(ret);
auto X = ret.value();
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, N, X);
@@ -154,7 +251,10 @@ TEST(DARETest, IdentitySystem_ABQR) {
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
Eigen::Matrix2d X = frc::DARE<2, 2>(A, B, Q, R);
auto ret = frc::DARE<2, 2>(A, B, Q, R);
EXPECT_TRUE(ret);
auto X = ret.value();
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, X);
@@ -167,7 +267,10 @@ TEST(DARETest, IdentitySystem_ABQRN) {
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
Eigen::Matrix2d X = frc::DARE<2, 2>(A, B, Q, R, N);
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
EXPECT_TRUE(ret);
auto X = ret.value();
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, N, X);
@@ -179,7 +282,10 @@ TEST(DARETest, MoreInputsThanStates_ABQR) {
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()};
Eigen::Matrix2d X = frc::DARE<2, 3>(A, B, Q, R);
auto ret = frc::DARE<2, 3>(A, B, Q, R);
EXPECT_TRUE(ret);
auto X = ret.value();
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, X);
@@ -192,54 +298,115 @@ TEST(DARETest, MoreInputsThanStates_ABQRN) {
const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()};
const frc::Matrixd<2, 3> N{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}};
Eigen::Matrix2d X = frc::DARE<2, 3>(A, B, Q, R, N);
auto ret = frc::DARE<2, 3>(A, B, Q, R, N);
EXPECT_TRUE(ret);
auto X = ret.value();
ExpectMatrixEqual(X, X.transpose(), 1e-10);
ExpectPositiveSemidefinite(X);
ExpectDARESolution(A, B, Q, R, N, X);
}
TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQR) {
TEST(DARETest, QNotSymmetric_ABQR) {
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d Q{{1.0, 1.0}, {0.0, 1.0}};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
auto ret = frc::DARE<2, 2>(A, B, Q, R);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::QNotSymmetric);
}
TEST(DARETest, QNotPositiveSemidefinite_ABQR) {
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d Q{-Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
auto ret = frc::DARE<2, 2>(A, B, Q, R);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::QNotPositiveSemidefinite);
}
TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQRN) {
TEST(DARETest, QNotSymmetric_ABQRN) {
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d Q{{1.0, 1.0}, {0.0, 1.0}};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d N{2.0 * Eigen::Matrix2d::Identity()};
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::QNotSymmetric);
}
TEST(DARETest, QNotPositiveSemidefinite_ABQRN) {
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d N{2.0 * Eigen::Matrix2d::Identity()};
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::QNotPositiveSemidefinite);
}
TEST(DARETest, RNotSymmetricPositiveDefinite_ABQR) {
TEST(DARETest, RNotSymmetric_ABQR) {
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R{{1.0, 1.0}, {0.0, 1.0}};
auto ret = frc::DARE<2, 2>(A, B, Q, R);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::RNotSymmetric);
}
TEST(DARETest, RNotPositiveDefinite_ABQR) {
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()};
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1)), std::invalid_argument);
auto ret1 = frc::DARE<2, 2>(A, B, Q, R1);
EXPECT_FALSE(ret1);
EXPECT_EQ(ret1.error(), frc::DAREError::RNotPositiveDefinite);
const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()};
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2)), std::invalid_argument);
auto ret2 = frc::DARE<2, 2>(A, B, Q, R2);
EXPECT_FALSE(ret2);
EXPECT_EQ(ret2.error(), frc::DAREError::RNotPositiveDefinite);
}
TEST(DARETest, RNotSymmetricPositiveDefinite_ABQRN) {
TEST(DARETest, RNotSymmetric_ABQRN) {
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R{{1.0, 1.0}, {0.0, 1.0}};
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::RNotSymmetric);
}
TEST(DARETest, RNotPositiveDefinite_ABQRN) {
const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()};
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1, N)), std::invalid_argument);
auto ret1 = frc::DARE<2, 2>(A, B, Q, R1, N);
EXPECT_FALSE(ret1);
EXPECT_EQ(ret1.error(), frc::DAREError::RNotPositiveDefinite);
const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()};
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2, N)), std::invalid_argument);
auto ret2 = frc::DARE<2, 2>(A, B, Q, R2, N);
EXPECT_FALSE(ret2);
EXPECT_EQ(ret2.error(), frc::DAREError::RNotPositiveDefinite);
}
TEST(DARETest, ABNotStabilizable_ABQR) {
@@ -248,7 +415,9 @@ TEST(DARETest, ABNotStabilizable_ABQR) {
const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
auto ret = frc::DARE<2, 2>(A, B, Q, R);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::ABNotStabilizable);
}
TEST(DARETest, ABNotStabilizable_ABQRN) {
@@ -258,7 +427,9 @@ TEST(DARETest, ABNotStabilizable_ABQRN) {
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::ABNotStabilizable);
}
TEST(DARETest, ACNotDetectable_ABQR) {
@@ -267,7 +438,9 @@ TEST(DARETest, ACNotDetectable_ABQR) {
const Eigen::Matrix2d Q{Eigen::Matrix2d::Zero()};
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
auto ret = frc::DARE<2, 2>(A, B, Q, R);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::ACNotDetectable);
}
TEST(DARETest, ACNotDetectable_ABQRN) {
@@ -277,7 +450,9 @@ TEST(DARETest, ACNotDetectable_ABQRN) {
const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
const Eigen::Matrix2d N{Eigen::Matrix2d::Zero()};
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
auto ret = frc::DARE<2, 2>(A, B, Q, R, N);
EXPECT_FALSE(ret);
EXPECT_EQ(ret.error(), frc::DAREError::ACNotDetectable);
}
TEST(DARETest, QDecomposition) {
@@ -290,10 +465,12 @@ TEST(DARETest, QDecomposition) {
// (A, C₁) should be detectable pair
const Eigen::Matrix2d C_1{{0.0, 0.0}, {1.0, 0.0}};
const Eigen::Matrix2d Q_1 = C_1.transpose() * C_1;
EXPECT_NO_THROW((frc::DARE<2, 2>(A, B, Q_1, R)));
auto ret1 = frc::DARE<2, 2>(A, B, Q_1, R);
EXPECT_TRUE(ret1);
// (A, C₂) shouldn't be detectable pair
const Eigen::Matrix2d C_2 = C_1.transpose();
const Eigen::Matrix2d Q_2 = C_2.transpose() * C_2;
EXPECT_THROW((frc::DARE<2, 2>(A, B, Q_2, R)), std::invalid_argument);
auto ret2 = frc::DARE<2, 2>(A, B, Q_2, R);
EXPECT_FALSE(ret2);
}

72
wpimath/src/test/native/cpp/DARETestUtil.cpp
View File

@@ -1,72 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// 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 "DARETestUtil.h"
#include <Eigen/Eigenvalues>
#include <gtest/gtest.h>
#include <wpi/print.h>
#include "frc/fmt/Eigen.h"
void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
double tolerance) {
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
EXPECT_NEAR(lhs(row, col), rhs(row, col), tolerance)
<< fmt::format("row = {}, col = {}", row, col);
}
}
if (::testing::Test::HasFailure()) {
wpi::print("lhs =\n{}\n", lhs);
wpi::print("rhs =\n{}\n", rhs);
wpi::print("delta =\n{}\n", Eigen::MatrixXd{lhs - rhs});
}
}
void ExpectPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& X) {
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigX{X,
Eigen::EigenvaluesOnly};
for (int i = 0; i < X.rows(); ++i) {
EXPECT_GE(eigX.eigenvalues()[i], 0.0);
}
}
void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& X) {
// Check that X is the solution to the DARE
// Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
// clang-format off
Eigen::MatrixXd Y =
A.transpose() * X * A
- X
- (A.transpose() * X * B * (B.transpose() * X * B + R).inverse()
* B.transpose() * X * A)
+ Q;
// clang-format on
ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
}
void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& N,
const Eigen::Ref<const Eigen::MatrixXd>& 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
// clang-format off
Eigen::MatrixXd Y =
A.transpose() * X * A
- X
- ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse()
* (B.transpose() * X * A + N.transpose()))
+ Q;
// clang-format on
ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
}

25
wpimath/src/test/native/cpp/DARETestUtil.h
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// 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.
#pragma once
#include <Eigen/Core>
void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
double tolerance);
void ExpectPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& X);
void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& X);
void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& N,
const Eigen::Ref<const Eigen::MatrixXd>& X);

18
wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp
View File

@@ -4,10 +4,14 @@
#include "frc/DARE.h"
template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R);
template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 2, 1>& N);
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R, bool checkPreconditions);
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 2, 1>& N, bool checkPreconditions);

18
wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp
View File

@@ -4,10 +4,14 @@
#include "frc/DARE.h"
template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R);
template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
const Eigen::Matrix<double, 2, 2>& N);
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R, bool checkPreconditions);
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 2>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R,
const Eigen::Matrix<double, 2, 2>& N, bool checkPreconditions);

18
wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp
View File

@@ -4,10 +4,14 @@
#include "frc/DARE.h"
template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R);
template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R,
const Eigen::Matrix<double, 2, 3>& N);
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 3, 3>& R, bool checkPreconditions);
template wpi::expected<Eigen::Matrix<double, 2, 2>, frc::DAREError>
frc::DARE<2, 3>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 3>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 3, 3>& R,
const Eigen::Matrix<double, 2, 3>& N, bool checkPreconditions);

20
wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp
View File

@@ -2,12 +2,18 @@
// 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 <wpi/expected>
#include "frc/DARE.h"
template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R);
template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 4, 1>& N);
template wpi::expected<Eigen::Matrix<double, 4, 4>, frc::DAREError>
frc::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q,
const Eigen::Matrix<double, 1, 1>& R, bool checkPreconditions);
template wpi::expected<Eigen::Matrix<double, 4, 4>, frc::DAREError>
frc::DARE<4, 1>(const Eigen::Matrix<double, 4, 4>& A,
const Eigen::Matrix<double, 4, 1>& B,
const Eigen::Matrix<double, 4, 4>& Q,
const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 4, 1>& N, bool checkPreconditions);