diff options
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 18 |
1 files changed, 16 insertions, 2 deletions
diff --git a/src/body.cpp b/src/body.cpp index 330c0a8..4741911 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -19,6 +19,20 @@ #include <Eigen/Dense> #include "body.h" +Matrix<double,9,9> +Body::Q (double dt) +{ + Matrix<double,9,9> Q; + Q = Matrix<double,9,9>::Zero(); + Q.block(0,0,3,3) = 0.25*dt*dt*dt*dt*Matrix<double,3,3>::Identity(); + Q.block(3,0,3,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity(); + Q.block(0,3,3,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(6,6,3,3) = dt*dt*5e-8*Matrix<double,3,3>::Identity(); + return Q; +} /* ----- end of method Body::q ----- */ + /* *-------------------------------------------------------------------------------------- * Class: Body @@ -56,7 +70,7 @@ Body::h ( const Matrix<double,9,1> &X, Matrix<double,1,1> &h ) * Description: Computes the Jacobian of the motion model of the body. *-------------------------------------------------------------------------------------- */ -void +Matrix<double,9,9> Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q ) { Matrix<double,9,9> F = Matrix<double,9,9>::Zero(); @@ -66,7 +80,7 @@ Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<dou F.block(0,3,3,3) = Rbw; F.block(3,3,3,3) = -W; F.block(3,6,3,3) = -Matrix<double,3,3>::Identity(); - return ; + return F; } /* ----- end of method Body::F ----- */ /* |