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] Fix LQR matrix constructor overload for Q, R, and N (#3884)

Browse Source 
It was using the continuous B matrix to compute the feedback gain
instead of the discrete B matrix.

Tests were added for the matrix constructor overloads.
This commit is contained in:
Tyler Veness
2022-01-08 23:23:53 -08:00
committed by GitHub
parent 8ac45f20bb
commit db0fbb6448
4 changed files with 216 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
View File

@@ -118,8 +118,13 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
var temp = discB.transpose().times(S).times(discB).plus(R);
m_K = temp.solve(discB.transpose().times(S).times(discA));
m_K =
discB
.transpose()
.times(S)
.times(discB)
.plus(R)
.solve(discB.transpose().times(S).times(discA));
m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
@@ -152,8 +157,13 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
var temp = discB.transpose().times(S).times(discB).plus(R);
m_K = temp.solve(discB.transpose().times(S).times(discA).plus(N.transpose()));
m_K =
discB
.transpose()
.times(S)
.times(discB)
.plus(R)
.solve(discB.transpose().times(S).times(discA).plus(N.transpose()));
m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));