Commit Graph

263 Commits

Author SHA1 Message Date
priseborough 13df6fb1c9 AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing'
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
2014-12-06 18:16:43 +11:00
priseborough dcb252cc05 AP_NavEKF : Fix bug in reset of position and velocity states
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
2014-12-06 18:16:43 +11:00
Jonathan Challinger 7692761f34 AP_NavEKF: Added getAccelNED function 2014-12-05 19:40:01 +09:00
priseborough 874d0780aa AP_NavEKF: Reduce vulnerability to ground fixed magnetic interference
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
2014-12-05 14:33:04 +11:00
Andrew Tridgell 46f601d703 AP_NavEKF: added getStaticMode() function 2014-11-22 18:27:51 +11:00
Andrew Tridgell 8acfbb2ee0 AP_NavEKF: add another health check in the EKF
if SV, SP and SH are all off then the most likely cause is divergence
of the EKF. This was done based on a flight log with bad gyro cal
2014-11-22 18:27:51 +11:00
Jonathan Challinger cc955b738b AP_NavEKF: Clean up flight detector logic 2014-11-14 10:34:49 +11:00
priseborough 5359da9c68 AP_NavEKF : Improved Magnetometer Error Handling
(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.
2014-11-14 10:34:48 +11:00
Jonathan Challinger 6c4d4713aa AP_NavEKF: Add parameter that specifies EKF-to-DCM/INAV fallback strictness 2014-11-06 10:03:52 +11:00
Jonathan Challinger 967986d5c6 AP_NavEKF: Split getAccelBias into getAccelZBias and getIMU1Weighting 2014-10-30 18:17:38 +11:00
Jonathan Challinger bb6d8fd44a AP_NavEKF: Set prevDelAng after using it 2014-10-30 18:17:37 +11:00
Jonathan Challinger 0727ac5c79 AP_NavEKF: clean up unused variable lastDivergeTime_ms 2014-10-30 18:17:37 +11:00
priseborough 5e51c09978 AP_NavEKF : Remove ineffective numerical divergence checks 2014-10-30 18:17:37 +11:00
priseborough 6f759fdb8e AP_AHRS : Tidy up logic used to enable synthetic sideslip fusion 2014-10-30 18:17:36 +11:00
priseborough 6a85753a5b AP_NavEKF : add INIT_GYRO_BIAS_UNCERTAINTY definition 2014-10-29 17:39:47 +09:00
priseborough fb14da7a4b AP_NavEKF : Add public method to reset gyro bias states 2014-10-29 15:32:21 +09:00
Randy Mackay 7d48704f82 AP_NavEKF: fix parameter descriptions
Errors pointed out by Richard (DK)
2014-10-27 22:20:03 +09:00
priseborough 2bc74934b8 AP_NavEKF: Track baro alt when pre-armed
This will help prevent spurious alt disparity warning messages for copter
2014-10-21 20:58:59 +09:00
Jonathan Challinger 6076f3fa22 AP_NavEKF: Quaternion::to_euler now uses references instead of pointers 2014-10-20 06:24:25 +11:00
priseborough 8aa267f75f AP_NavEKF : Fix bug in reset of GPS glitch offset
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.
2014-10-03 09:17:03 +10:00
Andrew Tridgell d6c56b8f7a AP_NavEKF: make it clear that all sat counts are covered 2014-10-01 12:55:29 +10:00
Andrew Tridgell a1f5be5b9e AP_NavEKF: simplify variable handling in EKF
use named state variables instead of states[] array where possible.
2014-10-01 12:55:29 +10:00
priseborough 8223a0d193 AP_NavEKF : Explicitly initialise gpsNoiseScaler to default value 2014-10-01 12:55:29 +10:00
priseborough b61a6c64d7 AP_NavEKF : Reduce weighting on GPS when not enough satellites
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.
2014-10-01 12:55:29 +10:00
priseborough f99f5759f5 AP_NavEKF : Fix bug in GPS innovation variance growth calculation
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
2014-10-01 12:55:29 +10:00
priseborough f0ee11e951 AP_NavEKF : Fix bug in reset of position, height and velocity states
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.
2014-10-01 12:55:28 +10:00
priseborough f71aeea61d AP_NavEKF : Reduce sensitivity on filter divergence check
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.
2014-09-17 10:22:55 +09:00
priseborough 36bf304f29 AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing'
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.
2014-09-17 10:22:49 +09:00
priseborough 5b72cb7610 AP_NavEKF : Clean up time stamps
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.
2014-09-17 10:22:45 +09:00
priseborough 0f62dbf6ed AP_NavEKF : Clear compass fail on transition between armed and disarmed
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.
2014-08-25 19:29:07 +10:00
Andrew Tridgell 5d40ec8014 AP_NavEKF: make use_compass() public 2014-08-24 21:00:24 +10:00
Andrew Tridgell 3ee3a71644 AP_NavEKF: prevent a possible numerical error on startup
fixes issue #1294
2014-08-08 15:55:20 +10:00
priseborough d5442fe64e AP_NavEKF : Modified compass fail logic
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
2014-07-31 21:12:11 +10:00
Andrew Tridgell 383070b9c0 AP_NavEKF: ensure get_position() fills in flags 2014-07-25 11:40:26 +10:00
Emile Castelnuovo 946a461873 VRBRAIN: added VRBRAIN to #if 2014-06-19 11:27:44 +02:00
priseborough edc79ca2a4 AP_NavEKF: Increase divergence test margin based on analysis of more user flight logs
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.
2014-05-24 22:20:24 +10:00
priseborough 5fe0d2c1b2 AP_NavEKF: Add protection for accel bias estimation errors
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
2014-05-18 08:09:00 +10:00
priseborough 3222e8f7cb AP_NavEKF: Default parameter adjustments
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
2014-05-18 08:08:49 +10:00
priseborough e40e50e2e1 AP_NavEKF: Prevent start-up transients re-tripping divergence test 2014-05-16 22:05:22 +10:00
priseborough a19015ed61 AP_NavEKF: Scale divergence check with covariance
This provides consistent behaviour for a range of gyro bias process
noise settings and automatically adjusts sensitivity as filter learns bias
2014-05-16 21:24:25 +10:00
priseborough 0337d44b2e AP_NavEKF: Fix bug causing immediate clearing of diverged status on reset 2014-05-16 21:24:04 +10:00
priseborough d150904dc6 AP_NavEKF: Increase gyro bias process noise
This is required to stop the bias estimate from becoming too static
towards the end of longer flights.
2014-05-16 21:24:04 +10:00
Andrew Tridgell 5edf4ba596 AP_NavEKF: fixed up handling of bitfields in faultStatus
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 21:42:39 +10:00
priseborough ce8e1daa16 AP_NavEKF: Improved divergence detection and reset
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
2014-05-15 21:14:21 +10:00
Andrew Tridgell 85ebe81ed3 AP_NavEKF: fixed logic for divergence test
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-11 18:00:38 +10:00
priseborough 40dc0e56fe AP_NavEKF: Add protection for badly conditioned innovation variances 2014-05-11 18:00:38 +10:00
priseborough 925c5751bd AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding
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
2014-05-11 18:00:38 +10:00
priseborough 3026ccee13 AP_NavEKF: Use baro data during launch transients whilst in static mode
Baro data can be used as it will still be valid during the launch and
is not a synthesised measurement.
2014-05-11 18:00:38 +10:00
priseborough 114bd56e2a AP_NavEKF: Fix display names in parameter list 2014-04-30 06:35:41 +10:00
priseborough 5fefce5899 AP_NavEKF: Synthetic sideslip fusion numerical error protections 2014-04-29 17:26:45 +10:00