Commit Graph

409 Commits

Author SHA1 Message Date
priseborough b56b68ce10 AP_NavEKF: Add public method reporting horizontal speed limit
This is required if using optical flow, as depending on height, the speed must be limited to prevent the sensor saturating
2014-12-06 18:16:50 +11:00
priseborough c8fb376cc4 AP_NavEKF: Fix bug in bias rate of change limiting
This doesn't change the behaviour, but means the result is achieved using the correct parameters, and comments are consistent
2014-12-06 18:16:50 +11:00
priseborough 599e53f3f2 AP_NavEKF: Prevent flow scale factor updating on ground 2014-12-06 18:16:49 +11:00
priseborough 083e22966c AP_NavEKF: Add public method to report available output data 2014-12-06 18:16:49 +11:00
priseborough e53d28854e AP_NavEKF: Add public method to inhibit GPS use and clean-up GPS use logic
This provides the calling vehicle software the abiity to request the EKF to not use GPS.
An integer is returned that indicates the type of operation available:
0 = request rejected (request will only be accepted if the EKF is in static mode, eg pre-armed)
1 = request accepted, attitude, vertical velocity and position estimates available
2 = request accepted, attitude, height rate, height, horizontal velocity and relative position estimates available
2014-12-06 18:16:49 +11:00
priseborough 8fb1d9cf8d AP_NavEKF: Fix bug preventing opt flow scale factor estimation
Also removes un-used variable declarations
2014-12-06 18:16:49 +11:00
priseborough a78920761c AP_NavEKF: Update comments in flow data interface 2014-12-06 18:16:49 +11:00
priseborough 52c7e56a4a AP_NavEKF: Add parameter for max valid optical flow rate magnitude 2014-12-06 18:16:49 +11:00
priseborough 6f8971d80a AP_NavEKF: Speed up flow sensor gyro bias correction 2014-12-06 18:16:49 +11:00
priseborough d7ad45ebda AP_NavEKF: Don't fuse flow measurements if too big 2014-12-06 18:16:49 +11:00
priseborough 84944fdd4e AP_NAvEKF: Revert back to 10Hz fusion 2014-12-06 18:16:49 +11:00
priseborough 1f78a73cdb AP_NavEKF: Make flow measurement delay vehicle specific
This takes into account the inter-sampling delay between the flow driver and the APM software which depends on the rate at which the dirver is being checked. This is 50Hz for plane and rover, and 200Hz for Copter.
2014-12-06 18:16:49 +11:00
priseborough 5f941655a8 AP_NavEKF: changes to support to 20Hz flow fusion rate 2014-12-06 18:16:48 +11:00
priseborough 0bf991eef9 AP_NavEKF: Fix bug in optical flow fusion smoothing 2014-12-06 18:16:48 +11:00
priseborough 839b814d12 AP_NavEKF : Inhibit flow and range fusion in static mode 2014-12-06 18:16:47 +11:00
priseborough 9b9934ac06 AP_NavEKF : fix velocity timeout bug during optical flow operation 2014-12-06 18:16:47 +11:00
priseborough 517026980b AP_NavEKF : Add fault and timeout logging 2014-12-06 18:16:47 +11:00
priseborough f640ec30ff AP_NavEKF : Reset held velocity on arming
This held velocity is used to constrain the drift in the optical flow solution before the vehicle is high enough for the optical flow sensor to work.
2014-12-06 18:16:47 +11:00
priseborough 06c19a3a4d AP_NavEKF : Update optical flow debug logging 2014-12-06 18:16:47 +11:00
priseborough 1791dec622 AP_NavEKF : Fix velocity hold mode 2014-12-06 18:16:47 +11:00
priseborough 84421e0a35 AP_NavEKF : Adjust initial flow scale factor and limits 2014-12-06 18:16:47 +11:00
priseborough 573633daf7 AP_NavEKF : Fix incorrect use of flow sensor time-stamp
flow sensor time stamp is now issued by the flow sensor and is on a different time-base
2014-12-06 18:16:47 +11:00
priseborough 9ea97c1a38 AP_NavEKF : Modify optical flow data interface
Sign conventions have changed
2014-12-06 18:16:46 +11:00
priseborough 8faeb190de AP_NavEKF : Rework logic to cope with bad flow data 2014-12-06 18:16:46 +11:00
priseborough 41f0231cfb AP_NavEKF : improve logic dealing with lack of flow or range data 2014-12-06 18:16:46 +11:00
priseborough 9ba46ad795 AP_NavEKF : Improve handling of flow sensor failure 2014-12-06 18:16:46 +11:00
priseborough 2ee1c549be AP_NavEKF : improve criteria used to inhibit scale factor estimation 2014-12-06 18:16:46 +11:00
priseborough e09ff84218 AP_NavEKF : Don't estimate focal length scale factor without reliable velocity 2014-12-06 18:16:46 +11:00
priseborough 4c92a5f23f AP_NavEKF : Use GPS velocity if PX4Flow sensor fails 2014-12-06 18:16:46 +11:00
priseborough 283811edcb AP_NavEKF : stop velocity change when flow measurements drop out 2014-12-06 18:16:46 +11:00
priseborough 9132fcfe43 AP_NavEKF : Change mag fusion to use corrected compass values
This enables compass units to be switched in-flight
2014-12-06 18:16:46 +11:00
priseborough c3be486c29 AP_NavEKF : Enable inhibiting of GPS measurements 2014-12-06 18:16:45 +11:00
priseborough 79698f7742 AP_NavEKF : improvements to range finder fusion 2014-12-06 18:16:45 +11:00
priseborough 1222559da0 AP_NavEKF : Fix bug affecting sonar fusion 2014-12-06 18:16:45 +11:00
priseborough 117bd2a998 AP_NavEKF : Add time based noise to terrain offset state 2014-12-06 18:16:45 +11:00
priseborough b1d3d5d9a3 AP_NavEKF : Don't update focal length scale factor at low speeds 2014-12-06 18:16:45 +11:00
priseborough 8dd1081f54 AP_NavEKF : Add range measurement to EKF debug message 2014-12-06 18:16:45 +11:00
priseborough 744c72d40b AP_NavEKF : Updates to range finder fusion 2014-12-06 18:16:45 +11:00
priseborough a20729f60f AP_NavEKF : Update names in optical flow debug logging 2014-12-06 18:16:45 +11:00
priseborough 235b3bebda AP_NavEKF : Add range finder health and enable range finder fusion
The 2-state auxiliary filter is also renamed.
2014-12-06 18:16:45 +11:00
priseborough 988de2a898 AP_NavEKF : Add smoothing to optical flow fusion 2014-12-06 18:16:44 +11:00
priseborough 70c779dbc2 AP_NavEKF : Optical flow numerical optimisations 2014-12-06 18:16:44 +11:00
priseborough 2a1420171a AP_NavEKF : reduce minimum accepted flow quality 2014-12-06 18:16:44 +11:00
priseborough 3fbedc9f98 AP_NAvEKF : update comments 2014-12-06 18:16:44 +11:00
priseborough 81b09c9361 AP_NavEKF : temporary mods to test use of flow sensor internal gyro data 2014-12-06 18:16:44 +11:00
priseborough cd418c946c AP_NavEKF : preliminary implementation for optical flow and range finder fusion
Range finder measurements are not input to EKF at this time, however the method for fusing them is
implemented.
2014-12-06 18:16:44 +11:00
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
priseborough ecc8e45eda AP_NavEKF: Fix bug in position reset logic 2014-04-23 18:16:02 +10:00
Andrew Tridgell 25667a11a0 AP_NavEKF: use AHRS vehicle class for sideslip calculation
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-21 18:13:12 +10:00
Andrew Tridgell 5acd17b843 AP_NavEKF: cleanup some build warnings
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-21 18:13:12 +10:00
priseborough bd28cdbdcf AP_NavEKF: Improved magnetometer consistency checks
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.
2014-04-21 16:31:31 +10:00
Andrew Tridgell 610a930612 AP_NavEKF: catch covarience errors and reset filter
this catches covariance values beyond a reasonable limit and resets
the filter is they happen
2014-04-21 15:37:08 +10:00
Andrew Tridgell 4756dbee84 AP_NavEKF: fixed millisecond subtraction for rollover
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-21 13:16:20 +10:00
Andrew Tridgell 68f1ae3036 AP_NavEKF: fixed some matlab ! -> ~ typos
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-20 21:44:37 +10:00
Andrew Tridgell 4abc2999a6 AP_NavEKF: start with a wind estimate of 3m/s, when no direct measurement
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>
2014-04-13 19:42:23 +10:00
priseborough d745dc2b6f AP_NavEKF : Increased position gate default to reduce impact of accel errors
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.
2014-04-13 19:37:22 +10:00
priseborough 188bea6bab AP_NavEKF : Enable calc of wind velocity when not using airspeed sensing
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.
2014-04-13 19:37:19 +10:00
Emile Castelnuovo 25f1c5774f AP_NavEKF: added #defines for VRBRAIN board 2014-04-08 16:19:19 +10:00
priseborough db043744a4 AP_NavEKF : Reduce Z accel bias process noise to provide a more stable estimate 2014-04-07 21:08:25 +10:00
priseborough 1f8b5a6d23 AP_NavEKF : Tighten GPS velocity glitch gate default setting for plane 2014-04-07 21:08:15 +10:00
priseborough 0c2489b07b AP_NavEKF : Fix bug in logging of airspeed innovation consistency ratio 2014-04-07 21:01:00 +10:00
priseborough 95c83255d7 AP_NavEKF : adjust default values for accelerometer process noise
Slows down estimate and allows for smaller values to be set
2014-04-07 21:00:43 +10:00
Andrew Tridgell 07d621c4be AP_NavEKF: used state structure in more places
makes the code a bit easier to read

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 21:30:16 +11:00
priseborough aa5335b16e AP_NavEKF : Improved GPS position glitch handling
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
2014-04-04 21:01:22 +11:00
priseborough b1786cf1e5 AP_NavEKF : Do not reset on GPS velocity timeout if good position data 2014-04-04 21:01:20 +11:00
priseborough d25883f712 AP_NavEKF : Limit rate of Z accel bias adaptation
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
2014-04-04 21:01:18 +11:00
priseborough 36b3cbc365 AP_NavEKF : Relax aliasing check limits
This reduces the chance of noisy data inhibiting GPS glitch protection
2014-04-04 21:01:15 +11:00
priseborough 0ae736c3a0 AP_NavEKF : clean up convoluted logic used in sensor health checks
This doesn't change the behavior, but does make the code easier to
understand
2014-04-04 21:01:13 +11:00
Andrew Tridgell cd3038fabd AP_NavEKF: fixed return of offset in getVariances() 2014-04-03 07:49:06 +11:00
Andrew Tridgell a7f213a5c5 AP_NavEKF: use pythagorous2() 2014-04-01 22:13:03 +11:00
priseborough 04036d7777 AP_NavEKF : Updated GPS glitch protection logic
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
2014-04-01 21:22:14 +11:00
Andrew Tridgell 5a2e84e792 AP_NavEKF: changes for new GPS API 2014-04-01 06:38:24 +11:00
priseborough 9c5f564dc5 AP_NavEKF : Fixes bug in initial earth magnetic field states
The calculation for these states was not being bias corrected
2014-03-30 07:43:52 +11:00
priseborough e1819bb53a AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane 2014-03-29 14:06:43 +11:00
priseborough 55c60b8f07 AP_NavEKF : Reduce Magnetometer innovation consistency check threshold 2014-03-26 21:29:47 +11:00
priseborough 45b1a2fa46 AP_NavEKF : Reduce GPS position innovation consistency fail threshold
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
2014-03-26 21:29:45 +11:00
Andrew Tridgell bafc664750 AP_NavEKF: use APM_BUILD_TYPE() macro 2014-03-26 12:42:11 +11:00
Andrew Tridgell e3792f9b26 AP_NavEKF: added a mechanism for per-vehicle-type defaults in EKF 2014-03-26 12:30:17 +11:00
priseborough 17cdac7bc8 AP_NavEKF : Clean up angle and mag field initialisation
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.
2014-03-24 19:54:48 +11:00
priseborough 7ae86b3979 AP_NavEKF : Track baro height observations pre-arm
This prevents copter from failing the pre-arm height discrepancy test
2014-03-23 18:25:50 +11:00
priseborough 7780d55788 AP_NavEKF : Prevent large magnetic field adaptations early in flight 2014-03-22 19:55:26 +11:00
Paul Riseborough f7007569d1 AP_NavEKF : Fix bug that limits copter sensor delay compensation to 125
msec
2014-03-21 21:52:20 +11:00
Paul Riseborough b2c0979947 AP_NavEKF : Fixes bug that causes accel bias to diverge in static mode, preventing copter arming 2014-03-21 21:49:50 +11:00
Paul Riseborough 8a2d16d13d AP_NavEKF : modifed Zaccel bias noise parameter to prevent unstable bias estimate 2014-03-20 08:50:00 +11:00
Paul Riseborough dacba5d911 AP_NavEKF : Update default tuning parameters (for plane use) 2014-03-14 06:07:58 +11:00
Paul Riseborough d0828d9c15 AP_NavEKF : Prevent bad user parameter causing incorrect GPS fusion 2014-03-14 06:07:58 +11:00
Paul Riseborough a24bfc1b8a AP_NavEKF : Use synthetic sideslip fusion during GPS denied operation with airspeed 2014-03-14 06:07:57 +11:00
Paul Riseborough 35811758d7 AP_NavEKF : Update comments and remove un-used function declarations 2014-03-14 06:07:57 +11:00
Paul Riseborough 784034170d AP_NavEKF : increased useage of helper functions 2014-03-10 21:54:03 +11:00
Paul Riseborough 7b3130cfcc AP_NaVEKF : Enable operation without airspeed and compass 2014-03-10 21:53:09 +11:00
Paul Riseborough 3a5acb9cea AP_NavEKF : improved on-ground, in-air check and GPS yaw alignment 2014-03-10 14:07:34 +11:00
Paul Riseborough b47a11edf6 AP_NavEKF : improved static and on-ground mode selection logic 2014-03-10 04:39:59 +11:00
Paul Riseborough 15a44571c2 AP_NavEKF : enable operation without a compass for planes 2014-03-09 20:01:45 +11:00
Paul Riseborough 2019708056 AP_NavEKF : Make all compass use autodetect 2014-03-09 19:59:19 +11:00
Paul Riseborough e485b246e7 AP_NavEKF : update comments 2014-03-09 19:59:18 +11:00
Paul Riseborough c8bd415b00 AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer 2014-03-09 19:59:17 +11:00
Paul Riseborough 52fabc822f AP_NavEKF : Add low speed magnetometer calibration option 2014-03-09 19:59:16 +11:00
priseborough 6ace31b6c1 AP_NavEKF : Fix bug that leaves height unconstrained in static mode 2014-03-09 19:27:21 +11:00
Paul Riseborough 94ff7522fc AP_NavEKF : Fix bug in initial East mag field state variance 2014-03-08 07:44:34 +11:00
Andrew Tridgell 449d09051e AP_NavEKF: cope with compass going offline while in flight 2014-03-02 14:32:10 +11:00
Andrew Tridgell 77f91e6250 AP_NavEKF: don't assume the number of gyros == number of accels 2014-03-02 13:53:19 +11:00