This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and the various calls to the hal.scheduler->millis() object have been consolidated.
Unused variables have been removed
If the inertial solution velocity or position needs to be reset to the GPS, the stored state history for the corresponding states should also be reset.
Otherwise the next GPS measurement will be compared to an invalid previous state and will be rejected.
The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s
Re-initialisation of the magnetic field states and yaw angle is now only performed a maximum of two times after start-up.
Once when coming out of static modefor the first time (first arm event)
Again (for copter only) when the altitude gain above the arming altitude exceeds 1.5m
this prevents magnetic interference present at arming (eg arming on a metal roof)from corrupting the magnetic field states enough to cause bad heading errors and toilet bowling on copter
(Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector.
Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight.
For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
The GPS glitch offset was being zeroed during position resets. This caused the filter to reject subsequent GPS measurements if the GPS error persisted long enough to invoke a timeout and a position reset.
GPS measurement variance is doubled if only 5 satellites, and quadrupled if 4 or less.
The GPS glitch rejection thresholds remain the same
This will reduce the impact of GPS glitches on attitude.
The uncertainty in acceleration is currently only scaled using horizontal accelerations, however during vertical plane aerobatics and high g pullups, misalignment in angles can cause significant horizontal acceleration error.
The scaling now uses the 3D acceleration vector length to better work during vertical plane high g maneouvres.
This error was found during flight testing with 8g pullups
If the inertial solution velocity or position needs to be reset to the GPS or baro, the stored state history for the corresponding states should also be reset.
Otherwise the next GPS or baro measurement will be compared to an invalid previous state and will be rejected. This is particularly a problem if IMU saturation or timeout has occurred because the previous states could be out by a large amount
The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s.
Flying aerobatics with Trad Heli has shown that the divergence check can be false triggered when large magnetometer errors and GPS dropouts are present.
This can also happen with multi rotors if large yaw rates are present.
This was an unintended consequence of the ekfsmoothing patch which improved filter stability during high rate manoeuvres, but made the divergence test more sensitive.
This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
This allows a compass that has been declared failed, possibly because of
external disturbances (eg movement of hatches, proximity of tools, etc)
to be given a second chance when the vehicle is armed.
If the vehicle can fly without a compass (a fly forward vehicle)
then if the compass times out (large errors for more than 10 seconds,
then it will be declared permanently failed and will not be
used until the filter is reset
Analysis of copter logs has shown cases with a healthy EKF where spikes in EKF4.DS of up to 25% of the threshold have occurred.
A value of closer to 10% for normal operation is preferred.
Don't do bias estimation if tilted by more than 60 degrees to prevent scale
factor errors affecting result unnecessarily.
Prevent Kalman gain from having the wrong sign due to numerical errors
associated with small process noise values.
Allow smaller Z accel bias process noise values to be set
Bring Plane glitch protection thresholds into alignment with copter and
rover
Slight increase in accelerometer bias process noise to prevent bias
estimate divergence into limits (Rover and Plane only as Copter does not
seem respond as well to this change)
effective increase in threshold on divergence test to allow increased
margin for bad GPS velocities
Divergence is now detected by looking for very large changes in the gyro
bias. This will cause the filter to be reset and declared unhealthy for
10 seconds.
Don't reset filterDiverged status immediately during reset
Set filterDiverged true if covariance blows up
Add fault status reporting
This will enable in-flight magnetometer calibration to be inhibited unconditionally,
This is required for long balloon carriage flights where ground speed can be high
enough to put it into in-air state, but with very poor observability of magnetic field
states causing bad state estimates and heading offsets to develop over time.
The covariance matrix no longer has rows and columns artificially zeroed when in static
mode. Instead booleans indicating whether wind or magentic field state estimation is
active are used to:
a) Set the process noise on these states to zero to stop their variances from increasing
unchecked when not being updated, and
b) Turn off updates for these states when measurement fusion is being performed.
This reduces the likelihood of a badly conditioned covariance matrix forming
during static mode operation.
A filter divergence check has also been added that will declare the filter unhealthy if
position, velocity and magnetic field observations are all failing their innovation
consistency checks. This unhealthy status will persist for 10 seconds after the
condition clears.
AP_NavEKF: Remove unnecessary zeroing of wind covariances
A magnetometer axis that fails the innovation consistency check will cause
all axes not to be used. If this condition continues for 10 seconds, a
magnetometer timeout condition will be declared. When the timeout has
occurred, if it is not a fly forward vehicle, then individual channels
will be used again, but with a reduced weighting.
this will cope better with users with low roll/pitch gains, to ensure
they get enough control on takeoff
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
Flight testing with windup turns has shown that the position gate threshold
can be tripped with good GPS data causing position jerks. This increases the
initial GPS glitch rejection threshold to effectively 5m when using the
default POSNE_NOISE value of 0.5m.
This patch also cleans up the logic associated with use of the synthetic
sideslip measurement so that it can never be used for a non fly-forward
vehicle type
This patch adds functionality that initialises the wind-speed vector to the
reciprocal of the ground speed vector, and scaled to 6 m/s. On average this gives
a better initial wind velocity estimate on launch by assuming:
a) launch will be into wind
b) wind speed is equal to global average
It also helps prevent a headwind causing initial underestimation of airspeed
causing high autopilot gains and limit cycles on climb-out, until first
turn when the EKF is able to estimate the wind.
When using GPS after previously rejecting it, the GPS position will
always be offset if outside the specified glitch radius. This was the
original intent of the design and makes handling of glitches smoother.
It has been tested on replay using glitchy flight data
Aliasing can causes the bias estimate to fluctuate very rapidly as it tries
to keep up, which degrades the benefit of switching between
accelerometers to avoid aliasing.
This patch give a much more stable bias estimate during aliasing, and
allows the bias to adapt at a maximum rate of 1.0 m/s2 in 50 seconds
This adds new functionality to the detection and compensation of GPS
glitches:
1) A maximum allowable innovation is calculated using the GPS noise
parameter multiplied by the gate, with an additional component allowing
for growth in position uncertainty due to acceleration error since
the last valid measurement
2) Includes per vehicle type values for the acceleration error limit
3) If the innovation length exceeds the maximum allowable, no fusion occurs
4) If no fusion has occurred for long enough such that the position uncertainty
exceeds the maximum set by a per vehicle parameter or a maximum time, an offset
is applied to the GPS data to so that it matches the value predicted by the filter
5) The offset is never allowed to be bigger than 100m
6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to
be accommodated gradually
7) The default velocity innovation gate has been tightened up for copter and rover
8) The variance data logging output has been updated to make it more useful
This patch reduces the maximum acceptable GPS jump from approximately 16 to 8 metres
This will provide copters with more protection for close in loiter situations
This path reduces duplicated code, eliminates unused variables and
causes the earth magnetic field states to be reset when exiting static mode
which will occur every time copter is armed. This enables copters to be
powered on and initialised inside vehicles or houses, without bad earth
field values affecting flight.
Constraining variances to a minimum value of 1e-9 was causing problems
with gyro bias and angular accuracy in noisy GPS environments.
Because the constraint is applied after every covariance prediction
and correction, a lower value of 0 is more appropriate.
priseborough
Jonathan Challinger
Andrew Tridgell
Randy Mackay
Emile Castelnuovo