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>
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 Google C++ protobuf implementation has issues with dynamic linkage across DLL boundaries because it uses global variables. It also has a compile-time dependency because the protoc version must exactly match the libprotobuf version. Using nanopb with a customized generator fixes both of these issues.
Co-authored-by: Gold856 <117957790+Gold856@users.noreply.github.com>
These test fixtures were adding complexity while only saving one line of
object initialization per test. Our other tests like this just make the
object at the top of each test.
Currently, a max iteration heuristic is used to determine when a spline
is malformed. Instead, we can report a failure immediately if dx and dy
are too small, because the heading won't be accurate either.
Fixes#6826.
We now use a wrapper (wpi::print) to catch exceptions since we can't patch
std::print() to not throw when we ultimately migrate to it.
fmtlib and std format/print throw the same exceptions and always have. We previously patched fmt::print() to not throw a write failure exception, but we can't do that for std::print(); wpi::print() is the migration plan.
LTVUnicycleController is a drop-in replacement with better tuning knobs.
The RamseteCommand examples were removed instead of retrofitted with
LTVUnicycleController because we're planning on removing the command
controller classes anyway, so it would be wasted effort. The
SimpleDifferentialDriveSimulation example shows direct
LTVUnicycleController usage.
