6. 数据融合 – EKF – robot_pose_ekf
此保融合odom/imu/ov等数据获得更精确的位姿信息。
并不要求各路传感器数据连续提供，某路例如imu可以的数据可以中断，不影响算法进行。
REF:
https://answers.ros.org/question/235228/how-is-the-orientation-of-frame-odom-initialized/
Subs:
* odom (nav_msgs/Odometry)
position and orientation of robot in ground plane
* imu_data (sensor_msgs/Imu)
RPY angles of robot base frame relative to a world reference frame.
R和P为绝对角度因为有重力 而Y为相对角度。
* vo (nav_msgs/Odometry)
the full position and orientation of robot
如果某传感器提供3D数据而只用到2D部分，则对不用部分赋值较大的协方差即可。
Pubs:
* robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)
The output of the filter (the estimated 3D robot pose).
TFs:
odom_combined -> base_footprint
发布一个名为odom_combined的新的坐标系，
并发布一个 odom_combined->base_footprint的TF变换。
Launchs:
不确定度方面的几个实践问题
6.1 协方差这个参数的来源问题
ERROR: Covariance specified for measurement on topic xxx is zero
A:
Each measurement that is processed by the robot pose ekf ,needs to have a covariance associated with it.
The diagonal elements of the covariance matrix , can not be zero.
When one of the diagonal elements is zero, this error is shown.
Messages with an invalid covariance, will not be used to update the filter.
So, you should reating the covariance matrices for individual sensors:
*1. For IMU and Odometry, the covariance matrix can be formed from the datasheet.
*2. For Visual Odometry, covariance matrix may be obtained from the measurement equation that relates the measured variables to the pose coordinates.
6.2 IMU的协方差
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;
}
for example 0.00017 means 0101deg/sec, this is so good imu, increase them if you get poor odometry.
For optimal values, perform an odometry calibration alogirthm such as UMBMark.
IMU is:
self.imu_msg.orientation_covariance = [-1, 0, 0,
0, -1, 0,
0, 0, -1 ] #sensor doesn’t have orientation
源代码是这样检查的
$ vi robot_pose_ekf/src/odom_estimation.cpp
void OdomEstimation::addMeasurement(const StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar)
{
// check covariance
for (unsigned int i=0; i<covar.rows(); i++){=”” if=”” (covar(i+1,i+1)=”=” 0){=”” ros_error(“covariance=”” specified=”” for=”” measurement=”” on=”” topic=”” %s=”” is=”” zero”,=”” meas.child_frame_id_.c_str());=”” return;=”” }=”” add=”” measurements=”” addmeasurement(meas);=”” (meas.child_frame_id_=”=” “wheelodom”)=”” odom_covariance_=”covar;” else=”” “imu”)=”” imu_covariance_=”covar;” “vo”)=”” vo_covariance_=”covar;” ros_error(“adding=”” a=”” an=”” unknown=”” sensor=”” %s”,=”” };=”” 6.3=”” odom的协方差=”” the=”” odometry=”” will=”” be=”” updated,=”” but=”” covariance=”” always=”” published=”” as=”” 0.=”” controller=”” don’t=”” include=”” information.=”” you=”” need=”” to=”” have=”” value=”” included=”” in=”” information=”” some=”” reason,=”” can=”” create=”” your=”” own=”” based=”” rosaria=”” and=”” value.=”” odom=”” 6×6=”” matrix,=”” because=”” 6=”” dof,=”” position=”” (x,=”” y,=”” z)=”” orientation=”” so=”” float[36]=”” matrix.=”” diagonal=”” terms=”” are=”” trust=”” each=”” dof.=”” estimate=”” or=”” algorithm=”” accuracy=”” with=”” experiment.=”” see=”” data=”” good=”” 1cm=”” translation=”” 0.1=”” radian=”” rotation=”” use=”” this=”” matrix:=”” [0.01=”” 0.0=”” 0.0,=”” 0.01=”” 0.1]=”” no=”” one=”” dof=”” put=”” huge=”” example=”” segway_rmp:=”” this-=””>odom_msg.pose.covariance[0] = 0.00001;
this->odom_msg.pose.covariance[7] = 0.00001;
this->odom_msg.pose.covariance[14] = 1000000000000.0;
this->odom_msg.pose.covariance[21] = 1000000000000.0;
this->odom_msg.pose.covariance[28] = 1000000000000.0;
this->odom_msg.pose.covariance[35] = 0.001;
above means, position z is not sure, ori of x and y is no tsure.</covar.rows();>
covariance
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = “odom”;
//set the position
odom.pose.pose.position.x = od->position.positionx;
odom.pose.pose.position.y = od->position.positiony;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
//set the velocity
odom.child_frame_id = “base_link”;
odom.twist.twist.linear.x = od->speed.speedx;
odom.twist.twist.linear.y = od->speed.speedy;
odom.twist.twist.angular.z = od->speed.speedr;
// set stddev
odom.pose.covariance[0] = pos_x_stddev;
odom.pose.covariance[7] = pos_y_stddev;
odom.pose.covariance[14] = pos_z_stddev;
odom.pose.covariance[21] = rot_x_stddev;
odom.pose.covariance[28] = rot_y_stddev;
odom.pose.covariance[35] = rot_z_stddev;
double pos_x_stddev
double pos_x_stddev
double pos_z_stddev
double rot_x_stddev
double rot_y_stddev
double rot_z_stddev
ros::NodeHandle private_node_handle(“~”);
private_node_handle.param(“pos_x_stddev”, pos_x_stddev, 0.11);
private_node_handle.param(“pos_y_stddev”, pos_y_stddev, 0.12);
private_node_handle.param(“pos_z_stddev”, pos_z_stddev, 1000000000000.0);
private_node_handle.param(“rot_x_stddev”, rot_x_stddev, 1000000000000.0);
private_node_handle.param(“rot_y_stddev”, rot_y_stddev, 1000000000000.0);
private_node_handle.param(“ros_z_stddev”, ros_z_stddev, 0.15);
REF:
Covariance matrices with a practical example
rris
rris