卡尔曼滤波器及matlab代码

1、信息融合大作业-一维纳最速下降法滤波器，卡尔曼滤波器设计及Matlab仿真时间：2010-12-6专业：信息工程班级：09030702学号：2007302171 姓名：马志强1. 滤波问题浅谈估计器或滤波器这一术语通常用来称呼一个系统，设计这样的系统是为了从 含有噪声的数据中提取人们感兴趣的，接近规定质量的信息。由于这样一个宽 目标，估计理论应用于诸如通信、雷达、声纳、导航、地震学、生物医学工程、 金融工程等众多不同的领域。例如，考虑一个数字通信系统，其基本形式由发 射机、信道和接收机连接组成。发射机的作用是把数字源(例如计算机)产生的0、1符号序列组成的消息信号变换成为适合于信道上传送的波形

2、。而由于符号 间干扰和噪声的存在，信道输出端收到的信号是含有噪声的或失真的发送信号。 接收机的作用是，操作接收信号并把原消息信号的一个可靠估值传递给系统输 出端的某个用户。随着通信系统复杂度的提高，对原消息信号的还原成为通信 系统中最为重要的环节，而噪声是接收端需要排除的最主要的干扰，人们也设 计出了针对各种不同条件应用的滤波器，其中最速下降算法是一种古老的最优 化技术，而卡尔曼滤波器随着应用条件的精简成为了普适性的高效滤波器。2. 维纳最速下降算法滤波器2.1最速下降算法的基本思想考虑一个代价函数？?(？?)，它是某个未知向量？?的连续可微分函数。函数 ?(?)将?？的元素映射为实数。这里，

3、我们要寻找一个最优解 ？。使它满足如 下条件?(?o) < ?(?)(2.1) 这也是无约束最优化的数学表示。特别适合于自适应滤波的一类无约束最优化算法基于局部迭代下降的算法： 从某一初始猜想？?(0)出发，产生一系列权向量？?(1),?,？，使得代价函 数?(?)在算法的每一次迭代都是下降的，即?(?(?+ 1) < ?(?(?)其中?(?)是权向量的过去值，而？?(？+ 1)是其更新值。我们希望算法最终收敛到最优值？?o。迭代下降的一种简单形式是最速下降法，该方法是沿最速下降方向连续调整权向量。为方便起见，我们将梯度向量 表示为?(?) ? = ?(?)=-?(2.2)因此，最

4、速下降法可以表示为?(?+ 1) =?(?)- 1?(?)(2.3)其中？?代表进程，？?是正常数，称为步长参数，1/2因子的引入是为了数学上 处理方便。在从？?到?+ 1的迭代中，权向量的调整量为1?(?) = ?(?+ 1) - ?(?) = - 2?(?)(2.4)为了证明最速下降算法满足式(2.1)，在?(?)处进行一阶泰勒展开，得到?(?(?+ 1)?(?(?)+ ?(?)?(?)(2.5)此式对于？?较小时是成立的。在式(2.4)中设？?为负值向量，因而梯度向量？?也 为负值向量，所以使用埃尔米特转置。将式(2.4)用到式(2.5)中，得到?(?(?+ 1) ? ?(?(?)- 1

5、?|?(?)|2此式表明当？?为正数时，?(?(?+ 1) < ?(?(?)。因此，随着?的增加,TO °代价函数？?(?)减小，当?= %时，代价函数趋于最小值j2.2最速下降算法应用于维纳滤波器考虑一个横向滤波器，其抽头输入为？?(？?),？?(？- 1),? ,?(?- ?+ 1)， 对应的抽头权值为？?o(?),?1(?),? ,?- 1 (?)。抽头输入是来自零均值、相 关矩阵为？?的广义平稳随机过程的抽样值。除了这些输入外，滤波器还要一个 期望响应?(?)，以便为最优滤波提供一个参考。在时刻 ？?抽头输入向量表示 为?(?)，滤波器输出端期望响应的估计值为？ ?|

6、?),其中？?是由抽头输 ?(?),?(?- 1),? ,?(?- ?+ 1)所张成的空间。空过比较期望响应 ?(?)及其估计值，可以得到一个估计误差？?(?)，即?(?) = ?(?)- ?(?|?) = ?(?) - ?(?)?(?)(2.6)这里?(?)?(?)是抽头权向量？?(？?)与抽头输入向量？?(？?)的内积。？?(？?)可以进一步表示为?(?) = ?0(?),?1 (?),? ,?- 1(?)同样，抽头输入向量？?(？?)可表示为?(?) = ?(?),?(?- 1),? ,?(?- ?+ 1)?如果抽头输入向量？?(？?)和期望响应？?(？?)是联合平稳的，此时均方误差或

7、者在时刻？?的代价函数？?(?)是抽头权向量的二次函数，于是可以得到?(?) = ?2? - ?(?) ? - ?(?) + ?(?) ?(?)(2.2) 其中，？?2?为目标函数？?(？?)的方差，？?抽头输入向量？?(？?)与期望响应?(?)的互相关向量，及？?为抽头输入向量？?(?)的相关矩阵。从而梯度向量可以写?(?)?0(?)?(?)?(?) = ?1(?)?(?)? (-?0(?)?(?)? ( )_ ?1 (?) = - 2?+ 2?( ?)?(?)?-1(?)?(?)?-1(?)(2.8)?(？)?(？)其中在列向量中 莎為和丽需分别是代价函数？?(？?)对应第？?个抽头权值?

8、(?)的实部？?(？?)和虚部？?(？?)的偏导数。对最速下降算法应用而言,假设式(2.8)中相关矩阵？?和互相关向量？已知，贝U对于给定的抽头权向量?(? + 1)为?(? + 1) = ?(?) + ? - ?(?)、 1 / 丿 、丿(2.9)它描述了为那滤波中最速下降法的数学表达式3. 卡尔曼滤波器3.1卡尔曼滤波器的基本思想卡尔曼滤波器是用状态空间概念描述其数学公式的，另外新颖的特点是， 他的解递归运算，可以不加修改地应用于平稳和非平稳环境。尤其是，其状态 的每一次更新估计都由前一次估计和新的输入数据计算得到，因此只需存储前 一次估计。除了不需要存储过去的所有观测数据外，卡尔曼滤波计

9、算比直接根 据滤波过程中每一步所有过去数据进行估值的方法都更加有效。?i(?)?(?+ 1)?(?)?(?):+?(?)图3.1线性动态离散时间系统的信号流图表示“状态”的概念是这种表示的基础。状态向量，简单地说状态，定义为数据的最小集合，这组数据足以唯一地描述系统的自然动态行为。换句话说，状态由预测系统未来特性时所素要的，与系统的过去行为有关的最少的数据组成。典型 地，比较有代表性的情况是，状态？?(？?)是未知的。为了估计它，我们使用一组 观测数据，在途中用向量？?(？?)表示。y(?)成为观测向量或者简称观测值，并 假设它是？?维的。在数学上，图3.1表示的信号流图隐含着一下两个方程：(

10、1) 过程方程?(? + 1) = ?(?+ 1,?)?(?) + ?1(?)(3.1) 式中，M X1向量？1(?)表示噪声过程，可建模为零均值的白噪声过程，且 其相关矩阵定义为?(?) ?= ?1(?)?(?) = ?(7 ?丄？?？(2) 测量方程?(?) = ?(?)?(?) + ?(?)(3.2)其中？?(?)是已知的N XM测量矩阵。N X1向量？?(？?)称为测量噪声， 建模为零均值的白噪声过程，其相关矩阵为?2(?) ?(?)=稳?)?；?？(3.2) 测量方程(3.2)确立了可测系统输出？?(?)与状态?(?)之间的关系，如 图3.1所示。3.2新息过程为了求解卡尔曼滤波问题

11、，我们将应用基于新息过程的方法。根据之前所述， 用向量? ?|?-1)表示?= 1时刻到?- 1时刻所有观测数据过去值给定的情 况下，你时刻观测数据?(?)的最小均方估计。过去的值用观测值 ?(1),?(2),? ,?(?- 1)表示，他们张成的向量空间用？?-1表示。从而可以定 义新息过程如下：?(?) = ?(?) - ? ?|?- 1)(3.3) 其中M X1向量?(?)表示观测数据？?(？?)的新息。3.3应用新息过程进行状态估计下面，我们根据信息过程导出状态？?(？?)的最小均方估计。根据推导，这个 估计可以表示成为新息过程？?(1),?(2),? ,?(?)序列的线性组合，即?(?

12、|?)=刀?(?)?(?)?=1(3.4) 其中?(?)?=1是一组待定的? X?矩阵。根据正交性原理，预测状态误差 向量与新息过程正交，即?(?,?)?(?) = ?(?) - ?| ?(?) = ?(3.6)将式(3.5)代入式(3.6)，并利用新息过程的正交性质，即得?(?)?(?) = ?(?) ?(?)?(?) = ?(?)?(?)(3.7)因此，式(3.7)两边同时右乘逆矩阵？厂(？?)，可得？?(?)的表达式为?(?) = ?(?)?(?)?-*?)(3.5) 最后，将式(3.8)带入式(3.5)，可得最小军方差估计?(?|?)=刀?(?)?(?)?-"?)?(?)?=

13、 1?-1=刀?(?)?(?)?-1(?)?(?)?=1+ ? ?(?) ?( ?) ?- 1 ( ?) ?(?)(3.6) 故对于?= ?+ 1,有?(?+ 1|?)?-1? 彳=刀?(?+ 1)?(?)?-1(?)?(?)?=1+ ?(? + 1)?(?) ?-1( ?)?(?)(3.7) 然而，?+ 1时刻的状态？?(？+ 1)与？?时刻的状态？?(？?)的关系式由式可以推导 出对于0 < k < n，有?(?+ 1)?-(?) = ? ?(?+ 1,?)?(?) + ?1(?) ?(?)?=?(? + 1 ?) ?(?)? (?)1 J /|_丿丿(3.8) 其中？?(？?

14、)只与观测数据？?(1),?(2),? ,?(?)有关。因此可知，？1(?)与?(?) 彼此正交(其中0 < k < n)。利用式(3.11)以及当?= ?时?(?|?)的计算公 式，可将式(3.10)右边的求和项改写为?-1刀?(?+ 1)?(?) ?-1 (?)?(?)?=1?-1=?(?+ 1 ?) X ? ?(?) ?(?) ?-1 (?) ?(?)?=1=?(?+ 1,?) ?(?l?-1)(3.9) 为了进一步讨论，引入如下基本定义。3.4卡尔曼增益定义M X N矩阵?(?) = ?(?+ 1)?(?) ?- 1(?)(3.10) 其中?(?+ 1)?(?)是状态向量？

15、?(？+ 1)和新息过程？?(？?)的互相关矩 阵。利用这一定义和式(3.12)的结果，可以将式(3.10)简单重写为?(?+ 1|?) = ?(? + 1,?)?(?|?-1)+ ?(?) ?(?)(3.11) 式(3.14)具有明确的物理意义。它标明：线性动态系统状态的最小均方估计 ?(? + 1|?)可以由前一个估计？?(？|?-1)求得。为了表示对卡尔曼开创性 贡献的认可，将矩阵？?(？?)称为卡尔曼增益。现在剩下唯一要解决的问题是，怎样以一种便于计算的形式来表示卡尔曼增 益?(?)。为此，首先将?(? + 1)与?(?)乘积的期望表示为?(?+ 1) ?(?) = ?(? + 1,

16、?)?(?) ?(?, ? - 1) ?(?)(3.12) 式中利用了状态?(?)与噪声向量？?(n)互不相关这一事实。其次，由于预测状 态误差向量?(?, ?- 1)与估计?(?|?-1)正交，因此?(?|?-1)与 ?(?,?- 1)乘机的期望为零。这样，用预测状态误差向量？?(？?,？- 1)代替 相乘因子?(?)，将不会引起式(3.15)变化，故有? ?(? + 1)?(?)=?(?+ 1 ?)?(? ?- 1)?(? ?- 1) ?(?)(3.13) 由此，可将上式进一步变化为?(?+ 1)?(?) = ?(? + 1,?) ?(?, ?- 1)?(?)(3.14) 现在我们重新定义

17、卡尔曼增益。为此，将式(3.17)代入式(3.13)得?(?) = ?(? + 1,?)?(?,? - 1)?(?) ?- 1(?)现在我们已经了解了卡尔曼滤波的整个过程和相应的参数设置,便利用计算机仿真实现，特将其中参数变量进行小结。(3.18)为了能够更为方卡尔曼变量和参数小结变量定义维数?(?)?时刻状态? X 1?(?)?时刻状态值? X 1?(?+ 1,?)从？时刻到?+ 1时刻的转移矩阵? X ?(?)?时刻的测量矩阵? X ?(?)过程噪声？?(？?)的相关矩阵? X ?(?)过程噪声？?(？?)的相关矩阵? X ?(?|?-1)给定观测值?(1),?(2),? ,?(?- 1)

18、在？时刻状态的预测估计? X 1?(?|?)给定观测值?久1),?(2),? ,?(?)在？时刻状态的滤波估计? X 1?(?)?时刻卡尔曼增益矩阵? X ?(?)?时刻新息向量? X1?(?)新息向量？?(？?)的相关矩阵? X ?(?,?- 1)?(?|?- 1)中误差相关矩阵? X ?(?)?(?1?)中误差相关矩阵? X ?基于单步预测的卡尔曼滤波器的小结观测值=?(1),?(2),? ,?(?- 1)转移矩阵 =?(?+ 1,?)测量矩阵=?(?)过程噪声的相关矩阵=?(?)测量噪声的相关矩阵=?(?)?(?) = ?(?+ 1,?)?(?,? - 1)?(?)?(?)?(?,?-

19、1)?(?)+ ?(?) -1?(?) = ?(?) - ?(?) ?(?|?一1)?(? + 1|?) = ?(?+ 1,?)?(?|?-1) + ?(?)?(?)?(?) = ?(?, ?- 1) - ?(?, ?+ 1) ?(?)?(?)?(?,?- 1)?(?+ 1,?) = ?(?+ 1, ?)?(?) ?(?+ 1,?)+ ?1(?)4 Matlab 仿真为了简化，这里只讨论简单的一维单输入一单输出线性系统模型，其中加入白噪声作为系统的扰动，具体仿真结果可以获得如下4.1维纳最速下降法滤波器仿真结果以上为最速下降法中不同的递归步长所导致的跟踪效果变化，对于最速下降法中的步长是影响其

20、算法稳定的关键，最速下降算法稳定的充分必要条件是条件步长 因子为小于输入自相关矩阵的最大特征值倒数的2倍。上面的序列分别从相关矩阵的随大特征之2倍的0.4倍开始变化至其1倍，最后一幅图象能够看出其已经 不再收敛，下面是大于输入相关矩阵的最大特征值 2倍步长时所表现的跟踪结果可以看出其已经明显发散，不再是我们所期望的滤波算法。因此可以总结出，对 于最速下降法来说，步长的选取是很重要的，根据不同条件的需求，选取正确的 步长，能为算法的快速高效提供基础。4.2卡尔曼滤波器仿真结果从图中可以发现，卡尔曼滤波器能够非常有效地在比较大的干扰下比较准确 地反映真实值，如果观测端加入干扰较大时，卡尔曼滤波器能

21、够较为有效地进行 滤波，不过当状态端的干扰增大时，卡尔曼滤波器的滤波效果也会随之下降。 如 下图，是加大了状态端的干扰，所呈现的滤波效果。如上图所示，状态端的干扰导致状态不稳定，卡尔曼滤波器的估计值也出现 了比较大的波动。如果将状态端的干扰再增大，则会出现更为严峻的滤波考验， 滤波效果如下。这是的状态已经很勉强了，所以，研究更为有效的多方法卡尔曼滤波器也显 得十分必要了。4.3 一种不需初始化的卡尔曼滤波器仿真这种滤波器只是实现了无需对部分变量进行初始化的设计，没有特别意义上的改进经典卡尔曼滤波器本身性能的特点。仿真图如下4.4后联平滑滤波的卡尔曼滤波器仿真只是在经典卡尔曼滤波器后端联接了平滑

22、滤波器，对性能改进的效果并不特别明显，仿真图如下如图中所表示，即使平滑过的估值与观测值之间的差别也不是特别令人满意， 所以，对于经典卡尔曼滤波的研究还需要更深一步进行，由于时间和能力有限， 本次的作业对于卡尔曼及其他滤波器的研究只能达到这种程度，希望在以后的学习中，能发现更好的对经典卡尔曼滤波器的改进方法。5 Matlab源代码(部分参考自互联网)5.1经典卡尔曼滤波器clearN=200;w(1)=0;x(1)=5;a=1;c=1;Q1 = randn(1,N)*1;% 过程噪声Q2 = randn(1,N);% 测量噪声for k=2:N;x(k)=a*x(k-1)+Q1(k-1); en

23、d%状态矩阵for k=1:N;Y(k)=c*x(k)+Q2(k);e ndp(1)=10;s(1)=1；for t=2:N;Rww=cov(Q1(1:t);Rvv=cov(Q2(1:t);p1(t)=a.A2*p(t-1)+Rww;b(t)=c*p1(t)/(c42*p1(t)+Rvv);%kalma n增益s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1);p(t)=p1(t)-c*b(t)*p1(t);endt=1:N;plot(t,s,'r',t,Y,'g',t,x,'b');%红色卡尔曼，绿色观测值，蓝色状态值leg

24、e nd('kalma n estimate','ovservati on s','truth');5.2最速下降法clcclear allN=30;q=2.1;%q>1 &&q<2/Ryx 最大特征值hn=zeros(1,N);hn (:)=5;vg=0;Rxx=xcorr(1);Ryx=mi n(min( corrcoef(1, 1+ra ndn);echo offfor i=1:N-1;%vg=2*Rxx*h n(:,i)-2*Ryx;%h n(:,i+1)=h n(:,i)-1/2*q*vg;vg=2*Rxx*

25、h n(i)-2*Ryx;hn (i+1)=h n(i)-1/2*q*vg;m(i)=1;endt=1:N-1;plot(t,h n(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;%加过程噪声的状态输出for k = 2 : NQ1 = cov(Q(1:k-1);%过程噪声协方差Q2 = cov(R(1:k-1);x(k) = x(k - 1);% 预估