4cb327fba3
ekf2: remove old GPS yaw autogenerated code
2022-11-22 09:59:31 -05:00
16a3fee54f
ekf2_test: check behavior of GNSS yaw fusion at singularities
2022-11-22 09:59:31 -05:00
4116de31ad
ekf2: compare GNSS yaw fusion sympy vs symforce
2022-11-22 09:59:31 -05:00
eff2c6adcf
ekf2: migrate GNSS yaw fusion to SymForce
2022-11-22 09:59:31 -05:00
da82757bf6
ekf2: accumulate multiple vel/pos/orientation reset deltas per update
...
- if in a single EKF update there are multiple resets we need to track the accumulated delta so the change consumed by the controllers is correct
2022-11-15 13:20:54 -05:00
22420a7bf1
ekf2: do not fuse ZVU if other velocity source is active
2022-11-15 11:17:07 -05:00
c67f03f383
ekf2: update change indicator
2022-11-14 11:51:32 -05:00
f319cc528b
ekf2: remove old flow fusion generated code
2022-11-14 11:51:32 -05:00
2a83dbf81d
ekf2_test: compare flow fusion sympy vs symforce
2022-11-14 11:51:32 -05:00
93564baccf
ekf2_flow: recalculate innovation after fusing 1st axis
...
The state changed so we need to recalculate the innovation of the 2nd
axis after fusing the 1st one
2022-11-14 11:51:32 -05:00
4dbdf23346
ekf2: update _R_to_earth when the state quat changed through fusion
2022-11-14 11:51:32 -05:00
28458340e6
ekf2_flow: check innov variance health after fusing 1st axis
2022-11-14 11:51:32 -05:00
73a8c388e8
ekf2: fuse by looping through axes
2022-11-14 11:51:32 -05:00
b54a4417fa
ekf2: migrate flow fusion to SymForce
2022-11-14 11:51:32 -05:00
639d1ddca2
ekf2: update flags (in_air, at_rest, etc) on delayed time horizon ( #20576 )
...
- in a lot of cases this won't really matter, although you can see it in logs (especially with things like vehicle_at_rest), but it's something we might as well do properly
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com >
2022-11-14 11:35:40 -05:00
7d1f1d0f84
ekf2: replace macro constants by typed constexpr
2022-11-14 10:45:23 -05:00
06702da003
ekf2: add GNSS yaw max interval
...
yaw data usually comes at lower rate than vel/pos
2022-11-14 10:45:23 -05:00
9834c7917b
ekf2: use yaw emergency estimator more aggressively
...
During the whole flight, if the difference between the yaw estimate from
EKF2 and the emergency estimator is large and that the GNSS velocity
fusion is failing continuously, immediately reset to the emergency yaw
estimate.
2022-11-14 10:45:23 -05:00
bd5bc9d207
ekf2: cleanup terrain estimator init (shouldn't be valid by default)
...
- with Ekf::initialiseFilter() resetting _time_last_hagl_fuse the terrain estimate was technically valid regardless of any range or flow data availability and as a result optical flow fusion is able to start and stay active
- only consider terrain estimate valid based on control status flags and recent fusion
2022-11-14 09:29:46 -05:00
84d1435880
ekf2: ensure minimum output buffer sizing
...
- buffer at least 2 samples for the IMU output predictor buffers
- dropping below 2 becomes problematic for the minimum observation
interval calculation and the vertical output buffer trapezoidal
integration
2022-11-09 09:49:35 -05:00
5239993c88
ekf2: move EV vel to new state machine, introduce EKF2_EV_CTRL param
2022-11-08 11:46:41 -05:00
688dae1108
ekf2: add new EKF2_EV_QMIN parameter
2022-11-08 11:46:41 -05:00
298cc61e07
ekf2: push fuse beta config into backend
2022-11-02 10:09:26 -04:00
a242a0210e
Update world_magnetic_model to latest Mon 24 Oct 2022 09:29:11 PM EDT
2022-10-25 09:20:01 -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
f0a0a3e545
ekf2_test: compare mag decl fusion sympy vs symforce
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
f77d7d9413
[AUTO COMMIT] update change indication
2022-10-19 17:34:31 +02:00
cf9859965a
ekf2: remove old yaw fusion auto-code
2022-10-19 17:34:31 +02:00
e33215b61c
ekf2_test: compare yaw fusion sympy vs symforce
2022-10-19 17:34:31 +02:00
42f683fa64
ekf2_test: test using positive definite covariance matrix
...
A covariance matrix needs to be positive definite
2022-10-19 17:34:31 +02:00
10f9ac148f
ekf2: migrate mag yaw fusion to SymForce
2022-10-19 17:34:31 +02:00
a41b6f416e
ekf2_test: verify heading innov variance in all orientations
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
0d2ff6e224
ekf2: test ekf_logger further reduce std::precision to minimize false positives
2022-10-18 14:19:16 -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
77a36219c6
ekf2_test: compare 3D mag fusion sympy vs symforce
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
458e5a6b0e
ekf2: update change indicator
2022-10-14 11:42:23 -04:00
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