Mathieu Bresciani and bresch
0d6c2c8ce9
EKF2: Error-State Kalman Filter ( #22262 )
...
* ekf derivation: change to error state formulation
* ekf2: update auto-generated code for error-state
* ekf2: adjust ekf2 code for error state formulation
* ekf2_tests: adjust unit tests for error-state EKF
* update change indicator for error-state EKF
* ekf2_derivation: allow disabling mag and wind states
---------
Co-authored-by: bresch <[brescianimathieu@gmail.com ](mailto:brescianimathieu@gmail.com )>
2023-10-31 10:02:18 -04:00
Daniel Agar
27f9b1b65a
ekf2: move zero gyro update to aid source class
2023-10-18 20:23:56 -04: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
408c30de13
ekf2: delete redundant aid src status getters
2023-10-18 15:21:51 -04:00
Daniel Agar
bdaf0acfca
ekf2: fully disable yaw estimator EKFGSF_yaw with CONFIG_EKF2_GNSS ( #22233 )
2023-10-18 13:30:17 -04:00
Daniel Agar
3d238b0275
ekf2: add kconfig to disable gravity fusion ( #22231 )
2023-10-18 10:50:51 -04:00
Daniel Agar
48e09a4dea
ekf2: move predict covariance IMU inhibit check to function
2023-10-17 09:58:10 -04:00
Daniel Agar
0b44852094
ekf2: move accel bias check out of fixCovarianceErrors
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
bresch
fc32820e19
ekf2: initialize wind covariance using symforce
2023-10-09 21:03:38 -04:00
Daniel Agar
5f87f3a046
ekf2: drag fusion add aid source status topic
2023-10-09 09:26:28 -04:00
Daniel Agar
028733e1c7
ekf2: add kconfig to disable wind estimation (off by default)
2023-10-09 09:22:29 -04:00
05fd8c5976
EKF2: centralized auto-generated state ( #22183 )
...
* ekf2_derivation: use single source of state definition
The state is defined as an ordered dictionary of group elements and
everything else is generated using that state definition
* ekf2: generated state sample add const reference getter
---------
Co-authored-by: bresch <[brescianimathieu@gmail.com ](mailto:brescianimathieu@gmail.com )>
Co-authored-by: Daniel Agar <daniel@agar.ca >
2023-10-06 10:28:21 -04:00
d61743412c
ekf2: fix flow gyro bias corrections ( #22145 )
...
* ekf2-flow: fix flow gyro bias compensation
* ekf2-flow: apply flow gyro bias when used
* ekf2: log optical flow gyro bias
* ekf2: optical flow control always use provided flow gyro (with bias applied)
* ekf2-flow: log flow gyro and gyro reference
* ekf2-flow: support senrors with XY flow gyro
---------
Co-authored-by: Daniel Agar <daniel@agar.ca >
Co-authored-by: bresch <[brescianimathieu@gmail.com ](mailto:brescianimathieu@gmail.com )>
2023-10-05 10:51:30 -04:00
bresch
10db6b6eda
ekf2: remove sparse vector optimization
...
The sparse vector template requires to know which states are non-zero in
the observation jacobian. This complicates the modularity of the code
when the state vector or the derivation is changed.
The computation cost difference is almost negligible for this size.
2023-10-02 10:12:53 -04:00
Daniel Agar
14a967e2ca
ekf2: remove aid src status fusion_enabled flag
2023-09-26 10:30:16 -04:00
bresch
99197919d7
ekf2: access state covariance using helper functions
2023-09-26 06:41:04 +02:00
b3f460f30c
ekf2: update quaternion covariance at yaw reset( #22123 )
...
- Preserve tilt variance while doing a yaw reset
- Yaw variance is now correctly set instead of increased
---------
Co-authored-by: Dominique <dominique.leblanc@donecle.com >
Co-authored-by: bresch <[brescianimathieu@gmail.com ](mailto:brescianimathieu@gmail.com )>
2023-09-25 09:36:42 -04:00
bresch
514e0330e5
ekf2_terrain: handle height reset
2023-09-25 09:34:14 -04:00
bresch
9e962f3efa
ekf2: update more hardcoded indexes
2023-09-21 08:27:52 -04:00
bresch
ebf962bf68
ekf2: remove size in name of state vector and matrix types
...
Then the state vector size can be changes without having to update the
name
2023-09-21 08:27:52 -04:00
bresch
dac337efc4
ekf2: auto-generate state vector size constant
2023-09-19 09:37:50 -04:00
bresch
51dbd8ee4c
ekf2: simplify state var constraint
2023-09-19 09:37:50 -04:00
bresch
779ea3f4d1
ekf2: access state covariance using enum
2023-09-19 09:37:50 -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
bresch
0282f85cd4
ekf2: compute quat tilt variance using SymForce
2023-08-29 10:57:49 -04:00
Mathieu Bresciani
74a54b3b12
EKF2: improve resilience against incorrect mag data
...
- when GNSS is used require low mag heading innovations during
horizontal acceleration (yaw observable) to validate the mag
- only fuse mag heading just enough to constrain the yaw estimate
variance to a sane value. Leave enough uncertainty to allow for a
correction when the yaw is observable through GNSS fusion
2023-08-17 09:55:15 -04:00
bresch
39a83ab138
ekf2_test: test yaw variance after reset
2023-08-08 12:09:56 -04:00
Daniel Agar and bresch
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 Agar and bresch
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
Daniel Agar
19d6e69b0b
ekf2: allow yaw estimator to perform yaw alignment
...
This can be useful if using a full EV + GNSS setup and you start indoors, then fly outside. Once GPS is good and the only missing requirement is yaw alignment the yaw estimator reset is performed and EV yaw will automatically stop itself.
2023-07-25 10:58:45 -04:00
Daniel Agar
1ef9ee7622
ekf2: simplify fuseYaw() signature and use consistently
...
- make it safe to call for other aid sources, no EKF state is changed unless fusion_enabled
2023-07-24 10:34:00 -04:00
bresch
e4a16bfc80
ekf2-mag: rework mag strength check
2023-07-24 10:16:37 -04:00
bresch
99824c445c
ekf2: add unit test for earth mag field reset to WMM
2023-07-24 09:52:52 +02:00
Daniel Agar
c8738e3a0d
ekf2: mag control cleanup mag reset
2023-07-24 09:52:52 +02:00
bresch
1128ada90a
ekf2: use correct AlphaFilter for 3D rotations
2023-07-17 13:43:18 +02:00
Mathieu Bresciani
288e3ae74a
EKF2: vision attitude error filter ( #21791 )
...
* ekf2-ev: filter q_error for frame correction
* ekf2: filter EV attitude error centrally
2023-07-03 11:13:59 -04:00
Mathieu Bresciani
24de623989
EKF2: Zero gyro update ( #21691 )
...
When at rest, directly fuse the gyro data as an observation of its bias.
This allows to strongly observe the gyro biases without having to fuse a
constant heading that makes the ekf too confident about its heading.
2023-06-09 21:13:18 -04: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
Matthias Grob
08d6465ce5
matrix: add explicit Vector4 type
2023-03-21 15:01:26 +01: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
Daniel Agar
98ff1afc19
ekf2: add kconfig option to enable/disable GNSS yaw
2023-03-20 10:12:17 -04:00
Daniel Agar
fe0e3acf09
ekf2: add kconfig option to enable/disable drag fusion
2023-03-20 10:12:17 -04:00
