基于Matlab的卡尔曼滤波算法仿真(word文档良心出品).docxVIP

基于Matlab的卡尔曼滤波算法仿真 卡尔曼滤波器原理 卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题， 它根据前一个估计 值和最近一个观察数据来估计信号的当前值。 它是用状态方程和递推方法进行估计的， 而它的解是以估计值(常常是状态变量的估计值)的形式给出其信号模型是从状态方 程和量测方程得到的。 卡尔曼滤波中信号和噪声是用状态方程和测量方程来表示的。 因此设计卡尔曼滤波 器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采用递推的方法计 算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统， 因而它比维纳过滤有更广泛的应用。 卡尔曼几个重要公式： s?h|n) = a s? -rt|n-1) + Gn[x(n) - ac sl?r(d)] (1) P(n) = a2 E (-1) + Q (2) Gn Gn = ????(??) ??+????(??) ?? E (n) =????? = (1 - n)G(n) 这组方程的递推计算过程如图1所示 式估)* EPink图1.卡尔曼滤波器递推运算流程图 式估)* E Pink 图1.卡尔曼滤波器递推运算流程图 卡尔曼滤波过程实际上是获取维纳解的递推运算过程，这一过程从某个初始状态启 动，经过迭代运算，最终到达稳定状态，即维纳滤波状态。递推计算按图1所示进行。 假设已经有了初始值s?（0|和（0），这样便可由式（2）计算P⑴，由式（3）计算Gi, 由式（4）计算（1），由式（1）计算s?（1|1） （1）和s?（1|1便成为下一轮迭代运算的 已知数据。在递推运算过程中，随着迭代次数 n的增加，E（n）将逐渐下降，知道最 终趋近于某个稳定值d这时 （n） = （n - 1）=直 为求得这个稳定值，将式（3）和式（2）代入式（4），得到 (1-?? 2)??+???? (1-?? 2)??+???????? ???? ^0 -乔=0 解此方程即可求出 基于Matlab的卡尔曼滤波器的仿真 Matlab代码如下： clear N=200; w(1)=0; x(1)=5; a=1; c=1; %过程噪声 Q1=randn (1,N)*1; %测量噪声 Q2=ra ndn (1,N); %状态矩阵 for k=2:N;x(k)=a*x(k-1)+Q1(k-1); end for k=1:N;Y(k)=c*x(k)+Q2(k); end p(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; %kalma n 增益 b(t)=c*p1(t)/(c.A2*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 FontSize=14; LineWidth=3; figure(); %画出温度计的测量值 plot(Y, k+ ); hold on; %画出最优估计值 plot(s, b- , LineWidth ,LineWidth) hold on; %画出真实值 plot(x, g- , LineWidth ,LineWidth); legend( 观测值 后验估计 , 真实值 ）; ,FontSize); ,FontSize); ,FontSize); ,FontSize); xl=xlabel( ); yl=ylabel( ); set(xl, fontsize set(yl, fontsize hold off ; set(gca, FontSize figure(); valid_iter = [2:N]; %画出最优估计值的方差 plot(valid_iter,p([valid_iter]), LineWidth ,LineWidth); ,FontSize); ,FontSize); ,FontSize); ,FontSize); legend（ 后验估计的误差估计 ）; xl=xlabel( ); yl=ylabel( ); set(xl, fontsize set(yl, fontsize set(gca, FontSize Matlab仿真结果如下: 卡尔曼滤波的结果: 估计的误差的方差: 卡尔曼滤波的实质是由测量值重构系统的状态向量 它以预测一实测一修正”的顺序 递推，根据系统的测量值来消除随机干扰，再现系统的状态，或根据系统的测量值从被噪 声污染的系统中恢复系统的本来面目。 从仿真结果可以看出， 卡尔曼滤波器能够有效地在 一定的噪声干扰下比较准确地反映真实值。 课程实验总结 通过现代数