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] Rewrite DARE solver (#5328)

Browse Source 
I timed the DARE unit tests, and the new solver is 0 to 100% faster in
all cases (that is, it's at least as fast as Drake's and up to 2x faster
in some cases).

The new solver is also much simpler, takes less time to compile, and
drops the libwpimath.so size from 325 MB to 301 MB.

I think most of the compilation time is coming from the eigenvalue
decompositions used to enforce argument preconditions.
This commit is contained in:
Tyler Veness
2023-05-14 22:23:00 -07:00
committed by GitHub
parent 3876a2523a
commit 52bd5b972d
32 changed files with 831 additions and 2024 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,7 +4,7 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.Drake;
import edu.wpi.first.math.DARE;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
@@ -111,7 +111,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
throw new IllegalArgumentException(msg);
}
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
var S = DARE.dare(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
m_K =
@@ -150,7 +150,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
var S = DARE.dare(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
m_K =