Commit Graph

84 Commits

Author SHA1 Message Date
Paul Riseborough 9f5e037015 AP_NavEKF3: Restore bug fix lost during rebase 2020-04-24 09:43:23 +10:00
Paul Riseborough 863f989130 AP_NavEKF3: Fix integration of GPS yaw options
AP_NavEKF3: fix failure to arm with EK3_MAG_CAL = 7

AP_NavEKF3: Fix failure to arm when not using magnetometer

AP_NavEKF3: Reduce yaw drift when EK3_MAG_CAL = 7
2020-04-24 09:43:22 +10:00
Paul Riseborough 410b5825fb AP_NavEKF3: Enable use of EKF-GSF yaw estimate
AP_NavEKF3: Add emergency yaw reset using a Gussian Sum Filter estimate

AP_NavEKF3: Reduce default minimum GPS velocity noise for Copters

Enables fail-safe to be set with more sensitivity and improves tracking accuracy.
Origin values were set using typical GPS performance for receivers on sale 6 years ago. Receiver performance has improved since then.

AP_NavEKF3: Prevent constant mag anomaly yaw resets

Prevents constant magnetic anomaly induced resets that can be triggered when flying with vehicle generated magnetic interference.
Allows for two resets per takeoff. Allowance for two resets is required, becasue a large ground based magnetic yaw anomaly can cause a sufficiently large yaw innovation that two resets in close succession are required.

AP_NavEKF3: Add option to fly without magnetometer

AP_NavEKF3: Rework emergency yaw reset logic

Use a separate external accessor function to request the yaw reset.
Allow reset requests to remain active for a defined period of time.
Tidy up reset function to split out accuracy check.

AP_NavEKF3: Fix vulnerability to lane switch race condition

Prevents the situation where a lane switch results in a lane being selected that does not have the correct yaw. This can occur if the primary lane becomes unhealthy before the external failsafe monitor has time to react.

AP_NavEKF3: Fix EKF_MAG_CAL = 6 behaviours

Fix bug causing the yaw alignment to be performed at startup before the GSF had a valid estimate.
Fix bug causing emergency yaw message to be output for a normal reset.
Fix vulnerability to reported negative yaw variance.
Remove duplicate timer checks.

AP_NavEKF3: Update EK3_MAG_CAL documentation

AP_NavEKF3: Relax yaw gyro bias convergence check when not using mag

AP_NavEKF3: Reduce yaw drift in hover with no yaw sensor

Uses the GSF yaw estimate if available which is better than the EKF's own yaw when no yaw sensor is available.
2020-04-24 09:43:22 +10:00
Andrew Tridgell be674fc36c AP_NavEKF3: make mag variance reporting consistent
logged scaled variance should match the value used in MAVLink
EKF_STATUS_REPORT
2020-04-06 17:30:29 +10:00
Peter Hall 47e3fda8e9 AP_NavEKF3: add initalized flag and change to uint32_t 2020-02-19 06:45:07 +11:00
Peter Barker 9ed37e4486 AP_NavEKF3: make send_status_report const 2019-12-11 07:54:41 +09:00
Peter Barker 4431d01230 AP_NavEKF3: stop relying on the presence of a RangeFinder 2019-12-11 08:09:01 +11:00
Paul Riseborough 5b1d9ed868 AP_NavEKF3: Add missing vertical position derivative calculation 2019-10-18 11:15:25 +09:00
Andrew Tridgell f3fc910abe AP_NavEKF3: continuously update gpsGoodToAlign
use it to determine how to handle a height datum reset
2019-07-07 19:20:32 +10:00
Michael du Breuil be74b631fe AP_NavEKF3: Fix AP_GPS.h include 2019-06-18 10:02:05 +10:00
Pierre Kancir b3a1c9c90c AP_NavEKF3: use get_distance_NE instead of location_diff 2019-04-08 08:00:52 -07:00
Paul Riseborough 35c82ef67f AP_NavEKF3: Improve optical flow height estimation 2019-04-02 10:51:12 +11:00
Dr.-Ing. Amilcar do Carmo Lucas 390e0fa601 AP_NavEKF3: replace location_offset() and get_distance() function calls with Location object member function calls
This allows removing duplicated code
2019-02-28 11:44:09 +11:00
Arjun Vinod f382a657bd AP_NavEKF3: fixed typos 2019-02-26 08:33:39 +11:00
Tom Pittenger 55377b234f AP_NavEKF3: remove HAL_CPU_CLASS_150 check, 150 is already a minimum requirement 2019-02-17 13:00:00 -08:00
Peter Barker dd8da7321c AP_NavEKF3: adjust for location flags being moved out of union 2019-01-16 11:45:29 +11:00
Michael du Breuil 1991f223ac AP_NavEKF3: Fix roundoff, and missing offset handling of getLLH 2018-09-21 00:18:49 +01:00
Andrew Tridgell 4869dd0ae4 AP_NavEKF3: send airspeed variance over mavlink 2018-04-30 15:41:32 +10:00
Peter Barker 5dc714bf5f AP_NavEKF3: use GPS singleton 2017-12-27 00:58:02 +00:00
Pierre Kancir be96a27fa8 AP_NavEKF3: only report terrain estimator innovations with valid rng value 2017-12-18 22:51:46 +00:00
Michael du Breuil efe70c8d3a AP_NavEKF3: Fix possible compass nullptr dereference 2017-08-01 15:25:05 +01:00
priseborough c7f6d52065 AP_NavEKF3: Add wheel encoder odometry
Uses the existing body frame odometry observation model.
Handles each sensored wheel as a separate sensor.
2017-07-27 16:52:27 +09:00
priseborough 0827b13feb AP_NavEKF3: Fix default behaviour for height origin corrections
Previous default was to apply in-flight height origin changes to local position instead of to reported origin height. This caused problems with copters that took off before getting GPS lock.
2017-07-17 02:22:02 +01:00
priseborough 6d14cf0ab8 AP_NavEKF3: Ensure reported local height = global height - origin height 2017-06-14 12:44:41 +01:00
priseborough 2b97d0f5c9 AP_NavEKF3: Add public accessor for state variances 2017-06-14 12:44:41 +01:00
priseborough 9da3caca47 AP_NavEKF3: Improve GPS reference height estimator
Fix rounding error bug preventing state from updating after initial convergence.
Decouple GPS reference height from published EKf origin height.
Add bitmask parameter to control update and publishing of GPS reference height.
2017-06-14 12:44:41 +01:00
murata 46cba2c970 libraries: Delete the setting value for specific editor. 2017-05-11 13:18:41 -07:00
priseborough fb7104f4e3 AP_NavEKF3: add fusion method for body frame odometry data 2017-04-19 11:04:40 +09:00
Randy Mackay 3f6a734a71 AP_NavEKF3: only use downward facing rangefinder 2017-02-27 15:18:11 +09:00
Michael du Breuil 1de198883d AP_NavEKF3: Check gps vertical accuracy for aligning the GPS 2017-02-22 11:53:42 -08:00
priseborough 2e5ac40ef6 AP_NavEKF3: remove unnecessary stdio includes 2017-01-27 16:57:42 +09:00
priseborough 7e8f3fca53 AP_NavEKF3: Address review comments
Removes a leftover debug printf statement.
Fixes documentation errors.
Replace remaining fmaxf function calls with Ardupilot MAX function
2017-01-27 16:57:42 +09:00
priseborough 92b8c33b19 AP_NavEKF3: publish 3-state range beacon estimator states 2017-01-27 16:57:42 +09:00
priseborough 39969e7d8e AP_NavEKF3: added EKF3 for EKF experimentation
AP_NavEKF3: Implement same maths as PX4/ecl EKF

Replace attitude vector states with quaternions
Remove gyro scale factor states
Add XY accel delta velocity bias estimation
Initial tuning
Add GPS body frame offset compensation

AP_NavEKF3: Fix bugs and consolidate aiding switch logic

Switching in and out of aiding modes was being performed in more than one place and was using two variables.
The reversion out of GPS mode due to prolonged loss of GPS was not working.
This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function.

AP_NavEKF3: prevent multiple fusion mode changes per filter update

AP_NavEKF3: Update tuning defaults

AP_NavEKF3: Fix bug causing switching in and out of aiding
If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding.
This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed.
An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS.

AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers
This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.

AP_NavEKF3: Improve switch-over to backup magnetometer
When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected.

AP_NavEKF3: enable automatic use of range finder height

AP_NavEKF3: Fix bug in handling of invalid range data

AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3:

AP_NavEKF3: Handle yaw jumps due to core switches

AP_NavEKF3: Enable simultaneous GPS and optical flow use

AP_NavEKF3: fix console status reporting

AP_NavEKF3: send messages to mavlink instead of console

This allows the GCS to better handle the display of messages to the user.

AP_NavEKF3: replace deprecated function call

AP_NavEKF3: Compensate for sensor body frame offsets

AP_NavEKF3: Fix bug in median filter code

AP_NavEKF3: save some memory in the position offsets in EKF3

We don't need to copy that vector3f for every sample. A uint8_t does the job

AP_NavEKF3: Add fusion of range beacon data

AP_NavEKF3: Bring up to date with EKF2

AP_NavEKF3: Misc range beacon updates

AP_NavEKF3: Add mising accessors

AP_NavEKF3: remove duplicate include

AP_NavEKF3: Prevent NaN's when accessing range beacon debug data

AP_NavEKF3: Update range beacon naming

AP_NavEKF3: updates

AP_NavEKF3: miscellaneous changes

AP_NavEKF3: misc updates

AP_NavEKF3: misc range beacons updates

AP_NavEKF3: add missing rover default param
2016-12-19 08:06:47 +11:00