298cc61e07
ekf2: push fuse beta config into backend
2022-11-02 10:09:26 -04:00
6f861ba889
ekf2: pos/vel reset helpers pass new variance
...
- optical flow velocity reset use already computed _flow_vel_ne
2022-10-24 10:59:58 -04:00
ed558e199f
ekf2: remove realignYawGPS() (replaced with yaw estimator)
2022-10-21 09:01:30 -04:00
96e7ea7a08
ekf2: remove old mag declination auto-code
2022-10-20 18:16:25 -04:00
2f3ea88099
ekf2: migrate mag declination to SymForce
2022-10-20 18:16:25 -04:00
5030b21d2e
ekf2: replace quatToInverseRotMat if only used once
2022-10-20 14:15:32 -04:00
fb3adc3faa
ekf2: move baro compensation to delayed time horizon and add validity check
...
- this removes an unnecessary virtual call and simplifies things a bit
Co-authored-by: bresch <brescianimathieu@gmail.com >
2022-10-20 09:17:55 -04:00
cea185268e
msg ROS2 compatibility, microdds_client improvements (timesync, reduced code size, added topics, etc), fastrtps purge
...
- update all msgs to be directly compatible with ROS2
- microdds_client improvements
- timesync
- reduced code size
- add to most default builds if we can afford it
- lots of other little changes
- purge fastrtps (I tried to save this multiple times, but kept hitting roadblocks)
2022-10-19 19:36:47 -04:00
f9509b442c
ekf2: height aid source consistency ( #20405 )
...
- working towards keeping all height source (baro/ev/gnss/rng) handling as consistent as possible, possibly refactoring these out into separate classes later
2022-10-19 15:35:07 -04:00
5ca28dd6dc
Use isAllFinite() in all places that check finiteness on entire vectors or matrices
2022-10-19 19:09:20 +02:00
cf9859965a
ekf2: remove old yaw fusion auto-code
2022-10-19 17:34:31 +02:00
10f9ac148f
ekf2: migrate mag yaw fusion to SymForce
2022-10-19 17:34:31 +02:00
f11908a266
ekf2: generate yaw fusion jacobians using symforce
2022-10-19 17:34:31 +02:00
b71fc63162
ekf2: fix sideslip timeout (typo)
2022-10-18 18:38:25 -04:00
535415a537
ekf2: add OF estimator aid src status
2022-10-18 14:19:16 -04:00
8b9ac2d7f3
ekf2: remove old mag 3D fusion auto-code
2022-10-17 16:14:56 -04:00
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
116bb6049f
ekf2: fusion helper consistency
...
- yaw fusion use measurementUpdate()
- fuseVelPosHeight() respect _accel_bias_inhibit (like measurementUpdate())
2022-10-17 10:17:31 -04:00
a8a3107c05
ekf2: remove old covariance prediction code
2022-10-14 11:42:23 -04:00
a4e511b90e
ekf2: migrate covariance prediction to SymForce
2022-10-14 11:42:23 -04:00
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
bdd043f27f
ekf2: fix sideslip fusion sign
2022-10-12 16:14:47 +02:00
53865118fb
ekf2: remove old sideslip fusion code
2022-10-12 09:55:35 -04:00
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
4b687beb3b
ekf2: remove old airspeed fusion code
2022-10-03 10:59:42 -04:00
29ebef1f74
ekf2: migrate fuse_airspeed to SymForce
2022-10-03 10:59:42 -04:00
41cda14126
ekf2: add symforce code generation helper target
2022-10-03 10:59:42 -04:00
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
bb790c9bec
ekf2: add vertical dead-reckoning timeout check for height validity
2022-09-29 14:16:36 -04:00
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
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
cfdaba35cc
Adds reset counter logic to EV height fusion
2022-09-26 11:48:18 +02:00
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
54c97db8b2
ekf2: fix covariance comment typo
2022-09-21 15:29:07 -04:00
facf01d69d
ekf2: report combined vertical position test ratio
2022-09-15 08:51:19 -04:00
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
ae1e12a6b5
ekf2: remove baro accumulation from initializeFilter
2022-09-10 12:29:29 -04:00
fe4a6ce8de
ekf2: allow initialization without baro data when fusion is disabled
2022-09-10 12:29:29 -04:00
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
aa716936bf
ekf2: move synthetic_position flag to control_status.flags.fake_pos
2022-09-10 12:29:29 -04:00
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
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
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
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
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
a2a29ba0dd
ekf2: verify parameter configuration for height sources
2022-08-24 09:16:11 -04:00
8dd5d0d1a3
ekf2: don't reset height when in conditional range aiding
2022-08-24 09:16:11 -04:00
2d39097f60
ekf2: move rng height to file
2022-08-24 09:16:11 -04:00
28b4cd0579
ekf2: move GNSS hgt control to file
2022-08-24 09:16:11 -04:00
b6f76b5eaa
ekf2: move ev control to file
2022-08-24 09:16:11 -04:00
Daniel Agar