kalman滤波理解三：协方差矩阵的计算

在整个kalmam滤波的操作过程中，有3个协方差矩阵是需要特殊注意的，也是很多人使用时不知如何设置和更新的，分别是状态协方差矩阵P，过程噪声协方差矩阵Q，测量噪声协方差矩阵R。

（一）状态协方差矩阵P

状态协方差矩阵P就是状态之间的协方差组成的矩阵，对角线元素是各个状态的方差，其余元素是相应元素的协方差，由此也能看出P是一个多维方阵，维度和状态数一致，且P是对称方阵。

比如状态X包含位置p和速度v两个量，则对应的协方差矩阵如下式所示：

                                         

 

由于相同变量之间的协方差就是其方差，因此对角线上元素分别是p和v的方差，其余两个元素分别是两元素间的协方差，由于协方差部分次序，协方差矩阵式对称的。

在使用时协方差矩阵P是一个迭代更新的量，每一轮预测和更新后，P都会更新一个新的值，因此初始化时可以根据估计协定，不用太苛求初始化的精准度，因为随着几轮迭代会越来越趋紧真实值。

---

（二）过程误差协方差矩阵Q

该矩阵的每一个元素分别是状态X的元素误差之间的协方差，以上文中的状态X为例，其包含位置p和速度v两个元素，其误差状态为：

                                          

其Q矩阵可用下式表示：

                              

该矩阵是由不确定的噪声引起的，确定Q的各元素大小是不容易的，使用时都是具体问题具体分析，比方说针对上文中的小车的状态，误差来源是移动中的打滑等，最底层的是由于力的变化导致的加速度的变化，因此我们找到了加速度方差，就可以推导出Q矩阵，假设加速度方差是D(a)。

根据，

                                        

得到，

                           

 因此，

                                       

---

 （三）测量噪声协方差矩阵R

测量噪声协方差的来源是传感器误差，即传感器的不准确性，在使用时一般传感器都会给出精度指标，该指标就可以直接转化到矩阵R中，还以上文小车为例，假设测量矩阵为下式所示：

                                             

 表示状态X中的位置p和速度v均可以测量，观测误差协方差矩阵R用下式表示：

                                     

此时对角线上就是各测量量的测量方差，从传感器说明书上均可以读到，另外针对状态之间的协方差吐过确定不了就可以设为0，即可以如下设置：

                                     

---

（四）总结

• P矩阵可以根据状态方差估计，不用太最求准确度，因为P矩阵会随着迭代次数的增加而收敛到准确值；
• Q矩阵是状态转移预测过程由于外部干扰产生，移动机器人中可以运动加速度的方差来推导，此时只要估计一个加速度的方差即可；
• R矩阵是传感器测量的不准确度，每个每个传感器都会给出测量对向的准确度，直接转换就能用。