bresch
421f13e4b5
ekf2: fix joseph covariance update for Schmidt-Kalman filter
...
If part of the Kalman gain is zeroed, the first step of the joseph
update does not produce a symmetrical matrix.
2024-03-07 15:11:47 +01:00
bresch
6f9a378247
yaw_est: force set gyro bias when at rest
...
The gyro bias estimate from EFK2 is really good when at rest and should
be used by the yaw estimator to prevent heading drifts due to poor
heading observability.
2024-03-05 14:00:06 -05:00
bresch
b80f15f7b5
ekf2-mag_auto: always use mag 3D after takeoff
2024-02-27 12:33:43 -05:00
bresch
051baec9c4
ekf2: allow wind dead-reckoning after manual position reset
...
Reset velocity using airspeed and start navigating
2024-02-27 09:34:05 -05:00
Daniel Agar
8001132d33
ekf2: ZeroGyroUpdate move to fuseDirectStateMeasurement
2024-02-26 12:32:59 -05:00
bresch
d501d8e1d4
ekf2: use Joseph stabilized update in direct state observations
2024-02-26 12:32:59 -05:00
bresch
9d9766c6cf
ekf2: use Joseph stabilized covariance update
2024-02-26 12:32:59 -05:00
bresch
37a40d3fc2
baro static pressure compensation tuning: remove dependency to baro bias
...
`estimator_baro_bias` requires to have GNSS and baro hgt active and GNSS as the reference. This is quite restrictive. Instead, we can simply use a high-passed version of the baro error.
2024-02-23 10:07:13 +01:00
Daniel Agar
506c60c471
ekf2: declination fusion don't use uninitialized parameter (EKF2_MAG_DECL)
2024-02-21 09:45:44 -05:00
Daniel Agar
8243b4f474
ekf2: move vel/pos reset helpers
2024-02-20 13:16:24 -05:00
Daniel Agar
22b957696d
ekf2: velocity/position fusion helper minor consistency cleanup
2024-02-20 13:16:24 -05:00
Daniel Agar
c338891677
ekf2: split vel_pos_fusion.cpp
2024-02-20 13:16:24 -05:00
Daniel Agar
c4c41c49e5
ekf2: move fuseVelPosHeight() -> fuseDirectStateMeasurement()
...
- don't bother keeping bad_vel_{N,E,D} and bad_pos_{N,E,D} fault status bits
2024-02-20 13:16:24 -05:00
Daniel Agar
021dd0d0af
ekf2: fix EV height bias predict call
...
- needs to be called every iteration
2024-02-20 11:47:53 -05:00
bresch
c221da27a7
ekf2: set attitude validity flag using centralized function
2024-02-20 11:33:30 -05:00
Daniel Agar
1032dd3470
ekf: fix measurementUpdate comment typo
2024-02-19 09:41:49 +01:00
Daniel Agar
8dc3975456
ekf2: only populate gnss pos aid src status if ref initialized
...
- this is a minor logging improvement when plotting the position from the beginning of the log (often a replay session)
2024-02-15 13:13:10 -05:00
muramura
f636414ca7
tuning_tools: Change 1G to a more accurate value
2024-02-09 10:26:57 -05:00
bresch
17d55dddd6
ekf2-drag: do not generate Kalman gain to save flash
2024-02-06 12:16:33 +01:00
bresch
1efb08375a
ekf2: let drag fusion affect the complete state vector
...
This improves tilt estimation and can extend the inertial dead-reckoning
validity period
2024-02-06 12:16:33 +01:00
bresch
9c02e384e6
ekf2-agp: follow measurement reset
2024-01-30 11:23:55 -05:00
bresch
5d9081b0dd
ekf2-agp: ensure logging of AGP aid_src topic
2024-01-30 11:23:55 -05:00
bresch
4268759d4a
ekf2-agp: reset to measurement on fusion timeout
2024-01-30 11:23:55 -05:00
bresch
80f20e619c
ekf2: zvup sequential fusion
2024-01-29 12:12:37 -05:00
Daniel Agar and bresch
51155f7a29
ekf2: treat inhibited states as "consider states" and remove conservative covariance matrix fixes ( #22597 )
...
When a state stops to be estimated it becomes a "consider state". Its value and variance are frozen but its covariance with other states continue to evolve normally.
- removes conservative accel bias variance limiting
- force symmetry is skipped after fusion of NED vel/pos (a direct measurement)
---------
Co-authored-by: bresch <brescianimathieu@gmail.com >
2024-01-24 14:14:09 -05:00
Mathieu Bresciani and bresch
cb396a6339
ekf2: fix computation of tilt and yaw variances add them to logging
...
Co-authored-by: bresch <bresch@users.noreply.github.com >
2024-01-24 12:14:15 -05:00
Daniel Agar
bf7da6430d
ekf2: consolidate LPOS & GPOS accuracy methods
2024-01-22 12:34:08 -05:00
bresch
7c7a3c117a
ekf2-gravity: nomalize gravity fusion and proper sequential fusion
2024-01-18 20:39:16 -05:00
bresch
d624fbba07
ekf2-grav: lower gate to reject real acceleration more effectively
2024-01-18 20:39:16 -05:00
bresch
c28972d15e
ekf2-grav: only use filtered accel norm to start/stop the fusion
...
Using the raw data makes the swith too sensitive to noise
2024-01-18 20:39:16 -05:00
Daniel Agar and bresch
ed0d26de8a
ekf2: improve attitude estimation without horizontal aiding
...
- fake_pos only if at rest or tilt variances becomes large
- fake pos: don't run when grav fusion is enabled
- gravity fusion enabled by default
- gravity: only fuse when accel norm and lpf norm are consistent
Co-authored-by: bresch <brescianimathieu@gmail.com >
2024-01-17 10:27:25 -05:00
Daniel Agar
64f28c4c07
ekf2: delete unused gps error norm field
2024-01-12 10:14:32 -05:00
Daniel Agar
8a031677d5
ekf2: verbose debug print status include state variances
2024-01-11 11:53:20 -05:00
Daniel Agar
8bcba6128a
ekf2: fix resetGlobalPosToExternalObservation whitespace
2024-01-11 10:59:56 -05:00
Roman Bapst
603c3f6636
added support to reset vehicle position based on external position ( #22444 )
...
via command MAV_CMD_EXTERNAL_POSITION_ESTIMATE
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2024-01-11 13:09:22 +01:00
Daniel Agar and bresch
d45c3d3407
ekf2: verbose (DEBUG_BUILD) print status with Matrix improved print
...
* ekf2: verbose print status
* matrix/Matrix improve print output
* bold diagonal elements, print ring buffer entry size
* print in scientific notation when >= 10 to respect max size
---------
Co-authored-by: bresch <brescianimathieu@gmail.com >
2024-01-10 09:46:04 -05:00
bresch
36eb319834
ekf2-yaw_est: split imu and velocity updates
2023-12-18 18:11:20 +01:00
bresch
a653073d4f
ekf2: perform GNSS checks at delayed-time horizon
...
- never fuse a measurement that is not passing the checks
- cleanup and simplify GNSS vel/pos control logic
2023-12-18 18:11:20 +01:00
bresch
5ca22df55c
ekf2-pos bias estimator: use enum
2023-12-05 11:22:59 -05:00
bresch
a3515a2474
ekf2: scope HeightSensor enum
2023-12-05 11:22:59 -05:00
bresch
6bd1145006
ekf2: scope RngCtrl enum
2023-12-05 11:22:59 -05:00
bresch
1df52df27d
ekf2: scope GnssCtrl enum
2023-12-05 11:22:59 -05:00
bresch
97423136d1
ekf2-AGP: scope control enum
2023-12-05 11:22:59 -05:00
Matthias Grob
4c0b6dbe86
Remove trailing whitespaces and trailing duplicate newlines
2023-12-01 13:11:00 -05:00
bresch and Daniel Agar
fe7988672f
ekf2: auxiliary position fusion
...
Co-authored-by: Daniel Agar <daniel@agar.ca >
2023-12-01 11:50:22 +01:00
bresch
6320ca64a9
ekf2-terr: allow range fusion before takeoff
2023-11-21 21:56:34 -05:00
bresch
0f3378e194
ekf2: fix terrain range finder logging
2023-11-21 21:56:34 -05:00
bresch
1c3a1183c8
ekf2-flow: refactor control logic
...
Use flow rates instead of integrals in backend. This allows us to delay
the data to the mitpoint integration time and simplifies the code in
general.
Gyro compensation can still be done in EKF2 if needed, but the
flow module normally already appends the correct gyro data to the flow
message.
2023-11-21 21:56:34 -05:00
Roman Bapst and Thomas Stastny
dd2322d622
Added PerformanceModel for fixed wing ( #22091 )
...
* created a Performance Model for fixed wing vehicle
- added compensation for maximum climbrate, minimum sinkrate, minimum airspeed and trim airspeed based on weight ratio and air density
- added atmosphere lib to standard atmosphere calculations
---------
Signed-off-by: RomanBapst <bapstroman@gmail.com >
Co-authored-by: Thomas Stastny <thomas.stastny@auterion.com >
2023-11-21 17:13:50 +01:00
Mathieu Bresciani and bresch
efa12ad224
EKF2 simplify covariance propagation ( #22344 )
...
* ekf2-derivation: optimize before generating cpp code
* update EKF2 change indicator
Slight changes due to simplifications done in the covariance prediction
---------
Co-authored-by: bresch <[brescianimathieu@gmail.com ](mailto:brescianimathieu@gmail.com )>
2023-11-14 11:22:16 -05:00
bresch