Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +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

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();
}