diff options
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 6 |
1 files changed, 4 insertions, 2 deletions
diff --git a/src/body.cpp b/src/body.cpp index f27a62f..e6bfb6c 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -105,7 +105,7 @@ Body::Q (double dt) Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity(); Q.block<3,3>(3,3) = dt*dt*Matrix<double,3,3>::Identity(); Q *= 800e-6; - Q.block<3,3>(6,6) = dt*dt*5e-8*Matrix<double,3,3>::Identity(); + Q.block<3,3>(6,6) = dt*dt*5e-3*Matrix<double,3,3>::Identity(); return Q; } /* ----- end of method Body::q ----- */ @@ -149,7 +149,7 @@ Body::h ( ) *-------------------------------------------------------------------------------------- */ Matrix<double,9,9> -Body::F ( const Vector3d &ang, const Quaterniond &q ) +Body::F ( const Vector3d &ang, const Quaterniond &q, double dt ) { Matrix<double,9,9> F = Matrix<double,9,9>::Zero(); Matrix<double,3,3> Rbw(q.toRotationMatrix()); @@ -158,6 +158,8 @@ Body::F ( const Vector3d &ang, const Quaterniond &q ) F.block<3,3>(0,3) = Rbw; F.block<3,3>(3,3) = -W; F.block<3,3>(3,6) = -Matrix<double,3,3>::Identity(); + F *= dt; + F += Matrix<double,9,9>::Identity(); return F; } /* ----- end of method Body::F ----- */ |