diff options
| author | Martin Miller | 2017-03-18 18:09:39 -0500 |
|---|---|---|
| committer | Martin Miller | 2017-03-18 18:09:39 -0500 |
| commit | c0727bbe1404e788b4ae82f6c6d99e105ecfacdf (patch) | |
| tree | d982eb15f94bbcdb319453f98d199a5fd9650651 /src/body.cpp | |
| parent | f2a48a3a12475b52e9fa525541eeebf0b3f7c7e8 (diff) | |
| download | refslam-c0727bbe1404e788b4ae82f6c6d99e105ecfacdf.zip refslam-c0727bbe1404e788b4ae82f6c6d99e105ecfacdf.tar.gz | |
Add fake z kalman update
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 ----- */ /* |