Reduces nesting by returning when the value is within the deadband.
Adjusts the algorithm to handle large values of maxMagnitude naturally (instead of needing a separate check).
Reformats the math comments.
Throughout the code the state sqrt covariance S and innovation covariance Sy are maintained as upper triangular cholesky factors of those covariance matrices. The original paper defines P=S*S', so S should be lower triangular. The functions in the paper reflect this. In the code implementation, the sqrt covariance matrices are upper triangular, but the algorithm expects them to be lower triangular.
This bug was likely missed because the incorrect version of the filter is able to converge for some systems where all the states are observed, and the test case is set up such that all states are observed.
To fix the bug, a couple things needed to be changed:
all instances of rankUpdate() needed to be changed to use the lower triangular cholesky factor,
In the unscented transform, when S is found via QR decomposition, we need to take the transpose because R is upper triangular,
P() and SetP() functions need to be modified to be P=S*S' instead of P=S'*S, and P.llt().matrixL() instead of P.llt().matrixU() respectively.
Each part of the algorithm has also had the comments changed to clarify exactly which equation from the paper it implements.
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
* Move units into API docs instead because suffixes make user code verbose and hard to read
* Rename trackWidth to trackwidth
* Make ultrasonic classes use meters instead of a mix of m, cm, mm, ft,
and inches
Small values of kₐ make the iterative solver ill-conditioned. This
change reverts to the constant-acceleration feedforward in that case. It
gives _very_ bad results (hence why we added the iterative solver in the
first place), but it's better than hanging.
```
TEST(ArmFeedforwardTest, CalculateIllConditioned) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 20_V / 1_rad_per_s;
constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
constexpr auto Kg = 0_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
// Calculate(currentAngle, currentVelocity, nextAngle, dt)
CalculateAndSimulate(armFF, 0_rad, 0_rad_per_s, 2_rad_per_s, 20_ms);
}
```
This produces 1 V and doesn't accelerate the system at all. Using
nextVelocity instead of currentVelocity in the feedforward outputs 41 V
and still only accelerates to 0.4 rad/s of the requested 2 rad/s.
I picked the kₐ cutoff by increasing kₐ until the iterative solver
started converging.
Fixes#7743.
It now uses SQP for problems without inequality constraints, which is
faster.
main:
```
[ RUN ] Ellipse2dTest.DistanceToPoint
0.203 ms
[ OK ] Ellipse2dTest.DistanceToPoint (0 ms)
[ RUN ] Ellipse2dTest.FindNearestPoint
0.019 ms
[ OK ] Ellipse2dTest.FindNearestPoint (0 ms)
```
upgrade:
```
[ RUN ] Ellipse2dTest.DistanceToPoint
0.197 ms
[ OK ] Ellipse2dTest.DistanceToPoint (0 ms)
[ RUN ] Ellipse2dTest.FindNearestPoint
0.015 ms
[ OK ] Ellipse2dTest.FindNearestPoint (0 ms)
```
This is easier to type and follows the naming of Pose2d::Nearest().
Since Ellipse2d and Rectangle2d were added for the 2025 season, we don't
need to add deprecation notices.
The Raspberry Pi 5 is fast enough that we no longer need it.
```
Running ./build/DAREBench
Run on (4 X 2400 MHz CPU s)
CPU Caches:
L1 Data 64 KiB (x4)
L1 Instruction 64 KiB (x4)
L2 Unified 512 KiB (x4)
L3 Unified 2048 KiB (x1)
Load Average: 0.47, 0.72, 0.45
***WARNING*** CPU scaling is enabled, the benchmark real time measurements may be noisy and will incur extra overhead.
-------------------------------------------------------------------------------
Benchmark Time CPU Iterations
-------------------------------------------------------------------------------
DARE_WPIMath_Dynamic 34.4 us 34.4 us 20315
DARE_WPIMath_NoPrecondChecks_Dynamic 21.7 us 21.7 us 32266
DARE_WPIMath_Static 15.2 us 15.2 us 45878
DARE_WPIMath_NoPrecondChecks_Static 7.84 us 7.84 us 89316
DARE_SLICOT 79.4 us 79.4 us 8789
DARE_Drake 34.9 us 34.9 us 20074
```
