bresch
cd77d52dc1
ekf2: calcRotVecVariances using SymForce
2023-08-07 14:28:06 -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
bresch
30bfe0d379
ekf2: remove artificial constraint on position uncertainty growth
...
There is no reason to contrsain the position uncertainty, estpecially
when flying with velocitiy aiding only.
Note that all the variances are already contrained to sane values at the
end of the covariance prediction
2023-07-24 15:10:07 -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
Daniel Agar and bresch
01bcc47fb1
ekf2: apply yaw reset to vision attitude error filter
...
- set vision attitude error filter uninitialized if vision data stops
- ev error filter is only compiled when ev config is selected
Co-authored-by: bresch <brescianimathieu@gmail.com >
2023-07-24 10:32:33 -04:00
bresch
a95c11d48a
ekf2-test: add test for inclination of mag earth field
2023-07-24 10:16:37 -04:00
bresch
72be724b86
ekf2: log mag inclination and strength for tuning
2023-07-24 10:16:37 -04:00
bresch
357bf024f6
ekf2-mag: add inclination check
2023-07-24 10:16:37 -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
bresch
672b29d555
ekf2: report dist bottom also when using flow for terrain
...
Terrain height can be estimated using a range finder and/or optical flow
2023-07-11 08:18:32 -07:00
alexklimaj
03528a6200
Add EKF2_OF_QMIN_GND to handle 0 optical flow quality when on ground
2023-07-11 08:18:06 -07:00
alexklimaj
bca0be47bc
Apply code review changes from @dagar
2023-07-11 08:18:06 -07:00
Eric Katzfey
9511248514
Removed the huge HAGL fuse timeout increase
2023-07-11 08:18:06 -07:00
Eric Katzfey
f5d9ac4526
Cleaned up some comments and debug code
2023-07-11 08:18:06 -07:00
mjturi-mai
56c794108d
actual fixes for velocity estimate errors and bad rng fusion
2023-07-11 08:18:06 -07: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
bresch
eb70c9f6f3
ekf2: update auto-generated code to SymForce 0.9.0
2023-06-26 14:10:54 -04:00
bresch
fdf7867a3e
ekf2: update derivation to SymForce 0.9.0
2023-06-26 14:10:54 -04:00
Daniel Agar
d9ae2ed7ac
ekf2: regenerate yaw estimator symforce
...
- remove underscore uppercase variable naming that can be problematic
2023-06-19 21:00:25 -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
bresch
9d7abf2552
ekf2: symforce - use builtin Rot3 and Quaternion operations
2023-06-05 11:58:42 -04:00
Daniel Agar
6535cc758e
ekf2: fuse mag update last heading fuse time if updating all states
...
- handle synthetic z special case
2023-05-26 08:48:08 -06:00
bresch
521abecbbf
ekf2: remove all remaining uses of EKF2_AID_MASK
2023-04-28 22:31:17 -04:00
bresch
2a2e43b620
ekf2: move flow fusion activation from AID_MASK to OF_CTRL
2023-04-28 22:31:17 -04:00
bresch
39c2f95669
ekf2: move drag fusion activation from AID_MASK to DRAG_CTRL
2023-04-28 22:31:17 -04:00
bresch
17806a9608
ekf2: robustify wind init using airspeed
...
Tests showed that sideslip fusion often starts just before airspeed
fusion, resetting the wind states to 0 instead of using the airspeed
data. A quick look at the wind uncertainty allows to know if the current
wind estimate is meaningful or if we should rather reset it using the
airspeed data.
2023-04-08 14:12:21 +02:00
bresch
fb3b4a3151
ekf2: remove duplicate buffer push of system flags
2023-04-04 15:56:09 -04:00
bresch
ed7c4bc5d7
ekf2: init tilt using quaternion from gravity vector
2023-03-31 10:15:12 +02:00
bresch
3e3307c0d0
ekf2: fix mag declination innovation angle wrapping
2023-03-21 21:07:28 -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
8b2205810b
ekf2: add kconfig option to enable/disable baro compensation
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
Daniel Agar
19a2b4ec31
ekf2: GNSS yaw only reset test ratio for new samples
...
- otherwise external test ratio monitoring could trigger a false positive
2023-03-14 21:49:24 -04:00
bresch
fd33e60f78
ekf: fix GNSS yaw fusion wrapping
2023-03-13 10:46:34 +01:00
Daniel Agar
b3cc945a5a
ekf2: merge runOnGroundYawReset() + runInAirYawReset() into unified magReset()
2023-03-09 09:08:27 -05:00
Daniel Agar
c1f244a6fd
ekf2: decrease EKF2_MAG_YAWLIM default 0.25 -> 0.2 rad/s ( #21264 )
2023-03-09 09:07:54 -05:00
Daniel Agar
4dda5a97d8
ekf2: mag_3d check mag bias variance before allowed to update all states (orientation)
2023-03-08 15:12:48 -05:00
Daniel Agar
d45aeae1de
ekf2: add and share centralized method to clear inhibited state Kalman gains
2023-03-07 13:27:57 -05:00
Daniel Agar
3fac85369e
ekf2: gps control use adjusted velocity and position for reset
2023-03-06 09:03:39 -05:00
bresch