![4c1ba49a86b43469ded63b4a4ca72491.png](https://i-blog.csdnimg.cn/blog_migrate/071974925ea5c3d315bc78d227cd021e.jpeg)
本文只是记录一下学习时公式推导过程,限于作者水平,不足之处请见谅。
卡尔曼滤波器解决的问题是:一般地,当可以由上一时刻状态估计当前时刻状态,且可通过传感器测量当前时刻状态,而两种方式都却有误差时,到底信哪一种方式得出的当前状态值更多些?即如何确定最优估计值。
![0dce9bf6579bbc6bd5df6671925c7801.png](https://i-blog.csdnimg.cn/blog_migrate/24af5bc302e3d600708bc78954280d3a.jpeg)
![3a9f6e3de13321efea6f57ded1ecc6c1.png](https://i-blog.csdnimg.cn/blog_migrate/07c515b8e80ec463f297cb9cbabf78de.jpeg)
![89d4f3c67f1ef5fa924deb0f16d93a66.png](https://i-blog.csdnimg.cn/blog_migrate/6d5874ae903706ff5ac4145cb3d68700.jpeg)
注:上面三张图中部分图片来自参考文献[2].
卡尔曼滤波器推导前的变量定义:
首先定义离散系统的状态方程
(1)
其中
设
两次估计的误差含义为:
误差
(2)
对于
(3)
先验估计
(4)
后验估计
(5)
因
(6)
因
(7)
至此,卡尔曼滤波基础原理中,差分方程、变量、误差、均方误差等变量的定义结束。
卡尔曼率滤波的5个基本公式:
公式 ①:状态预测,获得第k次状态的估计值
设激励
(8)
上式中
公式②:获得预测值
公式③:获得修正值
根据正交投影引理3,第
可以由第
(10)
根据正交投影引理
(12)
(13)
化简得
![5babed313e623c905042a3759fc23cee.png](https://i-blog.csdnimg.cn/blog_migrate/a487c6cb62b9a26a5e1c6df8d0c4d937.png)
(14)
公式④:获得卡尔曼增益
计算方法一:利用正交投影定理得
![1a46139d5d31a131d3c69add5e93a567.png](https://i-blog.csdnimg.cn/blog_migrate/3c964b07df341761edff12be37cd6639.png)
(15)
计算方法二:
为了估计最优,即后验估计
(16)
则,最优估计下卡尔曼增益应,如下式
(17)
公式⑤:获得第
(18)
带入卡尔曼增益
(19)
自此,卡尔曼滤波的基本原理与公式推导已经详细地给出。为了方便记忆和总结,卡尔曼的公式之间变量的关系及用到的数学技巧总结如图1。使用时只需依次迭代5个公式即可。
![e12eb06a7be65664a65b4043395ef8b3.png](https://i-blog.csdnimg.cn/blog_migrate/8aba5f220f1092f9b08dd6fb1c1092d5.jpeg)
注:
1、高斯分布的乘积还是高斯分布。
2、根据图1可以看出,卡尔曼增益的大小是由Q、R决定,标准的卡尔曼滤波在线性时不变系统下是可以离线计算出增益的。
卡尔曼增益最后会变成定值吗?www.zhihu.com参考文献:
[1] 赵树杰. 信号检测与估计理论[M]. 清华大学出版社有限公司, 2005
[2] Faragher R . Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation [Lecture Notes][J]. IEEE Signal Processing Magazine, 2012, 29(5):128-132.
[3] P.A. Bromiley;Products and Convolutions of Gaussian Probability Density Functions