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
00568985c0
yaw_est: use error-state covariance prediction
...
Convergence improvements in high yaw rate conditions
2023-11-13 11:37:27 -05:00
Silvan Fuhrer
ec20f92558
EKF2: sideslip_fusion: remove construced airspeed magnitude check to fuse beta
...
Signed-off-by: Silvan Fuhrer <silvan@auterion.com >
2023-11-09 09:42:43 +01:00
Daniel Agar
77baa7c24a
ekf2: EKFGSF yaw estimator always run
...
- if not in air the accel noise is doubled
- if landed don't init unless GPS velocity is non-negligible
- when inactive continue seeding with EKF gyro bias
- reset yaw estimator if GPS fusion is stopped
2023-11-07 15:48:49 -05:00
Sverre Velten Rothmund
94d4dc85f8
ekf2: Make stuck detector optional
2023-11-07 11:57:19 +01:00
bresch
a989e5338c
ekf2: reset globlal position uncertainty when GNSS is fused
...
There is no reason to keep an uncertainty on the origin as it is then
already contained in the local position estimate when GNSS data is fused
in the filter.
2023-11-02 13:11:40 -04:00
Mathieu Bresciani and bresch
0d6c2c8ce9
EKF2: Error-State Kalman Filter ( #22262 )
...
* ekf derivation: change to error state formulation
* ekf2: update auto-generated code for error-state
* ekf2: adjust ekf2 code for error state formulation
* ekf2_tests: adjust unit tests for error-state EKF
* update change indicator for error-state EKF
* ekf2_derivation: allow disabling mag and wind states
---------
Co-authored-by: bresch <[brescianimathieu@gmail.com ](mailto:brescianimathieu@gmail.com )>
2023-10-31 10:02:18 -04:00
Daniel Agar
27f9b1b65a
ekf2: move zero gyro update to aid source class
2023-10-18 20:23:56 -04:00
Daniel Agar
e79737a38d
ekf2: create simple estimator aid source base class and extract zero velocity update
2023-10-18 20:23:56 -04:00
Daniel Agar
408c30de13
ekf2: delete redundant aid src status getters
2023-10-18 15:21:51 -04:00
Daniel Agar
bdaf0acfca
ekf2: fully disable yaw estimator EKFGSF_yaw with CONFIG_EKF2_GNSS ( #22233 )
2023-10-18 13:30:17 -04:00
Daniel Agar
3d238b0275
ekf2: add kconfig to disable gravity fusion ( #22231 )
2023-10-18 10:50:51 -04:00
Daniel Agar
6eae9fb371
ekf2: fix barometer kconfig
2023-10-18 09:43:27 -04:00
Daniel Agar
48e09a4dea
ekf2: move predict covariance IMU inhibit check to function
2023-10-17 09:58:10 -04:00
Daniel Agar
0b44852094
ekf2: move accel bias check out of fixCovarianceErrors
2023-10-17 09:58:10 -04:00
bresch
1c9373e83b
update baro static pressure compensation tuning script
...
Field name changed in vehicle_gps_position
2023-10-16 09:45:28 -04:00
Daniel Agar
9676af2fe6
ekf2: predict covariance avoid explicit temporary nextP
2023-10-13 20:57:33 -04:00
Daniel Agar
d2b3e7fe16
ekf2: new kconfig to enable/disable GNSS (enabled by default)
2023-10-11 14:02:34 -04:00
bresch
ec15fe3d90
ekf2-derivation: fix terrain and yaw estimator derivations
...
fix compatibility issues with symforce-0.9.0
2023-10-11 09:49:17 -04:00
bresch
cf1c6a8b84
ekf2-derivation: remove old wind covariance derivation
2023-10-11 09:49:17 -04:00
Daniel Agar
5352a64042
ekf2: symforce derivation allow optionally disabling mag and wind states
2023-10-10 17:31:11 -04:00
Daniel Agar
b5f3d089c4
ekf2: mag_fusion remove direct state index usage
2023-10-10 09:18:04 -04:00
Daniel Agar
cf4c565e4a
ekf2: mag_fusion.cpp cleanup includes
2023-10-10 09:18:04 -04:00
Daniel Agar
c85840c4dd
ekf2: mag_fusion only set fault status flags if mag_3D updating all states
...
- other parts of the system are blanket checking for any fault status
flag
2023-10-10 09:18:04 -04:00
Daniel Agar
e58ceba4b1
ekf2: mag_fusion consolidate duplicate error handling
2023-10-10 09:18:04 -04:00
bresch
fc32820e19
ekf2: initialize wind covariance using symforce
2023-10-09 21:03:38 -04:00
Daniel Agar
28d58a947f
ekf2: cleanup more optional mag (CONFIG_EKF2_MAGNETOMETER)
2023-10-09 10:18:02 -04:00
Daniel Agar
5f87f3a046
ekf2: drag fusion add aid source status topic
2023-10-09 09:26:28 -04:00
Daniel Agar
028733e1c7
ekf2: add kconfig to disable wind estimation (off by default)
2023-10-09 09:22:29 -04:00
05fd8c5976
EKF2: centralized auto-generated state ( #22183 )
...
* ekf2_derivation: use single source of state definition
The state is defined as an ordered dictionary of group elements and
everything else is generated using that state definition
* ekf2: generated state sample add const reference getter
---------
Co-authored-by: bresch <[brescianimathieu@gmail.com ](mailto:brescianimathieu@gmail.com )>
Co-authored-by: Daniel Agar <daniel@agar.ca >
2023-10-06 10:28:21 -04:00
d61743412c
ekf2: fix flow gyro bias corrections ( #22145 )
...
* ekf2-flow: fix flow gyro bias compensation
* ekf2-flow: apply flow gyro bias when used
* ekf2: log optical flow gyro bias
* ekf2: optical flow control always use provided flow gyro (with bias applied)
* ekf2-flow: log flow gyro and gyro reference
* ekf2-flow: support senrors with XY flow gyro
---------
Co-authored-by: Daniel Agar <daniel@agar.ca >
Co-authored-by: bresch <[brescianimathieu@gmail.com ](mailto:brescianimathieu@gmail.com )>
2023-10-05 10:51:30 -04:00
bresch
10db6b6eda
ekf2: remove sparse vector optimization
...
The sparse vector template requires to know which states are non-zero in
the observation jacobian. This complicates the modularity of the code
when the state vector or the derivation is changed.
The computation cost difference is almost negligible for this size.
2023-10-02 10:12:53 -04:00
Daniel Agar
b817eb0370
ekf2: collect_gps() don't throttle updates until WMM set initially
2023-09-29 09:37:30 +02:00
Daniel Agar
1e594747ab
ekf2: fix WMM NAN checks
...
- if any of the stored WMM is NAN then it should be updated
2023-09-29 09:37:30 +02:00
Daniel Agar
14a967e2ca
ekf2: remove aid src status fusion_enabled flag
2023-09-26 10:30:16 -04:00
bresch
99197919d7
ekf2: access state covariance using helper functions
2023-09-26 06:41:04 +02:00
