卡尔曼滤波器及matlab代码

1、时间：2021.03. 06信息融合丸作业创作：欧阳道维纳最速下啥法谑波彖，卡余曼滤波器设计及Matlab仿真时间：2010-12-6专业：信息工程班级：09030702 学号：2007302171 期名：马志强1. 滤波问题肉淡估计器或滤波器这一术语通常用来称呼一个糸统， 设计这样的糸统是为了从含有噪步的数擔中提取人们 感兴趣.的，接近规定质量的信息。由于这样一个宽目 捺，估计理论应用于诸如通信、雷达、步纳、导航、 地震学、生扬医学工程、全融工程等众多不同的领 域。例如，考虑一个数字通信糸统，其基本形式由发 対机、信道和接收机连接组成。发射机的作用是把数 字源（例如计算机）产生的0、1符号序

2、列组成的谄息信 号变换成为适合于信道上传送的波形。而由于符号I可 干扰和噪步的存淮.，信道输出端收到的信号是含有噪 步的或夾真的发送信号。接收机的作用是，操作接收 信号并把原谄息信号的一个可靠估值传适给糸统输出 端的某个用户。随着通信糸统复杂度的提嵩，对原谄 息信号的还原成为通信糸统中最为重要的环节，而噪 步是接收端需要排除的最主要的干扰，人们也设计出 了针对各种不同条件应用的滤波器，其中最速下啥算 比是一种古老的最优化技术，而卡余曼滤波器随着应 用条件的精简成为了普适性的嵩效滤波器。2、维纳最速下阵算法滤波器2.1最速下啥算法的基本思想考虑一个代价函数/（"）,它是某个未知向量的连

3、续可 微分函数。曲数/3）将w的元素映鉗为卖数。这里，我 们要寻找一个最优鮮W。使它满足如下条件（2.1）这也是无约束最优化的数学表示。特别适合于自适应滤波的一类无约束最优化算法基 于局部迭代下阵的算法：从某一初始精想w（O）出发，产生一糸刊权向量W,w，使得代价函数j（w）往算法的毎一次迭代 都是下阵的，即其中W（n）是权向量的过去值，而、v（n十1）是其更新 值。我们希望算出最终收效到最优值叫。迭代下阵的一 种简单形式是最速下啥比，该方比是沿最速下阵方向 连续调整权向量。为方便起.见，我们将梯度向量表示 为(2.2)因此，最速下啥比可以表示为(2.3)其中斤代表进程，"是正常数，

4、称为步长参数，1/2因子 的引入是为了数学上戏理方便。柱从斤到斤+1的迭代 中，权向量的调整量为(2.4)为了证朗最速下啥算法满足式(2)，柱叽小处进行一阶 泰勒畏开，得到(2.5)此式对于"较小时是成立的。柱式(2.4)中设为负值向 量，因而梯度向量0也为负值向量，所以使用埃余采特 转置。将式(2.4)用到式(2.5)中,得到此式表朗当"为正数时，/(w® + l) v/(w(n)。因此，随 着几的增加，代价函数/)减小，当斤=8时，代价函数 趨于最小值4。2.2最速下阵算法应用于维纳滤波器考虑一个横向滤波器，其抽头输入为w(n),u(n- l),-,u(n-M

5、 + 1),对应的抽头权值为 叫何叫仇),一1何。抽头输入是来自零均值、柏关矩 阵为R的广义平稳随机过程的抽样值。除了这些输入 外，滤波器还要一个期望响应d(“)，以便为最优滤波提 供一个参考。柱时刻八抽头输入向量表示为“)，滤波 器输出端期望响应的估计值为(川),其中-是由抽头 输u(n),u(n-l),-,u(n-M + 1)所版成的空间。空过比较期 望响应d®)及其估计值，可以得到一个估计镁差0仇)， 即(2.6)这里w"(n)u(n)是抽头权向量w(n)与抽头输入向量u(n)的 内积。w(n)可以进一步表示为同样，抽头输入向量“)可表示为如果抽头输入向量“何和期望响

6、应d(n)是朕合平稳 的，此时均方谋差或者柱时刻八的代价函数/)是抽头 权向量的二次函数，于是可以得到(2.7)2其中，为目榇函数d®)的方差，卩抽头输入向量如)与 期望响应d(“)的互柑关向量，及R为抽头输入向量“)的柏关矩阵。从而梯度向量可以写为(2.8)刃(町刃)其中在刊向量中乔而和乔而分别是代价函数丿)对应第* 个抽头权值叫)的卖部叫)和虚部®)的偏导数。对 最速下啥算法应用而言，假设式(2.8)中相关矩阵R和互 相关向量P己知，则对于给走的抽头权向量w(n + l)为(2.9)它描述了为那滤波中最速下阵法的数学表达式。3. 卡余曼滤波器3.1卡余曼滤波器的基本思想

7、卡余曼谑、波器是用状态空间概念描述其数学公式的， 另外新颖的特点是，他的解递.归运算，可以不加修改 地应用于平稳和非平稳环境。尤其是，其状态的每一 次更新估计都由前一次估计和新的输入数据计算得 到，因此只需存儲前一次估计。除了不需要存储过去 的所有观测数据外，卡余曼滤波计算比直接根据滤波 过程中每一步所有过去数擔进行估值的方法都更加有 <o 地说状态，走义为数擔的最小集合，这组数擔足以唯一 地描述糸统的自然动态行为。换句话说，状态由预测糸 统未来特性时所素要的，与糸统的过去行为有关的最少 的数据俎成。典型地，比较有代表性的情况是，状态 兀）是未知的。为了估计它，我们使用一组观测数据， 柱

8、變中用向量y®）表示。y）成为观测向量或者简称观测 值，幷假设它是n维的。图3线性动态3 址糸统的信号流图表示状思向量，简月柱数学上，图3.1表示的信号流图隐含着一下两个 方程：（1）过程方程（3.1）式中，Mxl向量9（巧表示噪步过程，可建模为零均 值的勺噪步过程，且其相关矩阵定义为（2）测量方程（3.2）其中C）是己知的NxM测量矩阵。Nxl向量冬） 称为测量噪步，建模为零均值的勺噪步过程，其 相关矩阵为（3.3）测量方程（3.2）确立了可测糸统输出y®）与状态班巧 之间的关糸，如图3.1所示。3.2新息过程为了求解卡余曼滤波问題，我们将应用基于新息过 程的方法。根据之

9、前所述，用向量刃"|儿1)表示几=1时 刻到n-1时刻所有观测数擔过去值给定的情况下，你时 刻观测数据y)的最小均方估计。过去的值用观测值 刃1)丿(2)严孑("-1)表示，他们版成的向量空间用儿1表 示。从而可以定义新息过程如下：(3.4)其中M xl向量aS)表示观测数擔卩)的新息。3.3应用新息过程进行状态估计下面，我们根据信息过程导出状态兀的最小均方 估计。根据推导，这个牯计可以表示成为新息过程 况“，仇(n)序列的线性组合，即(3.4) 其中气伙儿“是一组待定的M xN矩阵。才艮擔正交性原 理，预测状态谋差向量与新息过程正交，即(3.6)琦式(3.5)代入式(3.

10、6),并利用新息过程的正交性质，即 得(3.7)因此，式(3.7)两边同时右乘逆矩阵R 5),可得耳(咖的表达式为(3.8)最后，将式(3.8)带入式(3.5),可得最小军方差估计(3.9)玫对于i = n + l,有(3.10)然而，兀+ 1时刻的状态兀(门+ 1)与八时刻的状态班町的关糸式由式可以推导出对于OSkSn,有(3.11)其中a(k)只与观测数擔y(i),y(2),y(k)有关。因此可知， ”1何与a(k)夜此正交(其中0H)。利用式(3.11)以及当 心乳时兀(计儿)的计算公式，可将式(30)右边的求和项改 写为(3.12)为了进一步讨论，引入如下基本定义。3.4卡余曼增盔文义

11、M xN矩阵(3.13)其中Ex(n + 1)丿®)是状态向量x(n + 1)和新息过程a(©的 互柏关矩阵。利用这一定义和式(3.12)的结黑,可以将 式(30)简单重写为（3.14）式（34）具有朗确的物理意义。它标朗：线性动态糸统 状态的最小均方估计斤+订儿）可以由前一个估计 兀（川儿1）求得。为了表示对卡余曼开创性贡献的认可， 舒矩阵G（n）称为卡余曼增益。现亦剩下唯一要解决的问題是，怎样以一种便于计 算的形式来表示卡余曼增Kc（n）o为此，首丸将班“ + 1） 与丿仇）乘积的期望表示为（3.15）式中利用了状态班巧与噪步向量勺）互不柏关这一事 卖。其次，由于预测状

12、态谖差向量£（兀机-1）与估计 %（nlyi）正交，因此xnyn-0与/（“-1）乘机的期望为 零。这样，用预测状态谋差向量£（S-1）代辱柏乘因子 班巧，將不会引起.式（35）变化，玫有（3.16）由此，可将上式进一步变化为（3.17）现往我们重新定义卡余曼增益。为此，将式（3.17）代入 式（3.13）得（3.18）现淮.我们己经了解了卡余曼谑、波的整个过程和相应的参数设置，为了能够更为方便利用计算机仿真卖现，特将 其中参数变量进行小结。卡余曼变量和参数小结变疑定义维数心)"时刻状态M x 1y(n)"时刻状态值NX1F(z? + lrn)从”时刻到

13、n + 1时刻的转移矩阵M xMC(“)”时刻的测量矩阵NXMQ©)过程噪声*(")的相关矩阵MxMQ2(町过程噪声巾的相关矩阵NxN弟bQ给定观测值7$(2),$("-1)在八时刻状态的预测估讣M x 1x(n|y)给定观测值y,丁,$(“)在八时刻状态的滤波估汁Af xlG(n)"时刻卡尔曼增益矩阵MxN心)八时刻新息向量Nxl/?何新息向的相关矩阵NxNK(n.n-l)珀1阮- J中误差相关矩阵MxMK何；("1儿)中误差相关矩阵MxM基于单步预测的卡余曼滤波器的小结观测值=y(i)，y(2),“,y(n 一 1)转移矩阵"5

14、+ tn)测量矩阵=C(n)过程噪声的相关矩阵=乞(“)测量噪声的相关矩阵G(n) = F(n + l,n)K(n,n- l)CW(n)C(n)lf(ntn - l)CH(n) + Q2(n) 1a(n) = y(n)-C(n)i(n|yn_1)+ 11 y) = F(n + l,n)i(n | x) 4-K(ri) = K(n,n-1)-F(ntn + l)G(n)C(n)K(n,n -1)K(n + l,n) = F(n + lji)K(n)Fw(n + l,n) + Qfii)4 Matlab 仿真为了简化，这里只讨论简单的一维单输 一单输出 线性糸统模型，其中加入自噪步作为糸统的扰动，

15、具体 仿真结果可以获得如下4.1维纳最速下啥比滤波器仿真结果以上为最速下阵法中不同的递.归步长所导致的跟踪效果 变化，对于最速下阵冻中的步长是影响其算法稔走的关 縫，最速下阵算出稔走的充分必、要条件是条件步长因子 为小于输入自相关矩阵的最大特征值倒数的2信。上面 的序列分别从相关矩阵的随大特征之2僖的0.4信开始 变化至其1僖，最后一幅图象能够看出其己经不再收 皴，下面是大于输入柏关矩阵的最大特征值2信步长时 所表现的跟踪结果可以看出其己经朗显发散，不再是我们所期望的滤波算 出。因此可以总、结出，对于最速下啥比来说，步长的选 取是很重要的，根据不同条件的需求，选取正确的步 长，能为算法的快速嵩

16、效提供基础。4.2卡余曼滤波器仿真结果从图中可以发现，卡余曼滤波彖能够非常有效地柱 比较大的干扰下比较准确地反映真卖值，如果观测端加 入干扰较大时，卡余曼滤波器能够较为有效地进行滤 波，不过当状态端的干扰增丸时，卡余曼滤波器的滤波 效果也会随之下啥。如下图，是加大了状态端的干扰， 所呈现的谑、波效果。如上图所示，状态端的干扰导玫状态不稳;t，卡余 曼滤波器的估计值也出现了比较大的波动。如果将状态 端的干扰再增大，则会出现更为严峻的滤波考验，滤波 效果如下。这是的状态己经很勉强了，所以，研究更为有效的 多方法卡余曼滤波器也显得十分必要了。4.3 -种不需初始化的卡余曼滤波器仿真这种谑、波器只是卖

17、现了无需对部分变量进行初始化 的设计，没有特别憲义上的改进经典卡余曼滤波器本身 性能的特点。仿真图如下4.4后联平滑滤波的卡余曼谑、波器仿真只是淮.经典卡余曼滤波器后端朕接了平滑谑、波彖，对性能改进的效果幷不特别朗显，仿真图如下如图中所表示，即使平滑过的估值与观测值之问的 差别也不是特别令人满意，所以，对于经典卡余曼滤波 的研究还需要更深一步进行，由于时间和能力有限，本 次的作业对于卡余曼及其他滤波器的研究只能达到这种 程度，希玺在以后的学习中，能发现更好的对经典卡余 曼滤波器的改进方出。5 Matlab源代码(部分参考自互朕网)5.1经典卡余曼滤波器clearN=200;w(l)=0;x(l

18、)=5;a=l;c=l;Q1 = randn( 1,N)*1;% 过程噪声Q2 = randn( 1 ,N);%测量噪步for k=2:N;x(k)=a*x(k-1 )+Q 1 (k-1); end%状态矩阵for k= 1 :N; Y (k)=c*x(k)+Q2(k);endp(l)=10;s(l)=l；for t=2:N;Rww=cov(Ql(l:t);Rvv=cov(Q2(l:t);p 1 (t)=a.A2*p(t-1 )+Rww;b(t)=c*p 1 (t)/(c.A2*p 1 (t)+Rvv);%kalman 增 _葢s(t)=a*s(t-1 )+b(t)*(Y(t)-a*c*s(t

19、-1);p(t)=pl(t)-c*b(t)*pl(t);endt=l:N;plot(t,s,Y,t,Y；g',t,x；b,);%红色卡余曼，绿色观测值，蓝色状态值legend(,kalman estimate','ovservations'/truth1);5.2最速T啥比clcclear allN=30;q=2.1 ;%q> 1 &&qv2/Ryx 最大特征值hn=zeros(l,N);hn(:)=5;vg=0;Rxx=xcorr(l);Ryx=min(min(corrcoef( 1, 1 +randn);echo offfor i=l:

20、N-l;%vg=2*Rxx*hn(:,i)2*Ryx;%hn(:,i+l)=hn(:,i)l/2*q*vg;vg=2*Rxx*hn(i)-2*Ryx;hn(i+1 )=hn(i)- l/2*q*vg;m(i)=l;endt=l:N-l;ploghn mm(t),b)；5.3后朕平滑滤波器的卡余曼滤波器clearclc;N=300;CON = 5;x = zeros(l,N);x(l)= 1；p= 10;Q = randn(l ,N)*0.2;%过程噪声协方差R = randn(l,N);%观测噪步协方差y = R + CON;%加过程噪声的状态输出 for k = 2 : NQ1 = cov(Q( 1 :k1);%过程噪声协方差Q2 = cov(R(l:k-l);x(k) = x(k - 1);%预估计k时刻状态麦量的值p = p + Q1;%对应于预估值的协方差kg = p / (p + Q2);%k