Also refactored RKF45 implementation to match the new style, which is
easier to read.
The tests were switched from RKF45 to RKDP since it's more accurate.
Some valid warnings like throwing NullPointerException or using a for
loop instead of System.arraycopy() were fixed.
Abstract classes marked with PMD.AbstractClassWithoutAbstractMethod were
made concrete because they already had protected constructors.
Fixes#1697.
This also fixes a member function name inconsistency between languages
and adds missing documentation to C++'s KalmanFilterLatencyCompensator.
Fixes#3229.
The implementation of wpi::circular_buffer has been effectively replaced
with a dynamically sized copy of wpi::static_circular_buffer with a
resize() member function.
The units for angular Kv and Ka were inconsistent with the derivation. A
second factory function overload was added for angular units that uses a
trackwidth to convert to the other form.
Notice how section 15.2 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf
defines the angular feedforward as u = Kv,angular v instead of u = Kv,angular + omega.
The units cancel for elements of A but not B, so just the B matrix was incorrect in our code.
This breaks existing C++ code since the units are part of the function
signature.
The gyro offset should be determined from the desired initial pose, not the current pose. This fix reflects the behavior of the odometry classes and the C++ holonomic pose estimators.
This was already removed from C++ in the offseason and replaced with
MathUtil.inputModulus(). We just neglected to do that for Java; it was
never intended to see a season release. Its implementation is incorrect
compared to inputModulus() as well.
See https://github.com/wpilibsuite/allwpilib/issues/3168 for discussion.
The stall torque, stall current, and free current are now multiplied by
the number of motors instead of just the stall torque. This produces the
same values for Kt and Kv regardless of the number of motors; the motor
resistance still affects the system response.
For an elevator model, the response should be the same as before since a
factor of "number of motors" shows up in the same place in the
acceleration calculation, but the current calculation will also be
correct now.
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.
Also update Checkstyle to 8.38.
Google changed their style guide from the last time we imported it. This PR brings in those naming changes. The change they made is allowing single letter member, parameter, and local variable names. They also added a lambda naming scheme and I thought it would be good to bring that in too.
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>
There were three options for where to put this function:
1. A free function in LinearQuadraticRegulator.h. Returning a K matrix
means the user can't use the LinearQuadraticRegulator in a loop
anymore.
2. A default argument added to ctors in LinearQuadraticRegulator for a
time delay (default of 0). This has the smallest API footprint from
the user perspective, but it bloats the already substantial
constructor overload set of LinearQuadraticRegulator.
3. A member function in LinearQuadraticRegulator that modifies the
internal K. This would still have to take in a LinearSystem or (A, B)
pair because the ctor doesn't store it. Storing it internally feels
like paying for what we don't use most of the time.
I went with option 3.
I verified the tests's expected values in Python with
scipy.linalg.fractional_matrix_power().
Closes#2877.
This address some problems with the LinearSystemLoop class that were discovered through testing.
The initial state estimate of the observer was set to the provided initial state rather than zero as previously, a non zero initial state passed into reset() would lead to a discrepancy between the current state estimate and the actual system state.
