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.
- 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>
- 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
- all sources of optical flow publish sensor_optical_flow
- sensor_optical_flow is aggregated by the sensors module, aligned with integrated gyro, and published as vehicle_optical_flow
Co-authored-by: alexklimaj <alex@arkelectron.com>
The noise spectral density, NSD, (square root of power spectral density) is a
continuous-time parameter that makes the tuning independent from the EKF
prediction rate.
NSD corresponds to the rate at which the state uncertainty increases
when no measurements are fused into the filter.
Given that the current prediction rate of EKF2 is 100Hz, the
same tuning is obtained by dividing the std_dev legacy parameter by 10:
nsd = sqrt(std_dev^2 / 100Hz)
bresch