mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 20:43:33 +08:00
ekf2: enable multiple height sources fusion
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.
This commit is contained in:
@@ -97,7 +97,7 @@ inline bool BiasEstimator::isTestRatioPassing(const float innov_test_ratio) cons
|
||||
|
||||
inline void BiasEstimator::updateState(const float K, const float innov)
|
||||
{
|
||||
_state = math::constrain(_state + K * innov, -_state_max, _state_max);
|
||||
_state = _state + K * innov;
|
||||
}
|
||||
|
||||
inline void BiasEstimator::updateStateCovariance(const float K)
|
||||
|
||||
Reference in New Issue
Block a user