function [Xf Yf]=VDF(Zx,Zy,sigmax,sigmay,sigma_ax,sigma_ay,T,N)
%變維濾波的自適應機動目標檢測自適應算法
%(Zx,Zy)觀測數據坐標
%sigmax,sigmay 測量誤差標準差
%sigma_ax,sigma_ay擾動噪聲標準差
%T 采樣周期
%N 觀測數據長度

%%%%%模型一:CV模型參數設置 X(k+1)=AX(k)+GW(k),Z(k)=HX(k)+V(k)%%%%%%%%%%%%%
A=[1 T 0 0;
    0 1 0 0;
    0 0 1 T;
    0 0 0 1];
G=[T^2/2,0;
    T,0;
    0,T^2/2;
    0,T];
 H=[1 0 0 0;
     0 0 1 0];
R=[sigmax^2 0;
   0 sigmay^2];%測量誤差協方差矩陣
% Q=[sigma_ax^2 0;
%     0 sigma_ay^2];%擾動噪聲協方差矩陣
Q=zeros(2,2);

%%%%%模型二:CA模型參數設置 Xm(k+1)=TmXm(k)+GmWmk),Zm(k)=HmXm(k)+Vm(k)%%%%%%%%%%
Tm=[1,T,0,0,T^2/2,0;
    0,1,0,0,T,0;
    0,0,1,T,0,T^2/2;
    0,0,0,1,0,T;
    0,0,0,0,1,0;
    0,0,0,0,0,1];
Gm=[T^2/2 0;
    T    0;
    0  T^2/4;
    0   T/2;
    1  0;
    0   1];
Hm=[1 0 0 0 0 0;
   0 0 1 0 0 0];
Rm=[sigmax^2 0;
   0 sigmay^2];%測量誤差協方差矩陣
Qm=[sigma_ax^2 0;
    0 sigma_ay^2];

%%%%%%%%%%濾波參數設置%%%%%%%%%%%%%%%%%%%%%%
alpha=0.8;%衰減因子
delta=int16(1/(1-alpha));%有效檢測窗口長度
Th=15;%低階向高階模型的機動檢測門限
Ta=10;%高階向低階模型的機動檢測門限
deltaN=3;%開始檢測機動的點

%%%%%%初始值和變量設置%%%%%%%%%%%%%%%%%%%%%%
%對于CV模型，參數變量設置
Xf=zeros(N,1);%濾波后X縱坐標值
Yf=zeros(N,1);%濾波后Y坐標值
Xkk=zeros(4,1);%每次X的濾波值
Xx=zeros(4,N);%X的濾波值
Xk1k=zeros(4,1);%X的預測值
Pk1k=zeros(4,4);%預測誤差協方差矩陣
Pkk=zeros(4,4);%每次濾波誤差協方差矩陣
Pp=zeros(4,4,N);%濾波誤差的協方差矩陣
Kk=zeros(4,2);%klaman增益
S=zeros(2,2);%濾波后位置估計(新息)的協方差矩陣
I=eye(4,4);%單位矩陣Xkk=zeros(4,1);%X的濾波值
%對于CA模型，參數變量設置
Xkkm=zeros(6,1);%X的濾波值
Xk1km=zeros(6,1);%X的預測值
Pk1km=zeros(6,6);%預測誤差協方差矩陣
Pkkm=zeros(6,6);%濾波誤差協方差矩陣
Kkm=zeros(6,2);%klaman增益
Sm=zeros(2,2);%濾波后位置估計(新息)的協方差矩陣
Im=eye(6,6);%單位矩陣
Xxm=zeros(6,N);
Ppm=zeros(6,6,N);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
D=zeros(2,N);%CA模型新息
Mu=0;%新息衰減平滑值
Nu=0;
sigmaa=zeros(1,N);
Pa=zeros(2,2);%加速度估計協方差
aa=zeros(2,1);%加速度估計
CA=0;%CA模型啟動標志
countk=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Xf(1)=Zx(1);Xf(2)=Zx(2);
Yf(1)=Zy(1);Yf(2)=Zy(2);
X0=[Zx(2) (Zx(2)-Zx(1))/T Zy(2) (Zy(2)-Zy(1))/T]';%初始坐標
P0=[sigmax^2 sigmax^2/T 0 0 ;
    sigmax^2/T sigma_ax^2*T^2/4+2*sigmax^2/T^2 0 0;
    0 0 sigmay^2 sigmay^2/T;
    0 0 sigmay^2/T sigma_ay^2*T^2/4+2*sigmay^2/T^2];%誤差協方差矩陣初始值

%%%%%%%%%%%%%%%%%%濾波過程%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Xkk=X0;
Pkk=P0;
for i=3:N
    if Mu<Th
        Xk1k=A*Xkk;%預測
        Pk1k=A*Pkk*A'+G*Q*G';%預測誤差協方差矩陣
        D(:,i)=[Zx(i) Zy(i)]'-H*Xk1k;%新息
        S=(H*Pk1k*H'+R); %濾波后位置估計(新息)的協方差矩陣
        Kk=Pk1k*H'/S;%kalman濾波增益
        Xkk=Xk1k+Kk*D(:,i);% 濾波
        Pkk=(I-Kk*H)*Pk1k;%濾波誤差協方差矩陣
        Xf(i)=Xkk(1);
        Yf(i)=Xkk(3);
        Xx(:,i)=Xkk;
        Pp(:,:,i)=Pkk;
        countk=countk+1;
        if countk>deltaN %啟動機動檢測
            Nu=D(:,i)'*inv(S)*D(:,i);
            Mu=alpha*Mu+Nu;
        end
    elseif Mu>Th %檢測機動發生，采用高階模型(CA),從i-delta-1開始有加速度加入
            %%%%%%%%%%初始化%%%%%%%%%%%%%%%%%%%%%%%%
            if CA==0
                Xkkm([5 6])=2/T^2*D(:,i-delta);%加速度初始值
                Xkkm([1 3])=[Zx(i-delta) Zy(i-delta)]';%位置估計
                Xkkm([2 4])=Xx([2,4],i-delta-1)+T*Xkkm([5 6]);%速度估計
                Pkkm(1,1)=R(1,1);%協方差初始估計
                Pkkm(1,2)=2/T*R(1,1);
                Pkkm(1,5)=2/T^2*R(1,1);
                Pkkm(5,5)=4/T^4*(R(1,1)+Pp(1,1,i-delta-1)+2*T*Pp(1,2,i-delta-1)+T^2*Pp(2,2,i-delta-1));
                Pkkm(2,2)=4/T^2*R(1,1)+4/T^2*Pp(1,1,i-delta-1)+Pp(2,2,i-delta-1)+4/T*Pp(1,2,i-delta-1);
                Pkkm(2,5)=4/T^3*R(1,1)+4/T^3*Pp(1,1,i-delta-1)+2/T*Pp(2,2,i-delta-1)+4/T*Pp(1,2,i-delta-1);
                Pkkm(3,3)=R(2,2);
                Pkkm(4,4)=R(1,1);
                Pkkm(6,6)=R(2,2);
                countk=0;
                CA=1;
                for j=i-delta:i-1 %采用高階模型濾波
                    Xk1km=Tm*Xkkm;%預測
                    Pk1km=Tm*Pkkm*Tm'+Gm*Qm*Gm';%預測誤差協方差矩陣
                    D(:,j)=[Zx(j) Zy(j)]'-Hm*Xk1km;%新息
                    Sm=(Hm*Pk1km*Hm'+Rm); %濾波后位置估計(新息)的協方差矩陣
                    Kkm=Pk1km*Hm'/Sm;%kalman濾波增益
                    Xkkm=Xk1km+Kkm*D(:,j);% 濾波
                    Pkkm=(Im-Kkm*Hm)*Pk1km;%濾波誤差協方差矩陣
                    Xf(j)=Xkkm(1);
                    Yf(j)=Xkkm(3);
                    Xxm(:,j)=Xkkm;
                    aa=Xkkm([5 6]);
                    Pa=[Pkkm(5,5) Pkkm(5,6);
                    Pkkm(6,5) Pkkm(6,6)];
                    sigmaa(j)=aa'*inv(Pa)*aa;
                end
            end
            Xk1km=Tm*Xkkm;%預測
            Pk1km=Tm*Pkkm*Tm'+Gm*Qm*Gm';%預測誤差協方差矩陣
            D(:,i)=[Zx(i) Zy(i)]'-Hm*Xk1km;%新息
            S=(Hm*Pk1km*Hm'+Rm); %濾波后位置估計(新息)的協方差矩陣
            Kkm=Pk1km*Hm'/S;%kalman濾波增益
            Xkkm=Xk1km+Kkm*D(:,i);% 濾波
            Pkkm=(Im-Kkm*Hm)*Pk1km;%濾波誤差協方差矩陣
            Xf(i)=Xkkm(1);
            Yf(i)=Xkkm(3);
            Xxm(:,i)=Xkkm;
            aa=Xkkm([5 6]);
            Pa=[Pkkm(5,5) Pkkm(5,6);
            Pkkm(6,5) Pkkm(6,6)];
            sigmaa(i)=aa'*inv(Pa)*aa;
            countk=countk+1;
            if countk>delta %啟動高階模型向低階模型的檢測
                if(sum(sigmaa(i-delta+1:i))<Ta) %退出機動模型CA
                    CA=0;
                    Mu=0;
                    Xkk=Xkkm(1:4);
                    Pkk=Pkkm(1:4,1:4);
                end
            end
    end
end