From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sun, 13 Dec 2020 04:45:31 +0000 (-0300)
Subject: Set initial values for acceleration and gyro.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=8dfa6d2b2d70a3438e64e944c2f67e357f003d78;p=imufusion_ros.git

Set initial values for acceleration and gyro.
---

diff --git a/src/ekf.cpp b/src/ekf.cpp
index 6cf4c93..59814ef 100644
--- a/src/ekf.cpp
+++ b/src/ekf.cpp
@@ -68,12 +68,20 @@ EkfNode::EkfNode(const ros::NodeHandle &node,const char *frameId)
 {
 	node_=node;
 	frameId_=frameId;
+
+	accel_.setZero();
+	gyro_.setZero();
+
+	accelCovariance_.setZero();
+	gyroCovariance_.setZero();
+
 	imuAvailable=false;
 	magAvailable=false;
+
 	imuSubscriber_=node_.subscribe("imu/data_raw",10,&EkfNode::imuCB,this);
 	magSubscriber_=node_.subscribe("imu/mag",10,&EkfNode::magCB,this);
 	imuPublisher_=node_.advertise<sensor_msgs::Imu>("imu/data",10);
-	
+
 	Eigen::Quaterniond q0;
 	q0.setIdentity();
 
@@ -107,7 +115,7 @@ EkfNode::EkfNode(const ros::NodeHandle &node,const char *frameId)
 		Pv=diag.asDiagonal();
 	}
 	else Pv*=1e-3*1e-3;
-	
+
 	const double T=0.02;	// 0.01 is too fast for an UART at 115200 bps
 
 	ekf=new imufusion::Ekf(Pw,Pv,T,100);
@@ -177,7 +185,7 @@ int main(int argc,char* argv[])
 	
 	if(argc != 2)
 	{
-		ROS_ERROR_STREAM("ekf: No base frame id.\n");
+		ROS_ERROR_STREAM("ekf: No frame id.\n");
 		return -1;
 	}