diff options
Diffstat (limited to 'src/state.cpp')
-rw-r--r-- | src/state.cpp | 58 |
1 files changed, 47 insertions, 11 deletions
diff --git a/src/state.cpp b/src/state.cpp index 9a2a6b3..f2344f0 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -60,26 +60,36 @@ State::update ( const Matrix<double,1,1> &z ) } void -State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q ) +State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q) { std::vector<measurement_t> featuresToAdd; for (auto i=z.begin(); i!=z.end(); ++i) { if (exists(i->id)) { - printf("Exists!\n"); + ; } else { featuresToAdd.push_back(*i); } } + addFeatures( featuresToAdd, q); - // Expand P matrix - expandP(featuresToAdd.size()); + return ; +} /* ----- end of method State::feature_update ----- */ +void +State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q) +{ + // Expand P matrix all at once + expandP(F.size()); + // // Add new features - for (auto i=featuresToAdd.begin(); i!=featuresToAdd.end(); ++i) { + for (auto i=F.begin(); i!=F.end(); ++i) { + // Estimate initial depth + Vector3d xib; + // Create feature - Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q); + Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, -0.44); features.push_back(f); - + // Initialize P values int j=features.size(); Matrix<double,3,3> Pi; @@ -87,7 +97,7 @@ State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond & initializePi(j,Pi); } return ; -} /* ----- end of method State::feature_update ----- */ +} /* ----- end of method State::addFeatures ----- */ /* *-------------------------------------------------------------------------------------- @@ -109,6 +119,30 @@ State::initializePi ( int i, const Matrix<double,3,3> &Pi ) return ; } /* ----- end of method State::initializePi ----- */ +MatrixXd +State::F ( const Quaterniond &q, const Vector3d &w, double dt ) +{ + Vector3d v; + v = body->vel(); + // Allocate matrix F + MatrixXd f; + int rows = 9 + 3*features.size(); + f = MatrixXd::Zero(rows,rows); + + // Set body F + f.topLeftCorner<9,9>() = body->F(w,q,dt); + // Set Fxi Fyi + { // limit i's scope + auto i = features.begin(); + for (int row=9; row<rows; row+=3,++i) { + f.block<3,9>(row,0) = (*i)->Fx(dt); + f.block<3,3>(row,row) = (*i)->Fy(v,w,dt); + } + } + + return f ; +} /* ----- end of method State::F ----- */ + /* *-------------------------------------------------------------------------------------- * Class: State @@ -119,10 +153,12 @@ State::initializePi ( int i, const Matrix<double,3,3> &Pi ) void State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt) { - Matrix<double,9,9> F,Q; + Matrix<double,9,9> Q; Q = body->Q(dt); - F = body->F(ang,q,dt); - P = F*P*F.transpose()+Q; + MatrixXd f; + f = F ( q, ang, dt ); + //cout << f << endl; + P = f*P*f.transpose()+Q; // Enforce symmetry P = 0.5*(P+P.transpose()); return ; |