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

[wpimath] Add DARE solver for Q, R, and N with LQR ctor overloads

Browse Source 
This is useful for implementing implicit model following.
This commit is contained in:
Tyler Veness
2021-02-15 18:17:55 -08:00
committed by Peter Johnson
parent b2c3b2dd8e
commit edd2f0232c
6 changed files with 216 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
View File

@@ -456,5 +456,18 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
return X;
}
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
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) {
DRAKE_DEMAND(N.rows() == B.rows() && N.cols() == B.cols());
Eigen::MatrixXd scrA = A - B * R.llt().solve(N.transpose());
Eigen::MatrixXd scrQ = Q - N * R.llt().solve(N.transpose());
return DiscreteAlgebraicRiccatiEquation(scrA, B, scrQ, R);
}
} // namespace math
} // namespace drake