问题探究

用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。

卡尔曼滤波及数据滤波

      卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

      斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。 关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。

      数据滤波是去除噪声还原真实数据的一种数据处理技术, Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态. 由于, 它便于计算机编程实现, 并能够对现场采集的数据进行实时的更新和处理, Kalman滤波是目前应用最为广泛的滤波方法, 在通信, 导航, 制导与控制等多领域得到了较好的应用

代码实现

clear all;

clc;

close all;

N=2000;

gv=0.0332;

m=500;

a=[-1.6 1.46 -0.616 0.1525];

Q2=0.005;%观测噪声方差

w=zeros(m*4,N);

alpher=zeros(m,N);

for j=1:m

   

   

v=randn(1,N)*sqrt(gv);

u=filter(1,a,v);

F=eye(4,4);

d=u;

C=zeros(4,N);

for i=5:N

   C(:,i)=[u(i-1);u(i-2);u(i-3);u(i-4)];

end

C(:,1)=[0;0;0;0];

C(:,2)=[d(1);0;0;0];

C(:,3)=[d(2);d(2);0;0];

C(:,4)=[d(3);d(2);d(1);0];

p_esti=eye(4,4);%状态误差自相关矩阵

for i=2:N

   w([4*j-3,4*j-2,4*j-1,4*j],i)=w([4*j-3,4*j-2,4*j-1,4*j],i-1);%一步预测误差

   alpher(j,i)=d(i)-C(:,i)'*w([4*j-3,4*j-2,4*j-1,4*j],i).'';%计算信息过程

   p_pre=p_esti;%一步预测误差自相关矩阵

   A=C(:,i)'*p_pre*C(:,i)+Q2;%新息过程自相关矩阵

   k=p_pre*C(:,i)/A;

   w([4*j-3,4*j-2,4*j-1,4*j],i)=w([4*j-3,4*j-2,4*j-1,4*j],i)+k*alpher(j,i);%状态估计

   p_esti=p_pre-k*C(:,i)'*p_pre;%状态估计误差自相关矩阵

end

end

MSE=sum(alpher.^2)/m;

MSE=MSE/max(MSE);

MSE=10*log10(MSE);

figure(1)

plot(MSE);

title(' 均方误差变化曲线');

xlabel('迭代次数n');ylabel('MSE');

w_aver=zeros(4,N);

for i=1:m

   w_aver(1,:)=w_aver(1,:)+w(4*i-3,:);

   w_aver(2,:)=w_aver(2,:)+w(4*i-2,:);

   w_aver(3,:)=w_aver(3,:)+w(4*i-1,:);

   w_aver(4,:)=w_aver(4,:)+w(4*i,:);

end

w_aver=w_aver/m;

figure(2)

hold on;

plot(w_aver(1,:),'b');

plot(w_aver(2,:),'b');

plot(w_aver(3,:),'b');

plot(w_aver(4,:),'b');

title('权向量的估计');

xlabel('迭代次数n');ylabel('抽头权值');

hold off;