Commit Graph

721 Commits

Author SHA1 Message Date
Paul Riseborough c828bdfbc9 AP_NavEKF3: Reduce output observer vertical velocity error when bad IMU 2021-09-23 18:55:28 +10:00
Paul Riseborough 8429c66860 AP_NavEKF3: Make subsequent bad IMU event detections faster 2021-09-23 18:55:28 +10:00
Paul Riseborough 458c67639d AP_NavEKF3: Force velocity state to follow GPS when IMU data is bad 2021-09-23 18:55:28 +10:00
Paul Riseborough c0d88e2673 AP_NavEKF3: Fix vertical velocity reset
Fixes bug that prevents the vertical velocity being reset to the GPS if the position aiding has already timed out and improves sensitivity of the bad IMU data check.
2021-09-23 18:55:28 +10:00
Paul Riseborough 17c2ba5ac1 AP_NavEKF3: Fix bug causing too frequent resets if bad IMU data detected 2021-09-23 18:55:28 +10:00
Peter Barker b66fd214f0 AP_NavEKF3: move from HAL_NO_GCS to HAL_GCS_ENABLED 2021-09-22 21:37:00 +10:00
Paul Riseborough e9c339a0a0 AP_NavEKF3: fix wind speed covariance initialisation bug 2021-09-21 17:16:36 +10:00
Josh Henderson 3006d50f97 AP_NavEKF3: use vector.xy().length() instead of norm(x,y) 2021-09-14 10:43:46 +10:00
Peter Barker dabba69b5e AP_NavEKF3: improve description of XKF4/NKF4 fields
Courtesy Paul Riseborough here: https://github.com/ArduPilot/ardupilot_wiki/issues/3641#issuecomment-913577132
2021-09-08 17:06:12 +10:00
Randy Mackay 63e579d738 AP_NavEKF3: add using_extnav_for_yaw 2021-08-31 09:20:17 +10:00
Randy Mackay 0edfd2634f AP_NavEKF3: rename using_external_yaw to using_noncompass_for_yaw 2021-08-31 09:20:17 +10:00
Randy Mackay d1f2acd813 AP_NavEKF3: rename varInnovOptFlow to flowVarInnov
also renamed innovOptFlow to flowInnov
2021-08-24 16:49:04 +09:00
Randy Mackay 85ade10e85 AP_NavEKF3: ofDataNew made local 2021-08-24 16:49:04 +09:00
Randy Mackay 11847cfcf5 AP_NavEKF3: always calculate optical flow innovations and variances 2021-08-24 16:49:04 +09:00
Randy Mackay 914a8dca49 AP_NavEKF3: for loops use uint8_t instead of unsigned 2021-08-24 16:49:04 +09:00
Randy Mackay 5779ebd177 AP_NavEKF3: remove unused Tbn_flow 2021-08-24 16:49:04 +09:00
Randy Mackay f9fd63e01e AP_NavEKF3: remove unused optflow calcs 2021-08-24 16:49:04 +09:00
TunaLobster d6a5622fcb AP_NavEKF3: Remove @RebootRequired: False 2021-08-19 10:00:16 +10:00
Andrew Tridgell 5c86cc6828 AP_NavEKF3: added EK3_PRIMARY parameter
allows for selection of which IMU to use on startup
2021-08-17 06:42:42 +10:00
Pierre Kancir 8216905f47 AP_NavEKF3: remove unecessary assignements 2021-08-12 10:39:49 +09:00
Randy Mackay 2a2cde68ef AP_NavEKF3: skip GSF reset count check if source actively changed
also only fail all compass on emergency reset
2021-08-10 09:44:57 +10:00
Andrew Tridgell ff59e0f72f AP_NavEKF3: fixed output LLH to account for IMU offset
thanks to Paul and Randy
2021-08-09 19:19:21 +10:00
Randy Mackay 2f1444772b AP_NavEKF3: XKF3.IYAW logs in degrees 2021-08-09 08:10:42 +09:00
Randy Mackay d961186e0c AP_NavEKF3: remove unused EK3_GSF_DELAY param 2021-08-06 07:00:46 +09:00
Andrew Tridgell 8681ecebfd AP_NavEKF3: mark log messages as streaming 2021-08-03 10:23:36 +10:00
Peter Barker c34284b6eb AP_NavEKF3: stop using AHRS as conduit for Compass pointer 2021-07-30 12:19:42 +10:00
Paul Riseborough 17ead96250 AP_NavEKF3: Revert IMU and wind speed process noise parameter defaults
These give noisier state estimates, but are more robust to rapid changes in IMU biases.
TODO implement a means of using the modified parameters when there are more than one EKF instance running with IMU's that are sampling at a higher rate.
2021-07-22 18:20:45 +10:00
Paul Riseborough aa6ac4a874 AP_NavEKF3: retune wind process noise for better airspeed fault detection 2021-07-22 18:20:45 +10:00
Paul Riseborough 8fd1e98701 AP_NavEKF3: Fix bug preventing copter wind estimation at low speed
Also re-tunes process noise default for smoother wind velocity state estimates.
2021-07-22 18:20:45 +10:00
Paul Riseborough 0088b0f3fe AP_NavEKF3: Revert EK3_BETA_MASK parameter extension
These are not required due to use of bit 7 in FLIGHT_OPTIONS to achieve the same function.
2021-07-22 18:20:45 +10:00
Paul Riseborough 59d31cc7d5 AP_NavEKF3: Rework non-airspeed wind estimation
Faster wind estimation when not using airspeed with acceptable noise in wind velocity estimates.
2021-07-22 18:20:45 +10:00
Paul Riseborough 5fa3ed5755 AP_NAvEKF3: use #define value for bad IMU hold time 2021-07-22 18:20:45 +10:00
Paul Riseborough 7497590363 AP_NavEKF3: Increase lower state variance limit when vibration affected
This makes state corrections from GPS and baro observations more aggressive
2021-07-22 18:20:45 +10:00
Paul Riseborough 3e123c0a30 AP_NavEKF3: Use sensor variance when checking for bad IMU 2021-07-22 18:20:45 +10:00
Paul Riseborough e3bdbcd8ea AP_NavEKF3: Make bad IMU status more persistent 2021-07-22 18:20:45 +10:00
Paul Riseborough ccc95d8726 AP_NavEKF3: Add accessor function for vibration affected status 2021-07-22 18:20:45 +10:00
Paul Riseborough ed61287410 AP_NavEKF3: Don't update accel bias states if vibration affected 2021-07-22 18:20:45 +10:00
Paul Riseborough 62d70a628e AP_NavEKF3: Use large accel process noise when IMU data is bad 2021-07-22 18:20:45 +10:00
Paul Riseborough 5eb7751682 AP_NavEKF3: Adjust gyro bias process noise tuning
NEw value is a compromise between roll/pitch angle and horizontal state velocity estimation errors and the noise in the gyro bias estimate
2021-07-22 18:20:45 +10:00
Paul Riseborough 72dc998509 AP_NavEKF3: Allow smaller dvel bias state variances for improved tuning 2021-07-22 18:20:45 +10:00
Paul Riseborough 0f2661c31c AP_NavEKF3: Retune IMU process noise
Required to achieve equivalent fusion noise and weighting on IMU vs other sources to previous param defaults with the old covariance prediction equations.
2021-07-22 18:20:45 +10:00
Paul Riseborough 171eed5d85 AP_NavEKF3: Make gyro bias learning less noisy 2021-07-22 18:20:45 +10:00
Paul Riseborough ce4d13091a AP_NavEKF3: Fix bug preventing learning of XY IMU dvel bias in flight 2021-07-22 18:20:45 +10:00
Paul Riseborough 6242ce19fa AP_NavEKF3: Change powf(x,2) to sq(x) 2021-07-22 18:20:45 +10:00
Paul Riseborough bb9eed28a9 AP_NavEKF3: Update covariance prediction equations 2021-07-22 18:20:45 +10:00
Paul Riseborough 7b8e935880 AP_NavEKF3: Use alternate form for quaternion to rotmat equations in derivation 2021-07-22 18:20:45 +10:00
Peter Barker 795f412264 AP_NavEKF3: pass NavEKF failures back up to callers 2021-07-21 18:02:26 +10:00
Andrew Tridgell fc2118f42b AP_NavEKF3: process GPS yaw independently of GPS fix
this processes GPS yaw with its own timestamp and as a separated step
from fusing position and velocity. This makes the yaw time handling
more accurate as yaw for moving baseline GPS comes in as a separate
piece of data from the position and velocity
2021-07-21 17:59:49 +10:00
Andrew Tridgell 0b9cde5812 AP_NavEKF3: fixed indentation in readGpsData() 2021-07-21 17:59:49 +10:00
Peter Barker a4410a4b2e AP_NavEKF3: log EKF data source set in XKFS 2021-07-21 16:31:53 +10:00
Andrew Tridgell 6b73c2151a AP_NavEKF3: implement moving origin
this shifts EKF_origin to the current location at 1Hz, while leaving
public_origin alone. All output APIs and logging are relative to
public_origin.

The effect of this change is to remove the distortion caused by a
spherical earth, which allows the EKF to operate without errors at
very long distances from the public_origin.
2021-07-14 17:34:40 +10:00
Andrew Tridgell 98fb4fcbe7 AP_NavEKF3: convert code_gen.py to ftype 2021-07-12 17:27:08 +10:00
Andrew Tridgell 44f098c86d AP_NavEKF3: convert powF(xx,2) calls to sq(xx) 2021-07-12 17:27:08 +10:00
Andrew Tridgell ae8dbe36bb AP_NavEKF3: convert EKF3 derivation to ftype 2021-07-12 17:27:08 +10:00
Andrew Tridgell 5319e3910f AP_NavEKF3: moved checkUpdateEarthField to be called less often 2021-07-10 07:20:41 +10:00
Andrew Tridgell a8c6d742aa AP_NavEKF3: update earth field at 1Hz
this prevents large mag errors on long distance flights
2021-07-10 07:20:41 +10:00
Andrew Tridgell 885e518741 AP_NavEKF3: allow for double EKF build 2021-07-10 07:20:41 +10:00
Randy Mackay 4fde26aa01 AP_NavEKF3: EK3_RNG_USE_HGT param references EK3_SRCx_POSZ 2021-07-05 08:42:06 +09:00
Paul Riseborough 1281033931 AP_NavEKF3: Remove unused class variable rngBcnTimeout 2021-06-23 13:26:02 +09:00
Paul Riseborough 9d85a4292e AP_NavEKF3: Ensure postion timeout flag and timer is always reset 2021-06-23 13:26:02 +09:00
Paul Riseborough de3c6d6e5c AP_NavEKF3: Fade each vert vel variance clip count over 1 second 2021-06-23 13:26:02 +09:00
Paul Riseborough ac92182153 AP_NavEKF3: Clean up initialisation of boolean array 2021-06-23 13:26:02 +09:00
Paul Riseborough b0763f04f1 AP_NavEKF3: Remove unwanted line 2021-06-23 13:26:02 +09:00
Paul Riseborough b7aad13cc8 AP_NavEKF3: Protect against collapse of velocity and position variances 2021-06-23 13:26:02 +09:00
Paul Riseborough 36160ba9ce AP_NavEKF3: Fix bug preventing horizontal position reset if badIMUdata 2021-06-23 13:26:02 +09:00
Paul Riseborough 5d00b7d042 AP_NavEKF3: Fix bug preventing height reset if badIMUdata 2021-06-23 13:26:02 +09:00
Paul Riseborough e4eb8339d0 AP_NavEKF3: Fix bug preventing velocity reset if badIMUdata 2021-06-23 13:26:02 +09:00
Josh Henderson 64dc3bbe41 AP_NavEKF3: Fix #17789 core's ekf origin altitude different if flying 2021-06-22 12:01:10 +10:00
Josh Henderson 0ae3730f11 AP_NavEKF3: non_GPS modes ensure EKF origin set only once and stays in sync
ekf3
2021-06-22 12:01:10 +10:00
Peter Barker d6348a0786 AP_NavEKF3: remove getBodyFrameOdomDebug
Not used after we moved logging to be within AP_NavEKF3
2021-06-07 09:28:52 +10:00
Peter Barker dd3ab29b2f AP_NavEKF3: remove getFilterGpsStatus
Not needed after we moved logging into NavEKF3
2021-06-07 09:25:53 +10:00
Siddharth Purohit cd5b764fd8 AP_NavEKF3: use first usable compass index to set magSelectIndex 2021-06-02 17:10:19 +10:00
Paul Riseborough a5a25411da AP_NavEKF3: Remove unnecessary baro limiting 2021-06-02 11:13:16 +09:00
Paul Riseborough 124f016e5b AP_NavEKF3: Use parameter to set baro ground effect dead-zone 2021-06-02 11:13:16 +09:00
Paul Riseborough 76d0dcc25c AP_NavEKF3: Use common method for handling baro ground effect 2021-06-02 11:13:16 +09:00
Andrew Tridgell dc9435a88d AP_NavEKF3: don't limit baro change when we are in fixed wing mode 2021-06-02 11:13:16 +09:00
Peter Barker c00f110f3d AP_NavEKF3: use DAL APIs for takeoff/touchdown expected 2021-06-02 11:13:16 +09:00
Peter Barker 15658362b5 AP_NavEKF3: make time-horizon OF data a local variable
Saves 40 bytes of RAM per core on stm32
2021-06-01 09:56:19 +10:00
Randy Mackay 02f6ca51bc AP_NavEKF3: init rngOnGnd to 5cm to avoid div-by-zero 2021-05-28 08:08:08 +09:00
Paul Riseborough 9014afc9d4 AP_NavEKF3: Fix typo in comment 2021-05-11 10:37:08 +10:00
Paul Riseborough b9abef37c3 AP_NavEKF3: Reset to GPS yaw if fusion times out when on ground. 2021-05-11 10:37:08 +10:00
Peter Barker f4591faeed AP_NavEKF3: simplify taking of GPS measurements 2021-03-23 15:11:33 +11:00
Paul Riseborough 1018801522 AP_NavEKF3: White space and comment fixes 2021-03-23 10:05:59 +11:00
Paul Riseborough 7113387d27 AP_NavEKF3: Use better defined data type 2021-03-23 10:05:59 +11:00
Paul Riseborough ac87cab6bc AP_NavEKF3: constify array index variable 2021-03-23 10:05:59 +11:00
Paul Riseborough 1498b516a3 AP_NavEKF3: Don't try to learn gyro biases that are poorly observable 2021-03-23 10:05:59 +11:00
Paul Riseborough 44be7161c0 AP_NavEKF3: Reduce yaw drift when operating without external aiding
Also fixes bug in the determination of the total angular variance threshold for when to fuse a zero innovation yaw measurement.
2021-03-23 10:05:59 +11:00
Paul Riseborough 53c4b163ce AP_NavEKF3: Allow adjustment of on ground not moving test sensitivity
Also re-tune default sensitivity based on user supplied logs with noisy gyro data.
2021-03-23 10:05:59 +11:00
Paul Riseborough e7b08c1ffc AP_NavEKF3: Update log message format description 2021-03-23 10:05:59 +11:00
Paul Riseborough 918606e71d AP_NavEKF3: Don't use fake measurements to learn dvel bias 2021-03-23 10:05:59 +11:00
Paul Riseborough c709ad9c80 AP_NavEKF3: Add missing axis specific dvel bias update inhibit 2021-03-23 10:05:59 +11:00
Paul Riseborough 073e273732 AP_NavEKF3: Don't allow yaw alignment until tilt alignment completed 2021-03-23 10:05:59 +11:00
Paul Riseborough a07427fd30 AP_NavEKF3: Don't learn poorly observed IMU dvel bias states before flight 2021-03-23 10:05:59 +11:00
priseborough 4b1c334bba AP_NavEKF3: improve stability of co-variance matrix processing
This patch prevents the delta velocity bias state variances collapsing due to insufficient process noise and causing bad bias estimates to form.
2021-03-23 10:05:59 +11:00
murata c5b1c832ba AP_NavEKF3: Change misnomer (NFC) 2021-03-15 18:21:15 +11:00
Peter Barker 4605870788 AP_NavEKF3: remove separate calls to get delta-times for vel and ang 2021-03-13 11:29:29 +11:00
Paul Riseborough e3eea0a54a AP_NavEKF3: Use specialised function for in-flight plane yaw alignment 2021-03-05 12:10:01 +11:00
Paul Riseborough 4675aea0bf AP_NavEKF3: Update parameter description 2021-03-05 12:10:01 +11:00
Paul Riseborough 246b80dc06 AP_NavEKF3: Don't return a 0,0 lat,lng unless absolutely necessary 2021-03-05 12:10:01 +11:00
Paul Riseborough a50be71b41 AP_NavEKF3: Fix inconsistency in reporting raw GPS position validity 2021-03-05 12:10:01 +11:00
Paul Riseborough 6a603019d5 AP_NavEKF3: Add accessor function for yaw alignment status 2021-03-05 12:10:01 +11:00
Paul Riseborough 3634a942a1 AP_NavEKF3: Fix bug causing unwanted use of default airspeed 2021-03-05 12:10:01 +11:00
Paul Riseborough 912db3512e AP_NavEKF3: Fix EKF selection bug 2021-03-05 12:10:01 +11:00
Paul Riseborough 85c0040fa3 AP_NavEKF3: Improve inFlight detection for fixed wing
Better handle case where no airspeed sensor is being used with a hand launch.
2021-03-05 12:10:01 +11:00
Paul Riseborough 295c1e1308 AP_NavEKF3: Reset covariances when falling back to non aiding mode 2021-03-05 12:10:01 +11:00
Paul Riseborough adae7365e1 AP_NavEKF3: Fix LLH reporting bug 2021-03-05 12:10:01 +11:00
Paul Riseborough ddadc45854 AP_NavEKF3: Reset states when falling back into non aiding mode. 2021-03-05 12:10:01 +11:00
Paul Riseborough b92eca7b66 AP_NavEKF3: Select startup aiding mode after landing without yaw sensor 2021-03-05 12:10:01 +11:00
Paul Riseborough 9a5a264aa0 AP_NavEKF3: Improve wind estimation without airspeed sensing
Enable default airspeed variance to be specified externally
Improve control of sideslip fusion
Don't modify non wind states unnecessarily when using synthesised air data measurements
2021-03-05 12:10:01 +11:00
murata ae565aef1a AP_NavEKF3: Change misnomer (NFC) 2021-03-03 20:49:55 +11:00
Iampete1 8173cd442a AP_NavEKF3 log position offset as float in meters 2021-03-02 10:03:12 +11:00
Paul Riseborough 0971ef55a4 AP_NavEKF3: Fix bug preventing EKFGSF running when needed for reset
The gpsAccuracyGood flag should not be used because it will go false if GPS innovations become high due to bad yaw which is when the EKFGSF is required. to keep running.
2021-03-02 07:28:24 +11:00
Peter Barker 65b780fad5 AP_NavEKF3: move GSF logging from EKF files into GSF files 2021-02-23 12:36:33 +11:00
Patrick José Pereira 3b0dac3198 AP_NavEKF3: Simplify boolean expression
Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
2021-02-23 10:30:05 +11:00
Paul Riseborough 342e6b35fa AP_NavEKF3: Fix use of incorrect ballistic coefficient parameter 2021-02-22 13:21:03 +09:00
Paul Riseborough 7370cd9f00 AP_NavEKF3: Fix typo and update parameter description 2021-02-22 13:21:03 +09:00
Randy Mackay 1814524fcb AP_NavEKF3: fix using_external_yaw when using external nav
the first if would always return true meaning the method would always return false when using external nav
thanks to David Sastre for finding this!
2021-02-09 11:35:39 +11:00
Randy Mackay f6cb0819ef AP_NavEKF3: pre_arm_check may skip position checks 2021-01-26 10:46:49 +11:00
Andrew Tridgell ddc4d08e8f AP_NavEKF3: make drag fusion optional 2021-01-24 11:56:52 +11:00
Andrew Tridgell 1ccda938cb AP_NavEKF3: make external navigation optional 2021-01-24 11:56:52 +11:00
Andrew Tridgell 8da511f039 AP_NavEKF3: make body odomotry build depend on vehicle type
saves about 11k of flash
2021-01-24 11:56:52 +11:00
Peter Barker 3582b69e9c AP_NavEKF3: remove getFilterTimeouts access methods 2021-01-22 13:04:36 +11:00
Peter Barker b3bacdd33c AP_NavEKF3: remove internal-only getTiltError method 2021-01-21 21:26:17 +11:00
Peter Barker 5ad0ac6727 AP_NavEKF3: remove unused getAccelNED method 2021-01-21 21:26:17 +11:00
Randy Mackay 4492e5024c AP_NavEKF3: enable ekf3 by default 2021-01-16 08:24:23 +09:00
Andy Piper 099d6aa72e AP_NavEKF3: add filter reset when vehicle is idle for too long 2021-01-15 15:47:20 +11:00
chobitsfan 26a3a02c81 AP_NavEKF3: use posxy_source in all places 2021-01-14 16:49:12 +09:00
chobits e73d28fe19 AP_NavEKF3: remove redundant extnav code 2021-01-12 21:56:36 +09:00
Andrew Tridgell 59fa794818 AP_NavEKF3: constrain vertical error
this prevents a floating point exception with external AHRS
2021-01-05 21:13:12 +11:00
Randy Mackay 647c568ab9 AP_NavEKF3: out-of-date comments removed
GPS, Static and ExtNav yaw buffers have been separated
2021-01-05 10:58:06 +11:00
Randy Mackay 27c998ad94 AP_NavEKF3: rename source and yawFusionMethod from EXTERNAL to GPS 2021-01-05 10:58:06 +11:00
Randy Mackay c14b4a8b6c AP_NavEKF3: add support for GSF as yaw source 2021-01-05 10:58:06 +11:00
Randy Mackay 5891c6ace8 AP_NavEKF3: separate GPS yaw from ExtNav yaw 2021-01-05 10:58:06 +11:00
Randy Mackay 6850b48ea1 AP_NavEKF3: add comments for yaw sensor variables 2021-01-05 10:58:06 +11:00
Andrew Tridgell e747edbcf2 AP_NavEKF3: fixed init order for scheduler
this can impact replay of EKF2
2021-01-02 10:30:26 +11:00
Randy Mackay 39be85cef4 AP_NavEKF3: integrate Source loss of init 2020-12-21 16:56:38 +09:00
Peter Barker 6dfd2f2ff9 AP_NavEKF3: use use_EKFGSFYaw to reduce code duplication 2020-12-17 16:00:29 +09:00
Randy Mackay a2cca60beb AP_NavEKF3: constify EKFGSF_getYaw 2020-12-16 19:18:29 +11:00
Randy Mackay 0c3fcfd9d6 AP_NavEKF3: add missing break to case statements 2020-12-16 19:18:29 +11:00
Randy Mackay 9cb529cfd6 AP_NavEKF3: minor formatting fix
also replace tab with spaces
2020-12-16 19:18:29 +11:00
Paul Riseborough 908b348c31 AP_NavEKF3: Fix typo in enum label 2020-12-15 19:29:16 +11:00
Randy Mackay e819dbdd64 AP_NavEKF3: add EKFGSF_getYaw to reduce duplicate code 2020-12-15 19:29:16 +11:00
Randy Mackay c2edae905f AP_NavEKF3: simplify logic when updating yawAngDataStatic 2020-12-15 19:29:16 +11:00
Randy Mackay 843ddb4fdc AP_NavEKF3: minor format fix 2020-12-15 19:29:16 +11:00
Paul Riseborough a9e76d44af AP_NavEKF3: Clean up yaw fusion logic 2020-12-15 19:29:16 +11:00
Paul Riseborough ccfd89240b AP_NavEKF3: Fix error in calculation of static 312 order yaw reference 2020-12-15 19:29:16 +11:00
Dr.-Ing. Amilcar do Carmo Lucas f77ab30f7a Revert "AP_NavEKF3: Remove unsupported parameter unit descriptions"
This reverts commit 4f4a2f446d.
Makes more sense to add those to the list of supported units
2020-12-15 18:22:36 +11:00
Paul Riseborough 8f32430294 AP_NavEKF3: Fix white spaces 2020-12-11 15:21:41 +11:00
Paul Riseborough bdc202378a AP_NavEKF3: fix white space 2020-12-11 15:21:41 +11:00
Paul Riseborough ed17d8e48f AP_NavEKF3: Improve parameter display names 2020-12-11 15:21:41 +11:00