The Joseph form of the error covariance update equation is more
numerically stable when the Kalman gain isn't optimal. Numerical
instability and filter divergence can occur if the user goes long time
periods between updates and the error covariance becomes ill-conditioned
(the ratio between the largest and smallest eigenvalue gets too large).
It would crash in C++ if the global measurement was sooner than all the
snapshots.
Align Java with the changes and better document computation approach.
The template argument order for UnscentedTransform was reversed to match
all the other UKF classes. Since UnscentedTransform is intended as a
class for internal use only, this shouldn't cause much breakage.
* Replace Matrix<> with Vector<> where vectors are explicitly intended.
I found these via `rg "Eigen::Matrix<double, \w+, 1>"`.
* Pass all Eigen matrices by const reference. I found these via `rg
"\(Eigen"` on main (the initializer list constructors make more false
positives).
* Replace MakeMatrix() and operator<< usage with initializer list
constructors. I found these via `rg MakeMatrix` and `rg "<<"`
respectively.
* Deprecate MakeMatrix()
Internal headers are no longer allowed as of
https://gitlab.com/libeigen/eigen/-/merge_requests/631. Based on
benchmarking I conducted in that thread, there doesn't seem to be a
performance penalty for including the full headers anymore.
This also fixes a member function name inconsistency between languages
and adds missing documentation to C++'s KalmanFilterLatencyCompensator.
Fixes#3229.
A lot of these are breaking changes. frc::Timer was replaced with the
contents of frc2::Timer. The others were in-place argument changes or
removing deprecated non-unit overloads.
The wpimath APIs use std::array, which doesn't do size checking. Passing
an array with the wrong size can result in uninitialized elements
instead of a compilation error.
This is a breaking change but is worthwhile to avoid hard-to-debug errors.
frc::NormalizeAngle(), units::math::NormalizeAngle(), and
frc::GetModulusError() were replaced with frc::InputModulus() and
frc::AngleModulus().
They were placed in wpimath/src/main/native/include/frc/MathUtil.h for
C++ and wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
for Java.
This makes code easier to read and more consistent between C++ and Java.
Also update clang-format settings to always add a line break (even if no braces are used).
This adds an overload of UnscentedKalmanFilter::Correct() that takes a
custom measurement covariance but uses default mean and residual
calculation functions.
Closes#2965.
Pose and state estimators can filter latency-compensated global measurements and fuse them with state-space drivetrain model information to estimate robot position. They are drop-in replacements for the existing odometry classes.
Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
By storing the previous dt, it can be moved into Correct() where it is
actually used. This lets us take the continuous R as an argument in the
user-provided R overload.
This helps reduce compilation overhead. I tried slimming down includes
of <Eigen/QR>, but the householderQr() function we use from there
requires including dependency headers from Eigen that don't fit with
lexographic ordering. It didn't seem worth the effort to work around.
This won't affect user code at all since all the Eigen feature usage
here is internal only; users generally only need <Eigen/Core>.
I didn't notice a performance difference between the original
implementation and this one for a flywheel simulation, so this
simplifies a lot of internals.
This class can no longer implement KalmanTypeFilter because that class
allows setting the error covariance for use in the
KalmanFilterLatencyCompensator class. This won't impact the holonomic pose
estimators that use KalmanFilterLatencyCompensator because they all use an EKF.
