immf.m

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

function [Xf Yf]=IMMF(Zx,Zy,sigmax,sigmay,sigma_ax,sigma_ay,T,N)
%交互多模算法(IMM)算法
%(Zx,Zy)观测数据坐标
%sigmax,sigmay 测量误差标准差
%sigma_ax,sigma_ay扰动噪声标准差
%T 采样周期
%N 观测数据长度
%采用两个模型，一个是非机动模型(CV),一个是机动模型(CA)

%%%%%模型一: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=zeros(2,2);%非机动模型下，扰动噪声协方差矩阵假定为0

%%%%%模型二: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];%机动模型下系统扰动噪声

%%%%%%%%%%%%%滤波参数设置%%%%%%%%%%%%%%%%%%%%%%
P=[0.95 0.05;
    0.05 0.95];%模型之间切换的转移概率矩阵
N0=5;%先从开始到N0点进行非机动模型滤波
mu1=0.95;%模型一的概率
mu2=0.05;%模型二的概率
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%对于CV模型，参数变量设置
Xf=zeros(N,1);%滤波后X纵坐标值
Yf=zeros(N,1);%滤波后Y坐标值
X1kk=zeros(4,1);%模型一每次X的滤波值
X2kk=zeros(4,1);%模型一每次X的滤波值
X01k=zeros(4,1);%每次滤波的输入值
Xx=zeros(4,N);%X的滤波值
Xk1k=zeros(4,1);%X的预测值
Pk1k=zeros(4,4);%预测误差协方差矩阵
P1kk=zeros(4,4);%模型一滤波误差协方差矩阵
P2kk=zeros(4,4);%模型二滤波误差协方差矩阵
P01k=zeros(4,4);%总的误差的协方差矩阵
Kk=zeros(4,2);%klaman增益
S=zeros(2,2);%滤波后位置估计(新息)的协方差矩阵
I=eye(4,4);%单位矩阵Xkk=zeros(4,1);%X的滤波值

%%%%%%%%%%%%%%%CV模型初始化%%%%%%%%%%%%%%%%%%%
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];%误差协方差矩阵初始值

%%%%%%%%%%%对于CA模型，参数变量设置%%%%%%%%%%%
X1kkm=zeros(6,1);%模型一X的滤波值
X2kkm=zeros(6,1);%模型一X的滤波值
X02k=zeros(6,1);%每次滤波模型二的输入值
Xk1km=zeros(6,1);%X的预测值
Pk1km=zeros(6,6);%预测误差协方差矩阵
P1kkm=zeros(6,6);%模型一误差协方差矩阵
P2kkm=zeros(6,6);%模型二误差协方差矩阵
P02k=zeros(6,6);%总的误差协方差矩阵
Kkm=zeros(6,2);%klaman增益
Dm=zeros(2,1);%新息
Sm=zeros(2,2);%滤波后位置估计(新息)的协方差矩阵
Im=eye(6,6);%单位矩阵

%%%%%%%%%%%%%%%%滤波过程%%%%%%%%%%%%%%%%%%
X1kk=X0;
P1kk=P0;
for i=3:N0-1 %先从开始到N0点进行非机动模型滤波
     Xk1k=A*X1kk;%预测
     Pk1k=A*P1kk*A'+G*Q*G';%预测误差协方差矩阵
     D=[Zx(i) Zy(i)]'-H*Xk1k;%新息
     S=(H*Pk1k*H'+R); %滤波后位置估计(新息)的协方差矩阵
     Kk=Pk1k*H'/S;%kalman滤波增益
     X1kk=Xk1k+Kk*D;% 滤波
     P1kk=(I-Kk*H)*Pk1k;%滤波误差协方差矩阵
     Xf(i)=X1kk(1);
     Yf(i)=X1kk(3);
end
%%%%%%%%%%%CA模型初始化%%%%%%%%%%%%%%%%%%%%%%%%%%
X2kkm([5 6])=2/T^2*D;%加速度初始值
X2kkm([1 3])=[Zx(i-1) Zy(i-1)]';%位置估计
X2kkm([2 4])=X1kk([2 4])+T*X2kkm([5 6]);%速度估计
P2kkm(1,1)=R(1,1);%协方差初始估计
P2kkm(1,2)=2/T*R(1,1);
P2kkm(1,5)=2/T^2*R(1,1);
P2kkm(5,5)=4/T^4*(R(1,1)+P1kk(1,1)+2*T*P1kk(1,2)+T^2*P1kk(2,2));
P2kkm(2,2)=4/T^2*R(1,1)+4/T^2*P1kk(1,1)+P1kk(2,2)+4/T*P1kk(1,2);
P2kkm(2,5)=4/T^3*R(1,1)+4/T^3*P1kk(1,1)+2/T*P1kk(2,2)+4/T*P1kk(1,2);
P2kkm(3,3)=R(2,2);
P2kkm(4,4)=R(1,1);
P2kkm(6,6)=R(2,2);
for i=N0:N %从N0点，加入CA模型，进行交互多模滤波
    
    %%%%***输入交互****%%%%%%%%%
    c1=P(1,1)*mu1+P(2,1)*mu2;
    c2=P(1,2)*mu1+P(2,2)*mu2;
    mu11=P(1,1)*mu1/c1;
    mu12=P(1,2)*mu1/c2;
    mu21=P(2,1)*mu2/c1;
    mu22=P(2,2)*mu2/c2;
    X2kk=X2kkm(1:4);
    X1kkm=[X1kk;2/T^2*D];
    X01k=X1kk*mu11+X2kk*mu21;
    X02k=X1kkm*mu12+X2kkm*mu22;
    P2kk=P2kkm(1:4,1:4);
    P1kkm(1:4,1:4)=P1kk;
    P1kkm(5,5)=S(1,1)*4/T^4;
    P1kkm(5,6)=S(1,2)*4/T^4;
    P1kkm(6,6)=S(2,2)*4/T^4;
    P1kkm(6,5)=S(2,1)*4/T^4;
    P01k=P1kk*mu11+P2kk*mu21+(X1kk-X01k)*(X1kk-X01k)'*mu11+(X2kk-X01k)*(X2kk-X01k)'*mu21;
    P02k=P1kkm*mu12+P2kkm*mu22+(X1kkm-X02k)*(X1kkm-X02k)'*mu12+(X2kkm-X02k)*(X2kkm-X02k)'*mu22;
    
    %%%%%%%%%%%%%%滤波过程%%%%%%%%%%
    %模型一滤波
    Xk1k=A*X01k;%预测
    Pk1k=A*P01k*A'+G*Q*G';%预测误差协方差矩阵
    D=[Zx(i) Zy(i)]'-H*Xk1k;%新息
    S=(H*Pk1k*H'+R); %滤波后位置估计(新息)的协方差矩阵
    Kk=Pk1k*H'/S;%kalman滤波增益
    X1kk=Xk1k+Kk*D;% 滤波
    P1kk=(I-Kk*H)*Pk1k;%滤波误差协方差矩阵
    pr1=exp(-1/2*D'*inv(S)*D)/((2*pi)*(det(S))^(1/2));
    %模型二滤波
    Xk1km=Tm*X2kkm;%预测
    Pk1km=Tm*P02k*Tm'+Gm*Qm*Gm';%预测误差协方差矩阵
    Dm=[Zx(i) Zy(i)]'-Hm*Xk1km;%新息
    Sm=(Hm*Pk1km*Hm'+Rm); %滤波后位置估计(新息)的协方差矩阵
    Kkm=Pk1km*Hm'/Sm;%kalman滤波增益
    X2kkm=Xk1km+Kkm*Dm;% 滤波
    P2kkm=(Im-Kkm*Hm)*Pk1km;%滤波误差协方差矩
    pr2=exp(-1/2*Dm'*inv(Sm)*Dm)/((2*pi)*(det(Sm))^(1/2));
    %模型概率更新
    c=pr1*c1+pr2*c2;
    mu1=(pr1*c1)/(c);
    mu2=(pr2*c2)/(c);
    
    %%%%%%%%%输出交互%%%%%%%%%%%%%%%
    Xf(i)=X1kk(1)*mu1+X2kkm(1)*mu2;
    Yf(i)=X1kk(3)*mu1+X2kkm(3)*mu2;
end