1.介绍
卡尔曼滤波的本质是参数化的贝叶斯模型,通过对下一时刻系统的初步状态估计(即状态的先验估计)以及测量得出的反馈相结合,最终得到该时刻较为准确的的状态估计(即状态的后验估计)。
其核心思想即为预测+测量反馈。
Kalman fliter是线性滤波器
2.经典的5个公式:
状态预测
“小帽子”表示估计,“ - ”表示用上一时刻的量估计而来的,
F 是状态转移矩阵,表示怎么由上一状态量怎么推测当前时刻状态;B是控制矩阵,表示控制量u怎么作用当前状态。
所有推测都包含噪声,所有噪声都假定为高斯白噪声,噪声越大不确定性越大,用协方差矩阵来表示不确定性。
主对角线上表示两个维度的方差,反对角线上表示两维度的协方差kalman filter中所有不确定性的描述都要用协方差矩阵。
噪声协方差矩阵的传递
每一时刻的协方差用P表示,协方差矩阵在每个时刻间传递
协方差矩阵Q表示P预测时的不确定性。
备注1:噪声协方差矩阵的推导
协方差矩阵的性质:
观察矩阵
v表示测量误差(噪声),观测噪声的协方差矩阵:R
测量矩阵H可以表示多种测量方式,这也体现了kalman filter的数据融合的特性
状态更新
括号里表示实际测量值与预测值之间的残差,为什么预测值要乘上观测矩阵H?因为让测量值和观测值维度一致,可以相减。
K为卡尔曼增益
定性分析:
1.权衡预测协方差P和观测协方差R的大小来决定相信预测模型多一点还是相信观测模型多一点
2.把残差的表现形式从观察域变到状态域(即让残差的维度与前者相同,可以相加)
噪声协方差矩阵的更新
这个值是留给下一次迭代用的
备注2:卡尔曼增益和协方差矩阵推导:
测量值与估计值之间的误差为:
由公式(2)(3)可得
此处有个小错误,应该是-K,结果是对的。
协方差矩阵为:
因为为向量,所以要加转置。
同理可得:
所以对展开得到:
因为协方差矩阵的对角线元素就是方差,而方差表示的就是噪声的能量,所以求的方差最小,则噪声也越小,所以求迹tr
对未知量K求导为零的值,则是方差和最小的对应的K
上式中的P越大,则K越大,从实际上P越大表示噪声越大估计值越不准确,所以需要测量值来修正,则K需越大
带入式中的:
公式(1)(2)表示预测,公式(3)(4)(5)用来更新
3.例子:小车位置与速度
2.http://blog.csdn.net/u012211419/article/details/49821393
转载请注明出处