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] Update drake with upstream (#3484)

Browse Source 
Our patches for the DARE and [[noreturn]] attributes were merged
upstream. We missed their monthly release window by a day, so we'll use
a commit hash for now.
This commit is contained in:
Tyler Veness
2021-07-22 17:48:48 -07:00
committed by GitHub
parent 1ef826d1da
commit ab8e8aa2a1
10 changed files with 97 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -382,12 +382,11 @@ void reorder_eigen(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
* DiscreteAlgebraicRiccatiEquation function
* computes the unique stabilizing solution X to the discrete-time algebraic
* Riccati equation:
* \f[
* A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0
* \f]
*
* @throws std::runtime_error if Q is not positive semi-definite.
* @throws std::runtime_error if R is not positive definite.
* AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
*
* @throws std::exception if Q is not positive semi-definite.
* @throws std::exception if R is not positive definite.
*
* Based on the Schur Vector approach outlined in this paper:
* "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
@@ -396,9 +395,9 @@ void reorder_eigen(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
*
* Note: When, for example, n = 100, m = 80, and entries of A, B, Q_half,
* R_half are sampled from standard normal distributions, where
* Q = Q_half'*Q_half and similar for R, the absolute error of the solution
* is 10^{-6}, while the absolute error of the solution computed by Matlab is
* 10^{-8}.
* Q = Q_halfᵀ Q_half and similar for R, the absolute error of the solution
* is 10⁻⁶, while the absolute error of the solution computed by Matlab is
* 10⁻⁸.
*
* TODO(weiqiao.han): I may overwrite the RealQZ function to improve the
* accuracy, together with more thorough tests.
@@ -464,9 +463,12 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
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);
// This is a change of variables to make the DARE that includes Q, R, and N
// cost matrices fit the form of the DARE that includes only Q and R cost
// matrices.
Eigen::MatrixXd A2 = A - B * R.llt().solve(N.transpose());
Eigen::MatrixXd Q2 = Q - N * R.llt().solve(N.transpose());
return DiscreteAlgebraicRiccatiEquation(A2, B, Q2, R);
}
} // namespace math