卡尔曼滤波器及matlab代码

欧阳学文信息融合大作业——维纳最速下降法滤波器，卡尔曼滤波器设计及Matlab仿真时间：126专业：信息工程班级：09030702学号：302171姓名：马志强滤波问题浅谈预计器或滤波器这一术语普通用来称呼一种系统，设计这样的系统是为了从含有噪声的数据中提取人们感爱好的，靠近规定质量的信息。由于这样一种宽目的，预计理论应用于诸如通信、雷达、声纳、导航、地震学、生物医学工程、金融工程等众多不同的领域。例如，考虑一种数字通信系统，其基本形式由发射机、信道和接受机连接构成。发射机的作用是把数字源(例如计算机)产生的0、1符号序列构成的消息信号变换成为适合于信道上传送的波形。而由于符号间干扰和噪声的存在，信道输出端收到的信号是含有噪声的或失真的发送信号。接受机的作用是，操作接受信号并把原消息信号的一种可靠估值传递给系统输出端的某个顾客。随着通信系统复杂度的提高，对原消息信号的还原成为通信系统中最为重要的环节，而噪声是接受端需要排除的最重要的干扰，人们也设计出了针对多个不同条件应用的滤波器，其中最速下降算法是一种古老的最优化技术，而卡尔曼滤波器随着应用条件的精简成为了普适性的高效滤波器。2．维纳最速下降算法滤波器2.1最速下降算法的基本思想考虑一种代价函数，它是某个未知向量的持续可微分函数。函数将的元素映射为实数。这里，我们要寻找一种最优解。使它满足以下条件 (2.1)这也是无约束最优化的数学表达。特别适合于自适应滤波的一类无约束最优化算法基于局部迭代下降的算法：从某一初始猜想出发，产生一系列权向量，使得代价函数在算法的每一次迭代都是下降的，即其中是权向量的过去值，而是其更新值。我们但愿算法最后收敛到最优值。迭代下降的一种简朴形式是最速下降法，该办法是沿最速下降方向持续调节权向量。为方便起见，我们将梯度向量表达为(2.2)因此，最速下降法能够表达为(2.3)其中代表进程，是正常数，称为步长参数，1/2因子的引入是为了数学上解决方便。在从到的迭代中，权向量的调节量为(2.4)为了证明最速下降算法满足式(2.1)，在处进行一阶泰勒展开，得到(2.5)此式对于较小时是成立的。在式(2.4)中设为负值向量，因而梯度向量也为负值向量，因此使用埃尔米特转置。将式(2.4)用到式(2.5)中，得到此式表明当为正数时，。因此，随着的增加，代价函数减小，当时，代价函数趋于最小值。2.2最速下降算法应用于维纳滤波器考虑一种横向滤波器，其抽头输入为，对应的抽头权值为。抽头输入是来自零均值、有关矩阵为的广义平稳随机过程的抽样值。除了这些输入外，滤波器还要一种盼望响应，方便为最优滤波提供一种参考。在时刻抽头输入向量表达为，滤波器输出端盼望响应的预计值为,其中是由抽头输所张成的空间。空过比较盼望响应及其预计值，能够得到一种预计误差，即(2.6)这里是抽头权向量与抽头输入向量的内积。能够进一步表达为同样，抽头输入向量可表达为 如果抽头输入向量和盼望响应是联合平稳的，此时均方误差或者在时刻的代价函数是抽头权向量的二次函数，于是能够得到(2.7)其中，为目的函数的方差，抽头输入向量与盼望响应的互有关向量，及为抽头输入向量的有关矩阵。从而梯度向量能够写为(2.8)其中在列向量中和分别是代价函数对应第个抽头权值的实部和虚部的偏导数。对最速下降算法应用而言，假设式(2.8)中有关矩阵和互有关向量已知，则对于给定的抽头权向量为(2.9)它描述了为那滤波中最速下降法的数学体现式。3.卡尔曼滤波器3.1卡尔曼滤波器的基本思想卡尔曼滤波器是用状态空间概念描述其数学公式的，另外新颖的特点是，他的解递归运算，能够不加修改地应用于平稳和非平稳环境。特别是，其状态的每一次更新预计都由前一次预计和新的输入数据计算得到，因此只需存储前一次预计。除了不需要存储过去的全部观察数据外，卡尔曼滤波计算比直接根据滤波过程中每一步全部过去数据进行估值的办法都更加有效。++++图3.1线性动态离散时间系统的信号流图表达“状态”的概念是这种表达的基础。状态向量，简朴地说状态，定义为数据的最小集合，这组数据足以唯一地描述系统的自然动态行为。换句话说，状态由预测系统将来特性时所素要的，与系统的过去行为有关的最少的数据构成。典型地，比较有代表性的状况是，状态是未知的。为了预计它，我们使用一组观察数据，在途中用向量表达。成为观察向量或者简称观察值，并假设它是维的。在数学上，图3.1表达的信号流图隐含着一下两个方程：过程方程(3.1)式中，向量表达噪声过程，可建模为零均值的白噪声过程，且其有关矩阵定义为测量方程(3.2)其中是已知的测量矩阵。向量称为测量噪声，建模为零均值的白噪声过程，其有关矩阵为(3.3)测量方程(3.2)确立了可测系统输出与状态之间的关系，如图3.1所示。3.2新息过程为了求解卡尔曼滤波问题，我们将应用基于新息过程的办法。根据之前所述，用向量表达时刻届时刻全部观察数据过去值给定的状况下，你时刻观察数据的最小均方预计。过去的值用观察值表达，他们张成的向量空间用表达。从而能够定义新息过程以下：(3.4)其中向量表达观察数据的新息。3.3应用新息过程进行状态预计下面，我们根据信息过程导出状态的最小均方预计。根据推导，这个预计能够表达成为新息过程序列的线性组合，即 (3.5)其中是一组待定的矩阵。根据正交性原理，预测状态误差向量与新息过程正交，即(3.6)将式(3.5)代入式(3.6)，并运用新息过程的正交性质，即得(3.7)因此，式(3.7)两边同时右乘逆矩阵，可得的体现式为 (3.8)最后，将式(3.8)带入式(3.5)，可得最小军方差预计 (3.9)故对于，有 (3.10)然而，时刻的状态与时刻的状态的关系式由式能够推导出对于，有 (3.11)其中只与观察数据有关。因此可知，与彼此正交(其中)。运用式(3.11)以及当时的计算公式，可将式(3.10)右边的求和项改写为 (3.12)为了进一步讨论，引入以下基本定义。3.4卡尔曼增益定义矩阵 (3.13)其中是状态向量和新息过程的互有关矩阵。运用这一定义和式(3.12)的成果，能够将式(3.10)简朴重写为 (3.14)式(3.14)含有明确的物理意义。它标明：线性动态系统状态的最小均方预计能够由前一种预计求得。为了表达对卡尔曼开创性奉献的承认，将矩阵称为卡尔曼增益。现在剩余唯一要解决的问题是，如何以一种便于计算的形式来表达卡尔曼增益。为此，首先将与乘积的盼望表达为 (3.15)式中运用了状态与噪声向量互不有关这一事实。另首先，由于预测状态误差向量与预计正交，因此与乘机的盼望为零。这样，用预测状态误差向量替代相乘因子，将不会引发式(3.15)变化，故有 (3.16)由此，可将上式进一步变化为 (3.17)现在我们重新定义卡尔曼增益。为此，将式(3.17)代入式(3.13)得 (3.18)现在我们已经理解了卡尔曼滤波的整个过程和对应的参数设立，为了能够更为方便运用计算机仿真实现，特将其中参数变量进行小结。卡尔曼变量和参数小结变量定义维数时刻状态时刻状态值从时刻届时刻的转移矩阵时刻的测量矩阵过程噪声的有关矩阵过程噪声的有关矩阵给定观察值在时刻状态的预测预计给定观察值在时刻状态的滤波预计时刻卡尔曼增益矩阵时刻新息向量新息向量的有关矩阵中误差有关矩阵中误差有关矩阵基于单步预测的卡尔曼滤波器的小结观察值=转移矩阵=测量矩阵=过程噪声的有关矩阵=测量噪声的有关矩阵=4Matlab仿真为了简化，这里只讨论简朴的一维单输入—单输出线性系统模型，其中加入白噪声作为系统的扰动，具体仿真成果能够获得以下4.1维纳最速下降法滤波器仿真成果以上为最速下降法中不同的递归步长所造成的跟踪效果变化，对于最速下降法中的步长是影响其算法稳定的核心，最速下降算法稳定的充足必要条件是条件步长因子为不大于输入自有关矩阵的最大特性值倒数的2倍。上面的序列分别从有关矩阵的随大特性之2倍的0.4倍开始变化至其1倍，最后一幅图象能够看出其已经不再收敛，下面是不不大于输入有关矩阵的最大特性值2倍步长时所体现的跟踪成果能够看出其已经明显发散，不再是我们所盼望的滤波算法。因此能够总结出，对于最速下降法来说，步长的选用是很重要的，根据不同条件的需求，选用对的的步长，能为算法的快速高效提供基础。4.2卡尔曼滤波器仿真成果从图中能够发现，卡尔曼滤波器能够非常有效地在比较大的干扰下比较精确地反映真实值，如果观察端加入干扰较大时，卡尔曼滤波器能够较为有效地进行滤波，但是当状态端的干扰增大时，卡尔曼滤波器的滤波效果也会随之下降。以下图，是加大了状态端的干扰，所呈现的滤波效果。如上图所示，状态端的干扰造成状态不稳定，卡尔曼滤波器的预计值也出现了比较大的波动。如果将状态端的干扰再增大，则会出现更为严峻的滤波考验，滤波效果以下。这是的状态已经很勉强了，因此，研究更为有效的多办法卡尔曼滤波器也显得十分必要了。4.3一种不需初始化的卡尔曼滤波器仿真这种滤波器只是实现了无需对部分变量进行初始化的设计，没有特别意义上的改善典型卡尔曼滤波器本身性能的特点。仿真图以下4.4后联平滑滤波的卡尔曼滤波器仿真只是在典型卡尔曼滤波器后端联接了平滑滤波器，对性能改善的效果并不特别明显，仿真图以下如图中所示，即使平滑过的估值与观察值之间的差别也不是特别令人满意，因此，对于典型卡尔曼滤波的研究还需要更深一步进行，由于时间和能力有限，本次的作业对于卡尔曼及其它滤波器的研究只能达成这种程度，但愿在后来的学习中，能发现更加好的对典型卡尔曼滤波器的改善办法。5Matlab源代码(部分参考自互联网)5.1典型卡尔曼滤波器clearN=200;w(1)=0;x(1)=5;a=1;c=1;Q1=randn(1,N)*1;%过程噪声Q2=randn(1,N);%测量噪声fork=2:N;x(k)=a*x(k1)+Q1(k1);end%状态矩阵fork=1:N;Y(k)=c*x(k)+Q2(k);endp(1)=10;s(1)=1;fort=2:N;Rww=cov(Q1(1:t));Rvv=cov(Q2(1:t));p1(t)=a.^2*p(t1)+Rww;b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);%kalman增益s(t)=a*s(t1)+b(t)*(Y(t)a*c*s(t1));p(t)=p1(t)c*b(t)*p1(t);endt=1:N;plot(t,s,'r',t,Y,'g',t,x,'b');%红色卡尔曼，绿色观察值，蓝色状态值legend('kalmanestimate','ovservations','truth');5.2最速下降法clcclearallN=30;q=2.1;%q>1&&q<2/Ryx最大特性值hn=zeros(1,N);hn(:)=5;vg=0;Rxx=xcorr(1);Ryx=min(min(corrcoef(1,1+randn)));echoofffori=1:N1;%vg=2*Rxx*hn(:,i)2*Ryx;%hn(:,i+1)=hn(:,i)1/2*q*vg;vg=2*Rxx*hn(i)2*Ryx;hn(i+1)=hn(i)1/2*q*vg;m(i)=1;endt=1:N1;plot(t,hn(t),'r',t,m(t),'b');5.3后联平滑滤波器的卡尔曼滤波器clearclc;N=300;CON=5;x=zeros(1,N);x(1)=1;p=10;Q=randn(1,N)*0.2;%过程噪声协方差R=randn(1,N);%观察噪声协方差y=R+CON;%加过程噪声的状态输出fork=2:NQ1=cov(Q(1:k1));%过程噪声协方差Q2=cov(R(1:k1));x(k)=x(k1);%预预计k时刻状态变量的值p=p+Q1;%对应于预估值的协方差kg=p/(p+Q2);%kalmangainx(k)=x(k)+kg*(y(k)x(k));p=(1kg)*p;endFilter_Wid=10;smooth_res=zeros(1,N);kalman_p=zeros(1,N);fori=Filter_Wid+1:Ntempsum=0;kalman_m=0;forj=iFilter_Wid:i1tempsum=tempsum+y(j);kalman_m=kalman_m+x(j);endkalman_p(i)=kalman_m/Filter_Wid;smooth_res(i)=tempsum/Filter_Wid;%平滑滤波end%figure(1);%hist(y);t=1:N;figure(1);expValue=zeros