From 161ac2e051e9f0f51ceeba3da263b740c309e19c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 8 May 2016 18:03:09 +1000 Subject: [PATCH] EKF: ensure position co-variances are reset When transitioning into optical flow nav from a non-aiding condition during ground operation, ensure position covariances are reset as they could be invalid. --- EKF/control.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 09115c6447..e5a1e5aa8f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -110,9 +110,15 @@ void Ekf::controlFusionModes() P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); if (!_in_air) { - // we are likely starting OF for the first time so reset the position and states + // we are likely starting OF for the first time so reset the horizontal position and vertical velocity states _state.pos(0) = 0.0f; _state.pos(1) = 0.0f; + + // reset the coresponding covariances + // we are by definition at the origin at commencement so variances are also zeroed + zeroRows(P,7,8); + zeroCols(P,7,8); + // align the output observer to the EKF states alignOutputFilter(); }