Commit Graph

471 Commits

Author SHA1 Message Date
Dr.-Ing. Amilcar Do Carmo Lucas d408d25f92 More spelling stuff 2017-01-06 10:57:39 -08:00
Dr.-Ing. Amilcar Do Carmo Lucas 8419045aea Spell in comments 2017-01-06 10:57:39 -08:00
murata 653030b038 AP_NavEKF2: Changed if statements to switch statement.
AP_NavEKF2: Change indentation.
2016-12-26 10:15:55 +09:00
priseborough 6dc7d25fde AP_NavEKF2: Fix bug in loss of aiding fallback
Fixes a bug introduced by the introduction of range beacon fusion to the fallback test.
2016-12-21 18:08:39 +11:00
priseborough fe69d9a0ea AP_NavEKF2: remove duplicate include 2016-12-18 10:28:04 +11:00
priseborough a8f3a374e2 AP_NavEKF2: replace sqrt function calls with sqrtf
Prevents compiler errors and un-wanted use of double precision types
2016-12-18 10:28:04 +11:00
priseborough 76106889d5 AP_NavEKF2: Prevent unwanted core switch on startup
Require primary core to be healthy for 10 seconds before starting selection logic
2016-12-13 01:18:28 +00:00
priseborough 69f3c83c20 AP_NavEKF2: Don't use speed switch criteria when speed estimate is invalid
Prevent unwanted switches from range finder to Baro height aiding due to triggering of the speed threshold
2016-12-09 21:17:28 +00:00
priseborough 14a0155f5e AP_NavEKF2: Allow threshold speed for range finder use to be adjusted 2016-12-09 21:17:28 +00:00
murata 25e47cce6b AP_NavEKF2: Changed sqrt method to sqrtf method.
AP_NavEKF2: Changed sqrt method to sqrtf method.
2016-12-07 15:41:41 +00:00
Randy Mackay fc5f4c20cd AP_NavEKF2: remove redundant check of beacon nullptr
thanks to muramura for spotting this
2016-12-01 14:53:26 +09:00
priseborough 665ba1c009 AP_NavEKF2: Fix bug in reporting of vertical position reset
Fixes a bug that allows the last reported reset time to to wind back and an out of date reset delta to be reported if a switch to a core that has previously been reset occurs.
Allows multiple consumers provided they access on the same frame.
2016-12-01 10:35:38 +09:00
priseborough 7f347e39dd AP_NavEKF2: Fix bug in reporting of horizontal position reset
Fixes a bug that allows the last reported reset time to to wind back and an out of date reset delta to be reported if a switch to a core that has previously been reset occurs.
Allows multiple consumers provided they access on the same frame.
2016-12-01 10:35:38 +09:00
priseborough 656d9bb2c4 AP_NavEKF2: Fix bug in reporting of yaw reset
Fixes a bug that allows the last reported reset time to to wind back and an out of date reset delta to be reported if a switch to a core that has previously been reset occurs.
Allows multiple consumers provided they access on the same frame
2016-12-01 10:35:38 +09:00
Randy Mackay 66a9093cd0 AP_NavEKF2: getRangeBeaconDebug returns false on failure to get beacon data 2016-11-30 17:56:51 +09:00
priseborough 0ec8f0932a AP_NavEKF2: Prevent output of NaN's in range beacon debug output
This can happen if this accessor function is called before the EKF states are initialised
2016-11-30 17:56:50 +09:00
Randy Mackay fbecda7eeb AP_NavEKF2: change order of variable initialisation
No functional change, this modifies the init order to match the declaration order.
2016-11-30 17:56:49 +09:00
Randy Mackay 82854953e7 AP_NavEKF2: minor comment fix 2016-11-30 17:56:48 +09:00
Randy Mackay 14a73e499a AP_NavEKF2: initialise two range beacon structures 2016-11-30 17:56:47 +09:00
priseborough 2ee8250807 AP_NavEKF2: Changes from code review by @muramura 2016-11-30 17:56:46 +09:00
Randy Mackay 1f4606e48a AP_NavEKF2: protect against null beacon 2016-11-30 17:56:45 +09:00
Randy Mackay 51ccfca31c AP_NavEKF2: fix format of statustext for beacon init 2016-11-30 17:56:44 +09:00
priseborough ef03a85cd3 AP_NavEKF2: add range beacon interface and fusion method 2016-11-30 17:56:43 +09:00
priseborough 0a177d04fd AP_NavEKF2: Simply core switch implementation
Functionally equivalent
2016-11-24 19:58:10 +09:00
priseborough 7d48054e6f AP_NavEKF2: Group GPS innovation tests when compiling error score 2016-11-24 19:58:07 +09:00
Randy Mackay e9ea06bd10 AP_NavEKF2: minor comment fix 2016-11-24 19:58:03 +09:00
priseborough 0df45703f1 AP_NavEKF2: Fix errors in position reset delta publishing 2016-11-24 19:57:47 +09:00
priseborough f6021c959d AP_NavEKF2: capture and publish vertical position reset deltas
Calculate the change in vertical position due to a internal EKF state reset or a EKF core switch
2016-11-24 19:57:45 +09:00
priseborough e62863f9a1 AP_NavEKF2: Reduce unnecessary EKF core switching
1) Do not switch to a core until its states have been updated.
2) Distinguish between mandated switches required for health failure and optional switches required due to innovation checks failing.
3) Apply hyseresis to innovation check levels
2016-11-24 19:57:41 +09:00
murata c808ee2f49 Global: To nullptr from NULL.
RC_Channel: To nullptr from NULL.

AC_Fence: To nullptr from NULL.

AC_Avoidance: To nullptr from NULL.

AC_PrecLand: To nullptr from NULL.

DataFlash: To nullptr from NULL.

SITL: To nullptr from NULL.

GCS_MAVLink: To nullptr from NULL.

DataFlash: To nullptr from NULL.

AP_Compass: To nullptr from NULL.

Global: To nullptr from NULL.

Global: To nullptr from NULL.
2016-11-02 16:04:47 -02:00
Andrew Tridgell 82365f8670 AP_NavEKF2: save some memory in the position offsets in EKF2
we don't need to copy that vector3f for every sample. A uint8_t does
the job
2016-10-27 17:09:06 +11:00
priseborough c93c3d54f3 AP_NavEKF2: Don't correct for zero IMU position offset
The IMU offset correction involves a significant number of floating point operations and most users will leave the offset parameter at zero.
2016-10-27 14:54:44 +11:00
priseborough 1890bbdafa AP_NavEKF2: Don't correct GPS for zero position offset 2016-10-27 14:54:44 +11:00
priseborough f025c96e63 AP_NavEKF2: Don't correct range finder for zero position offset 2016-10-27 14:54:43 +11:00
priseborough 9a0ce1d5eb AP_NavEKF2: Don't correct flow sensor for zero position offset 2016-10-27 14:54:43 +11:00
priseborough b40016db62 AP_NavEKF2: Fix documentation errors 2016-10-27 14:54:43 +11:00
priseborough 13ca62b1c7 AP_NavEKF2: Correct velocity and position outputs for IMU offset
This can improve position hold performance where it is not practical to have the IMU located at the centroid.
Although this enables the effect of IMU position offsets to be corrected, users will still need to be instructed to place the IMU as close to the vehicle c.g. as practical as correcting for large offsets makes the velocity estimates noisy.
2016-10-27 14:54:43 +11:00
priseborough 21dcf42c27 AP_NavEKF2: Add missing documentation 2016-10-27 14:54:43 +11:00
priseborough 8922861359 AP_NavEKF2: Correct optical flow data for sensor position offset
Correction requires the body rates averaged across the flow sensor sampling interval. This data has been added to the sensor buffer.
The body rate data from the flow sensor driver does not contain the Z component, so an equivalent value sampled from the navigation IMU has been used instead.
The variable omegaAcrossFlowTime has been moved out of the class and into the only function that uses it.
2016-10-27 14:54:43 +11:00
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