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.
