Commit Graph

82 Commits

Author SHA1 Message Date
Peter Barker 5da3759ff4 AP_NavEKF2: eliminate GCS_MAVLINK::send_statustext_all 2017-07-11 23:53:53 +01:00
Randy Mackay 7e7f78d4b5 AP_NavEKF2: final mag reset at 2.5m 2017-07-08 08:39:43 +09:00
priseborough 1985dcd366 AP_NavEKF2: Fix bug in declination fusion 2017-06-03 15:16:25 +01:00
Dr.-Ing. Amilcar Do Carmo Lucas ce37517268 AP_NavEKF2: Improve comments, typos 2017-05-03 08:44:49 +09:00
priseborough 596b20eb07 AP_NavEKF2: close perf counter when doing early return 2017-04-27 20:14:21 +10:00
priseborough 88469ce80c AP_NavEKF2: Reset covariance if bad mag fusion update 2017-04-27 20:14:21 +10:00
priseborough a63eac8073 AP_NavEKF2: Add missing perf_end 2017-01-27 16:57:42 +09:00
Dr.-Ing. Amilcar Do Carmo Lucas d408d25f92 More spelling stuff 2017-01-06 10:57:39 -08:00
priseborough ef03a85cd3 AP_NavEKF2: add range beacon interface and fusion method 2016-11-30 17:56:43 +09:00
Mathieu OTHACEHE 152edf7189 Global: remove mode line from headers
Using a global .dir-locals.el file is a better alternative than
reincluding the same emacs header in every file of the project.
2016-10-24 09:42:01 -02:00
Jonathan Challinger d1ecc63bf1 AP_NavEKF2: change priority of statustext messages 2016-07-29 13:19:51 +09:00
Andrew Tridgell 567d5cdbe6 AP_NavEKF2: use mavlink messages not printf for state change notifications 2016-07-19 12:16:51 +10:00
priseborough e34cdc6666 AP_NavEKF2: 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.
2016-07-19 12:16:50 +10:00
priseborough 8793c75da0 AP_NavEKF2: clean up mag field learned logic 2016-07-11 00:44:33 +10:00
priseborough 10470b2dc1 AP_NavEKF2: fix bug in initialisation of declination co-variances
Co-variances were being re-zeroed after being set. This meant that the initial declination learning was sensitive to measurement errors which could result in poor initial yaw accuracy.
2016-07-10 08:21:18 +10:00
priseborough 6b04a81b8d AP_NavEKF2: Fix plane in-flight yaw reset bug
Fixes bugs that prevented planes being able to reset yaw to GPS to recovery from takeoff with a bad magnetoemter.

1) If the velocity innovation check had not failed by the time the in-air transition occurred, then the yaw reset would not be performed
2) The velocity states were not being reset
3) The non fly-forward vehicle (copter) reset could occur first and effectively lock out the fly-forward vehicle (plane) yaw check.
2016-07-10 08:21:18 +10:00
priseborough 51dbed2338 AP_NavEKF2: remember mag field states between flight on same power cycle
Remember the mag bias and earth field states learned during flight when the vehicle  lands.
This improves performance for vehicles that do multiple flight on one power cycle
2016-07-10 08:21:18 +10:00
priseborough 136df7cb5c AP_NavEKF2: reduce declination errors on start of 3-axis fusion
Reset co-variances for NE field states.
2016-07-10 08:21:18 +10:00
priseborough 191c34612d AP_NavEKF2: Fix bug in use of corrected IMU data
IMU data was being corrected before being used by the co-variance prediction, whereas the delta angles and velocities in the derivation were supposed to be uncorrected.
This patch creates separate variable for the corrected data
2016-07-09 15:57:35 +10:00
Paul Riseborough 4a8689aa97 AP_NavEKF2: remove duplicate if statements from mag fusion 2016-06-28 14:20:13 +10:00
Paul Riseborough 1400dc9d02 AP_NavEKF2: Add tuning parameters for magentic yaw fusion
Enable simple magnetic yaw fusion observation noise and innovation consistency check gate width to be tuned independantly.
2016-06-28 14:20:12 +10:00
Paul Riseborough dc6836988c AP_NavEKF2: Improve ground based magnetic anomaly protection for copter
The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe.
A check for yaw errors due to a ground based magnetic anomaly has been introduced.
The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
2016-06-28 14:20:12 +10:00
Paul Riseborough 24d8cc62e2 AP_NavEKF2: rework yaw and magnetic heading reset logic
Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags.
Reduce the number of places where decisions to perform a yaw and field reset are made.
Don't perform a reset unless there is is data in the buffer
Don't use 3-axis fusion if the field states still need to be reset.
When starting 3-axis fusion request a reset if not previously performed.
Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 16:12:53 +10:00
Paul Riseborough de15928921 AP_NavEKF2: prevent potential arithmetic error in declination fusion 2016-05-27 09:00:41 +10:00
Paul Riseborough 11c6ea7ef6 AP_NavEKF2: attitude co-variance reset for all post alignment yaw resets
Whenever the yaw angle is changed, the correlation with other state errors will be incorrect and the terms should be zeroed.
2016-05-27 09:00:41 +10:00
Paul Riseborough fe9ddfdfeb AP_NavEKF2: remove code that prevents attitude updates after mag reset
This code assumes a vehicle is close to level and will not work for all vehicle types. It will be replaced by a different method.
2016-05-27 09:00:40 +10:00
Paul Riseborough fe85c68344 AP_NavEKF2: move yaw reset for no compass case
Should not be in a function that performs reset on magnetic field states
2016-05-27 09:00:40 +10:00
Paul Riseborough 74121fdaeb AP_NavEKF2: fix bug preventing in-flight alignment 2016-05-21 15:13:53 +10:00
Paul Riseborough 9aa3cdfaae AP_NavEKF2: Update GPS yaw re-alignment function
Updates to support initial alignment without magnetometer
2016-05-21 15:13:53 +10:00
Paul Riseborough e3c966661b AP_NavEKF2: Remove unnecessary variance reset
This reset is unnecessary give that a synthetic yaw angle is fused at a low rate to prevent variances from becoming badly conditioned.
2016-05-21 15:13:53 +10:00
Paul Riseborough 1179c08473 AP_NavEKF2: Handle case where we are in-flight and haven't yet aligned the yaw
This happens if planes are flown without a compass
2016-05-21 15:13:53 +10:00
Paul Riseborough 581c1aa0d4 AP_NavEKF2: Adjust attitude variance values used after a heading reset
Because these errors are in body frame, a spherical error should be used to accommodate different orientations
2016-05-21 15:13:53 +10:00
Paul Riseborough 64a8153b68 AP_NavEKF2: update function name
The primary purpose of this function is re-alignment
2016-05-21 15:13:52 +10:00
Paul Riseborough 42da33593d AP_NavEKF2: fix yaw fusion numerical health reporting 2016-05-21 15:13:51 +10:00
Paul Riseborough 6eb9d43507 AP_NavEKF2: add numerical error checking and isolation to mag decl fusion 2016-05-21 15:13:51 +10:00
Paul Riseborough b325c5faeb AP_NavEKF2: add error checking and isolation to mag heading fusion 2016-05-21 15:13:51 +10:00
Paul Riseborough a0b1dc5325 AP_NavEKF2: add error checking and isolation to 3D amg fusion 2016-05-21 15:13:51 +10:00
Ricardo de Almeida Gonzaga 64d14356b9 AP_NavEKF2: Fix typos 2016-05-13 19:20:06 -03:00
dgrat 41661f815f AP_Math: Replace the pythagorous* functions with a variadic template
The new function can deal with a variable number of function parameters.
Additionally, I renamed the functions to norm(), because this is the
standard name used in several other projects.
2016-05-10 11:41:26 -03:00
Paul Riseborough 1ecc206eee AP_NavEKF2: Allow use in planes without a magnetometer
Implements the following techniques to enable planes to operate without magnetometers.

1) When on ground with mag use inhibited, a synthetic heading equal to current heading is fused to prevent uncontrolled covariance growth.
2) When transitioning to in-flight, the delta between inertial and GPS velocity vector is used to align the yaw.
3) The yaw gyro bias state variance is reset following an in-flight heading reset to enable the yaw gyro bias to be learned faster.
2016-04-15 08:31:47 +10:00
Paul Riseborough 6d9ba8c527 AP_NavEKF2: Improve protection against ground based mag interference
Reset the mag field states and yaw earlier than the normal 5m height threshold if toilet bowling is detected.
2016-03-17 10:55:11 +11:00
Paul Riseborough 2888bdd6d5 AP_NavEKF: Protect against possible div by 0 2016-03-01 10:08:47 -03:00
Paul Riseborough 7d6b926749 AP_NavEKF2: Improved magnetic heading fusion
Use an Euler yaw heading that switches between a 321 and 312 rotation
sequence to avoid areas of singularity.  Using Euler yaw decouples the
observation from the roll and pitch states and prevents magnetic
disturbances from affecting roll and pitch via the magnetometer fusion
process.
2016-03-01 10:08:47 -03:00
dgrat 5148e41c1a AP_Math: Cleaned macro definitions
Moved Definitions into a separate header. Replaced PI with M_PI and
removed the M_PI_*_F macros.
2016-02-27 02:51:33 -03:00
Paul Riseborough 7459bfb96b AP_NavEKF2: Eliminate simple compass fusion singularities near +-90 deg pitch
The use of yaw angle fusion during startup and ground operation causes problems with tail-sitter vehicle types.
Instead of observing an Euler yaw angle, we now observe the yaw angle obtained by projecting the measured magnetic field onto the the horizontal plain.
This avoids the singularities associated with the observation of Euler yaw angle.
2016-02-22 16:29:36 +11:00
Paul Riseborough 4aefe1caee AP_NavEKF2: Fix sign error in magnetic heading innovation calculation 2016-02-20 08:55:48 +11:00
Paul Riseborough 047e9fabaf AP_NavEKF2: Fix bug in simple heading fusion
The innovation calculation should have been updated when the heading fusion maths was updated.

We now use a direct heading or yaw angle measurement in the derivation, not the difference between observed and published declination.
2016-02-19 15:35:11 +09:00
Paul Riseborough d3c9a0aef1 AP_NavEKF: Limit heading innovations after the consistency check 2016-02-18 08:53:47 +09:00
Paul Riseborough 6a34e4c384 AP_NavEKF2: Fix bug in magnetic heading and declination fusion equations.
The derivation incorrectly used a tan instead of an atan function. This applies the corrected auto-code.
2016-02-18 08:53:45 +09:00
Paul Riseborough 7e05646316 AP_NavEKF2: Improvements to non-GPS performance
Change to user adjustable fusion of constant position (as per legacy EKF) instead of constant velocity.
Enable user to specify use of 3-axis magnetometer fusion when operating without aiding.
Don't allow gyro scale factor learning without external aiding data as it can be unreliable
2016-02-18 08:53:41 +09:00