From 4c46b5fe7f70ceb8dc9d77202e6e073ccb18235a Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Fri, 27 Sep 2019 18:38:19 -0300 Subject: [PATCH] Add public function to get the covariance. --- include/ekf.hpp | 5 +++++ lib/ekf.cpp | 8 +++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/include/ekf.hpp b/include/ekf.hpp index f42b9ce..ff353a1 100644 --- a/include/ekf.hpp +++ b/include/ekf.hpp @@ -100,6 +100,11 @@ class Ekf */ Eigen::EulerAnglesZYXd euler(void); + /** @brief Get covariance estimation + @return covariance matrix + */ + Eigen::MatrixXd covariance(void); + // Force 16-byte aligment of fixed-size vectorizable Eigen types, EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/lib/ekf.cpp b/lib/ekf.cpp index a688340..f3695f6 100644 --- a/lib/ekf.cpp +++ b/lib/ekf.cpp @@ -61,7 +61,7 @@ void Ekf::update(const Eigen::Vector3d &acc,const Eigen::Vector3d &mag,const Eig P_=F(u)*P*F(u).transpose()+Pw_; // State prediction - x_=f(u); // Should be the last operation becuse changes x_ + x_=f(u); // Should be the last operation because it changes x_ } Eigen::Quaterniond Ekf::quaternion(void) @@ -79,6 +79,12 @@ Eigen::EulerAnglesZYXd Ekf::euler(void) return euler; } +Eigen::MatrixXd Ekf::covariance(void) +{ + return P_; +} + + Eigen::VectorXd Ekf::f(const Eigen::Vector3d u) { Eigen::MatrixXd Xi(4,3); -- 2.12.0