bresch
2fb7b35a8b
ekf2_terr: refactor terrain estimator - flow aiding
2022-02-11 10:57:45 -05:00
bresch
33fd1849e0
ekf2_terr: refactor terrain estimator - rng aiding
2022-02-11 10:57:45 -05:00
bresch
5818974f0f
ekf: add range finder "faulty" status
...
When delclared faulty, the range finder cannot be used anymore
2022-02-11 10:57:45 -05:00
Daniel Agar
6b1750d8be
ekf2: lower fake position observation variance when at rest
2022-02-04 21:48:32 -05:00
bresch
917910f3e2
ekf: pass a float by copy instead of a constant ref
2022-02-02 17:43:25 -05:00
bresch
3077f27821
ekf: use float instead of Vector2f for innov gate in pos/vel fusion
...
We always use the same innovation gate for X and Y anyway
2022-02-02 17:43:25 -05:00
Daniel Agar
5fb0084524
ekf2: remove unused shared fields for last velocity observation and variance
2022-02-02 17:43:25 -05:00
Daniel Agar
8a0581516c
ekf2: don't store _auxvel_sample_delayed
2022-02-02 17:43:25 -05:00
Daniel Agar
452b5e94b4
ekf2: don't store _drag_sample_delayed
2022-02-02 17:43:25 -05:00
Daniel Agar
88ffc177f7
ekf2: utils add getEulerYaw() that uses the best rotation sequence
2022-02-02 08:54:03 -05:00
Daniel Agar
c3e0b93fc8
ekf2: remove unnecessary inlines
2022-02-02 08:50:49 -05:00
Daniel Agar and : bresch
f3e2a197ad
move vehicle at rest detection ekf2 -> land_detector
...
- move vehicle at reset detection ekf2 -> land_detector
- ekf_unit: reduce init period
- Fake fusion is when at rest is quite strong and makes the variance reduce rapidly. Reduce the amount of time we wait before checking if the variances are still large enough.
- ekf_unit: reduce minimum vel/pos variance required after init
- Fake pos fusion has a low observation noise, making the vel/pos variances reduce quickly.
Co-authored-by:: bresch <brescianimathieu@gmail.com >
2022-02-01 17:50:19 -05:00
Daniel Agar
999737ddd5
ekf2: refactor mag control
...
- remove class _mag_sample_delayed
- update mag fusion call graph to use mag sample delayed functionally
- Ekf::resetMagHeading()
- use low pass mag directly, but check if valid (magnitude)
- MAG_FUSE_TYPE_INDOOR treat like auto if heading required
2022-01-31 11:47:10 -05:00
Daniel Agar
90358f078f
add odometry reset counter (ekf2, mavlink, etc)
2022-01-31 09:19:36 -05:00
Daniel Agar
49d9d1c987
ekf2: only set fault status bad_acc_clipping if clipping frequently
2022-01-27 09:59:50 -05:00
Daniel Agar
f499749c23
ekf2: mag field strength check respect requirements
...
- requires valid mag data (_mag_data_ready)
- respect mag_fusion_type (including MAG_FUSE_TYPE_INDOOR)
2022-01-27 09:05:13 -05:00
Daniel Agar
ae3070bbf1
ekf2: make filter update period configurable
...
- introduces new parameter EKF2_PREDICT_US to configure the filter
update period in microseconds
- actual filter update period will be an integer multiple of IMU
2022-01-26 10:57:09 -05:00
Oleg Kalachev and bresch
2945ce7c87
ekf2: fuse optical flow when magnetometer is disabled
...
Co-authored-by: bresch <brescianimathieu@gmail.com >
2022-01-25 15:42:10 +01:00
bresch
1502d77df2
ekf: refactor fake position control logic
2022-01-25 15:07:48 +01:00
bresch
a2260e53da
ekf2: replace yaw failure detection criteria
...
The existing logic using the angle between velocity vectors failed to
determine a yaw failure in practice because the state velocity is often
too small compared to its uncertainty to be used. In all the failures
reported, the yaw emergency estimator converged properly and yaw reset
would have fixed the issue.
A much simpler check using the yaw difference between the main EKF
and the emergency estimator is now used to tell if the vel/pos update
failure is most likely caused by a wrong heading.
2022-01-20 11:54:24 +01:00
bresch
e89e3c1b0c
ekf2: extract logic to test yaw emergency estimate quality
2022-01-19 09:05:41 -05:00
bresch
ea80c5027e
ekf2: split yaw estimator state getter into several functions
2022-01-19 09:05:41 -05:00
bresch
ffebd8e771
ekf2: initialize cos tilt variable assuming no tilt
2022-01-19 09:04:53 -05:00
Daniel Agar
92a5bbe97f
ekf2: improve ring buffer sizing and dynamically allocate
2022-01-19 09:04:53 -05:00
Daniel Agar
170849c8f8
ekf2: expand accel bias stability criteria
2022-01-04 13:57:49 -05:00
Daniel Agar
35502c249d
ekf2: don't use Vectors for height innovations (baro, rng, etc)
2022-01-03 09:20:20 -05:00
Daniel Agar
fc17797548
ekf2: delete obsolete _bad_vert_accel_detected
2021-12-13 09:13:39 -05:00
bresch
66290de745
ekf: improve range primary height source
...
allow range fusion to start with fake measurements if the sensor is
providing bad measurements because too close to the ground
2021-12-10 15:18:10 +01:00
bresch
9147d5ea5c
ekf2: use ground as ekf altitude datum when in range primary hgt mode
2021-12-10 15:18:10 +01:00
bresch
5e790da634
ekf2: reset the timeout timer in the reset function directly
2021-12-10 15:18:10 +01:00
bresch
0d874cf00a
ekf2: move aiding start details in dedicated function
...
When an aiding source needs to be started, simply call the corresponding
starting function and any required reset is handled in there.
2021-12-10 15:18:10 +01:00
Matthias Grob
68e2940f58
math: support max of three values additional to min and use it everywhere
2021-12-07 21:06:51 +01:00
bresch
c947562183
ekf: get rid of intermediate variable "fuse_height"
2021-12-07 09:16:21 +01:00
bresch
9651f78b0a
ekf: centralize the height fusion decision
2021-12-07 09:16:21 +01:00
bresch
d583ef6b79
ekf2: move height fusion code to separate source file
2021-12-07 09:16:21 +01:00
Matthias Grob
10ceea2fe6
geo: refactoring on comments and usage
2021-12-06 14:30:14 +01:00
Thomas Debrunner
8db7a6225b
geo: Moved the map_projection_* functions and state into a self-contained C++ class
2021-12-06 14:30:14 +01:00
Daniel Agar
bffd88ad81
ekf2: don't inhibit accel bias when not in air
2021-11-18 13:58:53 -05:00
bresch
394fa953b8
EKF: get status on yaw alignement in flight
...
1s after the in air mag reset is performed, the final yaw alignment is
complete and the yaw estimate can be used for yaw control.
2021-11-16 18:09:37 -05:00
bresch
9e54c6d1aa
ekf2: move generic functions to control.cpp
...
these functions aren't specific to GPS fusion
2021-11-01 13:59:34 +01:00
bresch
8aae39e82a
ekf2: move GPS control logic to separate source file
2021-11-01 13:59:34 +01:00
bresch
689ab12845
ekf2: refactor gps vel/pos fusion control logic
2021-11-01 13:59:34 +01:00
bresch
9afc390552
ekf2: move gps yaw reset in starting function
2021-11-01 13:59:34 +01:00
bresch
d0f89f7fff
ekf2: refactor wind reset functions
2021-10-26 10:18:56 +02:00
bresch
456dfcb4b9
ekf2: update getter for true airspeed
2021-10-26 10:18:56 +02:00
bresch
6e8f0e92ff
ekf2: refactor airspeed fusion control logic
2021-10-26 10:18:56 +02:00
bresch
8873e92c7c
ekf: force fallback to baro if GPS is stopped while in GPS height mode
...
Otherwise, no height aiding source is used
2021-10-26 10:05:28 +02:00
mcsauder
21163d859e
Whitespace cleanup.
2021-10-19 13:29:26 -04:00
Daniel Agar
4cf8eb8226
ekf2: EKF vehicle_at_rest always require some rotation in addition to vibration metrics
2021-10-18 21:39:40 -04:00
Daniel Agar
56823b5ac9
ekf2: EKF control don't allow invalid flow gyro to propagate
2021-10-14 16:31:22 -04:00
bresch