vdf.m

卡罗曼滤波 采用变维和交互多模算法实现机动检测

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