Commit Graph

325 Commits

Author SHA1 Message Date
priseborough 14bb4f4574 AP_NavEKF2: Publish output observer tracking errors 2016-07-09 15:57:35 +10:00
priseborough fa435d0323 AP_NavEKF2: Allow for faster accel bias change in-flight
Fixes a problem observed in a flight log where rapid temperature change caused the accel bias to change faster than the EKF could keep up.
This allows the bias to be learned faster but with acceptable level of noise in the estimate
2016-07-09 15:57:35 +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
priseborough 118d5b88b2 AP_NavEKF2: Clean up output observer and reduce pos vel time constant
Previous Time constant was too large leading to poor tracking of EKF states in the presence of bad quality IMU 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 e117bedf6a AP_NavEKF2: Adjust output observer tuning
Use more accurate method by default.
Reduce overshoot on quaternion tracking
2016-06-28 14:20:13 +10:00
Paul Riseborough 5c59922574 AP_NavEKF2: Fix bug that could publish bad compass offsets
Magnetometer bias states will subject to larger errors early in flight before flight motion makes the offsets observable and the state variances reduce.
Adds a check on state variances.
Replaces the parameter check with a check of the actual filter fusion method being used.
2016-06-28 14:20:13 +10:00
Paul Riseborough 165335b9e3 AP_NavEKF2: Improve ability to tune magnetic field learning
Allow different process noise to be set for body (sensor bias) and earth field states.
This allows a stable magnetometer bias estimate to be available at end of flight whilst still allowing for external magnetic anomalies during landing.
Adjust default values to give stable mag bias learning and fast learning of external anomalies.
2016-06-28 14:20:12 +10:00
Paul Riseborough 6523481c76 AP_NavEKF2: Improve tracking accuracy of output predictor
Automatically use the highest gain consistent with a 5% overshoot to minimise RMS tracking errors.
Provide an alternative correction method for the position and velocity states that allows the user to specify the time-constant. This can be used to fine tune the output observer for for platform specific sensor errors and control loop sensitivity estimation noise.
2016-06-28 14:20:12 +10:00
Paul Riseborough fe06606193 AP_NavEKF2: Ensure corrected IMU data used by EKF
Don't try to pop new IMU data from the buffer unless we have written data.
Correct IMU data as soon as it is popped from the buffer
2016-06-28 14:20:12 +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 703f56908f AP_NavEKF2: Correct comment 2016-06-28 14:20:12 +10:00
Paul Riseborough 26815ace7a AP_NavEKF2: Remove redundant logic check 2016-06-28 14:20:12 +10:00
Jonathan Challinger bd8c804ab5 AP_NavEKF2: correct comments 2016-06-28 14:20:12 +10:00
Jonathan Challinger ebae95d7f6 AP_NavEKF2: apply corrections to new inertial data when using for output prediction 2016-06-28 14:20:11 +10:00
Jonathan Challinger 2f709dfe86 AP_NavEKF2: improve inertial prediction 2016-06-28 14:20:11 +10:00
Paul Riseborough 262c5fe56d AP_NavEKF2: Make setter loop through all instances
For setters where a success boolean is returned, return false if any instance fails.
2016-06-28 14:20:11 +10:00
Jonathan Challinger 8baf064317 AP_NavEKF2: setTakeoffExpected and setTouchdownExpected loop through instances 2016-06-28 14:20:11 +10:00
Paul Riseborough ec5c460584 AP_NavEKF2: Fix predicted nav reporting
Predicted nav should never be false if current nav is true
2016-06-28 14:20:11 +10:00
Jonathan Challinger 99f481e098 AP_NavEKF2: always calcGpsGoodForFlight 2016-06-28 14:20:11 +10:00
Jonathan Challinger 3382e09580 AP_NavEKF2: add height constraint during takeoff 2016-06-28 14:20:11 +10:00
Paul Riseborough 4bb46af861 AP_NavEKF2: Increase allowable gyro bias offset 2016-06-28 14:20:10 +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 a94a19bd09 AP_NavEKF2: Fix bug causing continual yaw resets 2016-05-28 06:52:11 +10:00
Andrew Tridgell 97e2203e70 AP_NavEKF2: don't do 3D mag fusion on 2nd EKF2 core
this reduces the risk that mag fusion errors will badly affect
attitude estimation.
2016-05-27 21:27:06 +10:00
Paul Riseborough de15928921 AP_NavEKF2: prevent potential arithmetic error in declination fusion 2016-05-27 09:00:41 +10:00
Paul Riseborough 722eb0d706 AP_NavEKF2: Fix bug for start of wind speed estimation
The commencement of airspeed fusion could cause pitch errors due to small variances and large innovations. This issue is addressed by the following changes:
1) The airspeed measurement is used to set wind states to a value that reduces initial innovations.
2) The wind state variances are set to values that better reflect the wind speed uncertainty
2016-05-27 09:00:41 +10:00
Paul Riseborough ac329ec31c AP_NavEKF2: use observation noise to set initial magnetic field variances 2016-05-27 09:00:41 +10:00
Paul Riseborough 830751c0ae AP_NavEKF2: remove un-wanted functionality from mag and yaw reset
The function used to reset magnetic field states and yaw angle should not be used when there is no magnetometer. If it is incorrectly called without a magnetometer it should not change the attitude or field states.
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 cdd09df9ac AP_NavEKF: Add function to zero attitude state co-variances
When changing the vehicle yaw angle, the correlation between the attitude errors and errors in other states is invalid so the corresponding co-variance terms need to be zeroed.
This needs to be done in more than one place.
2016-05-27 09:00:41 +10:00
Paul Riseborough 6d34ac5ceb AP_NavEKF2: improve switch-over to use of mag field states
Set initial variances to measurement uncertainties
Always perform a field  and yaw reset
2016-05-27 09:00:40 +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 da0622827d AP_NavEKF: update tuning defaults
Increase speed of scale factor learning
2016-05-27 09:00:40 +10:00
Paul Riseborough 9eef97d108 AP_NavEKF2: use consistent parameter naming
M_NSE is a measurement noise
P_NSE is a observation noise
I_GATE is an innovation gate

This also ensures the new parameter values required to use the EKF2 will be enforced.
2016-05-25 12:06:04 +10:00
Paul Riseborough 74121fdaeb AP_NavEKF2: fix bug preventing in-flight alignment 2016-05-21 15:13:53 +10:00
Paul Riseborough 16cb7078cd AP_NavEKF2: Fix misleading pre-flight status reporting 2016-05-21 15:13:53 +10:00
Paul Riseborough 38d61f5281 AP_NavEKF2: Update status reporting to handle no-magnetometer startup 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 5ec8d523f6 AP_NavEKF: remove unwanted co-variance reset
The co-variance resets required following a velocity and position state reset is now performed in the stte reset function
2016-05-21 15:13:53 +10:00
Paul Riseborough 66e4d9d6e8 AP_NavEKF2: Reset co-variance values when doing a pos,vel,hgt reset
Ensures consistent behaviour after resets and reduces attitude disturbances
2016-05-21 15:13:52 +10:00
Paul Riseborough cf8175a073 AP_NavEKF2: Allow immediate use of GPS in-flight for plane without compass
Planes operating without a compass must align the yaw to the GPS heading immediately after launch or takeoff and cannot wait for GPS accuracy checks.
2016-05-21 15:13:52 +10:00
Paul Riseborough 08de4e6d95 AP_NavEKF: Do not perform start-up yaw alignment if no compass
Yaw cannot be aligned until in-flight when there is sufficient GPS velocity.
2016-05-21 15:13:52 +10:00
Paul Riseborough d2694fe5dc AP_NavEKF2: Remove duplicate transition into GPS aiding
This decision is made in one place in setAidingMode()
2016-05-21 15:13:52 +10:00
Paul Riseborough 7201f7d2fd AP_NavEKF2: fix comment 2016-05-21 15:13:52 +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 addd213af0 AP_NavEKF2: reduce effect of acceleration on non-GPS mode attitude 2016-05-21 15:13:52 +10:00
Paul Riseborough a3b78e6231 AP_NavEKF2: update tuning defaults 2016-05-21 15:13:51 +10:00
Paul Riseborough 65f63341d7 AP_NavEKF2: Improve numerical error protection in optical flow fusion
Abort fusion of data if variances will become negative and publish the fault
2016-05-21 15:13:51 +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
Paul Riseborough 51a3df0319 AP_NavEKF2: Add error checking and isolation to pos vel fusion 2016-05-21 15:13:51 +10:00
Paul Riseborough 71b589c89c AP_NavEKF2: extend fusion fault reporting coverage 2016-05-21 15:13:50 +10:00
Paul Riseborough 0982bd0b2b AP_NavEKF2: update parameter documentation 2016-05-21 15:13:50 +10:00
Paul Riseborough 46a2993a0d AP_NavEKF: use intuitive units for imu bias process noise parameters 2016-05-21 15:13:50 +10:00
Paul Riseborough 8c374fd679 AP_NavEKF2: correct comments 2016-05-21 15:13:50 +10:00
Paul Riseborough 6be9eaa524 AP_NavEKF2: use receiver estimated accuracy
Adjust the GPS observation noise based on receiver accuracy output if available.
2016-05-21 15:13:50 +10:00
Paul Riseborough dddc213836 AP_NavEKF2: Open up tuning limits
Done to assist tuning assessments
2016-05-21 15:13:50 +10:00
Paul Riseborough cf05e110fc AP_NavEKF2: fix scaling error in use of IMU noise parameters
Also remove process noise from state transition matrix where it was mistakenly left in after the derivation
2016-05-21 15:13:50 +10:00
Andrew Tridgell 4080c6091d AP_NavEKF2: fixed an assumption that _ahrs->get_compass() always works
if MAG_ENABLE=0 in plane there is no compass
2016-05-19 14:16:40 +10:00
Tom Pittenger d67b1edf10 AP_NavEKF2: (potential) compiler warning of float to double promotion 2016-05-17 20:30:51 -07: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
Andrew Tridgell ea508f1b80 AP_NavEKF2: use LOG_REPLAY and EK2_LOG_MASK parameters 2016-05-09 12:26:57 +10:00
Andrew Tridgell 32af886ba9 AP_NavEKF2: use consistent logging timestamps for sensors 2016-05-07 18:27:22 +10:00
Andrew Tridgell 4e4c575f16 AP_NavEKF2: added have_ekf_logging() 2016-05-07 18:27:20 +10:00
Andrew Tridgell b7ba0fa458 AP_NavEKF2: added ad-hoc logging example to EKF2 2016-05-07 18:27:20 +10:00
Andrew Tridgell 223c512188 AP_NavEKF2: added logging of sensor data in EKF2 2016-05-07 18:27:18 +10:00
Andrew Tridgell f92279f436 AP_NavEKF2: allow logging of IMT data from inside EKF2 2016-05-07 18:27:17 +10:00
Andrew Tridgell 9c4dd024bf AP_NavEKF2: auto change EK2_GPS_TYPE for NMEA
this fixes a problem where users of NMEA GPS receivers could not arm
with default EK2 parameters.
2016-05-05 19:46:33 +10:00
Andrew Tridgell ac60901b0c AP_NavEKF2: use vector comparison for new mag vector 2016-04-21 09:56:22 +10:00
Jonathan Challinger 97112ccd44 AP_NavEKF2: check mag instance id when returning mag offsets 2016-04-21 09:51:41 +10:00
Jonathan Challinger 1185cd1be7 AP_NavEKF2: move getMagOffsets into outputs 2016-04-21 09:51:41 +10:00
Jonathan Challinger acfaafe276 AP_NavEKF2: detect changes to magnetometer offset parameters and reset states 2016-04-21 09:51:41 +10: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
Lucas De Marchi e40b87cd0e AP_NavEKF2: replace header guard with pragma once 2016-03-16 18:40:42 +11:00
Paul Riseborough 38b3625ed8 AP_NavEKF2: Fix bug in initial alignment calculation
The bug caused the initial roll angle to be incorrect if the vehicle was powered up when inverted, causing long alignment times.
2016-03-02 09:10:09 +09: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
Paul Riseborough bb74371c58 AP_NavEKF2: Do not use GPS height if GPS accuracy is poor
If we are using GPS height, revert back to using Baro height if the GPS accuracy is poor.
2016-03-01 15:13:13 +09: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
Peter Barker 475a2040a1 AP_NavEKF2: avoid use of undefined #defines
Ensure EKF_DISABLE_INTERRUPTS is defined

Do not define MATH_CHECK_INDEXES, assume it is defined
2016-02-19 12:34:24 -02: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 59bf29198d AP_NavEKF2: Remove unnecessary logic preventing constant position
This removes a legacy design concept that is no longer required in this filter implementation. Planes will not be armed without EKF aiding and the proposed copter throw mode also requires EKF aiding to be operating.
The other problem with interrupting fusion during the launch is it doesn't reduce the corrections, it just delays them as wen the launch completes, the EKF inertial position estimate is still moving still moved and the corrections are therefore just delayed by the short launch interval.

Thank you to OXINARF for picking up the inconsistency with the previous logic
2016-02-18 08:53:43 +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
Paul Riseborough 8630874ef3 AP_NavEKF2: Send correct data over mavlink status message 2016-02-01 10:02:22 +09:00
Andrew Tridgell acb4885989 AP_NavEKF2: use get_delta_angle_dt() API 2016-01-19 09:50:22 +11:00
Paul Riseborough 287ebe8e6a AP_NavEKF2: Fix bug in application of sensor bias corrections
Sensor bias corrections were being applied to the incoming IMU data using the wrong delta time.
This was what was driving the different tuning between plane and copter for gyro bias process noise so the same gyro bias process noise default tuning value can now be used for all platform types.
Sensor bias corrections were being applied a a second time to the output observer inertial data.
2016-01-13 08:05:39 +11:00