robot_pose_ekf 和IMU 使用配置

robot_pose_ekf 和IMU使用配置

• EKF
• • 源码
  • 配置
  • 检查方法
  • 参考使用
• Xsens MTI使用

EKF

To integrating odometry information with IMU information and test the accuracy using bags from real experiments on husky/scout.

源码

git clone https://github.com/ros-planning/robot_pose_ekf.git

Ref:

robot_pose_ekf 源码解读1

robot_pose_ekf 源码解读2

贝叶斯滤波器包bfl 1

贝叶斯滤波器包bfl 2

配置

robot_pose_ekf.launch or test_robot_pose_ekf.launch

You need to change the topic and frame name

1. /odometry_throttle: Throttle messages on odometry to a particular rate(2hz)
   topic_tools/throttle
2. base_footprint_frame: make sure you have publish the tf tree between base_footprint_frame and imu’s frame like this<node pkg="tf" type="static_transform_publisher" name="world_broadcaster" args="0 0 0 0 0 0 base_link imu 100" />

[ WARN] [1601430080.460107326, 1601016468.862749824]: Could not transform imu message from /imu to base_link. Imu will not be activated yet.

4. Change the topic name and path of bags like <remap from="odom" to="base_odometry/odom" /> <remap from="imu_data" to="torso_lift_imu/data" />
   is correspond to <node pkg="rosbag" name="rosbag" type="play" args="--clock -hz 100 --d .4 $(find robot_pose_ekf)/test/ekf_test2_indexed.bag" />

5. The frequency of odom and imu needs to be the same(within 1s).

   [ERROR] [1601430156.351775252, 1601016452.792816901]: Timestamps of odometry and imu are 1.051269 seconds apart.

  <!-- Robot pose ekf -->
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="false"/>
    <param name="base_footprint_frame" value="base_link"/>
    <param name="output_frame" value="odom"/>

    <!-- <remap from="odom" to="base_odometry/odom" /> -->
    <!-- <remap from="imu_data" to="torso_lift_imu/data" /> -->

    <remap from="odom" to="/odometry" />
    <!-- <remap from="odom" to="/odometry_throttle" /> -->
    <remap from="imu_data" to="/imu/data" />
  </node>
  
  <node name="throttler1" type="throttle" pkg="topic_tools" args="messages /odometry 2/odometry_throttle" /> 

  <!-- <node pkg="rosbag" name="rosbag" type="play" args="-clock -hz 100 -d .4 $(find robot_pose_ekf)/test/ekf_test2_indexed.bag" /> -->
  <node pkg="rosbag" name="rosbag" type="play" args="--clock --hz 100 -d .4 /home/xywang/WORKSPACE/scout_cpc_ws/LOG/EKF/2020-09-25-14-47-30.bag" />
  
<node pkg="tf" type="static_transform_publisher" name="world_broadcaster" args="0 0 0 0 0 0 base_link imu 100" />
  

协方差参数的设置

[ERROR] [1587284286.565322789]: filter time older than odom message buffer
[ERROR] [1587284286.581064157]: Covariance specified for measurement on topic wheelodom is zero

If your odom and imu don’t have covariance, then add following code in odom_estimation_node.cpp

    // receive data 
    odom_stamp_ = odom->header.stamp;
    odom_time_  = Time::now();
    Quaternion q;
    tf::quaternionMsgToTF(odom->pose.pose.orientation, q);
    odom_meas_  = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0));
    for (unsigned int i=0; i<6; i++)
      for (unsigned int j=0; j<6; j++)
        odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j];

    if (odom_covariance_(1,1) == 0.0){//xinyi
      SymmetricMatrix measNoiseOdom_Cov(6);  measNoiseOdom_Cov = 0;
      measNoiseOdom_Cov(1,1) = pow(0.01221,2);  // = 0.01221 meters / sec
      measNoiseOdom_Cov(2,2) = pow(0.01221,2);  // = 0.01221 meters / sec
      measNoiseOdom_Cov(3,3) = pow(0.01221,2);  // = 0.01221 meters / sec
      measNoiseOdom_Cov(4,4) = pow(0.007175,2);  // = 0.41 degrees / sec
      measNoiseOdom_Cov(5,5) = pow(0.007175,2);  // = 0.41 degrees / sec
      measNoiseOdom_Cov(6,6) = pow(0.007175,2);  // = 0.41 degrees / sec
      odom_covariance_ = measNoiseOdom_Cov;
    }

    // manually set covariance untile imu sends covariance
    if (imu_covariance_(1,1) == 0.0){
      SymmetricMatrix measNoiseImu_Cov(3);  measNoiseImu_Cov = 0;
      measNoiseImu_Cov(1,1) = pow(0.00017,2);  // = 0.01 degrees / sec
      measNoiseImu_Cov(2,2) = pow(0.00017,2);  // = 0.01 degrees / sec
      measNoiseImu_Cov(3,3) = pow(0.00017,2);  // = 0.01 degrees / sec
      imu_covariance_ = measNoiseImu_Cov;
    }

Ref:

参考配置1

参考配置2

problem about tf tree

参考配置3

参考配置4

EKF 原理 & QR参数设置

I still have a problem with this part and if someone understands how to set Q and R in odom_estimation.cpp please feel free to tell me.

NonLinear System Model

    // create SYSTEM MODEL
    ColumnVector sysNoise_Mu(6);  sysNoise_Mu = 0;
    SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
    for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000,2);
    Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
    sys_pdf_   = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
    sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_);

    // create MEASUREMENT MODEL ODOM
    ColumnVector measNoiseOdom_Mu(6);  measNoiseOdom_Mu = 0;
    SymmetricMatrix measNoiseOdom_Cov(6);  measNoiseOdom_Cov = 0;
    for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
    Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
    Matrix Hodom(6,6);  Hodom = 0;
    Hodom(1,1) = 1;    Hodom(2,2) = 1;    Hodom(6,6) = 1;
    odom_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
    odom_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_);

    // create MEASUREMENT MODEL IMU
    ColumnVector measNoiseImu_Mu(3);  measNoiseImu_Mu = 0;
    SymmetricMatrix measNoiseImu_Cov(3);  measNoiseImu_Cov = 0;
    for (unsigned int i=1; i<=3; i++) measNoiseImu_Cov(i,i) = 1;
    Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov);
    Matrix Himu(3,6);  Himu = 0;
    Himu(1,4) = 1;    Himu(2,5) = 1;    Himu(3,6) = 1;
    imu_meas_pdf_   = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);
    imu_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(imu_meas_pdf_);

Ref:
EKF原理理解1
EKF原理理解2
参数理解
设置QR
problem about filter update

R measNoiseImu_Cov and measNoiseOdom_Cov

检查方法

1. Troubleshooting
2. I wrote a node to check the performance in rviz

<launch>
   <node pkg="robot_pose_ekf" type="showpath_robot_pose_ekf" name="showpath_robot_pose_ekf" output="screen" >
        <!-- <remap from="/odom" to="/UgvOdomTopic" /> -->
          <remap from="odom" to="/odometry" />
        <remap from="/combined_odom" to="/robot_pose_ekf/odom_combined" />
   </node>
   <node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find model_test)/cfg/showpath_ekf.rviz"/>
</launch>

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

ros::Publisher path_pub;
ros::Publisher path_pub2;
ros::Time current_time, last_time;
ros::Time current_time2, last_time2;
nav_msgs::Path path;
nav_msgs::Path path2;


void slam_odo_call_back(const nav_msgs::Odometry::ConstPtr &slam_odom) //for rplidar and carto
{

    current_time = ros::Time::now();

    geometry_msgs::PoseStamped this_pose_stamped;
    this_pose_stamped.pose.position.x = slam_odom->pose.pose.position.x;
    this_pose_stamped.pose.position.y = slam_odom->pose.pose.position.y;

    this_pose_stamped.pose.orientation.x = slam_odom->pose.pose.orientation.x;
    this_pose_stamped.pose.orientation.y = slam_odom->pose.pose.orientation.y;
    this_pose_stamped.pose.orientation.z = slam_odom->pose.pose.orientation.z;
    this_pose_stamped.pose.orientation.w = slam_odom->pose.pose.orientation.w;


    this_pose_stamped.header.stamp=current_time;
    this_pose_stamped.header.frame_id="world";
    path.poses.push_back(this_pose_stamped);

    path_pub.publish(path);
    // ROS_INFO("success!");
    last_time = current_time;
}
void combined_odo_call_back(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &combined_odom)
{
    
    current_time2 = ros::Time::now();

    geometry_msgs::PoseStamped this_pose_stamped2;
    this_pose_stamped2.pose.position.x = combined_odom->pose.pose.position.x;
    this_pose_stamped2.pose.position.y = combined_odom->pose.pose.position.y;
    
    this_pose_stamped2.pose.orientation.x = combined_odom->pose.pose.orientation.x;
    this_pose_stamped2.pose.orientation.y = combined_odom->pose.pose.orientation.y;
    this_pose_stamped2.pose.orientation.z = combined_odom->pose.pose.orientation.z;
    this_pose_stamped2.pose.orientation.w = combined_odom->pose.pose.orientation.w;

    this_pose_stamped2.header.stamp=current_time2;
    this_pose_stamped2.header.frame_id="world";
    path2.poses.push_back(this_pose_stamped2);

    path_pub2.publish(path2);
    // ROS_INFO("success2!");
    last_time2 = current_time2;
}


main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath");
    ros::NodeHandle ph;
    path_pub = ph.advertise<nav_msgs::Path>("traj_slam",1, true);
    path_pub2 = ph.advertise<nav_msgs::Path>("traj_ekf",1, true);
    ros::Subscriber slam_odom_sub = ph.subscribe("/odom", 1, &slam_odo_call_back);
    ros::Subscriber combined_odom_sub = ph.subscribe("/combined_odom", 1, &combined_odo_call_back);

    current_time = ros::Time::now();
    last_time = ros::Time::now();

    path.header.stamp=current_time;
    path.header.frame_id="world";

    current_time2 = ros::Time::now();
    last_time2 = ros::Time::now();

    path2.header.stamp=current_time2;
    path2.header.frame_id="world";
    
    ros::spin();

    return 0;
}

参考使用

https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/gyro.py

Xsens MTI使用

基于ros melodic 及 MTI-G-710测试
官方教程
this works for me

rviz display
MTi 600-series MT manager does not work on ubuntu16.04. Need to use it on Windows