今日主题--卡尔曼滤波器

原创 2012年03月23日 23:19:05

      自己开始做毕业设计到现在也都已经一个月了,接触的各种算法也都大概知道了轮廓,今天开始学习卡尔曼滤波器的实现。因为卡尔曼滤波器是自己学习的第一个主题,所以理论相比而言比较好,但是今天看完一个前辈的程序后,才更加深了理解。

    关于感触嘛,主要有两点:

       1.这篇文章是kalman先生的博士论文,可见是他年纪轻轻都提出的,值得学习,鞭策自己也写出如此经典 的文章。

       2.参数A和H说实话到现在还不知道该怎么确定,有经验的朋友可以指教下。


      以下载自网上资料,由于无法找到出处,无法确定作者。特此声明;

首先,我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation)来描述:
X(k)=A X(k-1)+B U(k)+W(k) 
再加上系统的测量值:
Z(k)=H X(k)+V(k) 
上两式子中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量。A和B是系统参数,对于多模型系统,他们为矩阵。Z(k)是k时刻的测量值,H是测量系统的参数,对于多测量系统,H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise),他们的covariance 分别是Q,R(这里我们假设他们不随系统状态变化而变化)。

对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出(类似上一节那个温度的例子)。

首先我们要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态:
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
式(1)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0。

到现在为止,我们的系统结果已经更新了,可是,对应于X(k|k-1)的covariance还没更新。我们用P表示covariance:
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
式(2)中,P(k|k-1)是X(k|k-1)对应的covariance,P(k-1|k-1)是X(k-1|k-1)对应的covariance,A’表示A的转置矩阵,Q是系统过程的covariance。式子1,2就是卡尔曼滤波器5个公式当中的前两个,也就是对系统的预测。

现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。结合预测值和测量值,我们可以得到现在状态(k)的最优化估算值X(k|k):
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain):
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

到现在为止,我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下X(k|k)的covariance:
P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)
其中I 为1的矩阵,对于单模型单测量,I=1。当系统进入k+1状态时,P(k|k)就是式子(2)的P(k-1|k-1)。这样,算法就可以自回归的运算下去。

卡尔曼滤波器的原理基本描述了,式子1,2,3,4和5就是他的5 个基本公式。根据这5个公式,可以很容易的实现计算机的程序。

下面,我会用程序举一个实际运行的例子。。。

4. 简单例子
(A Simple Example)

这里我们结合第二第三节,举一个非常简单的例子来说明卡尔曼滤波器的工作过程。所举的例子是进一步描述第二节的例子,而且还会配以程序模拟结果。

根据第二节的描述,把房间看成一个系统,然后对这个系统建模。当然,我们见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的,所以A=1。没有控制量,所以U(k)=0。因此得出:
X(k|k-1)=X(k-1|k-1) ……….. (6)
式子(2)可以改成:
P(k|k-1)=P(k-1|k-1) +Q ……… (7)

因为测量的值是温度计的,跟温度直接对应,所以H=1。式子3,4,5可以改成以下:
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=(1-Kg(k))P(k|k-1) ……… (10)

现在我们模拟一组测量值作为输入。假设房间的真实温度为25度,我模拟了200个测量值,这些测量值的平均值为25度,但是加入了标准偏差为几度的高斯白噪声(在图中为蓝线)。

为了令卡尔曼滤波器开始工作,我们需要告诉卡尔曼两个零时刻的初始值,是X(0|0)和P(0|0)。他们的值不用太在意,随便给一个就可以了,因为随着卡尔曼的工作,X会逐渐的收敛。但是对于P,一般不要取0,因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的,从而使算法不能收敛。我选了X(0|0)=1度,P(0|0)=10。

该系统的真实温度为25度,图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果(该结果在算法中设置了Q=1e-6,R=1e-1)。

××××××××××××××××××

附matlab下面的kalman滤波程序:

clear
N=200;
w(1)=0;
w=randn(1,N)
x(1)=0;
a=1;
for k=2:N;
x(k)=a*x(k-1)+w(k-1);
end


V=randn(1,N);
q1=std(V);
Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2; 
q3=std(w);
Rww=q3.^2;
c=0.2;
Y=c*x+V;

p(1)=0;
s(1)=0;
for t=2:N;
p1(t)=a.^2*p(t-1)+Rww;
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));
p(t)=p1(t)-c*b(t)*p1(t);
end

t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b');



版权声明:本文为博主原创文章,未经博主允许不得转载。

相关文章推荐

卡尔曼滤波器(THE KALMAN FILTER)的数学原理

0、内容提要这篇博客想试图证明卡尔曼滤波器(KALMAN FILTER),即就是说明KF(KALMAN FILTER)和EKF(EXTENDED KALMAN FILTER)算法中的步奏是怎么来的,为...

kalman 卡尔曼滤波器

理论基础 为什么需要协方差? 标准差和方差一般是用来描述一维数据的,但现实生活中我们常常会遇到含有多维数据的数据集,最简单的是大家上学时免不了要统计多个学科的考试成绩。面对这样的数据集...

浅谈卡尔曼滤波器

1795年,高斯为了测定行星轨道提出了最小二乘估计法。 1942年,维纳为了解决火力控制系统精度跟踪问题,提出了维纳滤波理论。 维纳滤波理论首次将数理统计理论与线性理论有机结合,形成了对随机信号最...

人工智能之卡尔曼滤波器浅析及matlab实战(很好的入门)

原文作者:Liu_LongPo 链接:http://blog.csdn.net/llp1992/article/details/45370025   卡尔曼滤波器是一种利用线性系统状态方程...

卡尔曼滤波器

卡尔曼滤波器 – Kalman Filter 1. 什么是卡尔曼滤波器 (What is the Kalman Filter?) 在学习卡尔曼滤波器之前,首先看看为什么叫“卡尔曼”。跟其他著名的理...

【初识】卡尔曼滤波器 Kalman Filter

卡尔曼滤波器 Kalman Filter (zz) 转自:http://carpa.bokee.com/4725695.html 关键词: 卡尔曼滤波器 Kalman Filter 在学...

卡尔曼滤波器 opencv

背景: 卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全及包含噪声的测量中,估计动态系统的状态。卡尔曼滤波的一个典型实例是从一组有限的,包含噪声的,对物体位置的观...

卡尔曼滤波器算法浅析及matlab实战

卡尔曼滤波器算法浅析及matlab实战 http://blog.csdn.net/llp1992/article/details/45370025 卡尔曼滤波器是一种利用线...

opencv 卡尔曼滤波器例子,自己修改过【转载】

【转载】http://hi.baidu.com/lin65505578/blog/item/7c03a0d4210b68c7a144dfe6.html   一、卡尔曼滤波器的理论解释      ...

卡尔曼(kalman)滤波器概述

kalman滤波器
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:深度学习:神经网络中的前向传播和反向传播算法推导
举报原因:
原因补充:

(最多只允许输入30个字)