Commit Graph

744 Commits

Author SHA1 Message Date
Clyde McQueen
630d3fa8a7 AP_NavEKF3: log mag fusion selection to XKFS 2024-07-11 16:16:27 +10:00
Peter Barker
c8a20726ff AP_NavEKF3: do not store prediction-enabled as state
this is only used in one place, and that place is called from the same routine setting the persistent state.  The only other place which calls readIMUData shouldn't be running the prediction step, but mmay, depending on the previous setting of the prediction step.

We are not initialising this state on filter reset, so it's possible that the state will be set when we do an InitialiseFilterBootstrap, which is probably not desired
2024-07-09 10:45:03 +10:00
Peter Barker
ce48932f4d AP_NavEKF3: remove storedRange member variable if rangefinder measurements disabled 2024-07-04 19:21:44 +10:00
Willian Galvani
cb74ebbddd EKF3: allow earth-frame fields to be estimated with an origin but no GPS 2024-07-03 16:49:27 +10:00
Peter Barker
13e7d04497 AP_NavEKF3: make AP_RANGEFINDER_ENABLED remove more code 2024-07-02 09:17:26 +10:00
Peter Barker
563c3e7c62 AP_NavEKF3: rename ins get_primary_accel to get_first_usable_accel 2024-06-26 17:12:12 +10:00
Clyde McQueen
0e6543f0e4 AP_NavEKF3: sub update for yaw reset and inFlight checks
on sub, request final yaw reset after diving 0.5m.
Also update rangefinder tests for sub
2024-06-25 18:09:37 +10:00
Paul Riseborough
61874da020 AP_NavEKF3: Rework method of synthesising airspeed for dead reckoning
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.
2024-06-09 14:25:02 +10:00
Paul Riseborough
720c2da807 AP_NavEKF3: Rework GPS jamming resiliency
Make it user selectable.
Remove potential for a race condition between decisions based on latest data and the EKF fusion processing which operates on a delayed time horizon. This is achieved by preventing data entering the buffer if awaiting checks to pass ensuring that no EKF fusion time horizon processes can use data that hasn't passed checks.
Log the waitingForGpsChecks class variable
2024-06-09 14:25:02 +10:00
Paul Riseborough
996bf7d4df AP_NavEKF3: Log gpsGoodToAlign 2024-06-09 14:25:02 +10:00
Paul Riseborough
eb2ff2192d AP_NavEKF3: Don't block no compass planes from running GPS alignment checks 2024-06-09 14:25:02 +10:00
Paul Riseborough
6baeb1cd9d AP_NavEKF3: Fix bug causing in flight yaw align to not complete 2024-06-09 14:25:02 +10:00
Paul Riseborough
ffde7f815c AP_NavEKF3: Allow wind to relearn rapidly when GPS is re-enabled 2024-06-09 14:25:02 +10:00
Paul Riseborough
87bf8d9997 AP_NavEKF3: Treat wind as truth when deadreckoning with no airspeed sensor 2024-06-09 14:25:02 +10:00
Paul Riseborough
568972c0f0 AP_NavEKF3: Fix bug preventing use of default or synthetic airspeed 2024-06-09 14:25:02 +10:00
Paul Riseborough
2c3174b77b AP_NavEKF3: Strengthen protection against GPS jamming
These changes prevent the EKF from consuming GPS data too soon when it is recovering from jamming if the EKF is able to navigate using dead reckoning.
2024-06-09 14:25:02 +10:00
Paul Riseborough
324d5da811 AP_NavEKF3: Use last observed wind states to enable dead reckoning
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
2024-06-09 14:25:02 +10:00
Paul Riseborough
e53416e77b AP_NavEKF3: Apply GPS quality checks following loss of 3D fix if velocity error is bounded 2024-06-09 14:25:02 +10:00
Andrew Tridgell
94cbd7cbfb AP_NavEKF3: use NEW_NOTHROW for new(std::nothrow) 2024-06-04 09:20:21 +10:00
Paul Riseborough
91423d4c71 AP_NavEKF3: Remove unncessary local position height reporting offset
The offset generated by the EK3_OGN_HGT_MASK parameter bit 2 option is applied to the baro or range finder sensor so it does not have to be applied to the local position height.
2024-05-23 10:35:51 +10:00
Paul Riseborough
5fded75eb6 AP_NavEKF3: Adjust sensor height when EK3_OGN_HGT_MASK bit 2 is set 2024-05-23 10:35:51 +10:00
Peter Barker
36e385fb22 AP_NavEKF3: set-origin failure comment improved 2024-05-21 09:56:02 +10:00
Randy Mackay
4e2dd7c399 AP_NavEKF3: accept set origin even when using GPS 2024-05-21 09:56:02 +10:00
Paul Riseborough
3c8b3be7a7 AP_NavEKF3: Change yaw source to stop compass use when calibrating 2024-04-23 15:19:56 +09:00
Andrew Tridgell
dce2492321 AP_NavEKF3: use filtered gyro in INS position correction
this reduces the impact of IMU noise on the output velocity from the
EKF
2024-04-17 14:53:59 +10:00
Iampete1
21df26de75 AP_NavEKF3: use set_and_defualt when changing imu mask 2024-03-26 11:35:36 +11:00
Randy Mackay
65b4db5ede AP_NavEKF3: define Yaw alignment min GPS speed per vehicle 2024-03-26 10:01:42 +11:00
Peter Barker
ce04d69d53 AP_NavEKF3: correct compilation in SITL when logging is disabled
this code is gated on HAL_BOARD_SITL already, and usually we have logging in SITL.  But not always
2024-03-12 09:25:17 +11:00
Marek S. Lukasiewicz
07681416f5 AP_NavEKF3: Match handling of MAV_CMD_EXTERNAL_POSITION_ESTIMATE to common MAVLink dialect 2024-03-07 18:13:51 +11:00
Peter Barker
21edc6aee1 AP_NavEKF3: do not trust number of beacons to not change
if the count from the beacon library changes we may end up looking at memory we shouldn't
2024-02-28 18:37:42 +11:00
Peter Barker
a517d5fed1 AP_NavEKF3: correct initialisation of BeaconFusion data structure
the fusionReport object is not being cleared when the filter undergoes a forced reset.

So delete and recreate the object.  Will also make this sensitive to the number of beacons changing.

Also don't attempt to allocate zero bytes.
2024-02-28 18:37:42 +11:00
Peter Barker
2df3cb98c6 AP_NavEKF3: move initialisation of rngBcn into BeaconFusion method 2024-02-27 12:04:14 +11:00
Peter Barker
e9d065c1cc AP_NavEKF3: tidy population of fusion reports
simply take a refefence and use it
2024-02-26 20:55:41 +11:00
Simon Hancock
07eecd3b67 AP_NavEKF3: Updates to log message units and help text
Set quaternion component units to no-unit from UNKNOWN
2024-01-21 14:26:54 +11:00
Peter Barker
0afed2f8c2 AP_NavEKF3: allow compilation with HAL_LOGGING_ENABLED false 2024-01-17 18:25:55 +11:00
Peter Barker
10c8af3409 AP_NavEKF3: do not use fmaxF on floating point values
it returns double, which will not fit into this float
2023-09-12 09:47:17 +10:00
Paul Riseborough
5aa7bd0b7a AP_NavEKF3: Allow operation with EK3_SRCx_POSZ = 0 (NONE) 2023-08-23 18:25:26 +10:00
Andy Piper
c57efa1b20 AP_NavEKF3: allow high values of EK3_ALT_M_NSE for boards without baros 2023-08-23 18:25:26 +10:00
Paul Riseborough
e3c0175bb4 AP_NavEKF3: Update EK3_GLITCH_RADIUS metadata 2023-08-16 17:56:43 +10:00
Paul Riseborough
842882355f AP_NavEKF3: increase innovation variance instead of clipping innovations 2023-08-16 17:56:43 +10:00
Paul Riseborough
573bd7c7f3 AP_NavEKF3: Provide option to clip velocity and position innovations 2023-08-16 17:56:43 +10:00
Andrew Tridgell
484312df93 AP_NavEKF3: fixed race condition in updateFilterStatus()
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
2023-08-12 17:47:56 +10:00
Andy Piper
2f97a9855a AP_NavEKF3: fix docs on ABIAS_P_NSE_DEFAULT 2023-07-14 08:33:05 +09:00
Andrew Tridgell
d11d4fa0bf AP_NavEKF3: fixed velocity reset on AID_NONE
The ResetVelocity() function is only supposed to reset XY states, not
Z state. Resetting the Z state for velocity results in a large
velocity glitch if a vehicle is descending or ascending when aiding
switches to AID_NONE

this fixes #19386
2023-06-26 18:09:31 +10:00
Paul Riseborough
4d03327470 AP_NavEKF3: Fix sign error in position fix delay compensation 2023-06-08 16:31:22 +10:00
Paul Riseborough
3677cb025d AP_NavEKF3: Add handlers for external lat lng position set 2023-06-06 15:19:12 +10:00
Paul Riseborough
c91909032f AP_NavEKF3: Increase delta velocity bias state process noise
Required due to state variance collapse on ground with some systems using RTK GPS.
2023-05-24 10:31:26 +10:00
Paul Riseborough
1053daaf90 AP_NavEKF3: Strengthen recovery from bad delta velocity bias learning 2023-05-24 10:31:26 +10:00
Paul Riseborough
b1111e79fe AP_NavEKF3: Increase default value of EK3_ABIAS_P_NSE
This is required because some hardware setups with RTK GPS have experienced a collapse of the delta velocity state variances.
2023-05-24 10:31:26 +10:00
Paul Riseborough
ef76890f9b AP_NavEKF3: Retune and fix delta velocity bias state variance protection 2023-05-24 10:31:26 +10:00