Use of ROBOT_POSE_EKF packets in Ros

1. How to use the package

1.1 Configuration

The default EKF node's launch file can be found under the package directory. The launch file contains several configuration parameters: Freq: Filter update and release frequency. The high frequency can give more robot posture data in a certain time, but it will not affect the estimation accuracy of pose. Senor_timeout: When the sensor stops sending data to the filter, the filter requires no sensor for much longer before continuing. Odom_used,imu_used,vo_used: To enable and lose energy.

Here are the configuration file contents, which you can change according to your needs:

<launch>
  <node pkg= "ROBOT_POSE_EKF" type= "ROBOT_POSE_EKF" name= "ROBOT_POSE_EKF" >
    <param Name= "Output_frame" value= "Odom"/>
    <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=" true "/>
    <param name=" Debug "value=" false "/>
    < param name= "Self_diagnose" value= "false"/>
  </node>
 </launch>

1.2 Running the compilation package

$ ROSDEP Install ROBOT_POSE_EKF
 $ roscd robot_pose_ekf
 $ rosmake

Run robot pose EKF

$ roslaunch Robot_pose_ekf.launch

2. Node

2.1 ROBOT_POSE_EKF

Perform an extended Kalman filter in order to estimate the robot pose.

2.2.1 Subscribe to the topic

Odom (Nav_msgs/odometry)
Pose (wheeled results): 2D posture includes the position and course of the planar robot, and the covariance of the corresponding pose. The message that sends this 2D gesture actually represents a 3D posture, but Z,roll,pitch is ignored.

Imu_data (Senser_msgs/imu)
3D Orientation (imu use): 3D direction provides the roll,pitch,yaw angle in the coordinate system of the robot in the world coordinate system. Roll and pitch angles are described as absolute angles, and the yaw angle is described as a relative angle. A covariance matrix indicates the uncertainty of direction measurement. When the message is received only on this topic, the robot pose EKF will not start; It also expects messages on/odom themes or/vo topics.

VO (nav_msgs/odometry)
3D pose (visual odometer): 3D posture represents the full position and direction of the robot, and the covariance of the posture. When the sensor detects only the part of the 3D pose, it simply specifies a large covariance in the undetected part.

ROBOT_POSE_EKF nodes do not require all three sensor sources to be available at the same time. Each source is given a bit pose estimate and a variance. The source runs at different frequencies and with different delays. The source can disappear or appear over time, and the node can automatically detect and use the available sources. To add your own sensor input, see the Adding a GPS sensor tutorial.

2.1. Topics published by 2 nodes

Robot_pose_ekf/odom_combined (geometry_msgs/posewithcovariancestamped)
Output of the filter (estimated 3D robot posture)

2.1.3 provides a TF conversion

Odom_combined→base_footprint

3. How Robot Pose EKF works

3.1 Position Description

All sensor sources that send information to the filter node have its own world reference coordinate system, and these coordinate systems can drift over time. Therefore, the absolute coordinate systems sent by different sensors cannot be compared to each other. The node updates the extended Kalman filter using the relative posture difference of each sensor.

3.2 Covariance Description

Because the robot moves around the circle, the uncertainty of its posture in the world coordinate system is constantly becoming bigger and larger. Over time, the covariance increases without bounds. Therefore, it is not used to publish the covariance in its own posture, instead of the covariance of the sensor source published covariance over time, that is, the variance of the velocity. Note that the use of the world's observations will reduce the uncertainty of robot posture, but this is a positioning rather than a mileage meter.

3.3 Choice of time

It is assumed that the robot pose filter has been recently updated at t_0 time. The node will update the robot pose filter until at least one measurement of each sensor arrives at a point in time after t_0.

4. Package Status

4.1 stability

The code is well tested and stable for a long time. The ROS API has evolved over time with message type changes.

4.2 Road Map This filter is currently set to three sensor signals. Will increase the speed to the EKF state.