mathlib: add second order reference model filter with optional rate feedback (#19246)
Reference models can be used as filters which exhibit a particular, chosen (reference) dynamic behavior. This PR implements a simple second order transfer function which can be used as such a reference model, additionally with rate feedback. The system is parameterized by explicitly set natural frequency and damping ratio. Another nice externality is that the output state and rate are kinematically consistent. Forward-euler and bilinear transform discretizations for the state space integration step are available.
- this simplifies the reset by allowing a notch filter to reset as
needed
- improves cascade initialization, on reset each filter will reset
properly from the previous
- numerically the cutoff frequency not being smaller than 1% of the
sample frequency is probably a better choice, but we're mostly filtering
16 bit data now stored in 32 bit float
- merge NotchFilterArray into regular NotchFilter (apply vs applyArray)
- only use direct form 1 to prevent reset confusion
- safe default field initialization
- update VehicleAngularVelocity usage
- rotates accel & gyro FIFO data before publication both to simplify downstream usage (including log review) and fix other issues
- to best handle int16_t data rotations are now either performed with swaps if possible, otherwise promoted to float, rotated using the full rotation matrix, then rounded back to int16_t
- fix sensors/vehicle_angular_velocity filter reset both with proper rotation and new calibration uncorrect helper
- in FIFO case filtering is done before calibration is applied, but we need to handle a possible reset from a completely different sensor (vehicle body angular velocity -> sensor frame uncorrected data)
* New NotchFilter methods ahead of RPMFilter implementation.
* Added Direct Form I implementation that support dynamic change of frequencies.
* Added update method to update frequency on an existing filter.
* Added setCoefficients method to easily and efficiently create clones of a filter.
* LowSide, HighSide, Onnotch and array tests testing the applyDF1 method.
Matthias Grob