针对的系统为:
状态方程 X(k)=AX(k-1)+Bu(k-1)+W(k-1)
测量方程 Z(k)=HX(k)+V(k) 0
W是状态预测的噪声 符合正态分布N(0,Q)
V 是测量的噪声 符合正态分布N(0,R)
理解:
一般测量值Z(k)为外界提供,大概是采用一种不同的方式去获取同一个目标值(值不一定相等),用来矫正预测的。于是我们得到了三个值,测量值Z(k),上一次综合预测值和测量值得到的最优值X(k-1|k-1),对上一次最优值通过状态方程预测的下一个结果
X(k|k-1)=AX(k-1|k-1)+Bu(k)。卡尔曼的滤波的目的是综合测量值和预测值得到一个最优值,这里采用的是分配权重的方式来综合结果X(k|k)=X(k|k-1)+K(k)(Z(k)-HX(k|k-1))。到此已经得到了最优的解,但是权重怎么计算?这里的权重K就是卡尔曼增益(Kalman gain)。K的确定是通过求使得估计值X(k|k)与真实值Xk的均方误差最小的K。得到K之后自然就是想办法使得卡尔曼滤波可以无限迭代下去,这好像是卡尔曼最牛逼的地方。
推导过程:
要看懂推导过程需要一些矩阵的计算知识以及统计学知识。
预测方程
![](https://img-blog.csdn.net/20171101221944441?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/Center)
综合预测值和测量值的最优解
![](https://img-blog.csdn.net/20171101222019008?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)
要求使得估计值与预测值的均方差最小,自然要先求均方差,这里是求协方差矩阵,协方差矩阵的对角线就是方差,只要求出了协方差矩阵就可以求该矩阵的痕迹即tr(矩阵的迹便是对角线上的元素累加起来),再对迹求导,使得求导后等于0,解方程即可得到K。
估计值X(k|k)和真实值X(k)之间的协方差矩阵
![](https://img-blog.csdn.net/20171101222034845?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)
预测值X(k|k-1)和真实值X(k)之间的协方差矩阵(别问为什么要求这个,这个是迭代的需要,具体我也不知道,接下来会用到)
![](https://img-blog.csdn.net/20171101222047899?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)
将0式带入2式,再将计算结果带入3式可以得到
![](https://img-blog.csdn.net/20171101222052562?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)
将5展开后将4式带入可以得到
![](https://img-blog.csdn.net/20171101222057435?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)
求6式的迹可以得到
![](https://img-blog.csdn.net/20171101222101931?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)
对7式求K(k)的偏导,并使之等于0可得到
![](https://img-blog.csdn.net/20171101222106746?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)
对8式移项可得
![](https://img-blog.csdn.net/20171101222110127?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)
将9式带入Pk可以得到
![](https://img-blog.csdn.net/20171101222114625?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)
到此还差P(k|k-1)未知
将4式展开计算即可以得到
![](https://img-blog.csdn.net/20171101222120010?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)
至此全部计算完毕。
标记黄色的即为卡尔曼的核心五条公式
使用:
使用步骤:
预测公式:
![](https://img-blog.csdn.net/20171101222134641?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)
更新公式:
![](https://img-blog.csdn.net/20171101222140939?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvRGFuZ19ib3k=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)
参考连接:
卡尔曼推导过程 http://blog.csdn.net/heyijia0327/article/details/17487467
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)