Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Print uncontrollable/unobservable models in LQR and KF (#3694)

Browse Source 
IsDetectable() was added to make the code easier to read.
This commit is contained in:
Tyler Veness
2021-10-29 00:03:02 -07:00
committed by GitHub
parent d5270d113b
commit a939cd9c89
12 changed files with 169 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -71,10 +71,7 @@ class ExtendedKalmanFilter {
Eigen::Matrix<double, Outputs, Outputs> discR =
DiscretizeR<Outputs>(m_contR, dt);
// IsStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
bool isObservable =
IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
if (isObservable && Outputs <= States) {
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR);
} else {
@@ -137,10 +134,7 @@ class ExtendedKalmanFilter {
Eigen::Matrix<double, Outputs, Outputs> discR =
DiscretizeR<Outputs>(m_contR, dt);
// IsStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
bool isObservable =
IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
if (isObservable && Outputs <= States) {
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR);
} else {

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

@@ -4,7 +4,10 @@
#pragma once
#include <frc/fmt/Eigen.h>
#include <cmath>
#include <string>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
@@ -65,14 +68,14 @@ class KalmanFilterImpl {
const auto& C = plant.C();
// IsStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
bool isObservable =
IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
if (!isObservable) {
wpi::math::MathSharedStore::ReportError(
"The system passed to the Kalman filter is not observable!");
throw std::invalid_argument(
"The system passed to the Kalman filter is not observable!");
if (!IsDetectable<States, Outputs>(discA, C)) {
std::string msg = fmt::format(
"The system passed to the Kalman filter is "
"unobservable!\n\nA =\n{}\nC =\n{}\n",
discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
Eigen::Matrix<double, States, States> P =