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

       卡尔曼滤波（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;