I timed the DARE unit tests, and the new solver is 0 to 100% faster in
all cases (that is, it's at least as fast as Drake's and up to 2x faster
in some cases).
The new solver is also much simpler, takes less time to compile, and
drops the libwpimath.so size from 325 MB to 301 MB.
I think most of the compilation time is coming from the eigenvalue
decompositions used to enforce argument preconditions.
This makes it possible to mock the timestamp for wpimath without affecting the rest of the library.
Co-authored-by: Peter Johnson <johnson.peter@gmail.com>
This effectively replaces the Unscented Kalman Filter used for Pose Estimation with the Odometry model, and uses a recalculable Kalman gain matrix to update pose estimations whenever a vision measurement is added.
Notably, this change removes the need for the confusing generics used in Java, and the C++ implementation got quite a bit less complex as well.
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
* Use explicit this capture required by C++20
* Use C++20 span
* Replace wpi::numbers with std::numbers
* Fix C++20 clang-tidy warning false positive in fmt
* Remove ciso646 include since C++20 removed that header
* Fix global-buffer-overflow asan warnings in ntcore tests
* Add DIOSetProxy constructor to HAL
* Upgrade MSVC compiler to 2022
* Bump native-utils to 2023.2.7 (changes to std=c++20)
Co-authored-by: Peter Johnson <johnson.peter@gmail.com>
Now, implicit narrowing conversions are only used with wpi::Now(). This
also fixes clang-tidy warnings about C-style casts. For example:
```
== clang-tidy /__w/allwpilib/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc ==
/__w/allwpilib/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc:95:18: warning: C-style casts are discouraged; use static_cast/const_cast/reinterpret_cast [google-readability-casting]
auto curTime = units::second_t(m_timer.Get());
^
```
In that case at least, the cast was removed entirely since Get() already
returns a units::second_t.
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).
