Commit Graph

77 Commits

Author SHA1 Message Date
Peter Barker 369292f7f3 AP_NavEKF2: stop relying on the presence of a RangeFinder 2019-12-11 08:09:01 +11:00
Peter Barker 99689f65f8 AP_NavEKF2: adjust for renaming of RangeFinder files 2019-11-12 07:14:37 +08:00
Paul Riseborough c26c6fd078 AP_NavEKF2: Add missing vertical position derivative calculation
Use a third order order complementary filter to estimate the rate of change of vertical position output.
2019-10-18 11:15:25 +09:00
Andrew Tridgell f7e138bddd AP_NavEKF2: use parent class for intermediate static variables
this makes the code faster as well as using less memory when both EK2
and EK3 are enabled
2019-10-01 15:39:03 +10:00
Andrew Tridgell 255981c60c AP_NavEKF2: moved some of the intermediate vars to the stack
this keeps stack frames below 1k, while giving faster access to the
variables and saving more memory
2019-09-24 12:51:54 +10:00
Randy Mackay 94a52b3525 EKF2: RNG_USE_HGT param only used when ALT_SOURCE = rangefinder 2019-09-10 18:44:52 +10:00
Andrew Tridgell 098701ec79 AP_NavEKF2: fixed baro innovation gate when on ground with AIDING_NONE
when on the ground without a position source we would disable the
innovation gate for the barometer. This meant that a single (or small
number of) really bad baro readings would be fused into the EKF,
causing it to destabilise

Fixes #11903
2019-07-30 08:23:21 +10:00
Andrew Tridgell 2427440e13 AP_NavEKF2: origin handling fixes from Francisco 2019-07-11 06:40:55 +10:00
Andrew Tridgell e8d9a1fae9 AP_NavEKF2: don't allow height datum reset when not on ground 2019-07-07 19:20:32 +10:00
Andrew Tridgell f5fd38aef2 AP_NavEKF2: set terrainState to zero on height datum reset 2019-07-07 19:20:32 +10:00
Andrew Tridgell 9372073f63 AP_NavEKF2: continuously update gpsGoodToAlign
use it to determine how to handle a height datum reset
2019-07-07 19:20:32 +10:00
Andrew Tridgell 20ebb2500b AP_NavEKF2: ensure that both AMSL and relative alt are correct after a reset
this forces the EKF origin to the GPS alt on a height datum reset if
we have GPS lock. If we don't do this then the reported AMSL alt will
drift over time away from the GPS alt when we reset while on the
ground
2019-07-07 19:20:32 +10:00
Michael du Breuil f04e47fc17 AP_NavEKF2: Reduce scope of AP_Baro.h 2019-06-27 14:56:21 +10:00
Michael du Breuil 892b687c09 AP_NavEKF2: Fix AP_GPS.h include 2019-06-18 10:02:05 +10:00
Arjun Vinod 78b165e36f AP_NavEKF2: fixed typos 2019-02-26 08:33:39 +11:00
Arjun Vinod a0e7c37f8c AP_NavEKF2: fixed typos 2019-02-22 10:21:35 +11:00
Tom Pittenger b32e8a4424 AP_NavEKF2: remove HAL_CPU_CLASS_150 check, 150 is already a minimum requirement 2019-02-17 12:59:52 -08:00
chobits 08b9bf4a89 AP_NavEKF2: fix extNav position noise 2018-10-24 19:14:54 +09:00
Paul Riseborough c680b931dc AP_NavEKF2: Enable fusion of external nav position data 2018-03-27 20:28:42 +09:00
Peter Barker 5ab89324b2 AP_NavEKF2: use baro singleton 2018-03-08 21:20:05 -08:00
Peter Barker 9c8466dc03 AP_NavEKF2: use GPS singleton 2017-12-27 00:58:02 +00:00
Peter Barker 1e83ef3c44 AP_NavEKF2: use rangefinder backend accessors 2017-08-15 18:30:45 +01:00
priseborough f2f3067326 AP_NavEKF2: Add interface to control GPS vertical velocity use 2017-08-10 19:49:45 +10:00
priseborough 36e7dfb5f6 AP_NavEKF2: Improve GPS reference height estimator
Fix rounding error bug preventing state from updating after initial convergence.
Decouple GPS reference height from published EKf origin height.
Add bitmask parameter to control update and publishing of GPS reference height.
2017-06-14 12:44:41 +01:00
Jonathan Challinger c53125f3b5 AP_NavEKF2: apply height innovation floor only when barometer is in use 2017-03-24 14:42:36 -07:00
priseborough 8c46d4316b AP_NavEKF2: handle position jump caused by change in GPS receiver
If the GPS receiver selection changes and we are using GPS for height, the
vertical position will be reset to the new GPS height measurement.
correct output observer history when doing a GPS height reset
2017-03-13 11:31:51 +11:00
Randy Mackay 563be389ee AP_NavEKF2: only use downward facing rangefinder 2017-02-27 15:18:10 +09:00
Francisco Ferreira c6875e1756
AP_NavEKF2: remove editor information wrongly added in d408d25 2017-01-09 16:16:14 +00:00
Dr.-Ing. Amilcar Do Carmo Lucas d408d25f92 More spelling stuff 2017-01-06 10:57:39 -08: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
priseborough ef03a85cd3 AP_NavEKF2: add range beacon interface and fusion method 2016-11-30 17:56:43 +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
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 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 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
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 cb1d3c7ed2 AP_NavEKF2: Allow for terrain gradient when using range finder for height 2016-09-13 17:45:03 +09: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
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
Jonathan Challinger 3382e09580 AP_NavEKF2: add height constraint during takeoff 2016-06-28 14:20:11 +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 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 51a3df0319 AP_NavEKF2: Add error checking and isolation to pos vel fusion 2016-05-21 15:13:51 +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
Ricardo de Almeida Gonzaga 64d14356b9 AP_NavEKF2: Fix typos 2016-05-13 19:20:06 -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
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