ros ekf融合odom imu ov信息

1.这里是ros_ekf_pose包的简单介绍:

这个包用于评估机器人的3D位姿，使用了来自不同源的位姿测量信息，它使用带有6D（3D position and 3D orientation）模型信息的扩展卡尔曼滤波器来整合来自轮子里程计，IMU传感器和视觉里程计的数据信息。基本思路就是用松耦合方式融合不同传感器信息实现位姿估计。

1 如何使用EKF

1.1 配置

EKF node的缺省启动文件位于robot_pose_ekf包中，文件中有许多配置参数：

freq: 滤波器更新和发布频率，注意：频率高仅仅意味着一段时间可以获得更多机器人位姿信息，但是并不表示可以提高每次位姿评估的精度
sensor_timeout: 当某传感器停止给滤波器发送信息时，滤波器应该等多长时间方才可以在没有该传感器信息状况下继续工作
odom_used, imu_used, vo_used: enable/disable输入源

启动文件中配置参数设置可以被修改，看起来大致如下所示：

<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 编译并运行该包

Build the package

 $ rosdep install robot_pose_ekf$ roscd robot_pose_ekf$ rosmake

Run the robot pose ekf
```
 $ roslaunch robot_pose_ekf.launch
```

2 Nodes

2.1 robot_pose_ekf

为确定机器人位姿，robot_pose_ekf包实现了扩展卡尔曼滤波器

2.1.1 Subscribed Topics

odom (nav_msgs/Odometry)

2D pose (used by wheel odometry): 该2D pose包含了机器人在地面的位置（position）和方位（orientation）信息以及该位姿的协方差（covariance）。
用来发送该2D位姿的消息实际上表示了一个3D位姿，只不过把z，pitch和roll分量简单忽略了。

imu_data (sensor_msgs/Imu)

3D orientation (used by the IMU): 3D方位提供机器人底座相对于世界坐标系的Roll, Pitch and Yaw偏角。 Roll and Pitch角是绝对角度（因为IMU使用了重力参考），而YAW角是相对角度。协方差矩阵用来指定方位测量的不确定度。当仅仅收到这个主题消息时，机器人位姿ekf还不会启动，因为它还需要来自主题'vo'或者'odom'的消息。

vo (nav_msgs/Odometry)

3D pose (used by Visual Odometry): 3D位姿可以完整表示机器人的位置和方位并给出位姿协方差。当用传感器只测量部分3D位姿(e.g. the wheel odometry only measures a 2D pose)时候, 可以给还未真正开始测量的部分3D位姿先简单指定一个大的协方差。

robot_pose_ekf node并不需要所有3个传感器源一直同时可用。每个源都能提供位姿和协方差，且这些源以不同速率和延时工作。随着时间推移某个源可能出现和消失，该node可以自动探测当前可用的源。如果要把自己想使用的传感器加入到输入源中，请参考指南 the Adding a GPS sensor tutorial。

2.1.2 Published Topics

robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)

滤波器输出 (估计的3D机器人位姿)

2.1.3 Provided tf Transforms

odom_combined → base_footprint

3 EKF如何工作

3.1 Pose interpretation

给滤波器node提供信息的所有传感器源都有自己的参考坐标系，并且随着时间推移都可能出现漂移现象。因此，每个传感器发出来的绝对位姿不能直接对比。因此该node使用每个传感器的相对位姿差异来更新扩展卡尔曼滤波器。

3.2 Covariance interpretation

当机器人在四周移动时候，随着时间推移位姿不确定性会变得越来越大，协方差会无边界的增长。这样一来发布位姿自身绝对协方差没有意义，因此传感器源会发布一段时间协方差的变化（例如，速速协方差）。

3.3 Timing

假定机器人上次更新位姿滤波器在t_0时刻，该node只有在收到每个传感器测量值（时间戳>t_0）之后才会进行下一次的滤波器更新。例如，在odom topic收到一条消息时间戳（t_1 > t_0）, 且在imu_data topic上也收到一条消息（其时间戳t_2 > t_1 > t_0）, 滤波器将被更新到所有传感器信息可用的最新时刻，这个时刻是t_1。在t_1时刻odom位姿直接给出了，但是imu位姿需要通过在t_0和t_2两时刻之间进行线性插值求得。在t_0到 t_1时间段，机器人位姿滤波器使用odom和IMU相对位姿进行更新。

上图是PR2机器人机器人的实验结果显示，从绿色初始位置开始移动最后回到出发位置。完美的odometry x-y曲线图应该是一个精确的闭环曲线图。上图蓝色线是轮式里程计的输入，蓝色点是其估计的结束位置。红色线是robot_pose_ekf的输出, robot_pose_ekf整合了轮式里程计和IMU的信息，给出了红色的结束位置点。

4 Package Status

4.1 Stability

该包的基础部分代码已经被充分测试且稳定较长时间。但是ROS API一直在随着消息类型的变化在被升级。

4.2 Roadmap

目前该滤波器设计被用于在PR2上使用的三种传感器 (wheel odometry, imu and vo)。下一步计划是让该包更加通用可以监听更多传感器源，所有发布均使用消息 (nav_msgs/Odometry) 。
增加速度到扩展卡尔曼滤波器状态中

5 Tutorials

robot_pose_ekf/Tutorials/AddingGpsSensor

2. 简单介绍协方差:转载http://blog.sina.com.cn/s/blog_4aa4593d01012am3.html

今天看论文的时候又看到了协方差矩阵这个破东西，以前看模式分类的时候就特困扰，没想到现在还是搞不清楚，索性开始查协方差矩阵的资料，恶补之后决定马上记录下来，嘿嘿~本文我将用自认为循序渐进的方式谈谈协方差矩阵。

统计学的基本概念

学过概率统计的孩子都知道，统计里最基本的概念就是样本的均值，方差，或者再加个标准差。首先我们给你一个含有n个样本的集合，依次给出这些概念的公式描述，这些高中学过数学的孩子都应该知道吧，一带而过。

均值：

标准差：

方差：

很显然，均值描述的是样本集合的中间点，它告诉我们的信息是很有限的，而标准差给我们描述的则是样本集合的各个样本点到均值的距离之平均。以这两个集合为例，[0，8，12，20]和[8，9，11，12]，两个集合的均值都是10，但显然两个集合差别是很大的，计算两者的标准差，前者是8.3，后者是 1.8，显然后者较为集中，故其标准差小一些，标准差描述的就是这种“散布度”。之所以除以n-1而不是除以n，是因为这样能使我们以较小的样本集更好的逼近总体的标准差，即统计上所谓的“无偏估计”。而方差则仅仅是标准差的平方。

为什么需要协方差？

上面几个统计量看似已经描述的差不多了，但我们应该注意到，标准差和方差一般是用来描述一维数据的，但现实生活我们常常遇到含有多维数据的数据集，最简单的大家上学时免不了要统计多个学科的考试成绩。面对这样的数据集，我们当然可以按照每一维独立的计算其方差，但是通常我们还想了解更多，比如，一个男孩子的猥琐程度跟他受女孩子欢迎程度是否存在一些联系啊，嘿嘿~协方差就是这样一种用来度量两个随机变量关系的统计量，我们可以仿照方差的定义：

来度量各个维度偏离其均值的程度，标准差可以这么来定义：

协方差的结果有什么意义呢？如果结果为正值，则说明两者是正相关的(从协方差可以引出“相关系数”的定义)，也就是说一个人越猥琐就越受女孩子欢迎，嘿嘿，那必须的~结果为负值就说明负相关的，越猥琐女孩子越讨厌，可能吗？如果为0，也是就是统计上说的“相互独立”。

从协方差的定义上我们也可以看出一些显而易见的性质，如：

协方差多了就是协方差矩阵

上一节提到的猥琐和受欢迎的问题是典型二维问题，而协方差也只能处理二维问题，那维数多了自然就需要计算

多个协方差，比如n维的数据集就需要计算个协方差，那自然而然的我们会想到使用矩阵来组织这些数据。给出协方差矩阵的定义：

这个定义还是很容易理解的，我们可以举一个简单的三维的例子，假设数据集有三个维度，则协方差矩阵为

可见，协方差矩阵是一个对称的矩阵，而且对角线是各个维度上的方差。

Matlab协方差实战

上面涉及的内容都比较容易，协方差矩阵似乎也很简单，但实战起来就很容易让人迷茫了。必须要明确一点，协方差矩阵计算的是不同维度之间的协方差，而不是不同样本之间的。这个我将结合下面的例子说明，以下的演示将使用Matlab，为了说明计算原理，不直接调用Matlab的cov函数(蓝色部分为Matlab代码)。

首先，随机产生一个10*3维的整数矩阵作为样本集，10为样本的个数，3为样本的维数。
MySample = fix(rand(10,3)*50)

根据公式，计算协方差需要计算均值，那是按行计算均值还是按列呢，我一开始就老是困扰这个问题。前面我们也特别强调了，协方差矩阵是计算不同维度间的协方差，要时刻牢记这一点。样本矩阵的每行是一个样本，每列为一个维度，所以我们要按列计算均值。为了描述方便，我们先将三个维度的数据分别赋值：

dim1 = MySample(:,1);
dim2 = MySample(:,2);
dim3 = MySample(:,3);

计算dim1与dim2，dim1与dim3，dim2与dim3的协方差：

sum( (dim1-mean(dim1)) .* (dim2-mean(dim2)) ) / ( size(MySample,1)-1 ) % 得到 74.5333
sum( (dim1-mean(dim1)) .* (dim3-mean(dim3)) ) / ( size(MySample,1)-1 ) % 得到 -10.0889
sum( (dim2-mean(dim2)) .* (dim3-mean(dim3)) ) / ( size(MySample,1)-1 ) % 得到 -10***000

搞清楚了这个后面就容易多了，协方差矩阵的对角线就是各个维度上的方差，下面我们依次计算：

std(dim1)^2 % 得到 108.3222
std(dim2)^2 % 得到 260.6222
std(dim3)^2 % 得到 94.1778

这样，我们就得到了计算协方差矩阵所需要的所有数据，调用Matlab自带的cov函数进行验证：

cov(MySample)

把我们计算的数据对号入座，是不是一摸一样？

Update：今天突然发现，原来协方差矩阵还可以这样计算，先让样本矩阵中心化，即每一维度减去该维度的均值，使每一维度上的均值为0，然后直接用新的到的样本矩阵乘上它的转置，然后除以(N-1)即可。其实这种方法也是由前面的公式通道而来，只不过理解起来不是很直观，但在抽象的公式推导时还是很常用的！同样给出 Matlab代码实现：

X = MySample – repmat(mean(MySample),10,1); % 中心化样本矩阵，使各维度均值为0
C = (X’*X)./(size(X,1)-1)

总结

理解协方差矩阵的关键就在于牢记它计算的是不同维度之间的协方差，而不是不同样本之间，拿到一个样本矩阵，我们最先要明确的就是一行是一个样本还是一个维度，心中明确这个整个计算过程就会顺流而下，这么一来就不会迷茫了~

P.S.写论文要选Latex，在wordpress里编辑公式还得用Latex，用Latex还真对得起咱学计算机这张脸~

例2：

a =
    -1     1     2
    -2     3     1
     4     0     3

for i=1:size(a,2)
    for j=1:size(a,2)
        c(i,j)=sum((a(:,i)-mean(a(:,i))).*(a(:,j)-mean(a(:,j))))/(size(a,1)-1);
    end
end

c =

10.3333 -4.1667 3.0000

-4.1667 2.3333 -1.5000

3.0000 -1.5000 1.0000

c为求得的协方差矩阵，在matlab以矩阵a的每一列为变量，对应的每一行为样本。这样在矩阵a中就有3个列变量分别为a（:,1), a(:,2), a(:,3)。

在协方差矩阵c中，每一个元素c(i,j)为对第i列与第j列的协方差，例如c(1,2) = -4.1667为第一列与第二列的协方差。

拿c(1,2)的求解过程来说

c(1,2)=sum((a(:,1)-mean(a(:,1))).*(a(:,2)-mean(a(:,2))))/(size(a,1)-1);

1. a(:,1)-mean(a(:,1))，第一列的元素减去该列的均值得到

-1.3333

-2.3333

3.6667

2, a(:,2)-mean(a(:,2))，第二列的元素减去该列的均值得到

-0.3333

1.6667

-1.3333

3, 再将第一步与第二部的结果相乘

-1.3333 -0.3333 0.4444

-2.3333 .* 1.6667 = -3.8889

3.6667 -1.3333 -4.8889

4, 再将结果求和/size(a,1)-1 得 -4.1667,该值即为c（1,2）的值。

再细看一下是不是与协方差公式：Cov(X,Y) = E{ [ (X-E(X) ] [ (Y-E(Y) ] } 过程基本一致呢，只是在第4步的时候matlab做了稍微的调整，自由度为n-1，减少了一行的样本值个数。

转载:https://blog.csdn.net/xiekaikaibing/article/details/80403264?utm_source=blogxgwz2

在使用robot_pose_ekf时，常遇到接收到的odometry数据格式错误的问题。一个可能的原因为底盘或其他设备发布odometry数据的协方差矩阵默认为0矩阵。解决的方法由两种：一种为在底盘将信息封装发布前对协方差矩阵进行初始化；另一种方法为在robot_pose_ekf中添加判断，如果接收到的odometry信息的协方差矩阵没有进行初始化，则进行初始化。

做工程时采用了第二种方法。初始化位于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确定协方差矩阵为0，故判断条件设得较为简单：odom_covariance_(1,1) == 0.0。判断条件应根据实际情况设定。协方差矩阵具体值可以考虑设置为精度的二次方。

关于卡尔曼滤波中协方差矩阵的设定没能深入学习理解，感觉较为复杂。但在做工程中确保odom的协方差矩阵对角线元素不均0则robot_pose_ekf即可工作。