Instead of having a single height source fused into the EKF and the
other ones "waiting" for a failure or the primary sensor, fuse all
sources in EKF2 at the same time. To prevent the sources from fighting against each
other, the "primary" source is set as reference and the other ones are
running a bias estimator in order to make all the secondary height
sources converge to the primary one.
If the reference isn't available, another one is automatically selected
from a priority list. This secondary reference keeps its current bias
estimate but stops updating it in order to be the new reference as close
as possible to the primary one.
Using the control status flags isn't robust as this part of the code
runs at the EKF update rate while the in_air transition is don at the
prediction rate. It was then likely to miss the transition
- replace float32[21] URT covariances with smaller dedicated position/velocity/orientation variances (the crossterms are unused, awkward, and relatively costly)
- these are easier to casually inspect and more representative of what's actually being used currently and reduces the size of vehicle_odometry_s quite a bit
- ekf2: add new helper to get roll/pitch/yaw covariances
- mavlink: receiver ODOMETRY handle more frame types for both pose (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU) and velocity (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU, MAV_FRAME_BODY_FRD)
- mavlink: delete unused ATT_POS_MOCAP stream (this is just a passthrough)
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
fix test by reducing the distance to the new origin: the maximum size of
the local position origin is a cube of 1e6m. If the origin is moved
further than this, the state is clipped to that maximum value
- refactor updateQuaternion() to compute the yaw jacobian directly (respecting the rotation sequence determination)
- fuseHeading()/fuseYaw321()/fuseYaw312() helpers are eliminated and now mag heading fusion and EV yaw fusion compute the innovation in place
- clear up logic for performing zero innovation heading fusion when quaternion variance exceeds threshold (no more _is_yaw_fusion_inhibited flag manipulation)
- when at rest continue fusing last static heading with very low variance even if other heading sources are active
As they are always moving horizontally, the check doesn't make sense
for fixed-wings.
Also don't run the check while on ground to prevent getting a failed
check during pre-takeoff manipulation.
When wind is already estimated, we don't reset the states using airspeed
data, so it could be that the fusion fails if the airspeed isn't
consistent with the filter (test ratio > 1). In this case, don't start
the fusion.
When wind isn't already estimated, the wind states are reset using
airspeed so the fusion can start regardless of the current test ratio.
split the fusion process into:
1. updateAirspeed: computes innov, innov_var, obs_var, ...
2. fuseAirspeed: uses data computed in 1. to generate K, H and fuse the
observation
bresch