Fixes https://github.com/wpilibsuite/allwpilib/issues/8284.
If we have vision updates at the time of the `Reset*` call, we can
correct the translation/rotation of the new odometry pose by adding a
new vision update where:
- `ResetTranslation`: the translation is hard-coded to the new
translation, and the rotation components are set to those of the latest
vision update (prior to clearing the map).
- `ResetRotation`: the rotation is hard-coded to the new rotation, and
the translation components are set to those of the latest vision update
(prior to clearing the map).
The spiraling issue occurs when the vision rotation standard deviation is very high relative to the odometry rotation standard deviation and the vision measurements have a large rotation error. (Scaling the rotation component of a twist without scaling the translation component causes the direction of overall translation to change, leading to spiraling around (either towards or away) the vision measurement instead of moving towards it.) Using a transform instead of a twist avoids this issue.
In general, scaling twist components is more mathematically correct than scaling transform components. However, although twists are correct for modeling uncertainty in an odometry-only pose estimate, they are not correct for the difference between the odometry-only pose estimate and a vision measurement. Since neither twists nor transforms are completely correct (and the pose estimator as a whole is not mathematically correct), but using transforms can guarantee that the pose estimate approaches the vision measurement (instead of potentially spiraling away), they are the least bad option.
clang-format 21 made some formatting changes. Since wpiformat's stdlib
task was removed, I removed NOLINT comments for it and removed some
std:: prefixes it added to comments.
Upstream no longer seems to have the commit we were pointing to. We'll
just use the tag since that hasn't changed since the official release
announcement.
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.
I upgraded all plugins I could see except org.ysb33r.doxygen. 2.0 made
breaking changes, and I couldn't figure out how to migrate.
Most of the changes are for suppressing new linter purification rites.
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>
The generator was calling a binary of the wrong architecture and failing
internally, but passing the build (incorrectly). Explode right away.
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Ensures java deprecated notation is paired with javadoc and vice versa.
Adds javadoc deprecation for MecanumControllerCommand, ArmFeedForward,
ElevatorFeedforward, and MecanumDriveMotorVoltages
Fixes#7736
Supersedes #7737
Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
A noteworthy change is the replacement of the `dp.startswith(os.path.join(".", "subdir"))` pattern. pathlib doesn't offer something with similar semantics besides `match` and `full_match`, so there's now a helper function that replicates the behavior.
Other notable changes include the addition of type annotations to ensure code correctness, using == to check file names instead of `endswith` for clarity (`endswith` is still used to check extensions), manual walking and copying being refactored in googletest, json, memory, nanopb, protobuf, and sleipnir to use `walk_cwd_and_copy_if`, and matching functions being shortened to the point where they can just be inlined into the lambda.
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: David Vo <auscompgeek@users.noreply.github.com>
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>
