odom和imu融合前添加协方差

writer：lIcht                      email：lIcht.gzl@gmail.com                 date:  2019.12.6

        单一的由轮速计算得到的里程计会有很大误差，为了尽可能的减少误差，需要利用robot_pose_ekf包对odom、imu、gps和vo信息进行融合，得到更加准确的里程计信息。根据自己已有的传感器，这里选择odom、imu和gps进行融合。在融合前有一个问题需要解决，不然robot_pose_ekf包无法工作。它要求入口数据要具有协方差信息，然而一般的里程计和imu是没有协方差信息的，此时需要自己在cpp文件里赋值一下。具体代码如下：

odom协方差（4X9）：

xxxxxxxxxx
//声明赋值：
const boost::array<double, 36> ODOM_POSE_COVARIANCE = {{1e-3, 0, 0, 0, 0, 0,
                                                        0, 1e-3, 0, 0, 0, 0,
                                                        0, 0, 1e6, 0, 0, 0,
                                                        0, 0, 0, 1e6, 0, 0,
                                                        0, 0, 0, 0, 1e6, 0,
                                                        0, 0, 0, 0, 0, 1e3}};
​
const boost::array<double, 36> ODOM_POSE_COVARIANCE2 = {{1e-9, 0, 0, 0, 0, 0,
                                                         0, 1e-3, 1e-9, 0, 0, 0,
                                                         0, 0, 1e6, 0, 0, 0,
                                                         0, 0, 0, 1e6, 0, 0,
                                                         0, 0, 0, 0, 1e6, 0,
                                                         0, 0, 0, 0, 0, 1e-9}};
​
const boost::array<double, 36> ODOM_TWIST_COVARIANCE = {{1e-3, 0, 0, 0, 0, 0,
                                                         0, 1e-3, 0, 0, 0, 0,
                                                         0, 0, 1e6, 0, 0, 0,
                                                         0, 0, 0, 1e6, 0, 0,
                                                         0, 0, 0, 0, 1e6, 0,
                                                         0, 0, 0, 0, 0, 1e3}};
​
const boost::array<double, 36> ODOM_TWIST_COVARIANCE2 = {{1e-9, 0, 0, 0, 0, 0,
                                                          0, 1e-3, 1e-9, 0, 0, 0,
                                                          0, 0, 1e6, 0, 0, 0,
                                                          0, 0, 0, 1e6, 0, 0,
                                                          0, 0, 0, 0, 1e6, 0,
                                                          0, 0, 0, 0, 0, 1e-9}};
​
​
给/odom主题的const boost::array<double, 36> ODOM_POSE_COVARIANCE = {{1e-3, 0, 0, 0, 0, 0,
                                                                    0, 1e-3, 0, 0, 0, 0,
                                                                    0, 0, 1e6, 0, 0, 0,
                                                                    0, 0, 0, 1e6, 0, 0,
                                                                    0, 0, 0, 0, 1e6, 0,
                                                                    0, 0, 0, 0, 0, 1e3}};
​
const boost::array<double, 36> ODOM_POSE_COVARIANCE2 = {{1e-9, 0, 0, 0, 0, 0,
                                                         0, 1e-3, 1e-9, 0, 0, 0,
                                                         0, 0, 1e6, 0, 0, 0,
                                                         0, 0, 0, 1e6, 0, 0,
                                                         0, 0, 0, 0, 1e6, 0,
                                                         0, 0, 0, 0, 0, 1e-9}};
​
const boost::array<double, 36> ODOM_TWIST_COVARIANCE = {{1e-3, 0, 0, 0, 0, 0,
                                                         0, 1e-3, 0, 0, 0, 0,
                                                         0, 0, 1e6, 0, 0, 0,
                                                         0, 0, 0, 1e6, 0, 0,
                                                         0, 0, 0, 0, 1e6, 0,
                                                         0, 0, 0, 0, 0, 1e3}};
​
const boost::array<double, 36> ODOM_TWIST_COVARIANCE2 = {{1e-9, 0, 0, 0, 0, 0,
                                                          0, 1e-3, 1e-9, 0, 0, 0,
                                                          0, 0, 1e6, 0, 0, 0,
                                                          0, 0, 0, 1e6, 0, 0,
                                                          0, 0, 0, 0, 1e6, 0,
                                                          0, 0, 0, 0, 0, 1e-9}};
​
​
​
​
​
//给 /odom 主题下的消息格式赋值：
//分为静止和运动：
//pose：
​
    if(vx==0 && vy==0 && vth==0){
      odom.pose.covariance = ODOM_POSE_COVARIANCE2;
  }
  else{
      odom.pose.covariance = ODOM_POSE_COVARIANCE;  
  }
​
​
​
//twist：
​
    if(vx==0 && vy==0 && vth==0){
      odom.twist.covariance = ODOM_TWIST_COVARIANCE2;
  }
  else{
      odom.twist.covariance = ODOM_TWIST_COVARIANCE;
  }
​
//写在 odom_pub.publish(odom);  之前就行了 常量
​

imu协方差赋值（3X3）：

xxxxxxxxxx
//imu协方差声明与赋值
boost::array<double, 9> covariance_li = {{-1,0,0,0,0,0,0,0,0}};
boost::array<double, 9> covariance_or = {{1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6}};
boost::array<double, 9> covariance_an = {{1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6}};
​
​
//给 /imu 主题下的消息赋值：
​
imu_data.orientation_covariance = covariance_or;
​
mu_data.angular_velocity_covariance = covariance_an;
​
imu_data.linear_acceleration_covariance = covariance_li;
​
​
​
​
​
​
//经过gps_common转换过后的gps的twist没有协方差，要不要赋值？它的消息结构与/odom一样，要不要赋值一样的协方差？
​