Commit Graph

662 Commits

Author SHA1 Message Date
Andrew Tridgell 0ca534bfab AP_NavEKF: only call calcGpsGoodToAlign if we need to
avoid calling it once we have an origin. This avoids some calculations
and string operations
2015-09-08 16:07:33 +10:00
Andrew Tridgell c10ce3ffbd AP_NavEKF: added prearm_failure_reason() 2015-09-08 16:07:33 +10:00
Paul Riseborough 21f873662c AP_NavEKF: Prevent false triggering of optical flow takeoff detection
Now that we have a pre-arm check in place to detect bad lidar, the motion check is unnecessary and can false trigger for copters with flexible undercarriages or on uneven ground.
2015-08-27 20:57:38 +09:00
Randy Mackay ec2bc5e41d NavEKF: use maxf when retrieving vibration levels
This reduces the performance hit that was caused by multiple calls to get_vibration_levels
2015-08-19 16:44:36 +09:00
Randy Mackay 629a5fd714 NavEKF: IMUSwitchState enum 2015-08-19 16:44:31 +09:00
Randy Mackay c179ed5253 NavEKF: IMU ratio set to primary accel when neither IMU is used
This forces the EKF IMU ratio to indicate which IMU is used except that it will be "0" in the unlikely case that the third IMU is used
2015-08-19 16:44:28 +09:00
Paul Riseborough 559cc29635 AP_NavEKF: Add hysteresis to IMU switching logic 2015-08-19 16:44:26 +09:00
Paul Riseborough 12e884ba6b AP_NavEKF: Modify method used to check for vibration errors
This method checks for consistency between accelerometer readings and switches to the unit with the lowest vibration of the difference exceeds 0.3g
The threshold of 1.7 m/s/s corresponds to a maximum tilt error of 10 deg assuming one IMU is good, one is bad and the EKF is using the bad IMU.
2015-08-19 16:44:24 +09:00
Randy Mackay 0b981d38e8 NavEKF: incorporate use_gyro and use_accel 2015-08-19 16:44:17 +09:00
Randy Mackay 8b17d32994 NavEKF: check baro health before consuming 2015-08-12 16:32:48 -07:00
Gustavo Jose de Sousa f7fd9e3d99 AP_NavEKF: standardize inclusion of libaries headers
This commit changes the way libraries headers are included in source files:

 - If the header is in the same directory the source belongs to, so the
 notation '#include ""' is used with the path relative to the directory
 containing the source.

 - If the header is outside the directory containing the source, then we use
 the notation '#include <>' with the path relative to libraries folder.

Some of the advantages of such approach:

 - Only one search path for libraries headers.

 - OSs like Windows may have a better lookup time.
2015-08-11 16:38:20 +10:00
Randy Mackay 0acfcbfa81 NavEKF: use primary compass instead of first instance
Thanks very much to staroselskii for finding this bug
2015-07-17 10:02:42 +09:00
Paul Riseborough 610595bfb9 AP_NavEKF: Prevent position drift between arming and takeoff when using OF 2015-07-13 10:54:36 +09:00
Paul Riseborough 8b4b0b9576 AP_NavEKF: Add accelerometer clip monitoring and isolation
Isolates an accelerometer that is clipping  and attempts to use the one that is clipping less.
2015-07-12 16:49:19 +09:00
Paul Riseborough 4fb7beba4b AP_NAvEKF: Prevent failure due to magnetometer fusion numerical errors
If a badly conditioned covariance matrix causes negative innovation variances, then the filter will diverge. The previous approach of increasing process noise was not effective in some cases, so a hard reset of the covariance matrix has been adopted to guarantee recovery.
This fixes a numerical error observed using the replay on flight log which had significant periods of compass rejection.
2015-07-02 18:07:22 +10:00
Andrew Tridgell 686b1137fa AP_NavEKF: added vehicle specific initial gyro bias uncertainty 2015-06-30 16:16:26 +10:00
Randy Mackay 9f17fc17ab NavEKF: update parameter descriptions 2015-06-24 16:25:14 +09:00
Paul Riseborough 87e6a24154 AP_NavEKF: Add public method to reset EKF height datum and baro alt 2015-06-24 16:25:11 +09:00
Paul Riseborough e3ccb74e12 AP_NavEKF: Publish amount of in-flight yaw angle reset 2015-06-24 11:49:29 +09:00
Paul Riseborough 6ba8cd05ff AP_NavEKF: Fix bug preventing GPS vertical velocity being used
This fixes a bug that prevents GPS velocity being used permanently if there is a temporary unavailability
2015-06-18 09:01:21 +10:00
Andrew Tridgell c9988d8b37 AP_NavEKF: rely on delta_velocity and delta_angles always being available 2015-06-17 13:10:26 +10:00
Andrew Tridgell a115182041 AP_NavEKF: fixed minimum deltat to be 100us 2015-06-17 13:10:26 +10:00
Andrew Tridgell c221959d5a AP_NavEKF: fixed corrected del velocity to use both accels 2015-06-17 13:10:25 +10:00
Paul Riseborough d3f83e8ec6 AP_NavEKF: Improve gyro bias learning rate for plane and rover
This patch increases initial gyro bias uncertainty and plane and rover specific process noise to improve the rate of gyro bias learning.
This reduces the likelihood of a navigation failure due to rapid temperature changes in the inertial sensors causing rapid changes in zero rate offset.

The increase in process noise cannot be applied to Copter due to different numerical stability limits arising from the faster update rate.
2015-06-03 11:46:17 +10:00
Andrew Tridgell d5da9fcca6 AP_NavEKF: fixed build warnings 2015-05-30 22:51:30 +10:00
Andrew Tridgell efd958f0b0 AP_NavEKF: rename file to remove spaces
spaces in file names break shell scripts
2015-05-23 17:14:52 +10:00
Andrew Tridgell 411a96b4b3 AP_NavEKF: take at most 30s to accept DCM attitude
velocity noise on the GPS can cause DCM to take a long time to
converge. If it takes longer than 30s then allow EKF to start
2015-05-20 15:22:15 +10:00
Andrew Tridgell 92c4c5cbcf AP_NavEKF: change using_gps threshold to 4s
this means plane will continue to use dead reckoning with GPS 3D lock
for 4s
2015-05-19 20:52:39 +10:00
Paul Riseborough 69ca654194 AP_NavEKF: Update declination when we know our location
This ensures that when we start using GPS, that the EKF will be using the correct declination for that location
If declination is not known it defaults to zero
2015-05-19 20:35:52 +10:00
Paul Riseborough c1c5e3598a AP_NavEKF: Enforce alignment of realigned earth mag field with declination
This prevents bad inertial or GPS data combined with the post takeoff heading alignment check used by plane from resulting in earth field states that have an incorrect declination
2015-05-19 20:35:52 +10:00
Paul Riseborough b1d8805114 AP_NavEKF: Reduce takeoff detection sensitivity
This patch reworks the in-air transition criteria to reduce the likelihood of false positives and to ensure that there will be enough ground speed to make the heading check work reliably.
2015-05-19 20:35:52 +10:00
Andrew Tridgell 6154db5cf8 AP_NavEKF: added using_gps status bit 2015-05-17 21:24:57 +10:00
Tom Pittenger e987173ffb AP_NavEKF: float to double promotion via tan instead of tanf 2015-05-16 08:21:49 +10:00
Paul Riseborough bd91b9727f AP_NavEKF: Add pre-arm check for horizontal inertial errors
This check will declare the EKF as unhealthy if the horizontal position innovations exceed a threshold  before motors are armed.
This will help to prevent a takeoff with bad inertial data caused by bad accel or gyro offsets.
2015-05-15 10:35:25 +09:00
Paul Riseborough cb59570938 AP_NavEKF: Remove bug preventing external selection of optical flow mode 2015-05-09 18:31:18 +10:00
Tom Pittenger 2deb0e08ea AP_NavEKF: replace fabs() with fabsf() 2015-05-09 09:57:27 +10:00
Paul Riseborough a232606fc9 AP_NavEKF: Prevent EKF yaw errors due to fast yaw spins
Prolonged yaw rotations with gyro scale factor errors can cause yaw errors and gyro bias estimation errors to build up to a point where EKF health checks fail.
This patch introduces the following protections:

1) The assumed yaw gyro error is scaled using a filtered yaw rate and an assumed 3% scale factor error (MPU6000 data sheet)
2) When the filtered yaw rate magnitude is greater than 1 rad/sec, the Z gyro bias process noise is zeroed and the state variance set to zero to inhibit modification of the bias state
3) When the filtered yaw rate magnitude is greater than 1 rad/sec, the magnetometer quaternion corrections are scaled by a factor of four to maintain tighter alignment with the compass
2015-05-08 16:47:31 +09:00
Paul Riseborough 08382373f1 AP_NavEKF: Reduce likelihood of in-air switch to GPS with a bad heading
Increases magnetometer weighting on yaw corrections when there there is no other aiding to constrain yaw drift.
Prevents switch to GPS if magnetometer data is failing innovation checks which indicates a bad yaw angle
2015-05-08 16:47:29 +09:00
Jonathan Challinger 142e018a18 AP_NavEKF: fix bug causing takeoff to fail 2015-05-08 09:15:45 +09:00
Andrew Tridgell a140a5e77f AP_NavEKF: revert AP_Math class change 2015-05-05 13:27:07 +10:00
Tom Pittenger 9b53961a7d AP_NavEKF: compiler warnings: float to double 2015-05-05 13:26:52 +10:00
Tom Pittenger 3276eec30e AP_NavEKF: compiler warnings: apply is_zero(float) or is_equal(float) 2015-05-05 13:26:52 +10:00
Andrew Tridgell c4d6f9d040 AP_NavEKF: fix for HAL_SITL rename 2015-05-05 09:45:55 +10:00
Andrew Tridgell ce0a863d01 AP_NavEKF: allow for optimisation of the EKF
disable optimisation when debugging needed
2015-05-05 09:45:53 +10:00
Paul Riseborough be04be9b43 AP_NavEKF: Fix bug preventing home position being set by copter
The interface definition has been modified so that it returns true for a position obtained usin geither the normal inertial navigation calculation, or a raw GPS measurement.
This enables this function to be used to set a home position before flight.
2015-05-02 17:20:21 +09:00
Randy Mackay fb7e75594f AP_NavEKF: replace memset with quaternion.initialise 2015-05-01 16:37:35 +09:00
Randy Mackay c366fbbc96 AP_NavEKF: remove unused haveDeltaAngles 2015-05-01 16:37:33 +09:00
Paul Riseborough b71b8f4bda AP_NavEKF: Always return a NED relative position if possible
If a calculated position is not available, the function will return a value based on raw GPS or last calculation if available, but the status will be set to false to indicate that it cannot be used for control.
2015-05-01 16:37:32 +09:00
Paul Riseborough 20ba1e6b1b AP_NavEKF: Always return a WGS-84 location if possible
If a calculated location is not available, the function will return raw GPS data if available, but the status will be set to false.
2015-05-01 16:37:31 +09:00
Paul Riseborough dd1e0b2f0b AP_NavEKF: Add missing function to return the NED to XYZ quaternion 2015-05-01 16:37:30 +09:00
Paul Riseborough 2c4572eb50 AP_NavEKF: Make ground effect variable names more descriptive 2015-05-01 16:37:29 +09:00
Paul Riseborough 3986851c51 AP_NavEKF: Prevent Z accel bias changing during ground effect takeoff
The inconsistent baro data during ground effect takeoff combined with the larger variances in the Z accel bias state early in flight can cause unwanted changes in bias estimate and therefore changes in height estimation error.
This patch turns of the process noise and state updates for the Z accel bias state when takeoff in ground effect is expected.
2015-05-01 16:37:28 +09:00
Paul Riseborough 2cba60c731 AP_NavEKF: Decouple takeoff ground effect compensation from arm transtion
Thsi fixes a potential bug where the vehicle could land at a lower location without disarming and re-enter ground effect takeoff mode wiht a baro height floor above the current altitude, causing unpredictable height gain
2015-05-01 16:37:27 +09:00
Paul Riseborough c4c0f819b5 AP_NavEKF: Ensure Covariance initialisation uses correct IMU time step 2015-05-01 16:37:26 +09:00
Paul Riseborough 5dc29699ab AP_NavEKF: Publish the INS delta quaternion 2015-05-01 16:37:25 +09:00
Paul Riseborough d273302ce7 AP_NavEKF: Improve rate of accel bias learning before flight 2015-05-01 16:37:24 +09:00
Paul Riseborough 8dc6354a54 AP_NavEKF: Prevent touchdown baro errors tripping height innovation check
Ground effect baro errors can cause a spike in height innovation on disarming if ground effect compensation was used during the landing. This causes a transient AHRS fault message if this innovation is outside the pre-arm check limits.
Resetting the vertical position state to the measured height after disarming prevents this.
2015-05-01 16:37:23 +09:00
Jonathan Challinger 2f38dd1b67 AP_NavEKF: use quaternion functions to apply IMU delta angles 2015-05-01 16:37:22 +09:00
Jonathan Challinger 9c374eb4a8 AP_NavEKF: apply constrained floor to barometer innovation while landing 2015-05-01 16:37:21 +09:00
Jonathan Challinger 3c6446fadd AP_NavEKF: refactor meaHgtAtTakeOff filter 2015-05-01 16:37:20 +09:00
Paul Riseborough 744de74c16 AP_NavEKF: Reduce ground effect baro induced height errors during takeoff 2015-05-01 16:37:19 +09:00
Paul Riseborough 13616d6436 AP_NavEKF: Add takeoff and touchdown expected to reported filter status 2015-05-01 16:37:19 +09:00
Jonathan Challinger 8c92524b8a AP_NavEKF: add takeoffExpected and touchdownExpected 2015-05-01 16:37:18 +09:00
Jonathan Challinger b6b55bf6f2 AP_NavEKF: integrate gravity over correct time period 2015-05-01 16:37:17 +09:00
Jonathan Challinger cb0c424da1 AP_NavEKF: rewrite readIMUData 2015-05-01 16:37:16 +09:00
Andrew Tridgell 514c349060 AP_NavEKF: raise DCM error_rp threshold to 0.05
it was failing on many boards with fixed wing
2015-04-28 14:07:39 +10:00
Randy Mackay fdf226ebc1 AP_NavEKF: include AP_RangeFinder.h 2015-04-24 15:49:11 +09:00
Tom Pittenger a340d13d01 NavEKF: fix compile warning re member init order 2015-04-24 14:14:12 +09:00
Tom Pittenger d6b34209b5 NavEKF: fix compile warnings re float constants 2015-04-24 14:04:02 +09:00
Randy Mackay b05bdd657d AP_NavEKF: getHeightControlLimit modifies height on success 2015-04-24 10:58:05 +09:00
Paul Riseborough cf04600710 AP_NavEKF: Add public function to limit height control during OF nav 2015-04-24 10:57:56 +09:00
Paul Riseborough 68b225de4d AP_NavEKF: Prevent pre-arm baro drift affecting mag field reset height 2015-04-24 10:57:54 +09:00
Paul Riseborough c2e6fdb56c AP_NavEKF: Reduce effect of noisy baro data on baro height offset estimate
This estimate is used to offset baro data if we need to switch across from range finder data due to sensor failure. The previous filter coefficients gave a 0.5 seconds time constant on the offset which was too susceptible to baro noise.
2015-04-24 10:57:51 +09:00
Paul Riseborough fd7fdc1ad9 AP_NavEKF: Remove unnecessary state resets on arm and disarm
Resetting states unnecessarily creates transients due to presence of bias errors
2015-04-24 10:57:49 +09:00
Paul Riseborough 3e67080002 AP_NavEKF: Use GPS vertical velocity observations to estimate Z accel bias 2015-04-24 10:57:47 +09:00
Paul Riseborough e48171ab11 AP_NavEKF: Improve takeoff with optical flow and range data loss
Range finder and optical flow data can drop-out and be reliable very close to ground. these patches enable the takeoff to be more relaibly detected and constrain optical flow navigation drift in the first part of takeoff.
2015-04-24 10:57:45 +09:00
Paul Riseborough 3e061b174e AP_NavEKF: Report unhealthy for all filter faults 2015-04-24 10:57:43 +09:00
Paul Riseborough 6c4c54c2ba AP_NavEKF: Use default on ground range parameter from range finder object 2015-04-24 10:57:41 +09:00
Jonathan Challinger 10476333d8 AP_NavEKF: don't run when previously disarmed and time has slipped 2015-04-24 10:57:39 +09:00
Jonathan Challinger 085faaac6a AP_NavEKF: fix delay detection so that filter properly resets after a delay 2015-04-24 10:57:36 +09:00
Paul Riseborough 1c8e3f9444 AP_NavEKF: Allow EKF to pull data from range finder object 2015-04-24 10:57:01 +09:00
Paul Riseborough 4a5bf0a266 AP_NavEKF: Reduce EKF start time
Makes EKF start conditional on DCM solution tilt error
2015-04-23 20:35:48 +09:00
Arthur Benemann e59c6ddbae AP_SmallEKF: increase start-up time of the SmallEKF 2015-04-21 21:45:51 +09:00
Arthur Benemann 4ad3e786a5 AP_NavEKF: fix initialization of the SmallEKF
The constructor 'states' variable was not being called. To make sure other variables where also zeroed now
2015-04-21 21:45:36 +09:00
Arthur Benemann 1d9beed42f AP_SmallEKF: add function to report if the EKF is stable 2015-04-21 21:45:29 +09:00
Arthur Benemann 7b28bf7d44 AP_NavEKF: Remove small EKF dependency on navigation EKF 2015-04-21 21:45:28 +09:00
Andrew Tridgell 4586de6637 AP_NavEKF: enable optimisation in Linux build
only really need debugging for SITL
2015-04-16 08:36:16 +10:00
Paul Riseborough dffa2e19bf AP_NavEKF: Make copter glitch accel consistent with timeout and radius 2015-04-15 17:32:48 +09:00
Paul Riseborough 5d70854c08 AP_NavEKF: Fix minor bug in calculation of innovation variance
the innovation variance for GPS should be the sum of squares of the state and measurement uncertainty.
2015-04-15 17:32:46 +09:00
Paul Riseborough 1008c6390c AP_NavEKF: Fail absolute position status if GPS repeatedly rejected 2015-04-15 17:32:44 +09:00
Paul Riseborough 77d3798278 AP_NavEKF: Reduce recovery time after a GPS fusion timeout 2015-04-15 17:32:41 +09:00
Paul Riseborough 0852aeab6e AP_NavEKF: Allow raw innovations to be monitored during timeouts 2015-04-15 17:32:39 +09:00
Paul Riseborough d3f4b4a02b AP_NavEKF: Fix name consistency for data check time stamps 2015-04-15 17:32:36 +09:00
Paul Riseborough e79ccf1fcc AP_NavEKF: Fix bug allowing terrain to be above vehicle position
The terrain state and vehicle state need to be compared at the same time horizon.
2015-04-11 15:51:08 +09:00
Paul Riseborough 6d58c63c4c AP_NavEKF: Prevent potential divide by zeros in OF fusion 2015-04-11 15:51:03 +09:00
Paul Riseborough 89142f1c5f AP_NavEKF: Prevent inadvertent use of DCM roll and pitch estimates.
the use of roll and pitch from the AHRS object is bad because that object could be returning estimates from the backup DCM algorithm.
2015-04-11 15:19:05 +09:00
Paul Riseborough 9268024094 AP_NavEKF: Update default parameters for copter optical flow fusion 2015-04-10 11:08:11 +09:00
Paul Riseborough 4fbdab27ff AP_NavEKF: Use range finder for primary hgt ref in opt flow mode
Falls back to baro if range finder is unavailable
Adds parameter enabling user to select which height source (baro or range finder) will be used during optical flow nav.
2015-04-10 11:08:07 +09:00
Paul Riseborough d618c55e2f AP_NavEKF: Improved handling of noisy GPS speed accuracy data 2015-04-10 11:07:34 +09:00
Paul Riseborough e98edaa6cb AP_NavEKF: Return more accurate validity status for height above ground 2015-04-10 11:07:24 +09:00
Paul Riseborough 586e4a7d2b AP_NavEKF: Add Matlab derivations and simulations behind small EKF 2015-04-10 11:07:21 +09:00
Paul Riseborough c57e25142c AP_NavEKF: Update optical flow fusion maths to reduce height errors 2015-04-07 20:51:18 -07:00
myly10 55befdc345 AP_NavEKF: Typo correction for EAS_NOISE description 2015-04-06 15:52:29 -07:00
Paul Riseborough 9b3656e77c AP_NavEKF: Fix bug introduced in Y axis flow fusion 2015-04-05 21:17:20 -07:00
Andrew Tridgell baf292def1 AP_NavEKF: prevent float exception on startup 2015-04-05 09:16:14 -07:00
Paul Riseborough 7fc0f026d2 AP_NavEKF: Fix bug in optical flow innovation variance integrity check
The check allowed negative innovation variances to pass. If this did occur, the filter would diverge.
2015-04-04 17:03:06 -07:00
Andrew Tridgell c1a0375562 AP_NavEKF: prevent divide by zero in EKF logging 2015-04-04 07:09:02 -07:00
Paul Riseborough 10f050c53b AP_NavEKF: Prevent baro drift causing toilet bowling
The magnetic field states are reset once at 1.5 metres and again at 5 metres. This height check was using the height at the first arm event as the reference. In the situation where there is baro drift and extgended time between the first arm event and flight, this can cause the magnetic field state to be reset when on the ground. If this happens when flying off a metallic surface, the resultant heading errors can cause sever toilet bowling.
2015-04-03 15:18:42 -07:00
Paul Riseborough d4c60ca956 AP_NavEKF: Fix bug preventing reset of velocity after OF fusion timeout 2015-04-03 15:18:39 -07:00
Jonathan Challinger 95cd3480ec AP_NavEKF: review all uses of dtIMU and use dtIMUactual where necessary
pair-programmed-with: Paul Riseborough <p_riseborough@live.com.au>
2015-04-03 15:18:09 -07:00
Andrew Tridgell 79b44d3988 AP_NavEKF: initialise gndEffectMode 2015-04-03 15:15:11 -07:00
Andrew Tridgell 3165c43dfe AP_NavEKF: initialise gpsSpdAccuracy 2015-04-03 15:15:11 -07:00
Paul Riseborough 98c32012fa AP_NavEKF: remove accel bias rate limit when disarmed 2015-04-03 15:15:11 -07:00
Paul Riseborough fe76cb4c0b AP_NavEKF : Make initial height variance consistent with baro noise
This makes sense to do because we initialise the state to the instantaneous baro reading
2015-04-03 15:15:11 -07:00
Paul Riseborough a976e9dad2 AP_NavEKF : Fix bug in scaling of initial Z accel bias state variance 2015-04-03 15:15:11 -07:00
Paul Riseborough 92df3adb5e AP_NavEKF : Fix bug in Z accel bias update for IMU1 2015-04-03 15:15:10 -07:00
Paul Riseborough 5d0952ba23 AP_NavEKF: eliminate onGndBaroNoise 2015-04-03 15:15:10 -07:00
Paul Riseborough fafb898341 AP_NavEKF: tuning change to accel bias learning 2015-04-03 15:15:10 -07:00
Paul Riseborough 398accd151 AP_NavEKF: Improve pre-flight ready checking 2015-04-03 15:15:10 -07:00
Paul Riseborough 5c1a226bef AP_NavEKF : Improvements to pre-arm IMU bias estimation 2015-04-03 15:15:10 -07:00
Jonathan Challinger a5924acb3d AP_NavEKF: set dtIMU from ins expected sample rate 2015-04-03 15:15:10 -07:00
Paul Riseborough a1351e73ab AP_NavEKF : Compensate mag bias states for external copass offset changes 2015-04-03 15:15:09 -07:00
Paul Riseborough 14795719f6 AP_NavEKF: Add public function for estimated magnetometer offsets 2015-04-03 15:15:09 -07:00
Paul Riseborough 1c244af3d8 AP_NavEKF: Fix bug affecting in-flight GPS acquisition
This bug caused velocities to be reset to zero
2015-04-03 15:15:09 -07:00
Paul Riseborough b9b6938b1d AP_NavEKF: Add ability to start using GPS in-flight
Improve the quality of the GPS required to set an EKF  origin
Eliminate repeated update of origin height - origin height updates once when EKF origin is set.
Operation in GPS mode is linked to setting of origin
2015-04-03 15:15:09 -07:00
Paul Riseborough 98fa918b84 AP_NavEKF: Add new compass learning option
Enables compass learning to be on continuously for non-position hold operation
2015-04-03 15:15:09 -07:00
Paul Riseborough 961faa59d9 AP_NavEKF: Don't let reported GPS accuracy modify horiz vel data checks 2015-04-03 15:15:08 -07:00
Paul Riseborough a607eb8469 AP_NavEKF: Reduce the GPS glitch tether length from 100 to 50m 2015-04-03 15:15:08 -07:00
Paul Riseborough 496d31ab83 AP_NavEKF: Reduce max time Copters can reject GPS
The maximum time copters can reject GPS has been reduced from 10 to 7 seconds as flight logs have show that inertial dead reckoning with vibration and calibration errors is not good enough to support 10 seconds without aiding.
2015-04-03 15:15:08 -07:00
Paul Riseborough 8c2029d896 AP_NavEKF: Accel bias learning improvements
Speed up pre-flight learning
Smoothen in-flight learning
2015-04-03 15:15:08 -07:00
Paul Riseborough 19d1b3b813 AP_NavEKF: Update EKF origin height whilst disarmed
This patch causes the EKF to update the height of its origin each time it receives a valid GPs message whislt disarmed.
The resultant EKF origin height represents the height of the zero baro alt datum relative to the GPS height datum.
2015-04-03 15:15:08 -07:00
Paul Riseborough 70afcd7e70 AP_NavEKF: Add second stage alignment of yaw and earth field states
Flight tests have shown that the magnetic field distortion associated with flight from steel structures can extend 3m or higher. To counteract this, a second and final yaw and magnetic field alignment has been added which is activated when the height exceeds 5m for the first time.
2015-04-03 15:15:08 -07:00
Paul Riseborough ed9c05cf2a AP_NavEKF: Remove logic used to delay fusion for load levelling
Logic used to delay optical flow and airspeed fusion to prevent it occurring on the same time step as magnetometer fusion has been removed. This is no longer required to efficiency improvements made at the firmware level.
2015-04-03 15:15:07 -07:00
Paul Riseborough c0d23ffc30 AP_NavEKF: Filter accuracy and stability improvements
Improvements in PX4 firmware have reduced the computational load  and mkae the previous practicwe of splitting magnetometer and optical flow fusion across multiple time steps unnecessary and make it possible to perform a covariance prediction prior to fusing data on the same time step. This patch:

1) Ensures that a covariance prediction is always performed prior to fusion of any observation
2) Removes the splitting of magnetometer fusion so that fusion of the X,Y and Z components occurs on the same time time step
3) Removes the splitting of optical flow fusion so that fusion of X and Y components occurs on the same time step
2015-04-03 15:15:07 -07:00
Jonathan Challinger de1f7f5e63 AP_NavEKF: use published delta velocities and delta angles if available 2015-04-03 15:15:07 -07:00
priseborough 3421a320b5 AP_NavEKF: Compensate for ground effect when takeoff or landing expected 2015-04-03 15:15:07 -07:00
Jonathan Challinger 20d92f5f9d AP_NavEKF: floor GPS velocity noise at parameter value for conservatism 2015-04-03 15:15:07 -07:00
priseborough 9a797a5d49 AP_NavEKF: Use GPS reported speed accuracy if available
UBlox receivers report an estimate of the speed accuracy that tests show correlates well to speed glitches. Using this to scale the GPS velocity observation noise will reduce the effect of bad GPS velocity data.
2015-04-03 15:15:06 -07:00
Andrew Tridgell 3289d38339 AP_NavEKF: make the init functions return bool
we need to know if it has initialised successfully
2015-03-28 10:51:38 -07:00
Andrew Tridgell de3f461a55 AP_NavEKF: use compass->last_update_usec() 2015-03-14 12:31:39 +11:00
Randy Mackay 230ca583d1 NavEKF: support sending EKF_STATUS_REPORT 2015-03-12 12:20:00 +09:00
Paul Riseborough 9f552eaa4b AP_NavEKF: Fix bug that resets position to origin when vehicle arms 2015-02-12 12:40:55 +11:00
Paul Riseborough b8d3da3846 AP_NavEKF: Report last known position when vehicle is disarmed 2015-02-12 12:40:55 +11:00
Andrew Tridgell 55041c7a7a AP_NavEKF: prevent division by zero in SmallEKF 2015-02-12 09:02:59 +11:00
Jonathan Challinger 50466848f7 AP_NavEKF: use hal.util soft_armed state 2015-02-11 20:25:11 +11:00
Grant Morphett 4860c84dff AP_NavEKF: Changes to fix the warnings in rover sitl build.
We are starting the process of resolving all the warnings in the
ardupilot builds of all vehicles and platforms.
2015-02-11 18:16:46 +11:00
Paul Riseborough aa94ff629d AP_NavEKF: Prevent bad GPS pre-arming casuing initial position errors
If the vehicle moves significantly or the GPS changes position significantly pre-armed, then the GPS glitch logic was being invoked when the first GPs measurements were fused. This patch resets the position to the GPS when the vehicle arms.
2015-02-03 15:57:10 +09:00
priseborough bc5581d634 AP_NavEKF: Prevent arming delays from failing GPS
Due to the way that gyro calibration is done, the EKF could be effectively not run for up to 30 seconds in extreme cases, making it possible that the GPS would be failed on arming and the copter put into a non-GPS mode.

the longer term solution is to update the gyro calibration so that it does not hold up other processing. the short tyermfix in thsi patch is to look for evidence of a 3D lock in the last received GPS message.
2015-02-03 15:57:09 +09:00
priseborough 2c012c2763 AP_NavEKF: Always check for new GPS data
This fixes a bug that meant that once the EKF had started up in a non-GPS mode, it would no longer read the GPS and therefore would never be able to use GPS again until reset.
2015-02-03 15:57:08 +09:00
Paul Riseborough 925d625ed1 AP_NavEKF: fix bug in small EKF velocity fusion 2015-02-03 09:49:17 +11:00