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