diff options
| -rw-r--r-- | src/body.cpp | 9 | ||||
| -rw-r--r-- | src/body.h | 2 | ||||
| -rw-r--r-- | src/state.cpp | 2 |
3 files changed, 7 insertions, 6 deletions
diff --git a/src/body.cpp b/src/body.cpp index 1fd66d4..e6afe8a 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -20,21 +20,22 @@ /* *-------------------------------------------------------------------------------------- * Class: Body - * Method: Body :: update + * Method: Body :: dx * Description: Increments the state by dx after a Kalman update. *-------------------------------------------------------------------------------------- */ void -Body::update ( const Matrix<double,9,1> &dx ) +Body::dx ( const Matrix<double,9,1> &del ) { - X += dx; + X += del; + // Constrain the height if (X[2]<-1.) { X[2]=-1.; } else if (X[2]>-0.2) { X[2]=-0.2; } return ; -} /* ----- end of method Body::update ----- */ +} /* ----- end of method Body::dx ----- */ void Body::vel ( const Matrix<double,3,1> &v ) @@ -27,7 +27,7 @@ class Body /* ==================== MUTATORS ======================================= */ void accelerometer_bias( const Vector3d &b); void pos( const UTM &utm); - void update(const Matrix<double,9,1> &dx); + void dx( const Matrix<double,9,1> &del); void vel(const Matrix<double,3,1> &v); /* ==================== OPERATORS ======================================= */ diff --git a/src/state.cpp b/src/state.cpp index 281d22e..1df68eb 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -172,7 +172,7 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, y = innovation(z,q); Matrix<double,Dynamic,1> dx; dx = K*y; - body->update(dx.segment<9>(0)); + body->dx(dx.segment<9>(0)); { int row=9; for (auto i=features.begin(); i!=features.end(); ++i, row+=3) { |