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

12
wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
View File

@@ -19,6 +19,12 @@ LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {}
LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 1, 1>& N, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, N, dt) {}
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const wpi::array<double, 2>& Qelems, const wpi::array<double, 1>& Relems,
@@ -32,4 +38,10 @@ LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {}
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
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, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, N, dt) {}
} // namespace frc

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