diff options
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 15 |
1 files changed, 12 insertions, 3 deletions
diff --git a/src/body.cpp b/src/body.cpp index 4741911..c4d8a82 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -19,6 +19,14 @@ #include <Eigen/Dense> #include "body.h" +Matrix<double,1,1> +Body::R() +{ + Matrix<double,1,1> R; + R << 1e-8; + return R; +} + Matrix<double,9,9> Body::Q (double dt) { @@ -56,11 +64,12 @@ Body::H ( Matrix<double,1,9> &H ) * Description: Writes the predicted measurement vector into h. *-------------------------------------------------------------------------------------- */ -void -Body::h ( const Matrix<double,9,1> &X, Matrix<double,1,1> &h ) +Matrix<double,1,1> +Body::h ( const Matrix<double,9,1> &X) { + Matrix<double,1,1> h; h[0] = X[2]; - return ; + return h; } /* ----- end of method Body::h_hat ----- */ /* |