bresch
8b9ac2d7f3
ekf2: remove old mag 3D fusion auto-code
2022-10-17 16:14:56 -04:00
bresch
77a36219c6
ekf2_test: compare 3D mag fusion sympy vs symforce
2022-10-17 16:14:56 -04:00
bresch
b92cbe12a0
ekf2: migrate mag 3D fusion to symforce
...
ekf2: merge mag 3d innov var, Hx and Kx computation to reduce flash
Slightly less code produced, almost no performance change
ekf2_mag3D: do not pre-compute Kalman gains
The vector of Kalaman gains is not too expensive to compute using
matrix-vector multiplication. Pre-generating it using CSE takes a lot of
flash space for little benefit.
2022-10-17 16:14:56 -04:00
Daniel Agar
116bb6049f
ekf2: fusion helper consistency
...
- yaw fusion use measurementUpdate()
- fuseVelPosHeight() respect _accel_bias_inhibit (like measurementUpdate())
2022-10-17 10:17:31 -04:00
bresch
458e5a6b0e
ekf2: update change indicator
2022-10-14 11:42:23 -04:00
bresch
1b4092abbb
symforce: temporarily remove custom cmake command
...
otherwise the generated files are removed with make clean
2022-10-14 11:42:23 -04:00
bresch
a8a3107c05
ekf2: remove old covariance prediction code
2022-10-14 11:42:23 -04:00
bresch
df084d65e3
ekf2_test: compare covariance prediction sympy vs symforce
2022-10-14 11:42:23 -04:00
bresch
a4e511b90e
ekf2: migrate covariance prediction to SymForce
2022-10-14 11:42:23 -04:00
Daniel Agar
2de990fd4b
estimator_aid_source split GNSS pos (3d) -> pos (2d) + hgt
...
- per estimator air source status only keep a single set of flags and
timestamp that applies to the entire source
2022-10-13 11:28:50 -04:00
bresch
bdd043f27f
ekf2: fix sideslip fusion sign
2022-10-12 16:14:47 +02:00
bresch
53865118fb
ekf2: remove old sideslip fusion code
2022-10-12 09:55:35 -04:00
bresch
f7c749c9cd
ekf2_test: compare sideslip fusion of Sympy and SymForce
2022-10-12 09:55:35 -04:00
bresch
d0f92bfbd5
ekf2_test: create helper functions for auto-generated code diff
2022-10-12 09:55:35 -04:00
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