diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 25 |
1 files changed, 24 insertions, 1 deletions
diff --git a/src/state.cpp b/src/state.cpp index dfe41fa..e53ad29 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -464,7 +464,7 @@ void #if STATESIZE==13 State::addFeatures ( std::vector<measurement_t> &F, double z) #else -State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z); +State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z) #endif { #if STATESIZE==13 @@ -1008,3 +1008,26 @@ State::asVector ( const Matrix<double,Dynamic,1> &m ) return ; } /* ----- end of method State::asVector ----- */ +#if STATESIZE==13 +Quaterniond +State::qhat ( ) +{ + return body->qhat() ; +} /* ----- end of method State::qhat ----- */ + +void +State::qhat ( const Quaterniond &q ) +{ + body->qhat(q); + return ; +} /* ----- end of method State::qhat ----- */ + + +void +State::quaternion_covariance ( ) +{ + P.block<4,4>(6,6) = 1e-5*Matrix<double,4,4>::Identity(); + return ; +} /* ----- end of method State::quaternion_covariance ----- */ + +#endif |