diff options
| -rw-r--r-- | src/feature.cpp | 46 | ||||
| -rw-r--r-- | src/feature.h | 5 |
2 files changed, 49 insertions, 2 deletions
diff --git a/src/feature.cpp b/src/feature.cpp index 45e24e7..e44d597 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -18,6 +18,52 @@ */ #include "feature.h" +/* + *-------------------------------------------------------------------------------------- + * Class: Feature + * Method: Feature :: L + * Description: Returns the motion model matrix L. These can be stacked to + * update the states of many features simultaneously. + *-------------------------------------------------------------------------------------- + */ +Matrix<double,3,6> +Feature::L ( ) +{ + double y0,y1,y2; + Matrix<double,3,6> L; + y0 = X[0]; + y1 = X[1]; + y2 = X[2]; + + L << -y2, 0, y2*y0, y0*y1, -(1+y0*y0), y1, + 0, -y2, y1*y2, 1+y1*y1, -y0*y1, -y0, + 0, 0, y2*y2, y2*y1, -y2*y0, 0; + + return L; +} /* ----- end of method Feature::L ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Feature + * Method: Feature :: motionModel + * Description: Updates the feature state according to the motion model. + *-------------------------------------------------------------------------------------- + */ +void +Feature::motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt) +{ + Matrix<double,3,6> L; + L = L(); + + Matrix<double,6,1> V; + V << vel[1], vel[2], vel[0], ang[1], ang[2], ang[0]; + + Matrix<double,3,1> ydot; + ydot = L*V; + X += ydot*dt; + return ; +} /* ----- end of method Feature::motionModel ----- */ + Matrix<double,6,1> Feature::h ( const Vector3d &x, const Quaterniond &q ) { diff --git a/src/feature.h b/src/feature.h index 2982276..971c952 100644 --- a/src/feature.h +++ b/src/feature.h @@ -22,6 +22,7 @@ class Feature /* ==================== ACCESSORS ======================================= */ /* ==================== MUTATORS ======================================= */ + void motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt); /* ==================== OPERATORS ======================================= */ Matrix<double,3,9> Fx(); @@ -29,9 +30,9 @@ class Feature Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q); Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q); Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q); + Matrix<double,3,6> L(); + /* - void motionModel ( const Vector3d &acc, const Vector3d &ang, - const Quaterniond &q, const double dt); Matrix<double,9,9> Q(double dt); Matrix<double,1,1> R(); Matrix<double,1,1> S(const Matrix<double,9,9> &P); |