diff options
-rw-r--r-- | src/state.cpp | 104 | ||||
-rw-r--r-- | src/state.h | 6 |
2 files changed, 105 insertions, 5 deletions
diff --git a/src/state.cpp b/src/state.cpp index dae6a4d..281fac3 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -41,11 +41,79 @@ State::enu ( const UTM &utm ) return ; } /* ----- end of method State::enu ----- */ +/* + *-------------------------------------------------------------------------------------- + * Class: State + * Method: State :: H + * Description: Returns the matrix H for the measurements z + *-------------------------------------------------------------------------------------- + */ +MatrixXd +State::H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ) +{ + MatrixXd h; + // Determine the size of H + int rows = 0; + for (auto i=z.begin(); i!=z.end(); ++i) { + switch ( i->z_type ) { + case REFLECTION: + rows += 6; + break; + + case MONO: + rows += 4; + break; + + case HEIGHT: + rows += 1; + break; + + default: + break; + } /* ----- end switch ----- */ + } + int cols = 9 + 3*features.size(); + h = MatrixXd::Zero(rows,cols); + for (int i=0,row=0; i<z.size(); ++i) { + switch ( z[i].z_type ) { + int col; + case REFLECTION: + col = rowById(z[i].id); + Feature *fi; + fi = featureById(z[i].id); + if (col==-1) { + fprintf(stderr, "Feature %d not found, quitting.\n", z[i].id); + exit(1); + } + + h.block<6,9>(row,0) = fi->Hx(pos,q); + h.block<6,3>(row,col) = fi->Hy(pos,q); + row += 6; + break; + + case MONO: + fprintf(stderr, "mono points not supported.\n"); + exit(1); + break; + + case HEIGHT: + h.block<1,9>(row,0) = body->H(); + row += 1; + break; + + default: + break; + } /* ----- end switch ----- */ + } + return h; +} /* ----- end of method State::H ----- */ + void -State::update ( const Matrix<double,1,1> &z ) +State::update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ) { - Matrix<double,1,9> H; - H=body->H(); + MatrixXd h; + h = H(pos,q,z); + /* Matrix<double,1,1> S; S=body->S(Pxx()); Matrix<double,9,1> K; @@ -57,6 +125,7 @@ State::update ( const Matrix<double,1,1> &z ) h = body->h(); dx = K*(z-h); body->update(dx); + */ } void @@ -196,7 +265,7 @@ void State::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt) { - //Pkk1(ang,q,dt); + Pkk1(ang,q,dt); body->motionModel(acc,ang,q,dt); Vector3d vel; @@ -334,6 +403,33 @@ State::exists ( int id ) /* *-------------------------------------------------------------------------------------- * Class: State + * Method: State :: rowById + * Description: Returns the first row of the feature with the given ID. Returns + * -1 if not found. + *-------------------------------------------------------------------------------------- + */ +int +State::rowById ( int id ) +{ + int j=9; + for (auto i=features.begin(); i!=features.end(); ++i, j+=3) { + if ((*i)->id()==id) return j; + } + return -1; +} /* ----- end of method State::rowById ----- */ + +Feature * +State::featureById ( int id ) +{ + for (auto i=features.begin(); i!=features.end(); ++i) { + if ((*i)->id()==id) return *i; + } + return NULL; +} /* ----- end of method State::featureById ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: State * Method: State :: expandP * Description: Expand the P matrix to fit one more feature. *-------------------------------------------------------------------------------------- diff --git a/src/state.h b/src/state.h index db45ba2..43342a7 100644 --- a/src/state.h +++ b/src/state.h @@ -11,6 +11,7 @@ #define MAXFEATURES 30 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once +#define FULLS // Comment out to treat each measurement independently. using Eigen::Matrix; using Eigen::MatrixXd; using Eigen::Dynamic; @@ -33,7 +34,10 @@ class State /* ==================== ACCESSORS ======================================= */ bool exists(int id); + int rowById(int id); + Feature *featureById(int id); MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); + MatrixXd H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ); MatrixXd Q(double dt); Matrix<double,9,9> Pxx(); Matrix<double,Dynamic,6> L(); @@ -50,7 +54,7 @@ class State std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, int j); void velocity_covariance(const Matrix<double,3,3> &p); void vel(const Matrix<double,3,1> &v); - void update ( const Matrix<double,1,1> &z); + void update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ); /* ==================== OPERATORS ======================================= */ void unicsv(); |