EKF: Don't assume large position uncertainty when starting optical fl…

…ow nav
PX4 · May 7, 2018 · bdf5b3e · bdf5b3e
1 parent e26711a
commit bdf5b3e
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
@@ -168,7 +168,9 @@ bool Ekf::resetPosition()
 
 		}
 
-		setDiag(P, 7, 8, sq(_params.pos_noaid_noise));
+		// estimate is relative to initial positon in this mode, so we start with zero error.
+		zeroCols(P,7,8);
+		zeroRows(P,7,8);
 
 	} else {
 		// Used when falling back to non-aiding mode of operation