Set initial values for acceleration and gyro.

diff --git a/src/ekf.cpp b/src/ekf.cpp
index 6cf4c93..59814ef 100644 (file)
--- 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;
        }