Commit Graph
77 Commits
Author SHA1 Message Date
bresch c33d79cfb4 ekf2: move gnss_checks to their own class 2025-06-03 09:22:32 +02:00
bresch a40377e544 ekf2: always update IMU filters 2025-05-02 10:23:43 +02:00
bresch 480c232bfd ekf2: use alpha filter class 2024-12-18 14:11:08 +01:00
bresch aeb182a8ed ekf2: rename resetAltitudeTo to initialiseAltitudeTo
This is to better show that the altitude is also used to set the origin.
2024-11-22 15:13:47 +01:00
bresch 814a2706f5 ekf2: compensate for coriolis and transport rate accelerations 2024-11-22 15:13:47 +01:00
bresch 842212df6c ekf2: update earth rate also without GNSS 2024-11-22 15:13:47 +01:00
bresch b19a6ee3b5 ekf2: store position state as lat/lon/alt
The position error state is still defined in a body-fixed NED frame but the
position state itself is latitude-longitude-altitude.
2024-11-22 15:13:47 +01:00
Marco HauswirthandMathieu Bresciani 0c451552c7 EKF2: add validity flags to global pos message (#23787)
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
2024-10-23 10:19:04 +02:00
Daniel Agar 9238656a33 ekf2: configurable velocity state constraint (EKF2_VEL_LIM)
- replace hardcoded 1000 m/s velocity state constraint with new EKF2_VEL_LIM parameter (default 100 m/s)
 - new velocity limit also used for GPS checks and external vision velocity
2024-10-07 10:35:48 +02:00
Marco Hauswirth 7dcea6b2e4 EKF2: range measurement rejection in rain/fog (#23579) 2024-08-30 17:25:56 +02:00
Daniel AgarandMathieu Bresciani 5b0014cb06 ekf2: remove legacy accel z bias checks (#23341)
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
2024-08-29 11:51:27 -04:00
bresch bab256bfe6 ekf2: handle external altitude resets 2024-08-28 11:02:26 +02:00
bresch 130fefb1e7 ekf2: initialize origin from corrent position when possible 2024-08-28 11:02:26 +02:00
Marco Hauswirth 5808dac4bc reset position-mode line following after position reset 2024-07-26 18:01:44 +02:00
Daniel Agar fd72578e98 ekf2: avoid constraining parameters every iteration 2024-07-25 09:51:35 +02:00
Daniel Agar c5c27a87f1 ekf2: track last terrain fuse time and update logic 2024-07-17 10:16:32 -04:00
Daniel Agar f93dc61770 ekf2: use bias corrected angular velocity
- avoid unnecessarily storing _ang_rate_delayed_raw
2024-07-10 21:20:47 -04:00
Daniel Agar c29b4ff87e ekf2: apply astyle formatting and enforce 2024-07-10 10:43:55 -04:00
Marco Hauswirth 4bc0286eb8 fix error from refactring commit, fix reset on ground (#23370) 2024-07-08 13:55:05 +02:00
Marco HauswirthandDaniel Agar e04c53241a EKF2: reset position by fusion (#23279)
* reset position by fusion

* handle local_pos_valid for fixed wing in gnss denied

* [WIP] ekf2: setEkfGlobalOrigin respect current height reference and vertical position aiding

* global origin, also reset vertical pos without gps as ref

* fix wo gnss, that bitcraze ci passes

* revert some changes as requested

* remove duplicate reset messages

* undo unrelated whitespace changes, I'll fix it everywhere in a followup

* [SQUASH] ekf2: add vehicle_command_ack

* resetGlobalPosToExternalObservation consolidate logic

* remove gnss check from local_pos validation check

* reset when  0<accuracy<1, otherwise fuse

* replace gps param with flag

* ekf2: dead reckon time exceeded, respect ZUPT preflight if air data or optical flow expected

* subtract timeout from last inertial dead-reck, change fake pos conditions, save flash

---------

Co-authored-by: Daniel Agar <daniel@agar.ca>
2024-07-07 22:43:55 +02:00
Daniel Agar 0742d356f5 ekf2: more conservative clipping checks for bad_acc_clipping fault status (#23337)
- track accel clipping count per axis
 - only set bad_acc_clipping fault_status if at least one axis is
   clipping continuously or if all have been clipping at warning level
 - Note: this doesn't impact the clipping projections that boost the
   accel process noise, pause bias estimation, and skip gravity fusion
   on a per sample basis
2024-06-28 16:45:08 -04:00
bresch 9001c23926 ekf2: clean up hagl vs terrain naming
Terrain is the state: terrain vertical position
Hagl (height above ground level) is the vertical distance between the
vertical position and the terrain vertical position
2024-06-26 11:05:38 +02:00
bresch 68980b59e2 ekf2: add terrain state 2024-06-26 11:05:38 +02:00
Daniel Agar 206488b844 ekf2: innovation sequence monitoring for all aid sources
- add new 'innovation_filtered' and 'test_ratio_filtered' fields to
   estimator_aid_source topics
2024-06-17 15:30:42 -04:00
bresch 51883fe5d4 ekf2: integrate mag heading into mag 3D 2024-04-05 22:26:03 -04:00
bresch b80f15f7b5 ekf2-mag_auto: always use mag 3D after takeoff 2024-02-27 12:33:43 -05:00
Daniel Agarandbresch 51155f7a29 ekf2: treat inhibited states as "consider states" and remove conservative covariance matrix fixes (#22597)
When a state stops to be estimated it becomes a "consider state". Its value and variance are frozen but its covariance with other states continue to evolve normally.

 - removes conservative accel bias variance limiting
 - force symmetry is skipped after fusion of NED vel/pos (a direct measurement)

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>
2024-01-24 14:14:09 -05:00
Daniel Agar 8a031677d5 ekf2: verbose debug print status include state variances 2024-01-11 11:53:20 -05:00
Daniel Agar 8bcba6128a ekf2: fix resetGlobalPosToExternalObservation whitespace 2024-01-11 10:59:56 -05:00
Roman Bapst 603c3f6636 added support to reset vehicle position based on external position (#22444)
via command MAV_CMD_EXTERNAL_POSITION_ESTIMATE

Signed-off-by: RomanBapst <bapstroman@gmail.com>
2024-01-11 13:09:22 +01:00
Daniel Agarandbresch d45c3d3407 ekf2: verbose (DEBUG_BUILD) print status with Matrix improved print
* ekf2: verbose print status
 * matrix/Matrix improve print output
 * bold diagonal elements, print ring buffer entry size
 * print in scientific notation when >= 10 to respect max size

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>
2024-01-10 09:46:04 -05:00
breschandDaniel Agar fe7988672f ekf2: auxiliary position fusion
Co-authored-by: Daniel Agar <daniel@agar.ca>
2023-12-01 11:50:22 +01:00
Daniel Agar e79737a38d ekf2: create simple estimator aid source base class and extract zero velocity update 2023-10-18 20:23:56 -04:00
Daniel Agar 48e09a4dea ekf2: move predict covariance IMU inhibit check to function 2023-10-17 09:58:10 -04:00
Daniel Agar d2b3e7fe16 ekf2: new kconfig to enable/disable GNSS (enabled by default) 2023-10-11 14:02:34 -04:00
Daniel Agar 5352a64042 ekf2: symforce derivation allow optionally disabling mag and wind states 2023-10-10 17:31:11 -04:00
Daniel Agar 28d58a947f ekf2: cleanup more optional mag (CONFIG_EKF2_MAGNETOMETER) 2023-10-09 10:18:02 -04:00
Daniel Agar 028733e1c7 ekf2: add kconfig to disable wind estimation (off by default) 2023-10-09 09:22:29 -04:00
Daniel Agar 153f7bbced ekf2: update all copyright headers 2023-09-15 10:02:09 -04:00
Daniel Agar 1e9f0ad2c6 ekf2: add kconfig for barometer support (enabled by default) 2023-09-15 10:02:09 -04:00
Daniel Agar 10b54d08fc ekf2: add dedicated EKF2_CONFIG_TERRAIN in kconfig
- new estimator_aid_src_terrain_range_finder for HAGL RNG
2023-09-15 10:02:09 -04:00
Daniel Agar 845b01a00d ekf2: add kconfig for magnetometer support (enabled by default) 2023-09-15 10:02:09 -04:00
Daniel Agarandbresch d75bb62a65 ekf2: separate mag and mag heading control logic (#21212)
- split mag_3d into new standalone mag fusion and mag fusion allowed to update all states (full mag_3d)
 - new dedicated control logic for mag/mag_3d fusion and standalone mag heading fusion
 - if WMM available use for mag_I and mag_B init
 - mag states reset if external yaw reset (yaw estimator, GPS yaw, etc)
 - mag reset if declination changed (eliminate _mag_yaw_reset_req)
 - mag fusion (but not mag_hdg or mag_3d) can be active during gps_yaw or ev_yaw (if yaw aligned north)

Co-authored-by: bresch <brescianimathieu@gmail.com>
2023-08-04 10:39:16 -04:00
Daniel Agarandbresch 84b6b472b4 ekf2: change delta angle and delta velocity bias states to accel and gyro bias (#21901)
* ekf2-test: remove outdated codegen comparison

The definition of states changed so the comparison with the old
derivation cannot work anymore.

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>
2023-07-28 09:31:44 -04:00
bresch ed7c4bc5d7 ekf2: init tilt using quaternion from gravity vector 2023-03-31 10:15:12 +02:00
Daniel Agar 32a5bd32ad ekf2: add kconfig option to enable/disable range finder fusion (and terrain estimator) 2023-03-21 11:25:34 -04:00
Daniel Agar 0784901a66 ekf2: add kconfig option to enable/disable optical flow fusion 2023-03-21 11:25:34 -04:00
Daniel Agar 4363b09421 ekf2: add kconfig option to enable/disable external vision fusion 2023-03-20 10:12:17 -04:00
Daniel Agar d47f96f1a5 ekf2: add kconfig option to enable/disable AUX velocity fusion 2023-03-20 10:12:17 -04:00
Daniel Agar 4270a303ab ekf2: add kconfig option to enable/disable airspeed and sideslip fusion 2023-03-20 10:12:17 -04:00