bresch
5f54f6fcda
ekf2: migrate sideslip fusion to SymForce
...
- split fusion into update (compute innov and innov_var) and actual fusion to the state vector
- use estimator_aid_source_1d struct to group the data
2022-10-12 09:55:35 -04:00
bresch
4b687beb3b
ekf2: remove old airspeed fusion code
2022-10-03 10:59:42 -04:00
bresch
299e6058e3
ekf2_test: test airspeed symforce vs sympy generated code
2022-10-03 10:59:42 -04:00
bresch
29ebef1f74
ekf2: migrate fuse_airspeed to SymForce
2022-10-03 10:59:42 -04:00
Daniel Agar
41cda14126
ekf2: add symforce code generation helper target
2022-10-03 10:59:42 -04:00
bresch
20b46ad33f
ekf2_test: test validity of local position when not at rest
...
At rest, the zero velocity update keeps the local position valid all the
time. Deactivate it to be able to test the validity checks
2022-09-29 14:16:36 -04:00
bresch
1914053db2
ekf2: synchronize z_valid and v_z_valid
...
mc_pos_control doesn't handle all the cases at the moment
2022-09-29 14:16:36 -04:00
bresch
f753b92cff
ekf2: start fake pos fusion after dead-reckoning timeout
...
Starting the fake position as soon as all the aiding sources stop makes
the local position immediately invalid while we could continue to navigate
for a couple of second with inertial dead-reckoning
2022-09-29 14:16:36 -04:00
bresch
bb790c9bec
ekf2: add vertical dead-reckoning timeout check for height validity
2022-09-29 14:16:36 -04:00
Ville Juven
59e86c490d
Remove include <log.h> from defines.h
...
The reason for this is that log.h includes drv_hrt.h which sucks in
half the project's include files with it, which causes strange build
issues
2022-09-29 07:56:24 +02:00
Daniel Agar
1f7080a710
ekf2: extend sensor bias stability requirement
...
- for accel/gyro/mag estimated bias only consider them stable (valid
for calibration updates) if the value isn't changing (10% of limit) over
the validity period
2022-09-28 10:40:52 -04:00
Charles Cross
cfdaba35cc
Adds reset counter logic to EV height fusion
2022-09-26 11:48:18 +02:00
bresch
cddade2047
ekf2: use validated airspeed from airspeed sensor only
...
don't use synthetic measurements to prevent circular dependency
2022-09-23 09:26:47 -04:00
Daniel Agar
38d332a553
ekf2: inhibit accel bias dynamically when fake position/height fusion active
...
- inhibit accel bias learning dynamically depending on the angle between an accelerometer and the vertical axis when fake position fusion is active
2022-09-22 20:32:20 -04:00
Daniel Agar
54c97db8b2
ekf2: fix covariance comment typo
2022-09-21 15:29:07 -04:00
bresch
facf01d69d
ekf2: report combined vertical position test ratio
2022-09-15 08:51:19 -04:00
bresch
34d8bd7988
update EKF2 change indicator
2022-09-10 12:29:29 -04:00
bresch
6bd81c0eb7
ekf2: do not store delayed baro sample in class
...
the sample is popped from the buffer and directly used; there is no
need to keep a local copy of it
2022-09-10 12:29:29 -04:00
bresch
ae1e12a6b5
ekf2: remove baro accumulation from initializeFilter
2022-09-10 12:29:29 -04:00
bresch
fe4a6ce8de
ekf2: allow initialization without baro data when fusion is disabled
2022-09-10 12:29:29 -04:00
bresch
a54fa7b9b1
ekf2: add fake height fusion logic
...
When there is no vertical aiding, fake height is started to constrain
the vertical channel of the EKF
2022-09-10 12:29:29 -04:00
bresch
aa716936bf
ekf2: move synthetic_position flag to control_status.flags.fake_pos
2022-09-10 12:29:29 -04:00
bresch
92fbd86b46
ekf2: add gps altitude drift test
2022-09-05 10:28:37 -04:00
bresch
a2a5093881
ekf2: relax zero velocity update
...
The ZVU is too strong and prevents the EKF from following variations in
the height aiding sources, creating large innovations.
2022-09-05 10:28:37 -04:00
Daniel Agar
d996af4647
ekf2: pass gpsSample around where required
...
- this minimizes potential misuse accessing _gps_sample_delayed and
makes the dependency clear
2022-09-05 10:27:19 -04:00
Daniel Agar
7bbdc220f5
ekf2: initialiseFilter() simplify mag heading init and resetQuatStateYaw
...
- most of resetQuatStateYaw doesn't apply to initial heading init, so
removing the special case keeps it simple
2022-09-02 08:46:10 -04:00
Daniel Agar
5cb44a521c
ekf2: estimator interface handle sample time with signed integers
...
- this prevents unsigned integer overflow if the timestamp is smaller
than the configured delay (mostly a simulation edge case)
2022-09-01 09:47:29 -04:00
dagar
75f6f22223
[AUTO COMMIT] update change indication
2022-08-31 08:52:30 -04:00
Daniel Agar
1948c5057a
ekf2: handle all time on delayed horizon (except for newest sample checks)
...
- a growing number of samples come into the backend with the time
already delayed (sensor's interrupt setting timestamp sample)
- if the incoming timestamp is already delayed then the new data checks
(relative to latest IMU) can be slightly wrong
- handle almost all timestamps and checks on delayed time horizon,
except for explicit checks of new samples
- isRecent() and isTimedOut() helpers use delayed time
- add new isNewestSampleRecent() used for checking the incoming
timestamp of the incoming (adjusted) data
2022-08-31 08:52:30 -04:00
Daniel Agar
9b0b0d1168
ekf2: update test change indication csv
2022-08-25 15:55:45 -04:00
bresch
0061ef8b89
[AUTO COMMIT] update change indication
2022-08-24 09:16:11 -04:00
bresch
090d03eea2
ekf2: publish GNSS, rng and EV height biases
2022-08-24 09:16:11 -04:00
bresch
a2a29ba0dd
ekf2: verify parameter configuration for height sources
2022-08-24 09:16:11 -04:00
bresch
8dd5d0d1a3
ekf2: don't reset height when in conditional range aiding
2022-08-24 09:16:11 -04:00
bresch
2d39097f60
ekf2: move rng height to file
2022-08-24 09:16:11 -04:00
bresch
28b4cd0579
ekf2: move GNSS hgt control to file
2022-08-24 09:16:11 -04:00
bresch
b6f76b5eaa
ekf2: move ev control to file
2022-08-24 09:16:11 -04:00
bresch
f555281c94
ekf2: move height control-related function to file
2022-08-24 09:16:11 -04:00
bresch
d87feee5b0
ekf2: move baro control logic to file
2022-08-24 09:16:11 -04:00
bresch
12e25eba62
ekf2: add common height fusion timeout parameter
2022-08-24 09:16:11 -04:00
bresch
52f726c5b7
ekf2: in bias estimator, use psd instead of var for prediction
...
PSD is independent from the sampling time while variance isn't
2022-08-24 09:16:11 -04:00
bresch
3fb218600a
ekf2_test: add multi height source unit tests
2022-08-24 09:16:11 -04:00
bresch
aba2eac0df
ekf2: publish GNSS, rng and EV height biases
2022-08-24 09:16:11 -04:00
bresch
8962cf2d25
ekf2: GPS, baro and range finder control parameters
...
Also remove the legacy "range aid" than can be achieved by setting the
height reference to range finder and the range finder control parameter
to "conditional".
Conditional range aiding cal also be set when the height reference isn't
the range finder. This prevents the ratchetting effect due to switching
between references.
2022-08-24 09:16:11 -04:00
bresch
578e1339ca
ekf2: re-implement range aid
...
range aid simply forces the range finder to be the height reference when
starting
2022-08-24 09:16:11 -04:00
bresch
88ac5ea210
ekf2_test: add test case for accel clipping handling
2022-08-24 09:16:11 -04:00
bresch
375753eba8
ekf2_test: add inertial nav falling detection tests
2022-08-24 09:16:11 -04:00
bresch
66ce1a002b
ekf2_test: rename VisionHeight -> ExternalVisionHeight
2022-08-24 09:16:11 -04:00
bresch
82ec7a495a
ekf2: refactor inertial nav falling check
2022-08-24 09:16:11 -04:00
bresch
f9188b2a14
ekf2: refactor vertical acceleration check for multiple height sources
2022-08-24 09:16:11 -04:00
bresch