diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 10 |
1 files changed, 5 insertions, 5 deletions
diff --git a/src/state.cpp b/src/state.cpp index 57ebbfa..998be20 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -21,7 +21,7 @@ State::State ( ) { body = new Body; P = Matrix<double,Dynamic,Dynamic>::Zero(9,9); - P.block(6,6,3,3) = 1e-6*Matrix3d::Identity(); + P.block(6,6,3,3) = 1e-9*Matrix3d::Identity(); return ; } /* ----- end of method State::State ----- */ @@ -204,9 +204,7 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q) fi = featureById(z[i].id); Matrix<double,6,1> hi, zi; hi = fi->h(body->ned(), q); - zi.segment<2>(0) = z[i].source.segment<2>(0); - zi.segment<2>(2) = fi->initialView(); - zi.segment<2>(4) = z[i].reflection.segment<2>(0); + zi = fi->measurement_vector(z[i].source, z[i].reflection); y.segment<6>(row) = zi-hi; row += 6; } else if (mt==MONO) { @@ -238,6 +236,7 @@ State::update ( const Quaterniond &q, const std::vector<measurement_t> &z ) MatrixXd S; S = h*P*h.transpose() + R(z); kalmanUpdate(h,S,z,q); + cout << features.size() << endl; #else /* ----- not FULLS ----- */ @@ -280,7 +279,8 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q) for (auto i=F.begin(); i!=F.end(); ++i) { if (features.size()>MAXFEATURES) break; // Create feature - Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, -0.44); + Vector3d pos = body->ned(); + Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, pos[2]); if (!f->sane()) { delete f; continue; |