Commit Graph

559 Commits

Author SHA1 Message Date
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