diff options
| author | Martin Miller | 2017-06-26 17:12:54 -0500 |
|---|---|---|
| committer | Martin Miller | 2017-06-26 17:12:54 -0500 |
| commit | 6c03b6e316d72d2bf8cf35a5b7890c646719ff48 (patch) | |
| tree | b8034eed498978b36bde7ed82e4a302bb171403d /src/body.cpp | |
| parent | a9a08ac971b8057e6e26bf781cb453edbcc71121 (diff) | |
| download | refslam-6c03b6e316d72d2bf8cf35a5b7890c646719ff48.zip refslam-6c03b6e316d72d2bf8cf35a5b7890c646719ff48.tar.gz | |
10pct mean error
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 3 |
1 files changed, 2 insertions, 1 deletions
diff --git a/src/body.cpp b/src/body.cpp index 885b2ad..018075b 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -125,11 +125,12 @@ Body::Q (double dt) Gb.block<3,3>(0,0) = -0.5*dt*dt*Matrix3d::Identity(); Gb.block<3,3>(3,0) = -dt*Matrix3d::Identity(); Gb.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero(); + Gb.block<3,3>(STATESIZE-3,0) = dt*Matrix3d::Identity(); Matrix<double,STATESIZE,3> Gw; Gw.block<3,3>(0,0) = 0.5*dt*dt*skewSymmetric(vel()); Gw.block<3,3>(3,0) = dt*skewSymmetric(vel()); - Gb.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero(); + Gw.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero(); #if STATESIZE==13 Matrix<double,STATESIZE,3> Gq; |