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/state.cpp | 88 ++++++++++++++++++++++++++++++++--------------------------- 1 file changed, 48 insertions(+), 40 deletions(-) (limited to 'src/state.cpp') 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 ----- */ -- cgit v1.1