From 69e0684df8568dac8815ae8e25b7e6561ec0486d Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Mon, 14 Dec 2020 03:31:43 -0300 Subject: [PATCH] Change initialization according to imufusion lib. --- config/ekf.yaml | 1 - src/ekf.cpp | 17 ++--------------- 2 files changed, 2 insertions(+), 16 deletions(-) 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 + Copyright (C) 2019, 2020 Walter Fetter Lages 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("imu/data",10); - Eigen::Quaterniond q0; - q0.setIdentity(); - - Eigen::MatrixXd P0(7,7); - P0.setIdentity(); - std::vector P0Diag; - ros::NodeHandle nh("~"); - if(nh.getParam("P",P0Diag)) - { - Eigen::Map diag(P0Diag.data(),P0.rows()); - P0=diag.asDiagonal(); - } - else P0*=1e5; - Eigen::MatrixXd Pw(7,7); Pw.setIdentity(); std::vector PwDiag; + ros::NodeHandle nh("~"); if(nh.getParam("Pw",PwDiag)) { Eigen::Map diag(PwDiag.data(),Pw.rows()); -- 2.12.0