diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 48 |
1 files changed, 47 insertions, 1 deletions
diff --git a/src/state.cpp b/src/state.cpp index 937a653..8a31535 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -204,7 +204,7 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S, // Get the update Matrix<double,Dynamic,1> dx; dx = K*y; - if (dx.segment<3>(3).norm()>0.04) { + if (dx.segment<3>(3).norm()>1) { cerr << "dx: " << dx.head<STATESIZE>().transpose() << endl; return K; } @@ -1035,3 +1035,49 @@ State::quaternion_covariance ( const Matrix<double,4,4> &covq ) } /* ----- end of method State::quaternion_covariance ----- */ #endif + +/* + *-------------------------------------------------------------------------------------- + * Class: State + * Method: State :: featuresAsMeasurements + * Description: Writes the feature locations to a measurement_t vector in body + * coordinates. + *-------------------------------------------------------------------------------------- + */ +void +#if STATESIZE==13 +State::featuresAsMeasurements ( std::vector<measurement_t> &yk ) +#else +State::featuresAsMeasurements ( std::vector<measurement_t> &yk, + const Quaterniond &q) +#endif +{ +#if STATESIZE==13 + Quaterniond q = body->qhat(); +#endif + for (auto i=features.begin(); i!=features.end(); ++i) { + // Get the predicted measurement + Matrix<double,6,1> h; + h = (*i)->h(body->ned(), q); + + Vector3d pbs, pbr; + pbs << h(0), h(1), 1; + pbr << h(4), h(5), 1; + + measurement_t z; + z.id = 0; + z.source = (*i)->p2x(pbs); + z.reflection = (*i)->p2x(pbr); + z.z_type = REFLECTION; + + // Get ellipse + Matrix<double,6,6> s; + s = (*i)->S(Pxx(), Pxy((*i)->id()), Pyy((*i)->id()), body->ned(), q); + z.Ssrc << s(0,0), s(1,1); + z.Sref << s(4,4), s(5,5); + + yk.push_back(z); + } + return ; +} /* ----- end of method State::featuresAsMeasurements ----- */ + |