imm.m

采用多模方式的卡尔曼滤波程序

function  IMM(a,M)
%采用交互多模算法的目标跟踪
%先生成目标运动的精确轨迹和观测数据
% a   做匀速圆周运动时的向心加速度
% M   蒙特卡洛仿真次数

r=300*300/a;                                           %做匀速圆周运动时的半径
N=fix((40000+pi*r)/3000)+1;                            %总的采样点数
n1=fix((20000+pi*r)/3000)+1;                           %做匀速圆周运动结束前的最后一个采样点
x=zeros(1,N);  y=x;
ex1=x;   ey1=y;                                        %用于存储滤波误差的均值
ex2=x;   ey2=y;                                        %用于存储滤波误差的标准差
X0=zeros(2,N);                                         %X0用于存储滤波输出值
for   n=1:7
    x(n)=-23000+3000*n;
end
for   n=8:n1
    x(n)=r*sin((3000*n-23000)/r);
    y(n)=-r+r*cos((3000*n-23000)/r);
end
for  n=n1+1:N
    x(n)=-3000*n+23000+pi*r;
    y(n)=-2*r;
end                                                    %目标运动的精确轨迹
X=[x;y];
C=[10000,0;0,10000];                                   %观测数据的协方差

%建立目标运动三个阶段的模型
%信号模型   X(k+1)=Q*X(k)+U
 Q1=eye(2);                 U1=[3000;0];
 e=cos(3000/r);  f=sin(3000/r); 
 Q2=[e,f;-f,e];           U2=r*[f;e-1];
 Q3=[1,0;0,1];   U3=-U1;
 P=[0.95,0.025,0.025;0.025,0.95,0.025;0.025,0.025,0.95]; %马尔科夫链模型转移概率

%进行M次仿真
for M_=1:M
    w1=100*randn(1,N);   w2=100*randn(1,N);
    Z=[x+w1;y+w2];                                          %观测数据
    
%前6个点采用非机动的卡尔曼滤波模型
X1=Z(:,1);X0(:,1)=X1;
ex1(1)=ex1(1)+x(1)-X0(1,1);   ey1(1)=ey1(1)+y(1)-X0(2,1);          %滤波误差与标准差计算
ex2(1)=ex2(1)+(x(1)-X0(1,1))^2;   ey2(1)=ey2(1)+(y(1)-X0(2,1))^2;
P1=[10000,0;0,10000];
for n=2:6
    X1=Q1*X1+U1;
    P1=Q1*P1*Q1';
    K=P1*inv(C+P1);
    X1=X1+K*(Z(:,n)-X1);
    X0(:,n)=X1;
    P1=(eye(2)-K)*P1;
    ex1(n)=ex1(n)+x(n)-X0(1,n);   ey1(n)=ey1(n)+y(n)-X0(2,n);          %滤波误差与标准差计算
    ex2(n)=ex2(n)+(x(n)-X0(1,n))^2;   ey2(n)=ey2(n)+(y(n)-X0(2,n))^2;
end

%从第7个点开始采用交互多模算法
u=[0.95,0.05,0];                                    %初始条件，任意指定的模型概率
X2=X1;X3=X1;P2=P1;P3=P1;
V=zeros(1,3);
u1=zeros(3,3);

for n=7:N
c=u*P;                                             %归一化常数
for m=1:3
    for t=1:3
        u1(m,t)=P(m,t)*u(m)/c(t);
    end
end

X01=X1*u1(1,1)+X2*u1(2,1)+X3*u1(3,1);
X02=X1*u1(1,2)+X2*u1(2,2)+X3*u1(3,2);
X03=X1*u1(1,3)+X2*u1(2,3)+X3*u1(3,3);

P01=u1(1,1)*(P1+(X1-X01)*(X1-X01)')+u1(2,1)*(P2+(X2-X01)*(X2-X01)')+u1(3,1)*(P3+(X3-X01)*(X3-X01)');
P02=u1(1,2)*(P1+(X1-X02)*(X1-X02)')+u1(2,2)*(P2+(X2-X02)*(X2-X02)')+u1(3,2)*(P3+(X3-X02)*(X3-X02)');
P03=u1(1,3)*(P1+(X1-X03)*(X1-X03)')+u1(2,3)*(P2+(X2-X03)*(X2-X03)')+u1(3,3)*(P3+(X3-X03)*(X3-X03)');

%预测
X1=Q1*X01+U1;   X2=Q2*X02+U2;   X3=Q3*X03+U3;

%预测误差协方差
P1=Q1*P01*Q1';   P2=Q2*P02*Q2';    P3=Q3*P03*Q3';

%模型概率更新
v1=Z(:,n)-X1;  V(1)=exp(-v1'*inv(P1+C)*v1/2)/(2*pi*sqrt(det(P1+C)));
v2=Z(:,n)-X2;  V(2)=exp(-v2'*inv(P2+C)*v2/2)/(2*pi*sqrt(det(P2+C)));
v3=Z(:,n)-X3;  V(3)=exp(-v3'*inv(P3+C)*v3/2)/(2*pi*sqrt(det(P3+C)));
c_=V*c';
u(1)=V(1)*c(1)/c_;  u(2)=V(2)*c(2)/c_;   u(3)=V(3)*c(3)/c_;

%卡尔曼增益
K1=P1*inv(P1+C);   K2=P2*inv(P2+C);   K3=P3*inv(P3+C);

%滤波
X1=X1+K1*(Z(:,n)-X1);  X2=X2+K2*(Z(:,n)-X2);   X3=X3+K3*(Z(:,n)-X3);

%滤波协方差
P1=(eye(2)-K1)*P1;    P2=(eye(2)-K2)*P2;     P3=(eye(2)-K3)*P3;

%输出交互
X0(:,n)=X1*u(1)+X2*u(2)+X3*u(3);

%滤波误差与标准差计算
ex1(n)=ex1(n)+x(n)-X0(1,n);   ey1(n)=ey1(n)+y(n)-X0(2,n);
ex2(n)=ex2(n)+(x(n)-X0(1,n))^2;   ey2(n)=ey2(n)+(y(n)-X0(2,n))^2;
end
end 

%计算滤波误差
ex1=ex1/M;
ey1=ey1/M;
ex2=sqrt(ex2/M-ex1.^2);
ey2=sqrt(ey2/M-ey1.^2);

%输出滤波结果和误差分析
figure(1);
plot(x,y,'k',Z(1,:),Z(2,:),'g',X0(1,:),X0(2,:),'r');
title('真实轨迹、观测值于滤波估计');
xlabel('x');ylabel('y');
legend('真实轨迹','观测值','滤波估计');
figure(2);
plot(1:N,ex1,'r',1:N,ey1,'b');
xlabel('k');ylabel('均值');
title('滤波误差的均值');
legend('x方向的误差均值','y方向上的误差均值');
figure(3);
plot(1:N,ex2,'r',1:N,ey2,'b');
xlabel('k');ylabel('标准差');
title('滤波误差的标准差');
legend('x方向的标准差','y方向上的标准差');