Change initialization according to imufusion lib.

diff --git a/config/ekf.yaml b/config/ekf.yaml
index a073c30..0ee1080 100644 (file)
--- a/config/ekf.yaml
+++ b/config/ekf.yaml
@@ -1,5 +1,4 @@
 # Floating point numbers in YAML require the dot, even if using "e".
 
-P: [1.0e6, 1.0e6, 1.0e6, 1.0e6, 1.0e6, 1.0e6, 1.0e6]
 Pw: [7.47601e-07, 7.47601e-07, 7.47601e-07, 7.47601e-07, 0.00274156, 0.00274156, 0.00274156]
 Pv: [0.000173706, 0.000173706, 0.000173706, 1.0e-12, 1.0e-12, 1.96e-12]

diff --git a/src/ekf.cpp b/src/ekf.cpp
index 59814ef..bea2d15 100644 (file)
--- a/src/ekf.cpp
+++ b/src/ekf.cpp
@@ -1,7 +1,7 @@
 /******************************************************************************
                          IMU Fusion Library ROS Wrapper
                             Extended Kalman Filter
-          Copyright (C) 2019 Walter Fetter Lages <w.fetter@ieee.org>
+          Copyright (C) 2019, 2020 Walter Fetter Lages <w.fetter@ieee.org>
 
         This program is free software: you can redistribute it and/or modify
         it under the terms of the GNU General Public License as published by
@@ -82,23 +82,10 @@ EkfNode::EkfNode(const ros::NodeHandle &node,const char *frameId)
        magSubscriber_=node_.subscribe("imu/mag",10,&EkfNode::magCB,this);
        imuPublisher_=node_.advertise<sensor_msgs::Imu>("imu/data",10);
 
-       Eigen::Quaterniond q0;
-       q0.setIdentity();
-
-       Eigen::MatrixXd P0(7,7);
-       P0.setIdentity();
-       std::vector<double> P0Diag;
-       ros::NodeHandle nh("~");
-       if(nh.getParam("P",P0Diag))
-       {
-               Eigen::Map<Eigen::VectorXd> diag(P0Diag.data(),P0.rows());
-               P0=diag.asDiagonal();
-       }
-       else P0*=1e5;
-
        Eigen::MatrixXd Pw(7,7);
        Pw.setIdentity();
        std::vector<double> PwDiag;
+       ros::NodeHandle nh("~");
        if(nh.getParam("Pw",PwDiag))
        {
                Eigen::Map<Eigen::VectorXd> diag(PwDiag.data(),Pw.rows());