Commit Graph
184 Commits
Author SHA1 Message Date
bresch c59101e8ed ekf2-gravity: do not estimate accel bias when gravity fusion is active
Gravity fusion uses the bias corrected accelerometer data to correct the
tilt estimate. We should not continue to estimate the accel bias when
this is active as it creates an unwanted feedback loop.
2025-05-02 10:23:43 +02:00
bresch 2aaa54037c ekf2: split gnss pos/vel flags
They can be selected independently in the control parameter, so there is
no reason why they should share the same flag.
2025-03-11 14:14:40 +01:00
Marco Hauswirth 4a5aa1e947 Fix max-hagl restriction to position/altitude control (#23667)
* fix max-hagl restriction to position/altitude control

* max hagl vel restriction in ManAcc position mode

* use interpolate func, change naming

* simplyfied vertical vel limitation

* move velocity-constraint adjustment to StickAccelXY
2025-01-20 15:50:21 +01:00
bresch b1773df441 ekf2: fix comments about primary height sources
Having the xxx_hgt flag set only means that the sensor is fused for
altitude/height aiding. It's not necessarily the height reference.
2025-01-07 10:23:13 +01:00
bresch fad9ae855d EKF2: reset aid src when resetting state to measurement
Filtered innovations and test ratios can be large before the reset and
would trigger pre-flight warnings
2024-12-13 14:26:57 +01:00
bresch 14fe6c2167 ekf2: resetHeightTo -> resetAltitudeTo
The vertical position state is now an altitude, not just a local height
2024-11-22 15:13:47 +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 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
bresch 5bfa6b3359 ekf2: remove redundant ned_origin_initialised flag 2024-10-01 10:37:20 -04:00
bresch 35efe44a6b ekf2: fusion always succeeds, do not return boolean 2024-09-24 10:48:14 -04:00
Marco Hauswirth 8afd267509 move more ekf function implementations to cpp 2024-09-23 12:18:41 -04:00
Marco Hauswirth e3f138862a move implementation of updateAidSrcStatus to cpp file, saves flash 2024-09-23 12:18:41 -04:00
bresch 15ddd94349 ekf2: move implementation to ekf_helper to reduce flash usage
saves about 4.8kB of flash on FMUv5
2024-09-23 10:27:42 -04:00
bresch cd2170deea ekf2-origin: backcompute based on lpos validity 2024-08-28 11:02:26 +02:00
bresch af752536b9 ekf2: extract setting origin from current lat/lon/alt
This is not only needed when GNSS is available but also for other global
sources of position (e.g.: aux global pos and manual pos updates)
2024-08-28 11:02:26 +02:00
bresch 9169a7c5fc ekf2: split horizontal and vertical origin reset
Allow partial resets (only lat/lon or only altitude)
2024-08-28 11:02:26 +02:00
Daniel Agar bbcf741e9e ekf2: make mag control responsible for WMM
- this further untangles mag control (which requires the WMM) from GPS
2024-08-27 16:16:55 +02:00
Daniel Agar ebbb880e92 ekf2: always use corrected accel/gyro for filtered metrics 2024-08-23 17:35:59 -04:00
bresch 1a0f97ebbd ekf2-fake pos: add valid fake position fusion
This is similar to fake pos but is only used when the ekf has an
external information telling it that the vehicle is not changing
position. This information can then be used to keep a valid local
position even when the vehicle isn't exactly at rest.
2024-08-23 11:17:21 +02:00
bresch 0931179579 ekf2: extract WMM update logic 2024-08-20 10:32:27 -04:00
Roman BapstandMarco Hauswirth 1b9f1b78e5 Added support for resetting wind states to external observation (#23277)
* added support for resetting wind states to external observation

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* moved wind related functions into separate file

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* added VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* correctly compute variances

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* ekf2: implement wind reset

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* only allow VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE during wind dead-reckoning
and increase horizontal velocity variance to allow velocity states to move
towards solution that is aligned with the newly set wind

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* only reset wind on ground

* still use wind reset using airspeed when it wasn't initialized

* exclude func for rover, change reset interface

* handle wind reset in drag-fusion

* replace state reset with variance reset in sideslip/drag fusion

* remove resetWind function

---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Marco Hauswirth <marco.hauswirth@auterion.com>
2024-07-19 14:33:08 +02:00
Daniel Agar eac14b7db2 ekf2/commander: simplify navigation filter preflight checks
- remove commander test ratio "tuning knobs" (COM_ARM_EKF_{HGT,POS,VEL,YAW})
   - these are effectively redundant with the actual tuning (noise & gate)
     in the estimator, plus most users have no idea why they'd be
     adjusting these other than to silence an annoying preflight complaint
 - remove ekf2 "PreFlightChecker" with hard coded innovation limits
 - ekf2 preflight innovation flags are now simply if any active source
   exceeds half the limit preflight
2024-07-18 16:39:18 +02:00
Daniel Agar 48f1687d3a ekf2: cleanup legacy EKF solution_status_flags 2024-07-16 10:11:27 +02:00
Daniel AgarandMathieu Bresciani 1cd7d54170 ekf2: consolidate GNSS yaw in gnss_yaw_control.cpp and fix naming (GPS->GNSS)
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
2024-07-15 12:50:51 -04:00
Daniel Agar c29b4ff87e ekf2: apply astyle formatting and enforce 2024-07-10 10:43:55 -04:00
Daniel Agar 3e3b886b5d ekf2: add terrain estimator_status_flags 2024-07-09 10:10:01 -04:00
Daniel Agar 64a6971bdb ekf2: only limit opt flow HAGL if range only terrain
- increase HALG limit from 75%->90% of sensor max
2024-07-09 10:10:01 -04: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
bresch 68980b59e2 ekf2: add terrain state 2024-06-26 11:05:38 +02:00
Daniel Agar dcb1103299 ekf2: move estimator_status test ratios to filtered values 2024-06-20 13:41:54 -04:00
Daniel Agar 157f7cf40b simplify world_magnetic_model interface (degrees in, degrees out)
- this hopefully helps avoid accidental mis-use
 - try to clarify units everywhere
2024-06-04 09:14:36 -04:00
Daniel Agar 224d6f2fa7 ekf2: ekf_helper.cpp remove duplicate method comments (comment on declaration only, not definition) 2024-05-02 17:40:58 -04:00
Daniel Agar c1fc893cca ekf2: move gyro/accel/wind covariance reset helpers to covariance.cpp 2024-05-02 17:40:58 -04:00
Daniel Agar 63c2ea33c1 ekf2: move Ekf::resetQuatStateYaw() to yaw_fusion.cpp 2024-05-02 17:40:58 -04:00
Daniel Agar 1ca4056b6a ekf2: delete unused Ekf::resetImuBias() 2024-05-02 17:40:58 -04:00
Daniel Agar 6b3b66619b ekf2: move baro dynamic pressure compensation to aid_sources/barometer 2024-05-02 17:40:58 -04:00
bresch 51883fe5d4 ekf2: integrate mag heading into mag 3D 2024-04-05 22:26:03 -04:00
bresch 12fefbcfbd ekf2: use global definition of quaternion error 2024-04-05 22:26:03 -04:00
bresch 6d819343aa ekf2: fix direct state measurement update for suboptimal K case
The duration of a unit test had to be increased because the incorrect
covariance matrix update, was making the unit test passing faster
(over-optimistic variance).
2024-03-20 14:13:49 +01:00
bresch 421f13e4b5 ekf2: fix joseph covariance update for Schmidt-Kalman filter
If part of the Kalman gain is zeroed, the first step of the joseph
update does not produce a symmetrical matrix.
2024-03-07 15:11:47 +01:00
bresch d501d8e1d4 ekf2: use Joseph stabilized update in direct state observations 2024-02-26 12:32:59 -05:00
Daniel Agar 8243b4f474 ekf2: move vel/pos reset helpers 2024-02-20 13:16:24 -05:00
Daniel Agar c4c41c49e5 ekf2: move fuseVelPosHeight() -> fuseDirectStateMeasurement()
- don't bother keeping bad_vel_{N,E,D} and bad_pos_{N,E,D} fault status bits
2024-02-20 13:16:24 -05:00
bresch c221da27a7 ekf2: set attitude validity flag using centralized function 2024-02-20 11:33:30 -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
Mathieu Brescianiandbresch cb396a6339 ekf2: fix computation of tilt and yaw variances add them to logging
Co-authored-by: bresch <bresch@users.noreply.github.com>
2024-01-24 12:14:15 -05:00
Daniel Agar bf7da6430d ekf2: consolidate LPOS & GPOS accuracy methods 2024-01-22 12:34:08 -05:00
bresch c28972d15e ekf2-grav: only use filtered accel norm to start/stop the fusion
Using the raw data makes the swith too sensitive to noise
2024-01-18 20:39:16 -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