在使用robot_pose_ekf時，常遇到接收到的odometry數(shù)據(jù)格式錯誤的問題。一個可能的原因為底盤或其他設備發(fā)布odometry數(shù)據(jù)的協(xié)方差矩陣默認為0矩陣。解決的方法由兩種：一種為在底盤將信息封裝發(fā)布前對協(xié)方差矩陣進行初始化；另一種方法為在robot_pose_ekf中添加判斷，如果接收到的odometry信息的協(xié)方差矩陣沒有進行初始化，則進行初始化。
做工程時采用了第二種方法。初始化位于odom_estimation_node.cpp中：
void OdomEstimationNode::odomCallback(const OdomConstPtr& odom)
{
…
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){
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;
}
my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, base_footprint_frame_, “wheelodom”), odom_covariance_);
…
}

由于對于我手頭情況而言odometry確定協(xié)方差矩陣為0，故判斷條件設得較為簡單：odom_covariance_(1,1) == 0.0。判斷條件應根據(jù)實際情況設定。協(xié)方差矩陣具體值可以考慮設置為精度的二次方。

關于卡爾曼濾波中協(xié)方差矩陣的設定沒能深入學習理解，感覺較為復雜。但在做工程中確保odom的協(xié)方差矩陣對角線元素不均0則robot_pose_ekf即可工作。

總結

以上是生活随笔為你收集整理的odom协方差初始化的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。