第三章卡尔曼(Kalman)滤波

1、第三章 卡尔曼(Kalman)滤波,第一节 引言,卡尔曼生平,卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们在现代控制理论中要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文A New Approach to Linear Filtering and Prediction Problems（线性滤波与预测问题的新方法）。,1.引言,卡尔曼(Kalman)滤波和维纳(Wiener)滤波都是以最小均方误差为准则的最佳线性估计或

2、滤波。,2.适用范围,维纳滤波只适用于平稳随机过程(信号) 卡尔曼滤波没有这个限制，信号可以是平稳的，也可以是非平稳的。,3.处理方法,维纳滤波器,根据全部过去的和当前的观测数据x(n),x(n-1), ,来估计信号的当前值,以均方误差最小条件下求解,系统的传递函数H(z)或单位冲激响应h(n),卡尔曼滤波,不需要全部过去的观察数据,来估计信号的当前值,它是用状态空间法描述系统， 即由状态方程和量测方程组成。,解是以估计值(是状态变量的估计值)的形式给出的,只根据前一个估计值 和最近一个观察数据,其算法是递推,且状态空间法采用 在时域内设计滤波器的方法,因而适用于多维随机过程的估计； 离散卡尔

3、曼算法适用计算机处理。,4.信号模型的建立,从信号模型的建立来看： 维纳滤波的信号模型是从信号与噪声的相关函数得到。 卡尔曼滤波的信号模型则是从状态方程和量测方程得到。,卡尔曼滤波器的特点是什么？,第二节 卡尔曼滤波器的信号模型 离散状态方程与量测方程,引入,在讨论维纳滤波时，提出一个基本概念： 任何具有有理功率谱密度的随机信号都可看作是白色噪声通过一个线性网络所形成。 由此得到维纳滤波器的信号模型,为了得到卡尔曼过滤的信号模型，必须首先讨论状态方程和量测方程。,一、离散状态方程及其解,离散状态方程的基本形式是：,其中x(k)代表一组状态变量组成的多维状态矢量， 而A，B都是矩阵，它们是由系统

4、的拓扑结构、元件性质和数值所确定的。,是激励信号。,状态方程是多维一阶的差分方程。 当已知初始状态x(0)， 可用递推的方法得到它的解,。,总结,二、离散时间系统的量测方程,卡尔曼滤波的信号模型,多维情况,卡尔曼滤波的信号模型,一维情况,例1,解：,第三节 卡尔曼滤波的方法,1、卡尔曼滤波的基本思想,2、研究对象离散系统,离散系统的n维状态方程：,离散系统的m维量测方程：,3、卡尔曼滤波一步递推法模型,4、求卡尔曼滤波递推公式,新息过程的性质,5、离散卡尔曼滤波算法总结,状态方程：,量测方程：,统计特性：,初始条件：,递推公式：,增益方程：,均方误差阵：,6、卡尔曼滤波算法的计算流程图,例2,

5、解：,7、一步预测估计的卡尔曼预测器,8、预测与滤波之间比较,9、同时有过滤和预测输出的方框图,由图可知：能够从卡尔曼滤波器中获得一步预测。,例3,解：,Kalman预测的跟踪性能,增益的变化曲线,10、卡尔曼滤波公式中各个参数之间关系,第四节 卡尔曼滤波与维纳滤波的关系,1、举例,2、结论,3、卡尔曼滤波与维纳滤波不同,(1)卡尔曼滤波与维纳滤波中解决最佳滤波的方法不相同。 维纳滤波：是用频域及传递函数的方法； 卡尔曼滤波：是用时域及状态变量的办法；,(2)卡尔曼在理论上是维纳滤波的推广和发展，特别在处理多变量系统、时变线性系统及非线性系统的最佳滤波等领域，为我们提供了一种比较有效的方法，克

6、服了基于频域处理所遇到的困难。 这些困难包括：维纳滤波要求平稳，而卡尔曼滤波则不要求； 卡尔曼容许初始时间不是负无穷大，这在很多情况下是有实际意义的；,(3)卡尔曼滤波的另一个不同点是把状态或信号过程的产生看成是白噪声激励有限维数系统的输出； 维纳滤波要求过程的自相关函数和互相关函数的简单知识，而卡尔曼滤波则要求时域中状态变量及信号产生过程的详细知识。,4、卡尔曼滤波的优点,在时域上采用线性递推形式对观测值进行处理，能实时地给出系统状态的最优估计，并突破了单维输入和输出的限制。 卡尔曼滤波算法的这些优点使它在信号和信息系统中得到比较广泛的应用。,5、卡尔曼滤波的缺点,（1）模型误差和数值发散。

7、 模型误差:卡尔曼滤波算法的关键是建立系统的状态模型。但实际系统有时很难得到精确描述，往往只能用近似模型来代替，因为即使能够获得精确的模型，也常会因为精确模型太复杂，维数过高而与实时处理必须减少计算量及尽量简化模型的要求相矛盾。近似或简化的模型都与精确模型之间存在误差，模型误差必然会给滤波带来影响，严重时还会造成滤波结果不收敛。 抑制方法：采用逐渐衰减记忆法、限定记忆法、限定下界法和人为增加模型输入噪声方差。,数值发散:舍入误差的影响以及递推算法使得舍入误差积累的影响。计算机存贮单元的长度有限，不可避免地存在舍入误差，它相当于在状态方程和量测方程中加入噪声，带来的后果是有可能改变某些矩阵的性质

8、，引起误差矩阵失去正定性和对称性，如均方误差阵列受到扰动而离开稳定解，如没失去正定性，仍可返回稳定解，可用双精度运算得以改善，但会增加运算量，目前采用平方根法，即求均方误差阵P改用其平方根P1/2实现。,(2)实时要求。影响卡尔曼滤波算法的实时性主要是状态维数n和增益矩阵的计算，它们往往有很大的计算量。 一般在计算中采取某些措施，例如应用定常系统新算法或在精度损失允许情况下尽量减小维数等措施，从而减小计算量以满足实时滤波的要求。,6、卡尔曼滤波的应用,在空间技术、工业过程控制与电子工程等领域得到了比较广泛的应用，特别在信号处理的二次加工数据处理方面应用更广，诸如雷达的位置、速度的估计，以及空中

9、交通管制系统对飞行器航迹的估计与导航等领域都得到了广泛而成功的应用。,(1)应用举例-雷达跟踪目标物,说明卡尔曼滤波的应用。 雷达跟踪目标的基本原理是通过发射脉冲，根据接收到的脉冲与发射脉冲的时间间隔，来确定目标物的距离和速度。 由于干扰的影响，接收到的脉冲波形变化很大，那么一次的测量结果可能存在很大的误差。 为了减小误差，往往采取发射一串脉冲的方法进行测量。,例4作业,解：,蒙特卡洛方法是指对多次产生随机数来模拟过程，并通过求平均来得到我们所需要的过程。这里就要求我们仿真100次。,MATLAB程序： clc; clear; T=5;%雷达扫描周期 num=100; %*产生真实轨迹* N=

10、800/T; x=zeros(N,1);y=zeros(N,1); vx=zeros(N,1);vy=zeros(N,1); x(1)=-2000; y(1)=500; vx=10; vy=0; ax=0; ay=0;%跟踪加速度 var=100;%观察目标位置的变化量,%产生真实轨迹 for i=1:N-1 x(i+1)=x(i)+vx*T+0.5*ax*T2; y(i+1)=y(i)+vy*T+0.5*ay*T2; end nx=zeros(N,1);ny=zeros(N,1); nx=var*randn(N,1); ny=var*randn(N,1); zx=x+nx; zy=y+ny;

11、 %真实目标轨迹,%滤波100次 for m=1:num z=2:1; xks(1)=zx(1); yks(1)=zy(1); xks(2)=zx(2); yks(2)=zy(2); o=4:4;g=4:2;h=2:4;q=2:2;xk=4:1;perr=4:4; o=1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1; h=1 0 0 0;0 0 1 0; g=T/2,0;T/2,0;0,T/2;0,T/2; q=10000 0;0 10000; perr=var2,var2/T,0,0;var*var/T, 2*var2/(T2),0,0;0,0,var2,var2/T;0,0

12、,var2/T,2*var2/(T2); vx=(zx(2)-zx(1)/2; vy=(zy(2)-zy(1)/2; xk=zx(1);vx;zy(1);vy;,%Kalman滤波开始 for r=3:N; z=zx(r);zy(r); xk1=o*xk; perr1=o*perr*o; k=perr1*h*inv(h*perr1*h+q); xk=xk1+k*(z-h*xk1); perr=(eye(4)-k*h)*perr1; xks(r)=xk(1,1); yks(r)=xk(3,1); vkxs(r)=xk(2,1); ykxs(r)=xk(4,1); xkls(r)=xk1(1,1)

13、; ykls(r)=xk1(3,1); perr11(r)=perr(1,1); perr12(r)=perr(1,2); perr22(r)=perr(2,2); rex(m,r)=xks(r); rey(m,r)=yks(r); end %结束一次滤波 end %结束滤波,ex=0;ey=0; eqx=0;eqy=0; ey1=0; ex1=N:1; ey1=N:1; %计算滤波的均值%计算滤波误差的均值 for i=1:N for j=1:num ex=ex+x(i)-rex(j,i); ey=ey+y(i)-rey(j,i); end ex1(i)=ex/num; ey1(i)=ey/num; ex=0;eqx=0;ey=0;eqy=0; end,%绘图 figure(1); plot(