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.
Loiter is only a horizontal position controller so it should not set the
z-axis position.
Moved pos_control.set_speed and accel functions so order matches
init_loiter_targets function order
AC_PosControl::init_xy_controller() has been added to PosControl and is
called by init_loiter_target.
Hybrid is currently using set_loiter_target function to init the loiter
controller. So we have to call init_xy_controller() by set_loiter_target
function.
What happens otherwise?
In AC_PosControl::update_xy_controller, we update "now" with
now = hal.scheduler->millis();
and, as _last_update_xy_ms has not been updated previously by
init_xy_controller(), we just call init_xy_controller().
So, _dt_xy will be negative and used anyways in all the functions and
PID called by update_xy_controller.
That will avoid at least _accel_target.x/y to be set to 0 but I'm not
sure for the high values, probably an I_term that is not reset and
reached very high value.
Or maybe a cast error somewhere... no clue at all
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
Robert Lefebvre
Andrew Tridgell
priseborough
Randy Mackay
Ju1ien
Jonathan Challinger