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

xingyun86 28天前 61

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矩阵是传感器测量的不准确度,每个每个传感器都会给出测量对向的准确度,直接转换就能用。

×
打赏作者
最新回复 (0)
只看楼主
全部楼主
返回