Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +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

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