%%%%%%%%%%%%%%%%%  基于勻速模型的濾波函數  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%系統模型  x(k+1)=Fk*x(k)+C*W(k)

   function cv=cvfilter(T,N,d,Z)                
  
 F=[1 T  0 0 
   0 1  0 0
   0 0  1 T 
   0 0  0 1 ];%狀態轉移矩陣
     

C=[T^2/2  0 ;
    T     0  ;
    0     T^2/2 ;
    0     T];%噪聲轉移矩陣


Qi=[0.03^2,0; 0,0.08^2];%%process covariance and measurement covariance

H=[1 0 0 0 
   0 0 1 0 ]; %量測轉移矩陣
Ri=d^2*eye(2);%量測方差
I=eye(4);
%T=1;%采樣間隔
%$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$%%%%%%%%%%%%%%%%%%%%%%%%%

%&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&%%%%%%%%%%%%%%%%%%%%%%%%%%
xx1=[30000 -172  30000 -246  ]';%初始位置

PP1=200^2*eye(4);%狀態預測協方差
cv=[];
%濾波遞推.
           
     for i=1:N
     x1=F*xx1;%計算一步預測估計值
     P11=F*PP1*F'+C*Qi*C';%計算預測濾波協方差矩陣
     K1=P11*H'*inv(H*P11*H'+Ri);%計算增益矩陣
     %S1=H*P11*H'+Ri;%濾波參差
     PP1=(I-K1*H)*P11;%預測協方差
     xx1=x1+K1*(Z(:,i)-H*x1);%計算估計值
     cv=[cv xx1];%輸出
     end