From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Mon, 14 Dec 2020 06:31:43 +0000 (-0300)
Subject: Change initialization according to imufusion lib.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=69e0684df8568dac8815ae8e25b7e6561ec0486d;p=imufusion_ros.git

Change initialization according to imufusion lib.
---

diff --git a/config/ekf.yaml b/config/ekf.yaml
index a073c30..0ee1080 100644
--- 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
--- 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());