diff options
| author | James Goppert <james.goppert@gmail.com> | 2013-01-17 12:18:20 -0500 |
|---|---|---|
| committer | James Goppert <james.goppert@gmail.com> | 2013-01-17 12:18:20 -0500 |
| commit | 13bb814f2015154cb5cadb814e37db1b6b2e634b (patch) | |
| tree | 74473907e30f384a772b46d5c38d9d5f6658a402 | |
| parent | c2c0baf843aa376681fcf7bf9dfcac480177b7fb (diff) | |
| download | px4-firmware-13bb814f2015154cb5cadb814e37db1b6b2e634b.tar.gz px4-firmware-13bb814f2015154cb5cadb814e37db1b6b2e634b.tar.bz2 px4-firmware-13bb814f2015154cb5cadb814e37db1b6b2e634b.zip | |
Prevented attitude correction from changing velocity when pos not init.
| -rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 8 |
1 files changed, 5 insertions, 3 deletions
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 15254f7c7..40166f3a1 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 |