diff options
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; |