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

[wpimath] Remove redundant internal DARE function (#7442)

Browse Source
This commit is contained in:
Tyler Veness
2024-11-28 21:24:13 -08:00
committed by GitHub
parent f377a9c573
commit a0af0fd572
6 changed files with 18 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
wpimath/src/main/native/include/frc/DARE.h
View File

@@ -143,89 +143,6 @@ Eigen::Matrix<double, States, States> DARE(
return H_k1;
}
/**
Computes the unique stabilizing solution X to the discrete-time algebraic
Riccati equation:
AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
This is equivalent to solving the original DARE:
A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
where A₂ and Q₂ are a change of variables:
A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
This overload of the DARE is useful for finding the control law uₖ that
minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
@verbatim
∞ [xₖ]ᵀ[Q N][xₖ]
J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
k=0
@endverbatim
This is a more general form of the following. The linear-quadratic regulator
is the feedback control law uₖ that minimizes the following cost function
subject to xₖ₊₁ = Axₖ + Buₖ:
@verbatim
∞
J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
k=0
@endverbatim
This can be refactored as:
@verbatim
∞ [xₖ]ᵀ[Q 0][xₖ]
J = Σ [uₖ] [0 R][uₖ] ΔT
k=0
@endverbatim
This internal function skips expensive precondition checks for increased
performance. The solver may hang if any of the following occur:
<ul>
<li>Q₂ isn't symmetric positive semidefinite</li>
<li>R isn't symmetric positive definite</li>
<li>The (A₂, B) pair isn't stabilizable</li>
<li>The (A₂, C) pair where Q₂ = CᵀC isn't detectable</li>
</ul>
Only use this function if you're sure the preconditions are met.
@tparam States Number of states.
@tparam Inputs Number of inputs.
@param A The system matrix.
@param B The input matrix.
@param Q The state cost matrix.
@param R_llt The LLT decomposition of the input cost matrix.
@param N The state-input cross cost matrix.
@return Solution to the DARE.
*/
template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt,
const Eigen::Matrix<double, Inputs, Inputs>& N) {
// 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.
//
// This is equivalent to solving the original DARE:
//
// A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
//
// where A₂ and Q₂ are a change of variables:
//
// A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
return detail::DARE<States, Inputs>(A - B * R_llt.solve(N.transpose()), B,
Q - N * R_llt.solve(N.transpose()),
R_llt);
}
} // namespace detail
/**