Fix retrieve of ROS parameters.

diff --git a/src/ekf.cpp b/src/ekf.cpp
index 80490ab..62d21ba 100644 (file)
--- a/src/ekf.cpp
+++ b/src/ekf.cpp
@@ -75,18 +75,18 @@ EkfNode::EkfNode(const ros::NodeHandle &node,const char *frameId)
        Eigen::MatrixXd P0(7,7);
        P0.setIdentity();
        std::vector<double> P0Diag;
-       if(node_.getParam("P",P0Diag))
+       ros::NodeHandle nh("~");
+       if(nh.getParam("P",P0Diag))
        {
                Eigen::Map<Eigen::VectorXd> diag(P0Diag.data(),P0.rows());
                P0=diag.asDiagonal();
        }
-       else P0*=1e-5;
+       else P0*=1e5;
 
        Eigen::MatrixXd Pw(7,7);
        Pw.setIdentity();
        std::vector<double> PwDiag;
-       ros::NodeHandle nh("~");
-       if(node_.getParam("/ekf/Pw",PwDiag))
+       if(nh.getParam("Pw",PwDiag))
        {
                Eigen::Map<Eigen::VectorXd> diag(PwDiag.data(),Pw.rows());
                Pw=diag.asDiagonal();