The UKF test was calling `.value()` on an implicit
`units::millisecond_t` type assuming it was `units::second_t`.
I normalized the rest of the dt declarations while I was at it.
Adds methods to compute the dot and cross products between Translation2ds and Translation3ds, as well as methods to compute the square of Distance and Norm, which allows avoiding some calls to sqrt in many cases.
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
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.
Java generics are too limited to do what we need. This refactors generic code previously in Unit and Measure into unit-specific classes that can have unit-safe math operations (notably, times and divide) that can return values in known units instead of a wildcarded Measure<?>.
Unit-specific measure implementations are automatically generated by ./wpiunits/generate_units.py, which generates generic interfaces and mutable and immutable implementations of those interfaces. These make up the bulk of the diff of this PR (approximately 9300 LOC).
This also adds units for angular and linear velocities, accelerations, and momenta; moment of inertia; and torque.
