Silvan Fuhrer
1ae96d6509
EKF2: fix builds without CONFIG_EKF2_RANGE_FINDER
...
Signed-off-by: Silvan Fuhrer <silvan@auterion.com >
2024-06-26 11:05:38 +02:00
bresch
a50ef2eb5e
ekf2-terrain: make terrain validity based on uncertainty
...
When using optical flow for terrain aiding, we cannot assume that the
terrain estimate is valid if flow is fused as is can only be observed
during motion. When no direct observation is available, the terrain is
assumed to be valid if its 1sigma uncertainty is < 10% of the hagl.
2024-06-26 11:05:38 +02:00
bresch
a665764b0e
ekf2: remove unused EKF2_TERR_MASK
2024-06-26 11:05:38 +02:00
bresch
7903ddf5df
ekf2-terrain: terrain is not a separate estimator
2024-06-26 11:05:38 +02: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
30ce560e3a
ekf2: mag control reset filtered test ratio on start (if aligning yaw)
2024-06-20 13:41:54 -04:00
Daniel Agar
dcb1103299
ekf2: move estimator_status test ratios to filtered values
2024-06-20 13:41:54 -04: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
Roman Bapst
0ce64e1b92
ekf2: don't fuse optical flow samples when the current distance to the ground is larger than the reported maximum flow sensor distance
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2024-06-11 21:20:40 -04:00
Daniel Agar
fb659ae200
ekf2: stopMagFusion() reset yaw_align if mag heading was active
...
- we also need to clear mag_aligned_in_flight
2024-06-06 10:08:20 -04:00
Daniel Agar
bfe3c86aeb
ekf2: merge mag yaw angle observability into heading consistency
...
- the additional hyteresis logic for "yaw angle observable" was
needed when it controlled time dependent mag_3d
2024-06-06 10:07:31 -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
bresch
0a665a526c
ekf2: add mag type init
...
In this mode, the mag is used to inilialize the heading.
During flight, the heading is predicted using gyros and corrected
by GNSS measurements if available.
2024-06-03 13:28:12 +02:00
bresch
53210dd8f3
ekf2-mag: with NE aiding, constrain heading drift only before takeoff
...
After takeoff, the heading is easily observable
2024-05-31 10:38:17 -04:00
bresch
3dac9af09e
ekf2-mag: do not reset on WMM change when NE aiding is active
...
The mag field states are observed. No need to reset them.
2024-05-31 10:38:17 -04:00
bresch
ee765e7859
ekf2-mag: do not reset when NE aiding is active
2024-05-31 10:38:17 -04:00
bresch
6515935b52
ekf2-mag: do not limit the earth mag field estimate
...
The EKF can recover from an initial bad earth mag field estimate.
Constraining the field is not necessary and can lead to an unpredicted
behavior of the filter.
Declination fusion is safe to run even when the horizontal field is 0
2024-05-31 10:38:17 -04:00
bresch
774b6ed3b8
ekf2-mag: do not use yaw emergency estimator to reset mag states
...
On slowly moving vehicles (e.g.: boats, rovers), the yaw estimator has
worse convergence than the main EKF. Resetting the mag states using the
yaw estimator as reference can lead to poor heading. Also, the EKF can
recover really well from initially incorrect mag states.
2024-05-31 10:38:17 -04:00
bresch
c3d984703c
ekf2-mag: remove immediate declination fusion after reset
2024-05-31 10:38:17 -04:00
bresch
a6007e4b93
ekf2-mag: turn around update_all_states condition
...
Non-functional
2024-05-31 10:38:17 -04:00
bresch
c11c75d32e
ekf2-mag: always add process noise until initial value
2024-05-31 10:38:17 -04:00
bresch
993782cffa
ekf2: only trigger position timeout reset when hpos fusion is active
2024-05-29 20:49:14 -04:00
Daniel Agar and bresch
d359f6236e
ekf2: symforce zero more efficiently ( #23133 )
...
- increase symforce CppConfig zero_initialization_sparsity_threshold so
that a Matrix setZero() call is performed instead of individually zeroing
Co-authored-by: bresch <brescianimathieu@gmail.com >
2024-05-17 11:20:30 +02:00
bresch
ea39032b45
mag_ctrl: combine common conditions for mag_hdg and mag_3d
2024-05-17 11:19:04 +02:00
bresch
d796009302
mag_ctrl: do not fuse synthetic mag but do not zero the innovation
2024-05-17 11:19:04 +02:00
Daniel Agar
bb5dfc7d51
integrationtests: mavros/mission_test.py bump yaw_error_std threshold (heading init is delayed, but not wrong)
2024-05-17 11:19:04 +02:00
Daniel Agar
5173830718
ekf2: mag fusion don't update all states or tilt by default
...
- cleanup some of the legacy mag flags
2024-05-17 11:19:04 +02:00
Daniel Agar
bfc39cf341
ekf2: mag control always populate estimator aid src
2024-05-17 11:19:04 +02:00
Daniel Agar
95ae5a657d
ekf2: merge mag_3d_control + mag_control
2024-05-17 11:19:04 +02: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
Daniel Agar
4f0eb72fc9
ekf2: move IMU down sampler to imu_down_sampler/
2024-05-02 17:40:58 -04:00
Daniel Agar
58637d3825
ekf2: move terrain estimator and derivation to terrain_estimator/
2024-05-02 17:40:58 -04:00
Daniel Agar
58de8cbb77
ekf2: move fake_height, fake_pos, zero_innovation_heading to aid_sources/
2024-05-02 17:40:58 -04:00
Daniel Agar
49c782bad9
ekf2: move bias estimators to bias_estimtor/
2024-05-02 17:40:58 -04:00
Daniel Agar
e262fde4dc
ekf2: move aux global position fusion to aid_sources/aux_global_position
2024-05-02 17:40:58 -04:00
Daniel Agar
b8d46e60a5
ekf2: move mag fusion to aid_sources/magnetometer
2024-05-02 17:40:58 -04:00
Daniel Agar
3f6c3e0649
ekf2: move output predictor to output_predictor/
2024-05-02 17:40:58 -04:00
Daniel Agar
24fdd696cb
ekf2: move range finder files to aid_sources/range_finder
2024-05-02 17:40:58 -04:00
Daniel Agar
3dbd3f8a1a
ekf2: move airspeed fusion file to aid_sources/airspeed
2024-05-02 17:40:58 -04:00
Daniel Agar
789b2b3d8a
ekf2: move sideslip fusion file to aid_sources/sideslip
2024-05-02 17:40:58 -04:00
Daniel Agar
eb8ee74066
ekf2: move baro height file to aid_sources/barometer
2024-05-02 17:40:58 -04:00
Daniel Agar
de178b1435
ekf2: move gravity fusion file to aid_sources/gravity
2024-05-02 17:40:58 -04:00
Daniel Agar
78f2ccbb60
ekf2: move optical flow files to aid_sources/optical_flow
2024-05-02 17:40:58 -04:00
Daniel Agar
fcf94e7670
ekf2: move GNSS files to aid_sources/gnss
2024-05-02 17:40:58 -04:00
Daniel Agar
31ae5b77fe
ekf2: move drag_fusion file to aid_sources/drag
2024-05-02 17:40:58 -04:00
Silvan Fuhrer