卡爾曼濾波（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;