卡尔曼滤波原理
神笔马良ppt-档案管理规范
2023年3月18日发(作者：上古卷轴5技能).
word版
卡尔曼滤波器–KalmanFilter
1．什么是卡尔曼滤波器
〔WhatistheKalmanFilter〕
在学习卡尔曼滤波器之前，首先看看为什么叫“卡
尔曼〞。跟其他著名的理论〔例如傅立叶变换，泰
勒级数等等〕一样，卡尔曼也是一个人的名字，而
跟他们不同的是，他是个现代人！
卡尔曼全名RudolfEmilKalman，匈牙利数学家，
1930年出生于匈牙利首都布达佩斯。1953，1954年
于麻省理工学院分别获得电机工程学士及硕士学
位。1957年于哥伦比亚大学获得博士学位。我们现
在要学习的卡尔曼滤波器，正是源于他的博士论文
和1960年发表的论文?ANewApproachtoLinear
FilteringandPredictionProblems?〔线性滤波
与预测问题的新方法〕。如果对这编论文有兴趣，
可以到这里的地址下载：。
简单来说，卡尔曼滤波器是一个“optimalrecurs
ivedataprocessingalgorithm〔最优化自回归数
据处理算法〕〞。对于解决很大局部的问题，他是
最优，效率最高甚至是最有用的。他的广泛应用已
经超过30年，包括机器人导航，控制，传感器数据
.
word版
融合甚至在军事方面的雷达系统以及导弹追踪等
等。近年来更被应用于计算机图像处理，例如头脸
识别，图像分割，图像边缘检测等等。
2．卡尔曼滤波器的介绍
〔IntroductiontotheKalmanFilter〕
为了可以更加容易的理解卡尔曼滤波器，这里会应
用形象的描述方法来讲解，而不是像大多数参考书
那样罗列一大堆的数学公式和数学符号。但是，他
的5条公式是其核心内容。结合现代的计算机，其
实卡尔曼的程序相当的简单，只要你理解了他的那
5条公式。
在介绍他的5条公式之前，先让我们来根据下面的
例子一步一步的探索。
假设我们要研究的对象是一个房间的温度。根据你
的经历判断，这个房间的温度是恒定的，也就是下
一分钟的温度等于现在这一分钟的温度〔假设我们
用一分钟来做时间单位〕。假设你对你的经历不是
100%的相信，可能会有上下偏差几度。我们把这些
偏差看成是高斯白噪声〔WhiteGaussianNoise〕，
也就是这些偏差跟前后时间是没有关系的而且符合
高斯分配〔GaussianDistribution〕。另外，我们
.
word版
在房间里放一个温度计，但是这个温度计也不准确
的，测量值会比实际值偏差。我们也把这些偏差看
成是高斯白噪声。
好了，现在对于某一分钟我们有两个有关于该房间
的温度值：你根据经历的预测值〔系统的预测值〕
和温度计的值〔测量值〕。下面我们要用这两个值
结合他们各自的噪声来估算出房间的实际温度值。
假设我们要估算k时刻的是实际温度值。首先你要
根据k-1时刻的温度值，来预测k时刻的温度。因
为你相信温度是恒定的，所以你会得到k时刻的温
度预测值是跟k-1时刻一样的，假设是23度，同时
该值的高斯噪声的偏差是5度〔5是这样得到的：
如果k-1时刻估算出的最优温度值的偏差是3，你
对自己预测的不确定度是4度，他们平方相加再开
方，就是5〕。然后，你从温度计那里得到了k时
刻的温度值，假设是25度，同时该值的偏差是4度。
由于我们用于估算k时刻的实际温度有两个温度
值，分别是23度和25度。终究实际温度是多少呢？
相信自己还是相信温度计呢？终究相信谁多一点，
我们可以用他们的covariance来判断。因为Kg^2=
5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k
时刻的实际温度值是：23+0.78*(25-23)=24.56度。
.
word版
可以看出，因为温度计的covariance比拟小〔比拟
相信温度计〕，所以估算出的最优温度值偏向温度
计的值。
现在我们已经得到k时刻的最优温度值了，下一步
就是要进入k+1时刻，进展新的最优估算。到现在
为止，好似还没看到什么自回归的东西出现。对了，
在进入k+1时刻之前，我们还要算出k时刻那个最
优值〔24.56度〕的偏差。算法如下：((1-Kg)*5^2)
^0.5=2.35。这里的5就是上面的k时刻你预测的那
个23度温度值的偏差，得出的2.35就是进入k+1
时刻以后k时刻估算出的最优温度值的偏差〔对应
于上面的3〕。
就是这样，卡尔曼滤波器就不断的把covariance递
归，从而估算出最优的温度值。他运行的很快，而
且它只保存了上一时刻的covariance。上面的Kg，
就是卡尔曼增益〔KalmanGain〕。他可以随不同的
时刻而改变他自己的值，是不是很神奇！
下面就要言归正传，讨论真正工程系统上的卡尔曼。
3．卡尔曼滤波器算法
〔TheKalmanFilterAlgorithm〕
.
word版
在这一局部，我们就来描述源于DrKalman的卡尔
曼滤波器。下面的描述，会涉及一些根本的概念知
识，包括概率〔Probability〕，随即变量〔Rando
mVariable〕，高斯或正态分配〔GaussianDistr
ibution〕还有State-spaceModel等等。但对于卡
尔曼滤波器的详细证明，这里不能一一描述。
首先，我们先要引入一个离散控制过程的系统。该
系统可用一个线性随机微分方程〔LinearStochas
ticDifferenceequation〕来描述：
X(k)=AX(k-1)+BU(k)+W(k)
再加上系统的测量值：
Z(k)=HX(k)+V(k)
上两式子中，X(k)是k时刻的系统状态，U(k)是k
时刻对系统的控制量。A和B是系统参数，对于多
模型系统，他们为矩阵。Z(k)是k时刻的测量值，H
是测量系统的参数，对于多测量系统，H为矩阵。W
(k)和V(k)分别表示过程和测量的噪声。他们被假
设成高斯白噪声(WhiteGaussianNoise)，他们的
covariance分别是Q，R〔这里我们假设他们不随
系统状态变化而变化〕。
对于满足上面的条件(线性随机微分系统，过程和测
量都是高斯白噪声)，卡尔曼滤波器是最优的信息处
.
word版
理器。下面我们来用他们结合他们的covariances
来估算系统的最优化输出〔类似上一节那个温度的
例子〕。
首先我们要利用系统的过程模型，来预测下一状态
的系统。假设现在的系统状态是k，根据系统的模
型，可以基于系统的上一状态而预测出现在状态：
X(k|k-1)=AX(k-1|k-1)+BU(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)=AP(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)：
.
word版
X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))
………(3)
其中Kg为卡尔曼增益(KalmanGain)：
Kg(k)=P(k|k-1)H’/(HP(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个公式，可
以很容易的实现计算机的程序。
下面，用Matlab程序举一个实际运行的例子。
4．简单例子
〔ASimpleExample〕
这里我们结合第二第三节，举一个非常简单的例子
来说明卡尔曼滤波器的工作过程。所举的例子是进
.
word版
一步描述第二节的例子，而且还会配以程序模拟结
果。
根据第二节的描述，把房间看成一个系统，然后对
这个系统建模。当然，我们见的模型不需要非常地
准确。我们所知道的这个房间的温度是跟前一时刻
的温度一样的，所以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度，但是参加了标准偏差为几度的
高斯白噪声〔在图中为蓝线〕。
.
word版
为了令卡尔曼滤波器开场工作，我们需要告诉卡尔
曼两个零时刻的初始值，是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〕。
clear
N=200;
w(1)=0;
w=randn(1,N)
x(1)=0;
a=1;
fork=2:N;
x(k)=a*x(k-1)+w(k-1);
end
V=randn(1,N);
q1=std(V);
Rvv=q1.^2;
.
word版
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;
fort=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');
用matlab做的kalman滤波程序,已通过测试
－－－－－－－－－－－－－－－－－－－－－－
－－－－
.
word版
还有下面一个Matlab源程序，显示效果更好。
clear
clc;
N=300;
CON=25;%房间温度，假定温度是恒定的
%%%%%%%%%%%%%%%kalmanfilte
r%%%%%%%%%%%%%%%%%%%%%%
x=zeros(1,N);
y=2^0.5*randn(1,N)+CON;%加过程噪声的状
态输出
x(1)=1;
p=10;
Q=cov(randn(1,N));%过程噪声协方差
R=cov(randn(1,N));%观测噪声协方差
fork=2:N
x(k)=x(k-1);%预估计k时刻状态变量
的值
p=p+Q;%对应于预估值的协方差
kg=p/(p+R);%kalmangain
x(k)=x(k)+kg*(y(k)-x(k));
.
word版
p=(1-kg)*p;
end
%%%%%%%%%%%SmoothnessFilte
r%%%%%%%%%%%%%%%%%%%%%%%%
Filter_Wid=10;
smooth_res=zeros(1,N);
fori=Filter_Wid+1:N
tempsum=0;
forj=i-Filter_Wid:i-1
tempsum=tempsum+y(j);
end
smooth_res(i)=tempsum/Filter_Wid;
end
%figure(1);
%hist(y);
t=1:N;
figure(1);
expValue=zeros(1,N);
fori=1:N
.
word版
expValue(i)=CON;
end
plot(t,expValue,'r',t,x,'g',t,y,'b',t,smooth
_res,'k');
legend('expected','estimate','measure','smoo
thresult');
axis([0N2030])
xlabel('Sampletime');
ylabel('RoomTemperature');
title('SmoothfilterVSkalmanfilter');