- collapse updateOptFlow() and startFlowFusion() to avoid recomputing H
- this is a relatively expensive call we can easily avoid with the
right structure
- adjust flow sample gyro_rate immediately after popping from ring
buffer
- always update flow gyro bias (calcOptFlowBodyRateComp()) regardless
of flow quality or magnitude
* reset position by fusion
* handle local_pos_valid for fixed wing in gnss denied
* [WIP] ekf2: setEkfGlobalOrigin respect current height reference and vertical position aiding
* global origin, also reset vertical pos without gps as ref
* fix wo gnss, that bitcraze ci passes
* revert some changes as requested
* remove duplicate reset messages
* undo unrelated whitespace changes, I'll fix it everywhere in a followup
* [SQUASH] ekf2: add vehicle_command_ack
* resetGlobalPosToExternalObservation consolidate logic
* remove gnss check from local_pos validation check
* reset when 0<accuracy<1, otherwise fuse
* replace gps param with flag
* ekf2: dead reckon time exceeded, respect ZUPT preflight if air data or optical flow expected
* subtract timeout from last inertial dead-reck, change fake pos conditions, save flash
---------
Co-authored-by: Daniel Agar <daniel@agar.ca>
- track accel clipping count per axis
- only set bad_acc_clipping fault_status if at least one axis is
clipping continuously or if all have been clipping at warning level
- Note: this doesn't impact the clipping projections that boost the
accel process noise, pause bias estimation, and skip gravity fusion
on a per sample basis
Terrain is the state: terrain vertical position
Hagl (height above ground level) is the vertical distance between the
vertical position and the terrain vertical position
The EKF can recover from an initial bad earth mag field estimate.
Constraining the field is not necessary and can lead to an unpredicted
behavior of the filter.
Declination fusion is safe to run even when the horizontal field is 0
When North-East (e.g.: GNSS pos/vel) aiding is active, the heading
estimate is constrained and consistent with the vel/pos aiding. Reset to
GNSS heading should only occur if no N-E aiding is active or if the
filter is not yes aligned. Otherwise, just wait for the consistency
check to pass again (will pass at some point if the heading uncertainty
of the filter is getting too high).
The duration of a unit test had to be increased because the incorrect
covariance matrix update, was making the unit test passing faster
(over-optimistic variance).
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>
- 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>
Daniel Agar