In preparation for changing the regular `P` name to const, in
preparation for auditing the code so that writes to the matrix keep its
necessary numeric properties.
Sadly there is not a better way than a per-file `#define` to make the
switch. Making `P` a reference to `Pmut` substantially changes the
compiler output. Defining `P` in the header file conflicts with other
includes. Doing the rename at the top of each file allows each file to
be fixed independently.
this is an input to the EKF, but also populated based on the AHRS current state.
Make the data come from the sensor, not the *output* of the AHRS backends!
These are always called together and always called with `P` as the first
argument. `P` must remain symmetric, so merging them removes chances for
bugs and saves flash.
Add a new magnetometer calibration mode that enables 3-axis mag field
learning on the ground and after the first in-flight yaw and mag reset,
but inhibits learning during the initial climb phase.
This addresses two issues:
- Allows mag field to be learned on the ground before takeoff, which is
needed when swapping batteries with different magnetic signatures
- Inhibits learning during initial climb when motor magnetic interference
is strongest, preventing EKF core divergence from corrupted body offsets
Reject setOriginLLH() calls while statesInitialised is false.
InitialiseVariables() resets validOrigin during bootstrap, so an
origin set before that point (e.g. from AHRS use_recorded_origin_maybe)
is silently lost. Callers already handle the false return by
retrying on the next cycle.
This replaces the previous AHRS-level workaround that detected and
re-applied the lost origin.
The previous method resulted in data incest and fusion of predicted airspeed on every EKF internal time step.
This was not apparent during flight where the vehicle was turning, but during long straight legs did not constrain along track drift.
These changes enable the EKF to use the last observable wind velocity vector estimate to synthesise an airspeed measurement if operating without an airspeed sensor and when all other measurement types that can constrain velocoty drift are lost. This enables the EKF to use dead reckoning to continue after loss of GPS when there is no air speed sensor fitted and without the need to set a default airspeed value.
The logic used to fuse a default airspeed value has also been cleaned up and the call to FuseAirSpeed() from inside SelectBetaDragFusion() has been removed.
AP_NavEKF3: Fix error in default airspeed observation variance
AP_NavEKF3: Enable shadow fusion of airspeed when sensor is disabled
filter status was initially set to zero then updated. This interacts
with the IMU filtering code which checks filter status from a
different thread to determine active_EKF_type(). When the race
condition is hit then the IMU we are running notch filters on changes
for a single sample, causing a notch filter glitch
Innovations and innovation test ratios will still be calculated and reported for an unhealthy sensor, but the EKF states wnd covariance matrix will not be modified.
When returning our last-known-position we were supplying lat/lng but not
altitude.
This usually really doesn't matter as this result generally isn't used,
especially for altitude. OTOH, it may prevent a bug into the future.
Fixes bug that prevents the vertical velocity being reset to the GPS if the position aiding has already timed out and improves sensitivity of the bad IMU data check.
this shifts EKF_origin to the current location at 1Hz, while leaving
public_origin alone. All output APIs and logging are relative to
public_origin.
The effect of this change is to remove the distortion caused by a
spherical earth, which allows the EKF to operate without errors at
very long distances from the public_origin.
Thomas Watson
Peter Barker
Iampete1
Andy Piper
snktshrma
rishabsingh3003
Randy Mackayandsnktshrma
Willian Galvani
Paul Riseborough
Paul Riseborough
Andrew Tridgell
Josh Henderson