From 13bb814f2015154cb5cadb814e37db1b6b2e634b Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 17 Jan 2013 12:18:20 -0500 Subject: [PATCH] Prevented attitude correction from changing velocity when pos not init. --- apps/examples/kalman_demo/KalmanNav.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 15254f7c79..40166f3a14 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -590,9 +590,11 @@ int KalmanNav::correctAtt() psi += xCorrect(PSI); // attitude also affects nav velocities - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); + if (_positionInitialized) { + vN += xCorrect(VN); + vE += xCorrect(VE); + vD += xCorrect(VD); + } // update state covariance // http://en.wikipedia.org/wiki/Extended_Kalman_filter