* fix(mathlib): correct MedianFilter comparator to satisfy strict-weak ordering
- Add explicit NaN handling before comparison operators
- NaN sorted to high end, ensuring finite values cluster at median index
- Guard NaN checks with if constexpr for non-floating-point types
- Replace float equality check with < and > to avoid -Wfloat-equal
Fixes#25917
* fix(mathlib): remove type_traits dependency in MedianFilter
Replace std::is_floating_point_v<T> with a self-contained template
specialization to avoid <type_traits> header, which is unavailable
on NuttX targets compiled with -nostdinc++.
* fixed formating
* test(mathlib): add majority-finite and majority-NaN window tests for MedianFilter
* Moved structs inside namespace math
* clean up
* add two more tests
---------
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
* lib: add FilteredDerivative class
* AirspeedValidator: add first principle check
- filters throttle, pitch and airspeed rate, and triggers
if the airspeed rate is negative even though the vehicle
is pitching down and giving high throttle.
Check has to fail for duration defined by ASPD_FP_T_WINDOW
to trigger an airspeed failure.
* AirspeedValidator: define constants for first principle check
* FilteredDerivative: set initialised to false if sample interval is invalid
* airspeed_selector: improved comment
* increase IAS derivative filter time constant from 4 to 5
* use legacy parameter handling for FW_PSP_OFF
* handle FW_THR_MAX as well
* ROMFS/airframes: exclude some airframes for v6x and v4pro to save flash on them
---------
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
- MPC_VEL_LP: new velocity first order low pass filter (off by default)
- MPC_VEL_NF_FRQ/MPC_VEL_NF_BW: new velocity notch filter (off by default)
- MPC_VELD_LP: existing velocity derivative low pass filter, but I've dropped the remaining controllib usage
- WelfordMeanVector now computes covariance
- use Kahan summation for Welford mean (but continue using float32 for actual mean, etc)
- WelfordMean and WelfordMeanVector handle initial value and count roll over
- Welford mean count rollover at 16 bit max to prevent numerical issues and shift weight to newer samples
- sensors/vehicle_imu: update Welford mean usage (now simplified with resets removed)
- fix vehicle_imu_status accel var, now properly rotated with full covariance matrix
- gyro_calibration: update Welford mean usage
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
