Add public function to get the covariance.

diff --git a/include/ekf.hpp b/include/ekf.hpp
index f42b9ce..ff353a1 100644 (file)
--- 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 (file)
--- 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);