From e0734384252675e2a37f4d9287184cc48ab68b05 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Mon, 27 Mar 2017 14:23:43 -0500 Subject: EKF is working --- src/body.cpp | 10 +++---- src/main.cpp | 2 +- src/state.cpp | 88 ++++++++++++++++++++++++++++++++--------------------------- src/state.h | 7 ++--- 4 files changed, 57 insertions(+), 50 deletions(-) diff --git a/src/body.cpp b/src/body.cpp index e6afe8a..ff54e89 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -40,7 +40,7 @@ Body::dx ( const Matrix &del ) void Body::vel ( const Matrix &v ) { - X.segment(3,3) = v; + X.segment<3>(3) = v; return ; } /* ----- end of method Body::vel ----- */ @@ -127,7 +127,7 @@ Body::H ( ) { Matrix H; H = Matrix::Zero(); - H[0,2] = 1; + H(2) = 1.; return H ; } /* ----- end of method Body::H ----- */ @@ -192,9 +192,9 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond Matrix W; W = skewSymmetric(ang); A.block<3,3>(3,0) = -W; - b.segment(3,3) = acc-bias+Rbw.transpose()*gravity_world; + b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world; - X.segment(0,6) += (A*X.segment(3,3)+b)*dt; + X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt; return ; } /* ----- end of method Body::motionModel ----- */ @@ -226,7 +226,7 @@ Body::unicsv ( ) void Body::accelerometer_bias ( const Vector3d &b ) { - X.segment(6,3) = b; + X.segment<3>(6) = b; return ; } /* ----- end of method State::accelerometer_bias ----- */ diff --git a/src/main.cpp b/src/main.cpp index bb2a3b3..897a319 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -98,7 +98,7 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q) if (fclose(fin)==EOF) { err_sys("fclose"); } - mu.feature_update(z,q); + mu.handle_measurements(z,q); } return; diff --git a/src/state.cpp b/src/state.cpp index 1df68eb..c33c053 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -21,7 +21,7 @@ State::State ( ) { body = new Body; P = Matrix::Zero(9,9); - P.block(6,6,3,3) = 1e-9*Matrix3d::Identity(); + P.block<3,3>(6,6) = 1e-9*Matrix3d::Identity(); return ; } /* ----- end of method State::State ----- */ @@ -63,13 +63,14 @@ State::H ( const Quaterniond &q, const std::vector &z ) 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); } + Feature *fi; + fi = featureById(z[i].id); + h.block<6,9>(row,0) = fi->Hx(pos,q); h.block<6,3>(row,col) = fi->Hy(pos,q); row += 6; @@ -92,6 +93,14 @@ State::H ( const Quaterniond &q, const std::vector &z ) return h; } /* ----- end of method State::H ----- */ +/* + *-------------------------------------------------------------------------------------- + * Class: State + * Method: State :: Hrows + * Description: Returns the number of rows in the measurement vector based on + * the number and types of measurements. + *-------------------------------------------------------------------------------------- + */ int State::Hrows ( const std::vector &z) { @@ -117,6 +126,13 @@ State::Hrows ( const std::vector &z) return rows ; } /* ----- end of method State::Hrows ----- */ +/* + *-------------------------------------------------------------------------------------- + * Class: State + * Method: State :: R + * Description: Returns the measurement noise matrix. + *-------------------------------------------------------------------------------------- + */ MatrixXd State::R ( const std::vector &z ) { @@ -165,11 +181,16 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, MatrixXd K; // P^T is implied here since P is symmetric K = S.partialPivLu().solve(h*P).transpose(); + + // Compute P_k|k P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P; P = 0.5*(P+P.transpose()); + // Compute the innovation or error Matrix y; y = innovation(z,q); + + // Get the update Matrix dx; dx = K*y; body->dx(dx.segment<9>(0)); @@ -187,7 +208,7 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, *-------------------------------------------------------------------------------------- * Class: State * Method: State :: innovation - * Description: Returns the innocation vector z-hhat + * Description: Returns the innovation vector z-hhat *-------------------------------------------------------------------------------------- */ Matrix @@ -227,31 +248,15 @@ State::innovation( const std::vector &z, const Quaterniond &q) return y; } -void -State::update ( const Quaterniond &q, const std::vector &z ) -{ -#ifdef FULLS - MatrixXd h; - h = H(q,z); - MatrixXd S; - S = h*P*h.transpose() + R(z); - kalmanUpdate(h,S,z,q); - cout << features.size() << endl; - -#else /* ----- not FULLS ----- */ - -#endif /* ----- not FULLS ----- */ - /* - Matrix dx; - Matrix h; - h = body->h(); - dx = K*(z-h); - body->update(dx); - */ -} - +/* + *-------------------------------------------------------------------------------------- + * Class: State + * Method: State :: handle_measurements + * Description: Update EKF, and adds new features to state. + *-------------------------------------------------------------------------------------- + */ void -State::feature_update ( const std::vector &z, const Quaterniond &q) +State::handle_measurements ( const std::vector &z, const Quaterniond &q) { std::vector featuresToAdd; std::vector featuresToUpdate; @@ -265,7 +270,15 @@ State::feature_update ( const std::vector &z, const Quaterniond & } } if (featuresToUpdate.size()>0) { - update(q, featuresToUpdate); +#ifdef FULLS + MatrixXd h; + h = H(q,featuresToUpdate); + MatrixXd S; + S = h*P*h.transpose() + R(featuresToUpdate); + kalmanUpdate(h,S,featuresToUpdate,q); +#else /* ----- not FULLS ----- */ + +#endif /* ----- not FULLS ----- */ } addFeatures( featuresToAdd, q); @@ -276,11 +289,11 @@ void State::addFeatures ( std::vector &F, const Quaterniond &q) { // Add new features + Vector3d pos = body->ned(); for (auto i=F.begin(); i!=F.end(); ++i) { if (features.size()>MAXFEATURES) break; // Create feature - Vector3d pos = body->ned(); - Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, pos[2]); + Feature *f = new Feature(i->id, i->source, i->reflection, pos, q); if (!f->sane()) { delete f; continue; @@ -288,13 +301,7 @@ State::addFeatures ( std::vector &F, const Quaterniond &q) features.push_back(f); // Expand P - expandP(); - - // Initialize P values - int j=features.size(); - Matrix Pi; - Pi = f->P0(); - initializePi(j,Pi); + expandP(f->P0()); } return ; } /* ----- end of method State::addFeatures ----- */ @@ -564,9 +571,10 @@ State::featureById ( int id ) *-------------------------------------------------------------------------------------- */ void -State::expandP ( ) +State::expandP ( const Matrix3d &Pi) { - P.conservativeResize(3+P.rows(),3+P.cols()); + P.conservativeResizeLike(MatrixXd::Zero(3+P.cols(),3+P.cols())); + P.bottomRightCorner<3,3>() = Pi; return ; } /* ----- end of method State::expandP ----- */ diff --git a/src/state.h b/src/state.h index ea8cfef..4001216 100644 --- a/src/state.h +++ b/src/state.h @@ -9,7 +9,7 @@ #include "feature.h" #include "types.h" -#define MAXFEATURES 30 +#define MAXFEATURES 50 //#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::Dynamic; @@ -48,7 +48,7 @@ class State /* ==================== MUTATORS ======================================= */ void accelerometer_bias(const Vector3d &b); void pos(const UTM &utm); - void feature_update( const std::vector & z, const Quaterniond &q); + void handle_measurements( const std::vector & z, const Quaterniond &q); void initializePi(int i, const Matrix &Pi); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, const std::vector &z, const Quaterniond &q); @@ -59,7 +59,6 @@ class State std::list::iterator removeFeature(std::list::iterator &i, int j); void velocity_covariance(const Matrix &p); void vel(const Matrix &v); - void update ( const Quaterniond &q, const std::vector &z ); /* ==================== OPERATORS ======================================= */ Matrix innovation( const std::vector &z, const Quaterniond &q); @@ -73,7 +72,7 @@ class State private: /* ==================== METHODS ======================================= */ void addFeatures(std::vector &F, const Quaterniond &q); - void expandP(); + void expandP ( const Matrix3d &Pi); void shrinkP( int i ); /* ==================== DATA MEMBERS ======================================= */ -- cgit v1.1