diff options
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 108 |
1 files changed, 97 insertions, 11 deletions
diff --git a/src/body.cpp b/src/body.cpp index c4d8a82..2832ea4 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -15,10 +15,71 @@ * * ===================================================================================== */ - -#include <Eigen/Dense> #include "body.h" +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: update + * Description: Increments the state by dx after a Kalman update. + *-------------------------------------------------------------------------------------- + */ +void +Body::update ( const Matrix<double,9,1> &dx ) +{ + X += dx; + return ; +} /* ----- end of method Body::update ----- */ + +void +Body::vel ( const Matrix<double,3,1> &v ) +{ + X.segment(3,3) = v; + return ; +} /* ----- end of method Body::vel ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: S + * Description: Returns the matrix S=HPH.T+R. HPH.T is trivial, so Body::H() is + * not called explicitly. + *-------------------------------------------------------------------------------------- + */ +Matrix<double,1,1> +Body::S ( const Matrix<double,9,9> &P ) +{ + Matrix<double,1,1> S; + S << P(2,2); + return S+R(); +} /* ----- end of method Body::S ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: enu + * Description: Stores the position input as ENU. Internally stored as NED. + *-------------------------------------------------------------------------------------- + */ +void +Body::enu ( const UTM &utm ) +{ + utm_c = utm.zone_c; + utm_i = utm.zone_i; + X[0] = utm.northing; + X[1] = utm.easting; + X[2] = -utm.up; + return ; +} /* ----- end of method Body::enu ----- */ + + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: R + * Description: Returns R matrix of measurement noise for height measurement. + *-------------------------------------------------------------------------------------- + */ Matrix<double,1,1> Body::R() { @@ -27,6 +88,13 @@ Body::R() return R; } +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: Q + * Description: Returns Q matrix of process noise for the body. + *-------------------------------------------------------------------------------------- + */ Matrix<double,9,9> Body::Q (double dt) { @@ -45,15 +113,16 @@ Body::Q (double dt) *-------------------------------------------------------------------------------------- * Class: Body * Method: Body :: H - * Description: Writes the Jacobian of the measurement model into H. + * Description: Returns the Jacobian of the measurement model, H. *-------------------------------------------------------------------------------------- */ -void -Body::H ( Matrix<double,1,9> &H ) +Matrix<double,1,9> +Body::H ( ) { + Matrix<double,1,9> H; H = Matrix<double,1,9>::Zero(); H[0,2] = 1; - return ; + return H ; } /* ----- end of method Body::H ----- */ @@ -61,11 +130,11 @@ Body::H ( Matrix<double,1,9> &H ) *-------------------------------------------------------------------------------------- * Class: Body * Method: Body :: h - * Description: Writes the predicted measurement vector into h. + * Description: Returns the predicted measurement vector, h. *-------------------------------------------------------------------------------------- */ Matrix<double,1,1> -Body::h ( const Matrix<double,9,1> &X) +Body::h ( ) { Matrix<double,1,1> h; h[0] = X[2]; @@ -76,11 +145,11 @@ Body::h ( const Matrix<double,9,1> &X) *-------------------------------------------------------------------------------------- * Class: Body * Method: Body :: F - * Description: Computes the Jacobian of the motion model of the body. + * Description: Returns the Jacobian of the motion model of the body. *-------------------------------------------------------------------------------------- */ Matrix<double,9,9> -Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q ) +Body::F ( const Vector3d &ang, const Quaternion<double> &q ) { Matrix<double,9,9> F = Matrix<double,9,9>::Zero(); Matrix<double,3,3> Rbw(q.toRotationMatrix()); @@ -100,7 +169,7 @@ Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<dou *-------------------------------------------------------------------------------------- */ void -Body::motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt) +Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt) { Vector3d bias(X.segment(6,3)); Vector3d gravity_world(0.,0.,9.80655); @@ -136,3 +205,20 @@ Body::skewSymmetric ( const Vector3d &x, Matrix<double,3,3> &y ) } /* ----- end of method Body::skewSymmetric ----- */ +void +Body::unicsv ( ) +{ + printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", utm_i, utm_c, + X[1], X[0], -X[2], + X[3], X[4], X[5], + X[6], X[7], X[8]); + return ; +} /* ----- end of method Body::unicsv ----- */ + + void +Body::accelerometer_bias ( const Vector3d &b ) +{ + X.segment(6,3) = b; + return ; +} /* ----- end of method State::accelerometer_bias ----- */ + |