From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Fri, 27 Sep 2019 21:38:19 +0000 (-0300)
Subject: Add public function to get the covariance.
X-Git-Tag: f1~2
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=4c46b5fe7f70ceb8dc9d77202e6e073ccb18235a;p=imufusion.git

Add public function to get the covariance.
---

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);