Commit Graph

682 Commits

Author SHA1 Message Date
priseborough 16f021c466 AP_NavEKF2: Correct range finder data for body frame position offset 2016-10-27 14:54:43 +11:00
priseborough e884e9cc6e AP_NavEKF2: Correct GPS data for antenna body frame offset 2016-10-27 14:54:43 +11:00
priseborough 064a106808 AP_NavEKF2: Get IMU accelerometer body position offset data 2016-10-27 14:54:42 +11:00
priseborough 397033b7c3 AP_NavEKF2: Add flow sensor body position offset to data buffer 2016-10-27 14:54:42 +11:00
priseborough 92c086b40e AP_NavEKF2: Add rangefinder body position offset to data buffer 2016-10-27 14:54:42 +11:00
priseborough 3148ad4623 AP_NavEKF2: Add GPS antenna position offset data to data buffer 2016-10-27 14:54:42 +11:00
priseborough fd905c23e1 AP_NavEKF2: Add body position offset to optical flow interface 2016-10-27 14:54:42 +11: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
priseborough 9779511425 AP_NavEKF2: Fix bug in median filter code 2016-10-19 19:45:00 +09:00
Jonathan Challinger dac59cb5da AP_NavEKF2: use exact matrix for trim rotation 2016-10-13 10:02:14 +11:00
priseborough 5d09c78f34 AP_NavEKF2: Always perform optical flow takeoff check when receiving data
A specialised takeoff check is now always performed when we receive new flow data as the default behaviour is to try and use flow data whenever it is received, rather than limit its use to a use to a flow-only mode of operation that had to be selected via user parameter.
2016-10-10 14:49:48 +09:00
priseborough a75a383ef2 AP_NavEKF2: Update protection for out of focus flow data 2016-10-10 14:49:41 +09:00
priseborough 635826c056 AP_NavEKF2: Fix reporting of terrain estimator innovations
Terrain height is relevant whenever optical flow data is present
2016-10-10 14:49:35 +09:00
priseborough b0072b587c AP_NavEKF2: fix reporting of optical flow use status 2016-10-10 14:49:24 +09:00
priseborough ed9ecb28fb AP_NavEKF2: Enable entry into relative position mode on start-up 2016-10-10 14:49:21 +09:00
priseborough e0b8c54194 AP_NavEKF: Update GPS type parameter description 2016-10-10 14:49:18 +09:00
priseborough f9018fcc1b AP_NavEKF2: Enable simultaneous optical flow and GPS use
Enables simultaneous use of GPS and optical flow data with automatic fallback to relative position mode if GPS is lost and automatic switch-up to absolute position status if GPS gained/re-gained.
2016-10-10 14:49:02 +09:00
priseborough ab55991b33 AP_NavEKF2: Report position jumps due to lane switches
Also moves code required to update reset data due to lane switches into separate functionscto improve readability.
2016-10-10 11:56:14 +09:00
Francisco Ferreira 42cd8e9721 AP_NavEKF2: Handle yaw jumps due to core switches
Based on work from Paul Riseborough (priseborough)
2016-09-25 18:24:11 -07:00
Francisco Ferreira 8da22e441b Revert 3 commits about yaw reset core switch
Revert "AP_NavEKF2: Fix bug in published yaw reset value found during code review"
commit 175faf1e41.

Revert "AP_NavEKF2: use a struct for all yaw step class variables"
commit 77fad065d1.

Partially revert "AP_NavEKF2: Handle yaw jumps due to core switches"
commit 885bfd1b4e.
2016-09-25 18:24:11 -07:00
Randy Mackay 2ebb81b02f AP_NavEKF2: switching compass message to GCS downgraded to INFO 2016-09-21 21:42:44 +09:00
priseborough b6867e085d AP_NavEKF2: Don't report range finder variance on mavlink if not required 2016-09-21 10:41:21 +09:00
priseborough cb1d3c7ed2 AP_NavEKF2: Allow for terrain gradient when using range finder for height 2016-09-13 17:45:03 +09:00
priseborough 60d8adcca0 AP_NavEKF2: Fix height drift on ground using range finder without GPS 2016-09-13 17:44:59 +09:00
priseborough 8797714203 AP_NavEKF2: Fix bug in handling of invalid range finder data 2016-09-13 17:44:43 +09:00
priseborough 175faf1e41 AP_NavEKF2: Fix bug in published yaw reset value found during code review 2016-09-13 17:35:18 +09:00
priseborough 77fad065d1 AP_NavEKF2: use a struct for all yaw step class variables 2016-09-13 17:35:16 +09:00
priseborough 885bfd1b4e AP_NavEKF2: Handle yaw jumps due to core switches 2016-09-13 17:35:14 +09:00
Andrew Tridgell dd812cfc0c AP_NavEKF2: added getPrimaryCoreIMUIndex()
needed for correct AHRS gyro estimate
2016-09-05 12:53:53 +10:00
priseborough e6f36f04db AP_NavEKF2: Enable automatic use of range finder height
The EK2_RNG_USE_HGT parameter sets the height (expressed as a percentage of the maximum range of the range finder as set by the RNGFND_MAX_CM parameter) below which the range finder will be used as the primary height source when the vehicle is moving slowly.

When using a height reference other than GPS, the height datum can drift due to air pressure changes if using baro, or due to terrain height changes if using range finder as the primary height source. To ensure that a consistent height datum is available when switching between altitude sources, the WGS-84 height estimate of the EKF's local positi norigin is updated using a
single state Bayes estimator,

If rngfinder or gps height data is lost whilst being used, there will be a fall-back to baro data.
2016-08-08 10:56:44 +09: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 bd7bf21475 AP_NavEKF2: Speed improve switch-over to backup magnetometer
When switching over to a back up magnetometer, ensure that the earth field estimate are reset. other wise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected.
2016-07-19 12:16:50 +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 e2b8807260 AP_NavEKF2: 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.
2016-07-19 12:16:50 +10:00
priseborough fcc07b5560 AP_NavEKF2: Prevent multiple aiding mode changes per update 2016-07-19 12:16:50 +10:00
priseborough f4f347fb75 AP_NavEKF2: add missing parenthesis and console message 2016-07-19 12:16:50 +10:00
priseborough 9a23152ee4 AP_NavEKF2: 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.
2016-07-19 12:16:50 +10:00
priseborough 230ba2700f AP_NavEKF2: fix documentation errors 2016-07-19 12:16:50 +10:00
priseborough 341d070db8 AP_NavEKF2: Separate filter status update and get functions
The filter status logic calculations were being repeated every time the get function was called.
The logic is now updated once per filter update step and a separate get function added
2016-07-19 12:16:49 +10:00
priseborough b1717649b1 AP_NavEKF2: remove combined NED local position interface 2016-07-19 12:16:49 +10:00
priseborough e374ec634d AP_NavEKF2: Add separate horizontal/vertical local position interfaces 2016-07-19 12:16:49 +10:00
priseborough 04e8726d8a AP_NavEKF2: relax bias convergence check
Enables bias convergence check to pass within 30 seconds when vehicle is static.
2016-07-19 12:16:48 +10:00
priseborough 280395afa1 AP_NavEKF2: Don't declare ready to do aiding unless gyro bias is learned 2016-07-19 12:16:48 +10:00
priseborough 169cd6625d AP_NavEKF2: clean up output predictor 2016-07-19 12:16:48 +10:00
priseborough aaab250f13 AP_NavEKF2: Don't start GPS aiding until gyro calibration has stabilised
If we start GPS aiding before the gyro bias variances have reduced, glitches on the GPS can cause attitude disturbances that degrade flight accuracy during early flight.
2016-07-19 12:16:48 +10:00
priseborough 8793c75da0 AP_NavEKF2: clean up mag field learned logic 2016-07-11 00:44:33 +10:00
priseborough e6592186fc AP_NavEKF2: Tuning update
Slow down magnetic field learning
2016-07-10 08:21:18 +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 744f19cd2d AP_NavEKF2: Improve scaling of output predictor I gain
Provide consistent overshoot of 5% across a wider range of time constants and prevent selection of larger time constants causing 'ringing' in the position and velocity outputs.
2016-07-09 15:57:36 +10:00
priseborough a49c16d63c AP_NavEKF2: Update output filter tuning
Updated tuning to take advantage of corrected time delta dtEkfAvg
2016-07-09 15:57:36 +10:00
priseborough 55dee347e0 AP_NavEKF2: Fix bug in averaged filter delta time
Average EKF time delta was not being updated.
2016-07-09 15:57:36 +10:00
priseborough 927186339c AP_NavEKF2: Improved output predictor tracking
Implement a PI feedback controller for velocity and position state tracking
2016-07-09 15:57:35 +10:00
priseborough 253f744824 AP_NavEKF2: Collect output predictor tracking data 2016-07-09 15:57:35 +10:00
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
Paul Riseborough 20923da23a AP_NavEKF2: Allow tuning of non-GPS mode
Eliminate the use of horizontal position states during non-aiding operation to make it easier to tune.
Explicitly set the horizontal position associated Kalman gains to zero and the coresponding covariance entries to zero after avery fusion operation.
Make the horizontal velocity observation noise used during non-aiding operation adjustable.
Use a fixed value of velocity noise during initial alignment so that the flight peformance can be tuned without affecting the initial alignment.
2016-01-13 08:05:39 +11:00
Paul Riseborough 23038e7243 AP_NavEKF2: Use measurement uncertainties to initialise covariance 2016-01-13 08:05:39 +11:00
Paul Riseborough e80fb8b3fa AP_NavEKF2: Improve non-GPS in-flight attitude accuracy
The non-GPS mode was not being activated for small height gains - eg indoor flight.
The incorrect innovation consistency check was being applied to the synthetic velocity observations.
2016-01-06 20:49:21 +11:00
Andrew Tridgell c34626ec4e AP_NavEKF2: automatically cut back EK2_IMU_MASK to suit number of IMUs 2016-01-05 16:40:43 +11:00
Andrew Tridgell 0831661b3c AP_NavEKF2: mark EK2_ENABLE as an ENABLE parameter 2016-01-04 11:14:42 +11:00
Andrew Tridgell 23cef70846 AP_NavEKF2: use get_loop_delta_t() from INS 2015-12-27 14:57:17 +09:00
Don Gagne 8573245963 AP_NavEKF2: fix invalid Range meta data 2015-12-27 16:10:44 +11:00
Lucas De Marchi 2591261af6 Global: rename min and max macros to uppercase
The problem with using min() and max() is that they conflict with some
C++ headers. Name the macros in uppercase instead. We may go case by
case later converting them to be typesafe.

Changes generated with:

	git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)max(/\1MAX(/g'
	git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)min(/\1MIN(/g'
2015-12-01 16:28:09 -02:00
Paul Riseborough ddb7d92fc8 AP_NavEKF2: Fix timing jitter in airspeed fusion
The airspeed observation buffer was only being checked when new data arrived instead of every frame which introduced some timing jitter. The buffer is now checked every filer update step.
The duplication and inconsistent naming of booleans used to indicate availability f data has been fixed.
2015-11-26 09:22:03 +11:00
Siddharth Purohit and Paul Riseborough 3014eb4001 AP_NavEKF2: Rework measurement buffer refactor
These changes were pair coded an tested by Siddharth Purohit and Paul Riseborough

Fix indexing errors
Move buffer code into a separate file
Split observer and IMU/output buffers and remove duplicate sample time
Optimise observation buffer search
Reduce maximum allowed fusion age to 100 msec
2015-11-23 19:42:06 +11:00
Siddharth Bharat Purohit b3c8dcee34 AP_NavEKF2: measurement buffer refactor 2015-11-23 19:34:50 +11:00
Caio Marcelo de Oliveira Filho 8d882b06dd AP_NavEKF2: use millis/micros/panic functions 2015-11-20 12:32:41 +09:00
Paul Riseborough ff2782b790 AP_NavEKF2: Parameter changes to reduce noise on gyro bias estimates 2015-11-18 20:17:03 +09:00
Paul Riseborough 2193103586 AP_NavEKF: Reduce time to switch after an IMU fault
This revised threshold value is still double the maximum that has been observed in flight logs so far with healthy sensors
The previous value was too slow to switch for sudden IMU gyro faults
We can afford an ocasional false trigger becasue the front end will only select another instance if it is healthy and has lower errors
2015-11-18 20:17:00 +09:00
Paul Riseborough 20798316e1 AP_NavEKF2: fix bug in performance timer 2015-11-18 20:16:58 +09:00
Paul Riseborough 2fb5a4489b AP_NavEKF2: Prevent airspeed faults from causing excessive loss of accuracy 2015-11-18 11:39:54 +11:00
Paul Riseborough fc6978e4d9 AP_NavEKF2: Fix error in parameter documentations 2015-11-18 11:39:54 +11:00
Paul Riseborough f9cadaf15c AP_NavEKF2: Increase resolution of innovation consistency gate parameters
Also adds protection against setting the gate to a number that would casue numerical errors.
2015-11-18 11:39:53 +11:00
Andrew Tridgell 08ef00b431 AP_NavEKF2: fixed build with IMU_BUFFER_LENGTH change 2015-11-18 11:33:51 +11:00
Paul Riseborough af3507ef3c AP_NavEKF2: Stop bad INS velocity spoiling height reset
We should reset the velocity as well as the height if a reasonable reset value is available.
2015-11-18 11:18:50 +11:00
Paul Riseborough 97799ef0b3 AP_NavEKF2: Ensure that GPS origin is set before using data for height 2015-11-18 11:18:50 +11:00
Paul Riseborough b8427e5d95 AP_NavEKF2: Only allow rangefinder height option during optical flow nav 2015-11-18 11:18:50 +11:00
Paul Riseborough c9eea98142 AP_NavEKF2: Improve terrain height estimation
The ad-hoc scaling of error growth has been replaced with a consistent method that uses the main nav filters published vertical velocity uncertainty and the terrain gradient assumption.
2015-11-18 11:18:46 +11:00
Paul Riseborough d820a538d5 AP_NavEKF2: Make magnetometer data available status global
This assists with debugging and makes it consistent with other measurements that are buffered
2015-11-18 11:18:46 +11:00
Paul Riseborough 04228e0b3b AP_NavEKF2: Rework selection of height measurements for fusion
GPS height has been added as a measurement option along with range finder and baro
Selection of the height measurement source has been moved into a separate function
Each height source is assigned its own measurement noise
If GPS or baro alt is not able to be used, it reverts to baro
When baro is not being used, an offset is continually calculated which enables a switch to baro without a height step.
2015-11-18 11:18:42 +11:00
Paul Riseborough 80bc64ee7a AP_NavEKF2: Update parameter documentation with GPS height source option 2015-11-18 11:18:18 +11:00
Paul Riseborough 4c72a14e22 AP_NavEKF2: Add function to LPF and spike filter baro data 2015-11-18 11:18:17 +11:00
Paul Riseborough 34311bedfa AP_NavEKF2: Add data buffers for range finder data 2015-11-18 11:18:17 +11:00
Andrew Tridgell 99da195e37 AP_NavEKF2: make IMU buffer length depend on main loop rate
this fixes a problem with Replay and baro data
2015-11-18 10:20:39 +11:00
Paul Riseborough 9b82b2200c AP_NavEKF2: Fix plane in-flight yaw reset
The copter method was being used for plane and the plane method was not being run due to the change in flight status not being detected.
The plane reset method did not trigger if the EKF had already dragged the velocity states along with the GPS or could align to an incorrect heading.
The method has been reworked so that it resets to the GPS course, but only if there are inconsistent angles and large innovations.
To stop a failed magnetometer causing a loss of yaw reference later in flight, if all available sensors have been tried in flight and timed out, then no further magnetoemter data will be used
2015-11-16 09:05:42 +11:00
Paul Riseborough e8706db382 AP_NavEKF2: Update IMU tuning parameter limits 2015-11-16 09:05:42 +11:00
Paul Riseborough 0562529729 AP_NavEKF2: Tuning changes to make attitude less sensitive to GPS and compass errors 2015-11-16 09:05:42 +11:00
Paul Riseborough 6522fb2621 AP_NavEKF2: Allow user to relax pre-flight GPS checks 2015-11-12 20:39:15 +11:00
Paul Riseborough 0b653bb82b AP_NavEKF2: Update MAVLnk parameter documentation 2015-11-11 15:16:36 +11:00
Paul Riseborough ca31ced2b4 AP_NavEKF2: Fix bug preventing use of optical flow with multiple IMU's 2015-11-10 18:17:16 +11:00
Paul Riseborough 577670ccee AP_NavEKF2: Reduce memory required by 6KB when running at 400Hz
Down-sample the IMU and output observer state data to 100Hz for storage in the buffer.
This reduces storage requirements for Copter by 75% or 6KB
It does not affect memory required by plane which already uses short buffers due to its 50Hz execution rate.
This means that the EKF filter operations operate at a maximum rate of 100Hz.
The output observer continues to operate at 400Hz and coning and sculling corrections are applied during the down-sampling so there is no loss of accuracy.
2015-11-10 15:51:18 +11:00
Paul Riseborough e33aa7898a AP_NavEKF2: Prevent return of uninitialised variable 2015-11-09 22:30:38 +11:00
Paul Riseborough 3d8e74df26 AP_NavEKF2: Don't switch magnetometers when on the ground or just started
Large magnetometer innovations on the ground could be caused by factors that will disappear when flying, eg:

a) Bad initial gyro bias
b) External magnetic field disturbances (adjacent metal structures, placement of hatches with magnets, etc)

To avoid unnecessary switches, we inhibit switching until off-ground and when sufficient time has lapsed from power on to learn gyro bias offsets.
2015-11-09 22:07:10 +11:00
Paul Riseborough 3099d94e78 AP_NavEKF2: Retrieve correct time-stamp for active compass 2015-11-09 22:07:10 +11:00
Paul Riseborough 5d2f78996a AP_NavEKF2: Remove un-used data in IMU buffer
Recovers 416 Bytes of memory per EKF2 instance for Copter
2015-11-09 22:07:10 +11:00
Paul Riseborough f00b1ff22d AP_NavEKF2: Publish the magnetometer selection index 2015-11-09 22:07:10 +11:00
Paul Riseborough 7294c8004b AP_NavEKF2: Enable use of backup magnetometers after a timeout
If the magnetometer fails innovation consistency checks for too long (currently 10 sec), then the next available sensor approved for yaw measurement will be used.
2015-11-09 22:07:09 +11:00
Paul Riseborough 6deabe28c2 AP_NavEKF2: Require all magnetometer axes to pass innovation test
The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis.
This was not achieved with the previous implementation.
These changes move the innovation consistency checks for all three axes to the top before any axes are fused.
Unnecessary performance timers have been removed.
2015-11-09 22:07:09 +11:00
Paul Riseborough afeadfca51 AP_NavEKF2: Don't attempt to compensate for external mag calibration
This was problematic to implement with magnetometer switching. It is likely that slow magnetometer learning can still be performed externally (eg plane) but this will need to be monitored to see if it causes issues.
2015-11-09 22:07:09 +11:00
Paul Riseborough f08a4af751 AP_NavEKF2: Remove redundant if statement
Thank you to OXINARF for picking up this one
2015-11-08 17:06:57 +11:00
Paul Riseborough 9efc2152fc AP_NavEKF2: Fix comment 2015-11-08 16:26:04 +11:00
Paul Riseborough a8983d075f AP_NavEKF2: Enable clean entry into GPS aiding in-flight
The setting of the EKF origin and the entry into GPS aiding mode have been separated to make the logic clear.
The order of operations has been changed to ensure that when a reset to GPS is performed, a valid GPS measurement is available in the buffer
Declaration of GPS availability is not made unless the GPS data has been entered into the buffer
2015-11-08 16:26:04 +11:00
Paul Riseborough f99dbc70f9 AP_NavEKF2: Don't return ready to use GPS true if user has inhibited GPS 2015-11-08 16:26:04 +11:00
Paul Riseborough d6f7156f4e AP_NavEKF2: Fix timing offset in position reset calculation
the correction for GPS latency now uses the correct time for that observation
2015-11-08 16:26:04 +11:00
Paul Riseborough 0b64ad0123 AP_NavEKF2: Enable EKF instance to be specified when requesting public data
Only applied to interfaces required for data logging.
If an invalid instance is requested, the data for the primary instance is returned. This allows the primary data to be returned by calling with a -1 instance value.
2015-11-08 15:37:28 +11:00
Paul Riseborough 6c2dc3ccf8 AP_NavEKF2: Publish the number of instances 2015-11-08 15:37:27 +11:00
Paul Riseborough 65e4a4ab94 AP_NavEKF2: Use correct casting of integers when printing
Changes in response to review comments
2015-11-08 15:37:27 +11:00
Paul Riseborough 2dcaeb0304 AP_NavEKF2: Update publishing of primary core index
Changes made in response to review comments
2015-11-08 15:37:27 +11:00
Paul Riseborough 1858da8307 AP_NavEKF2: Better switching in response to faults
Make selection sticky
If fault detected or unhealthy, then switch to healthy core with lowest fault score.
If no healthy core found, do not switch.
2015-11-08 15:37:27 +11:00
Paul Riseborough 5f6d1d8d25 AP_NavEKF2: Add fault score calculation 2015-11-08 15:37:27 +11:00
Paul Riseborough b24507d33c AP_NavEKF2: Publish the primary EKF index 2015-11-08 15:37:27 +11:00
Paul Riseborough 04165a60a7 AP_NavEKF2: Reduce false positive on EKF health check
Apply filtering to baro innovation check and and don't apply innovation checks once aiding has commenced because GPS and baro disturbances on the ground and during launch could generate a false positive
2015-11-08 15:37:27 +11:00
Paul Riseborough 49f9ea317c AP_NavEKF: Update console message and param descriptions for multiple EKF's 2015-11-08 15:37:26 +11:00
Paul Riseborough 2340e18fdc AP_NavEKF2: Offset the fusion time horizon between multiple instances
Prevents frame over-runs due to simultaneous fusion of measurements on each instance.
The offset is only applied if less than 5msec available between frames
2015-11-08 15:37:26 +11:00
Paul Riseborough 3eeff8dbc6 AP_NavEKF2: Remove unused code
This method of correcting for the fusion time horizon delay was too computationally expensive for our application and did not smooth fusion noise.
2015-11-08 15:37:26 +11:00
Paul Riseborough d48454ee2d AP_NavEKF2: Use nominated IMU unless unavailable
We now run a separate instance of the EKF for each IMU, so input data fusion of IMU's is no longer required.
2015-11-08 15:37:26 +11:00
Andrew Tridgell d52279af27 AP_NavEKF2: don't try to allocate last bytes of memory to EKF2 2015-11-06 10:42:58 +11:00
Andrew Tridgell ee78e2d618 AP_NavEKF2: set primary to first healthy core 2015-11-06 10:42:58 +11:00
Andrew Tridgell 2ab2afc86a AP_NavEKF2: added EK2_IMU_MASK for multiple IMUs
no voting between multiple IMUs yet
2015-11-06 10:42:58 +11:00
Lucas De Marchi e0e7b73ceb AP_NavEKF2: remove checks for HAL_BOARD_APM2 and HAL_BOARD_APM1 2015-11-04 12:14:12 +11:00
Paul Riseborough aa14de9d39 AP_NavEKF2: Fix failure to start mag cal due to gyro noise
Vibration in the 400Hz delta angles could cause the angular rate condition check for in-flight magnetic field alignment to fail.
The symptons were failure to start magnetic field learning as expected when EK2_MAG_CAL=3 was set.
2015-11-02 14:12:17 +11:00
Paul Riseborough 2ebce110b7 AP_NavEKF2: Explicitly define plane build type for default parameters
Use Copter parameters if build type is unknown
2015-11-02 14:12:17 +11:00
Paul Riseborough b6d63d4b4e AP_NavEKF2: Fix reporting of pre-flight GPS checks
Re-order checks so that that less important messages are not hidden when the 40 character buffer overflows
Add required output methods
2015-11-02 14:12:16 +11:00
Paul Riseborough 7bd61a484c AP_NavEKF2: Faster initial accel bias learning 2015-11-02 14:12:16 +11:00
Paul Riseborough b15bf3243e AP_NavEKF2: Remove potential for division by zero 2015-10-30 15:34:26 +09:00
Paul Riseborough 5533a9a149 AP_NavEKF2: Add improved health monitoring when using simple compass yaw fusion 2015-10-30 15:34:23 +09:00
Paul Riseborough f539b597a3 AP_NavEKF2: Improve initialisation of magnetic field learning
Use the more robust, but less accurate compass heading fusion up to 5m altitude
Wait for the magnetometer data fusion time offset to be correct before using data to reset states
Don't reset magnetic field states if the vehicle is rotating rapidly as timing offsets will produce large errors
When doing the yaw angle reset, apply the reset increment to all quaternions stored in the output buffer to avoid transients produced by yaw rotations and the 0.25 second fusion time horizon offset.
Only do the one yaw and mag reset at 5m, not two at 1.5 and 5.0m
Always re-do the yaw and mag reset when leaving the ground.
2015-10-30 15:34:20 +09:00
Paul Riseborough 844ed95718 AP_NavEKF2: Add method to rotate output quaternion history 2015-10-30 15:34:17 +09:00
Lucas De Marchi 831d8acca5 Remove use of PROGMEM
Now variables don't have to be declared with PROGMEM anymore, so remove
them. This was automated with:

    git grep -l -z PROGMEM | xargs -0 sed -i 's/ PROGMEM / /g'
    git grep -l -z PROGMEM | xargs -0 sed -i 's/PROGMEM//g'

The 2 commands were done so we don't leave behind spurious spaces.

AVR-specific places were not changed.
2015-10-30 14:35:16 +09:00
Lucas De Marchi 2c38e31c93 Remove use of PSTR
The PSTR is already define as a NOP for all supported platforms. It's
only needed for AVR so here we remove all the uses throughout the
codebase.

This was automated with a simple python script so it also converts
places which spans to multiple lines, removing the matching parentheses.

AVR-specific places were not changed.
2015-10-30 14:35:04 +09:00
Randy Mackay 4d458833dc AP_NavEKF2: remove unnecessary PV_AidingMode check
Thanks to OXINARF for catching this
2015-10-30 12:52:49 +09:00
Paul Riseborough b459d937ad AP_NavEKF2: Clean up GPS fusion timeout logic and comments 2015-10-30 12:24:19 +09:00
Randy Mackay 94adaba305 AP_NavEKF2: constify getLastPosNorthEastReset
Also constify getLastVelNorthEastReset
2015-10-30 12:24:16 +09:00
Randy Mackay 52ed075405 AP_NavEKF2: constify getLastYawResetAngle 2015-10-30 12:24:12 +09:00
Paul Riseborough 0b1a6a2157 AP_NavEKF2: Clean up GPS timeout logic 2015-10-30 12:24:06 +09:00
Paul Riseborough 74da4d8e57 AP_NavEKF2: Remove GPS glitch offset logic
Correction for steps in position and velocity caused by resets following GPS glitches and other events are now handled by the control loops.
2015-10-30 12:24:04 +09:00
Paul Riseborough 4fdec67546 AP_NavEKF2: Publish the position and velocity state reset deltas
Used by the control loops to compensate for step changes in position and velocity when the EKF needs to do a reset of these states.
2015-10-30 12:24:02 +09:00
Andrew Tridgell c228ea4371 AP_NavEKF2: prevent a valgrind error on startup 2015-10-26 11:34:32 +11:00
Paul Riseborough 87ea9acc7f AP_NavEKF2: Fix mavlink parameter label 2015-10-26 08:05:15 +11:00
Paul Riseborough 1cf626692c AP_NavEKF: Reduce impact of altimeter flow disturbance error on Copter alt hold
This increase in assumed altimeter noise and reduction in accel noise has been flight tested by L.Hall with noticeable reduction in the immediate response to Baro errors during moving flight.
This increases the time constant of response to baro errors such that the pilot can more easily compensate.
2015-10-26 08:05:15 +11:00
Paul Riseborough e692e30988 AP_NavEKF2: Reduce allowance for magnetometer timing errors
The previous gain from rate to magnetometer error was excessive. The revised value is equivalent to a magnetic field length of 0.5 with a timing uncertainty of 0.01 sec
2015-10-26 08:05:15 +11:00
Paul Riseborough 4c5ba60578 AP_NavEKF2: Remove dead code 2015-10-26 08:05:15 +11:00
Paul Riseborough 55ac8f0230 AP_NavEKF2: Update parameter values.
Testing on different platforms has shown that the new EKF has smaller innovations enabling innovation consistency checks that reject GPS and baro errors to be tightened.
The position and velocity thresholds for plane have been left the same because planes are less sensitive to GPS glitches as they fly higher and with more separation to surrounding objects. They are also more prone to bad inertial data due to the installation practices.
The altitude noise has been increased on plane to allow for the larger baro disturbances that result from the higher speeds and lack of a proper static pressure source. The innovation consistency gate has been adjusted to provide the same baro error limit of ~20m before baro is rejected.
2015-10-24 07:26:50 +11:00
Paul Riseborough 1e7ac873b9 AP_NavEKF2: Improve handling of GPS loss and recovery for planes
Extended GPS loss can result in the earth field states becoming  rotated and making it difficult for the EKF to recover its heading when GPS is regained.
During prolonged GPS outages, the position covariance can become large enough to cause the reset function to continually activate. This is fixed by ensuring that position covariances are always reset when the position is reset.
The innovation variance was being used incorrectly instead of the state variance to trigger the glitch reset.
2015-10-24 07:17:11 +11:00
Paul Riseborough cd5ec3a3e0 AP_NavEKF2: Reduce angle errors when flying without GPS
Turn off aiding during >1g manoeuvres when not using GPS
Use larger velocity and position uncertainties when flying without GPS
2015-10-23 19:04:30 +11:00
Paul Riseborough 1eaf318b9b AP_NavEKF2: Faster learning of gyro scale factors 2015-10-23 19:04:30 +11:00
Paul Riseborough a2f5962f77 AP_NavEKF2: Reduce effect of baro disturbances 2015-10-23 19:04:30 +11:00
Paul Riseborough 5eb7cf1fbf AP_NavEKF: Reduce roll/pitch disturbance when magnetic field is reset
Because we have changed the yaw angle and have taken a point sample on the magnetic field, covariances associated with the magnetic field states will be invalid and subsequent innovations could cause an unwanted disturbance in roll and pitch.
The reset of the Euler angles to a new yaw orientation was being done using roll and pitch from the output observer states, not the EKF state vector which meant that when roll and pitch were changing, the reset to a new yaw angle would also cause a roll and pitch disturbance.
2015-10-23 19:04:30 +11:00
Paul Riseborough b08817554e AP_NavEKF2: Ensure consistent position and velocity fusion rates when not using GPS
This sets the fusion of the synthetic position and velocity to occur at the same time as the barometer

This makes filter tuning more consistent between GPS and non-GPS useage
2015-10-21 17:27:00 +11:00
Paul Riseborough 1c347e8859 AP_NavEKF: Prevent high measurement data rates from overflowing buffers
High measurement data rates can fill buffers with data that is always new and never fused because it is over-written before it falls behind the measurement time horizon.
2015-10-21 17:27:00 +11:00
Paul Riseborough d1a090dda8 AP_NavEKF: Fix bug fetching data from measurement buffers
This bug can result in valid data being rejected and fused at the wrong time horizon.
2015-10-21 17:27:00 +11:00
Paul Riseborough aabb9b4e02 AP_NavEKF2: handle uncertain build type 2015-10-21 17:27:00 +11:00
Paul Riseborough e6312d91ce AP_NavEKF2: Correct variable name 2015-10-21 17:27:00 +11:00
Paul Riseborough 4640673cb1 AP_NavEKF2: Fix bug causing incorrect fusion timing 2015-10-21 17:27:00 +11:00
Paul Riseborough cd8b9c7d26 AP_NavEKF: prevent high rate mag data locking out other data fusion 2015-10-20 20:16:04 +11:00
Paul Riseborough ab8c28a7cc AP_NavEKF: Fix bug allowing takeoff in GPS modes without aiding
The legacy EKF switches GPs aiding on on arming, whereas the new EKF switches it on based on GPS data quality.
This means the decision to arm and therefore the predicted solution flags must now reflect the actual status of the navigation solution as it will no longer change when motor arming occurs.
2015-10-20 20:16:00 +11:00
Paul Riseborough e076ed7f66 AP_NavEKF2: Remove compiler warning message 2015-10-20 18:16:16 +11:00
Andrew Tridgell cb0f7cb370 NavEKF2: use new perf counter API 2015-10-20 18:16:15 +11:00
Paul Riseborough 60c2e81d19 AP_NavEKF2: Apply optimisations to declination fusion covariance update
These are the same type of optimisations that were successful with the magnetometer fusion
2015-10-20 15:21:41 +11:00
Paul Riseborough 0054291cf4 AP_NavEKF2: Apply optimisations to side-slip fusion covariance update
These are the same optimisations that were successful with the magnetometer fusion
2015-10-20 15:21:40 +11:00
Paul Riseborough 5e4bc4e954 AP_NavEKF2: Apply optimisations to airspeed fusion covariance update
These are the same optimisations that were successful with the magnetometer fusion
2015-10-20 15:21:40 +11:00
Paul Riseborough a29147d6d2 AP_NavEKF2: Apply optimisations to optical flow covariance update
These are the same optimisations that were successful with the magnetometer fusion
2015-10-20 15:21:40 +11:00
Paul Riseborough b5e43288d4 AP_NavEKF2: Down to 12 from 14 usec for perf test[8] 2015-10-20 15:21:40 +11:00
Paul Riseborough 0f530bb5a0 AP_NavEKF2: Correct comments 2015-10-20 15:21:40 +11:00
Andrew Tridgell d1dfd5fd01 AP_NavEKF2: use common header for optimisation level and irq disable 2015-10-20 15:21:40 +11:00
Andrew Tridgell a017ae7e00 AP_NavEKF2: down to 218us for test[9] 2015-10-20 15:21:40 +11:00
Andrew Tridgell fc23be8025 AP_NavEKF2: enable fine grained perf tuning for mag fusion 2015-10-20 15:21:40 +11:00
Andrew Tridgell b6c1352e4c AP_NavEKF2: added 10 test perf counters
used for fine grained performance tuning
2015-10-20 15:21:40 +11:00
Paul Riseborough 824436dfb6 AP_NavEKF2: Explicitly define constants as floats 2015-10-20 15:21:40 +11:00
Paul Riseborough 2fbd050418 AP_NavEKF2: Use blended accelerometer data
If high vibration levels cause offsets between the two, it switches to the accelerometer with lower vibration levels. The default behaviour is to use the average of both accelerometers.
2015-10-20 15:21:40 +11:00
Paul Riseborough 2b13020c19 AP_NavEKF2: Fix bug in vertical position derivative calculation 2015-10-20 15:21:39 +11:00
Paul Riseborough f6ad79688e AP_NavEKF2: Update start sequence console messages 2015-10-20 15:21:39 +11:00
Paul Riseborough 1a1236f764 AP_NavEKF2: Update magnetometer fusion documentation 2015-10-20 15:21:39 +11:00
Paul Riseborough 8526a8ba7e AP_NavEKF2: Level processor loading between frames
Don't fuse other measurements on the same frame that magnetometer measurements arrive if running at a high frame rate as there will be insufficient time to complete other operations.
2015-10-20 15:21:39 +11:00
Paul Riseborough 225b81baf4 AP_NavEKF2: Prevent users breaking filter by setting excessive time delays
If time deelays greater than the IMU buffer length are used, then it is possible for measurements to be dropped.
2015-10-20 15:21:39 +11:00
Paul Riseborough a1d6c7ae1e AP_NavEKF2: Improve timing accuracy
This parameter is a compromise between numerical accuracy of the covariance prediction and sensor timing jitter
Further testing has shown that doing covariance prediction and sensor fusion every 10msec has no observable effect on fusion health and reduces timing hitter noise on magnetometer observations during high rate maneovures
2015-10-20 15:21:39 +11:00
Paul Riseborough e0ed2dab63 AP_NavEKF2: Adjust IMU FIFO buffer for different IMU data rates
The values chosen ensure that up to consistent 250 msec of sensor delay compensation is available for different platform types
The revised values also ensure that fusion occurs at different time to when the 10Hz magnetometer measurements are read
2015-10-20 15:21:39 +11:00
Paul Riseborough baa8692960 AP_NavEKF2: Allow use of magnetometer learning during optical flow nav
Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference.
This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
2015-10-20 15:21:38 +11:00
Paul Riseborough 1b8a93ef0c AP_NavEKF2: Update function header comments 2015-10-20 15:21:38 +11:00
Paul Riseborough 6899767d28 AP_NavEKF2: Disable magnetic field learning if we have no absolute position reference 2015-10-20 15:21:38 +11:00
Paul Riseborough 242b9641c8 AP_NavEKF2: Fix in-air logic bug 2015-10-20 15:21:38 +11:00
Paul Riseborough e7de2d3ea3 AP_NavEKF: Update magnetic field learning options
Provide an option to always do learning
Make field learning decision logic clearer
Change defaults so that plane learns when airborne
Change defaults so that Rover does not learn (large external magnetic interference)
2015-10-20 15:21:38 +11:00
Paul Riseborough 495809f211 AP_NavEKF2: Allow magnetic field learning to be faster 2015-10-20 15:21:38 +11:00
Paul Riseborough 84a02efd52 AP_NavEKF2: Improve efficiency of Z magnetometer fusion 2015-10-20 15:21:37 +11:00
Paul Riseborough 35b08849f7 AP_NavEKF2: Reduce effect of rounding errors on covariance prediction 2015-10-20 15:21:37 +11:00
Paul Riseborough 4e928bf294 AP_NavEKF2: Fix bug preventing pre-takeoff reference measurements 2015-10-20 15:21:37 +11:00
Paul Riseborough 71c399674a AP_NavEKF2: Delay use of magnetic field states until off-ground
Magnetic interference whilst on the ground can adversely affect filter states. This patch ensures that the simpler and more robust magnetic heading observation method is used until the vehicle has cleared the ground.
2015-10-20 15:21:37 +11:00
Paul Riseborough 99c2dc41e0 AP_NavEKF2: Miscellaneous tuning changes 2015-10-20 15:21:37 +11:00
Paul Riseborough 47ae0f35f6 AP_NavEKF2: Make EKF2 PosDownDerivative interface follow coding conventions
Updates arising from peer review.
2015-10-20 15:21:37 +11:00
Paul Riseborough 27393855f1 AP_NavEKF2: Change assumed gyro calibration accuracy
If an external gyro calibration has been performed, we should assume that it has been done under static conditions
Otherwise it is pointless and we should allow the EKF to find its own gyro bias offsets.
2015-10-20 15:21:36 +11:00
Paul Riseborough 67a669fcdc AP_NavEKF2: Correct comments for height measurement 2015-10-20 15:21:36 +11:00
Paul Riseborough 0dc570b5a5 AP_NavEKF2: Improve optical flow takeoff detection logic
Ensure takeoff detect status goes to false when on ground
2015-10-20 15:21:36 +11:00
Paul Riseborough d3a6690e4f AP_NavEKF2: Fix bugs preventing use of optical flow navigation 2015-10-20 15:21:36 +11:00
Paul Riseborough 52fd369b2f AP_NavEKF2: Add methods to output and calculate vertical position derivative
This patch calculates a derivative of the vertical position (positive down).
The derivative is exposed via a public function.
2015-10-20 15:21:35 +11:00
Andrew Tridgell 2ae0811458 AP_NavEK2: fixed perf counter names for EKF2 2015-10-20 14:36:49 +11:00
Randy Mackay 35769035d4 AP_NavEKF2: resolve compiler warning related to init order 2015-10-15 20:03:05 +09:00
Paul Riseborough 0c61e09b70 AP_NavEKF2: Critical big fix - states not initialised
The failure to initialise the magnetometer bias states to zero can result in a large jump in yaw gyro bias and heading when a heading reset is performed.
2015-10-15 09:52:55 +11:00
Paul Riseborough e3013b493b AP_NavEKF2: Critical bug fix - perf counter not initialised 2015-10-15 09:48:13 +11:00
Paul Riseborough 20a3f9782e AP_NavEKF2: Add full set of selectable pre-flight GPS checks 2015-10-10 21:22:53 +09:00
Paul Riseborough db4dfce7b1 AP_NavEKF2: Add fix status to GPS check report message 2015-10-10 14:49:07 +09:00
Paul Riseborough 22920aafad AP_NavEKF2: Improve GPS status reporting 2015-10-10 14:48:59 +09:00
Paul Riseborough 1cb2220107 AP_NavEKF2: Critical Bugfix 2015-10-10 14:48:54 +09:00
Paul Riseborough f467a89fa3 AP_NavEKF2: Update comments for filter status output function 2015-10-10 14:48:52 +09:00
Paul Riseborough b142cc7fd2 AP_NavEKF2: Rename files and re-distribute content 2015-10-10 14:48:50 +09:00
Siddharth Bharat Purohit 1ce3276d74 AP_NavEKF2: split EKF control and output get functions from state specific libs 2015-10-10 14:48:48 +09:00
Siddharth Bharat Purohit 2e388fb2f9 AP_NavEKF2: split otp flow from PosVelNED 2015-10-10 14:48:46 +09:00
Siddharth Bharat Purohit 290ea0e1e8 AP_NavEKF2: split up EKF_core into different files 2015-10-10 14:48:44 +09:00
Paul Riseborough cde140354a AP_NavEKF2: Clean up control logic 2015-10-05 13:35:32 +09:00
Paul Riseborough f4db78fc11 AP_NavEKF2: Allow for larger gyro bias errors
MPU6000 data sheet indicates that variation on gyro ZRO across temperature range from -40 to +85 is +-20 deg/sec.
The limits on the gyro bias states have been increased to allow for this.
To enable the EKF to accommodate such large gyro bias values in yaw without the yaw error wrapping, leading to continual heading drift, an unwrap function has been applied to the compass heading error.
2015-10-05 13:35:29 +09:00
Paul Riseborough 325f4139fe AP_NavEKF2: Clean up mode change logic 2015-10-05 13:35:26 +09:00
Paul Riseborough ef624199f9 AP_NavEKF2: Enable GPS velocity fusion to be inhibited 2015-10-05 13:35:23 +09:00
Paul Riseborough 53e58f1075 AP_NavEKF2: Update default parameters
Updates from preliminary tuning in Replay
2015-09-25 19:22:56 +10:00
Paul Riseborough 2fb72b6e6a AP_NavEKF2: Clean up loss of GPS logic 2015-09-25 19:20:52 +10:00
Paul Riseborough df0eb9d9d7 AP_NavEKF2: Don't run GPS checks when not required 2015-09-25 19:20:52 +10:00
Paul Riseborough 1986af021f AP_NavEKF2: Remove un-used flight mode and duplicate variable 2015-09-25 19:20:52 +10:00
Paul Riseborough 7230472516 AP_NavEKF2: Adjust parameter defaults 2015-09-25 19:20:52 +10:00
Paul Riseborough 8bcedb228b AP_NavEKF2: Bring pre-flight GPS checks up to date with EKF1 2015-09-25 19:20:52 +10:00
Randy Mackay 015f700bc0 AP_NavEKF2: getLastYawResetAngle returns last reset time 2015-09-24 16:57:38 +09:00
Paul Riseborough 80e182f827 AP_NavEKF2: Update EKF2 data logging 2015-09-24 12:58:44 +10:00
Paul Riseborough 4acd6c129a AP_NavEKF2: Update parameters 2015-09-24 12:58:43 +10:00
Paul Riseborough 9c5e48e7e9 AP_NavEKF2: Remove unused variables and improve variable names 2015-09-24 12:58:43 +10:00
Paul Riseborough 4a7714e15d AP_NavEKF2: Remove unused parameters and improve naming consistency 2015-09-24 12:58:43 +10:00
Paul Riseborough 8afb26087d AP_NavEKF2: Remove unused function 2015-09-24 12:58:43 +10:00
Andrew Tridgell fe76662faf AP_NavEKF2: added set_enable() API 2015-09-23 18:55:12 +10:00
Paul Riseborough f270573acc AP_NavEKF2: Use library functions for quaternion corrections 2015-09-23 17:48:48 +10:00
Andrew Tridgell cbc62238a9 AP_NavEKF2: run astyle for reformatting 2015-09-23 17:29:28 +10:00
Andrew Tridgell 760dafbb9d AP_NavEKF2: added enabled() function 2015-09-23 12:09:26 +10:00
Andrew Tridgell b4555f30a5 AP_NavEKF2: added frontend calls to core code 2015-09-23 11:56:42 +10:00
Andrew Tridgell 3ac75aeffb AP_NavEKF2: added EK2_ENABLE parameter 2015-09-23 11:56:42 +10:00
Paul Riseborough f500474a86 AP_NavEKF2: initial import of new maths EKF 2015-09-23 09:51:14 +10:00