diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 13 |
1 files changed, 13 insertions, 0 deletions
diff --git a/src/state.cpp b/src/state.cpp index 8aadb64..294b522 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -17,6 +17,19 @@ */ #include "state.h" +void +State::update (Matrix<double,9,1> &X, Matrix<double,9,9> &P, + const Matrix<double,1,9> H, const Matrix<double,1,1> &h, + const Matrix<double,1,1> &z, const Matrix<double,1,1> &R) +{ + Matrix<double,1,1> S; + Matrix<double,9,1> K; + S = H*P*H.transpose() + R; + K = P*H.transpose()*S.inverse(); + P = (Matrix<double,9,9>::Identity()-K*H)*P; + P = 0.5*(P+P.transpose()); + X += K*(z-h); +} /* *-------------------------------------------------------------------------------------- |