基于Matlab的卡尔曼滤波算法仿真

基于Matlab的卡尔曼滤波算法仿真1. 卡尔曼滤波器原理卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的。卡尔曼滤波中信号和噪声是用状态方程和测量方程来表示的。因此设计卡尔曼滤波器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。卡尔曼几个重要公式：s(n|n) = a s (n-1|n-1) + Gnx(n) ac s (n-1|n-1) (1)P(n) = a2(n-1) + Q (2)Gn = cP(n)R+c2P(n) (3)(n) = RcGn = (1 cGn)P(n) (4)这组方程的递推计算过程如图1所示。 图1. 卡尔曼滤波器递推运算流程图卡尔曼滤波过程实际上是获取维纳解的递推运算过程，这一过程从某个初始状态启动，经过迭代运算，最终到达稳定状态，即维纳滤波状态。递推计算按图1所示进行。假设已经有了初始值s(0|0)和（0），这样便可由式（2）计算P(1)，由式（3）计算G1，由式（4）计算（1），由式（1）计算s(1|1)。（1）和s(1|1)便成为下一轮迭代运算的已知数据。在递推运算过程中，随着迭代次数n的增加，（n）将逐渐下降，知道最终趋近于某个稳定值0。这时 （n） = （n - 1）= 0为求得这个稳定值，将式（3）和式（2）代入式（4），得到 02 + 1-a2R+c2Qc2a20-QRc2a2=0解此方程即可求出0。2. 基于Matlab的卡尔曼滤波器的仿真Matlab代码如下：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);end for k=1:N;Y(k)=c*x(k)+Q2(k);endp(1)=10;s(1)=1;for t=2:N; Rww = cov(Q1(1:t); Rvv = cov(Q2(1:t);p1(t)=a.2*p(t-1)+Rww;%kalman 增益 b(t)=c*p1(t)/(c.2*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);endFontSize=14;LineWidth=3;figure();%画出温度计的测量值plot(Y,k+); hold on;%画出最优估计值plot(s,b-,LineWidth,LineWidth) hold on;%画出真实值plot(x,g-,LineWidth,LineWidth); legend(观测值, 后验估计, 真实值);xl=xlabel();yl=ylabel();set(xl,fontsize,FontSize);set(yl,fontsize,FontSize);hold off;set(gca,FontSize,FontSize);figure();valid_iter = 2:N; %画出最优估计值的方差plot(valid_iter,p(valid_iter),LineWidth,LineWidth); legend(后验估计的误差估计);xl=xlabel();yl=ylabel();set(xl,fontsize,FontSize);set(yl,fontsize,FontSize);set(gca,FontSize,FontSize);Matlab仿真结果如下：卡尔曼滤波的结果：估计的误差的方差：卡尔曼滤波的实质是由测量值重构系统的状态向量。它以“预测实测修正”的顺序递推，根据系统的测量值来消除随机干扰，再现系统的状态，或根据系统的测量值从被噪声污染的系统中恢复系统的本来面目。从仿真结果可以看出，卡尔曼滤波器能够有效地在一定的噪声干扰下比较准确地反映真实值。3. 课程实验总结通过现代数字信号处理课程的相关学习，我了解到了各种各样的数字信号处理方法，例如维纳滤波、卡尔曼滤波和自适应滤波等，并且每一种滤波器又有不同的算法进行实现，但对于这些数字信号处理方法具体怎么实现以及各种方法适用于进行什么样数字信号处理场合没有进行具体