diff options
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 69 |
1 files changed, 67 insertions, 2 deletions
diff --git a/src/body.cpp b/src/body.cpp index daf4e69..330c0a8 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -19,7 +19,64 @@ #include <Eigen/Dense> #include "body.h" - void +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: H + * Description: Writes the Jacobian of the measurement model into H. + *-------------------------------------------------------------------------------------- + */ +void +Body::H ( Matrix<double,1,9> &H ) +{ + H = Matrix<double,1,9>::Zero(); + H[0,2] = 1; + return ; +} /* ----- end of method Body::H ----- */ + + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: h + * Description: Writes the predicted measurement vector into h. + *-------------------------------------------------------------------------------------- + */ +void +Body::h ( const Matrix<double,9,1> &X, Matrix<double,1,1> &h ) +{ + h[0] = X[2]; + return ; +} /* ----- end of method Body::h_hat ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: F + * Description: Computes the Jacobian of the motion model of the body. + *-------------------------------------------------------------------------------------- + */ +void +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(); + Matrix<double,3,3> Rbw(q.toRotationMatrix()); + Matrix<double,3,3> W; + skewSymmetric(ang,W); + 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 ; +} /* ----- end of method Body::F ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: motionModel + * Description: Propagates the motion model of the body into vector X. + *-------------------------------------------------------------------------------------- + */ +void Body::motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt) { Vector3d bias(X.segment(6,3)); @@ -41,10 +98,18 @@ Body::motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d & return ; } /* ----- end of method Body::motionModel ----- */ - void +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: skewSymmetric + * Description: Create the skew symmetric matrix y from the vector x. + *-------------------------------------------------------------------------------------- + */ +void Body::skewSymmetric ( const Vector3d &x, Matrix<double,3,3> &y ) { y << 0.,-x[2], x[1], x[2],0.,-x[0],-x[1],x[0],0.; return ; } /* ----- end of method Body::skewSymmetric ----- */ + |