diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/feature.cpp | 63 | ||||
| -rw-r--r-- | src/feature.h | 12 |
2 files changed, 65 insertions, 10 deletions
diff --git a/src/feature.cpp b/src/feature.cpp index e44d597..b83f8ee 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -21,6 +21,47 @@ /* *-------------------------------------------------------------------------------------- * Class: Feature + * Method: Feature :: S + * Description: Returns S, the information matrix for an independent + * measurement of the feature. For correlated measurements, combine Hx and Hy + * with other measurements outside of the instance and compute S. + *-------------------------------------------------------------------------------------- + */ +Matrix<double,6,6> +Feature::S ( const Matrix<double,9,9> &Pxx, const Matrix<double,9,3> &Pxy, + const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q) +{ + Matrix<double,6,9> hx; + Matrix<double,6,3> hy; + hx = Hx( pos, q); + hy = Hy( pos, q); + Matrix<double,6,6> S; + S = hx*Pxx*hx.transpose() + 2*hx*Pxy*hy.transpose() + hy*Pyy*hy.transpose() + R(); + + return S; +} /* ----- end of method Feature::S ----- */ + +Matrix<double,3,3> +Feature::Q ( const double dt ) +{ + Matrix<double,3,3> Q; + Q = Matrix<double,3,3>::Identity(); + Q *= dt*dt*1e-5; + return Q; +} /* ----- end of method Feature::q ----- */ + +Matrix<double,6,6> +Feature::R ( ) +{ + Matrix<double,6,6> R; + R = Matrix<double,6,6>::Identity(); + R *= 1e-9; + return R; +} /* ----- end of method Feature::R ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Feature * Method: Feature :: L * Description: Returns the motion model matrix L. These can be stacked to * update the states of many features simultaneously. @@ -52,14 +93,14 @@ Feature::L ( ) void Feature::motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt) { - Matrix<double,3,6> L; - L = L(); + Matrix<double,3,6> f; + f = 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; + ydot = f*V; X += ydot*dt; return ; } /* ----- end of method Feature::motionModel ----- */ @@ -138,6 +179,14 @@ Feature::x2p ( const Vector3d &x ) +/* + *-------------------------------------------------------------------------------------- + * Class: Feature + * Method: Feature :: Fx + * Description: Returns the Jacobian of the motion model with respect to the + * body. + *-------------------------------------------------------------------------------------- + */ Matrix<double,3,9> Feature::Fx ( ) { @@ -152,6 +201,14 @@ Feature::Fx ( ) return F; } /* ----- end of method Feature::Fx ----- */ +/* + *-------------------------------------------------------------------------------------- + * Class: Feature + * Method: Feature :: Fy + * Description: Returns the Jacobian of the motion model with respect to the + * feature. + *-------------------------------------------------------------------------------------- + */ Matrix<double,3,3> Feature::Fy ( const Vector3d &v, const Vector3d &w ) { diff --git a/src/feature.h b/src/feature.h index 971c952..2a492c2 100644 --- a/src/feature.h +++ b/src/feature.h @@ -5,6 +5,7 @@ using Eigen::Vector3d; using Eigen::Matrix; +using Eigen::MatrixXd; using Eigen::Quaterniond; /* @@ -32,13 +33,10 @@ class Feature Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q); Matrix<double,3,6> L(); - /* - Matrix<double,9,9> Q(double dt); - Matrix<double,1,1> R(); - Matrix<double,1,1> S(const Matrix<double,9,9> &P); - void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y); - void unicsv(); - */ + Matrix<double,3,3> Q(const double dt); + Matrix<double,6,6> R(); + Matrix<double,6,6> S ( const Matrix<double,9,9> &Pxx, const Matrix<double,9,3> &Pxy, + const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q); protected: /* ==================== METHODS ======================================= */ |