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
Daniel Agar
5fa3b9d86a
lib/world_magnetic_model: fetch_noaa_table.py refactor and scaling improvements
...
- upate to NOAA grid API to build WMM table in one pass
- refactor declination/inclination/totalintensity table printing to
shared method
- compute scaling factor to maximize resolution
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
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
Daniel Agar
c3fb0b1090
ekf2: move auxvel file to aid_sources/auxvel
2024-05-02 17:40:58 -04:00
Daniel Agar
b5d1e87368
ekf2: move EV files to aid_sources/external_vision
2024-05-02 17:40:58 -04:00
Daniel Agar
b6da0b141d
ekf2: GPS yaw only invalidate yaw_align if stopping due to fusion failure
2024-04-29 21:04:21 -04:00
bresch
9dc7719d4a
ekf2: Only reset to GNSS heading if necessary
...
When North-East (e.g.: GNSS pos/vel) aiding is active, the heading
estimate is constrained and consistent with the vel/pos aiding. Reset to
GNSS heading should only occur if no N-E aiding is active or if the
filter is not yes aligned. Otherwise, just wait for the consistency
check to pass again (will pass at some point if the heading uncertainty
of the filter is getting too high).
2024-04-29 07:22:01 -07:00
bresch
8bf1cf0b15
ekf2_params: reduce "short" description
2024-04-24 15:09:57 -04:00
bresch
4cf51e5833
baro comp: set hpf optional
2024-04-22 15:59:50 +02:00
Daniel Agar
ec3ceae45e
ekf2: move yaw_estimator and derivation to dedicated folder
2024-04-18 15:14:59 -04:00
bresch
a8a67fbf8f
ekf2: set horizon using specific parameter
...
Some sensors can have their delay included in the timestamp. In this
case, the buffer cannot be sized using the _DELAY parameter.
2024-04-17 09:16:16 -04:00
