📄 imm2.m
字号:
trajectory3;
x(:,1)=X(:,1); %初始状态向量
R=diag([200^2 10^2 200^2 10^2]);
P=50*eye(6); %初始状态向量协方差
X1(:,1)=x(:,1);
X2(:,1)=x(:,1);
X3(:,1)=x(:,1);
P1=P;
P2=P;
P3=P;
Pt=[0.8 0.15 0.05
0.3 0.4 0.3
0.05 0.15 0.8];
u=zeros(3,length(z)+1);
u(:,2)=[1/2 1/4 1/4]';
%singer模型
a=1/60;
F=[1 T -(1-a*T-exp(-a*T))/a^2 0 0 0
0 1 (1-exp(-a*T))/a 0 0 0
0 0 exp(-a*T) 0 0 0
0 0 0 1 T -(1-a*T-exp(-a*T))/a^2
0 0 0 0 1 (1-exp(-a*T))/a
0 0 0 0 0 exp(-a*T) ];
q11= 1/(2*a^5)*(1-exp(-2*a*T)+2*a*T+2/3*a^3*T^3-2*a^2*T^2-4*a*T*exp(-a*T));
q12= 1/(2*a^4)*(exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2);
q13=1/(2*a^3)*(1-exp(-2*a*T)-2*a*T*exp(-a*T));
q21=q12;
q22=1/(2*a^3)*(4*exp(-a*T)-3-exp(-2*a*T)+2*a*T);
q23=1/(2*a^2)*(exp(-2*a*T)+1-2*exp(-a*T));
q31=q13;
q32=q23;
q33=1/(2*a)*(1-exp(-2*a*T));
am=80; %最大机动加速度
pm=0.5; %am的概率
p0=0.5; %非机动概率
delta=am^2*(1+4*pm-p0)/3;
Q1=2*a*[ q11 q12 q13 0 0 0
q21 q22 q23 0 0 0
q31 q32 q33 0 0 0
0 0 0 q11 q12 q13
0 0 0 q21 q22 q23
0 0 0 q31 q32 q33]*delta*0.5;
%CA模型
F=[1 T T^2/2 0 0 0
0 1 T 0 0 0
0 0 1 0 0 0
0 0 0 1 T T^2/2
0 0 0 0 1 T
0 0 0 0 0 1];
Q=[T^5/20 T^4/8 T^3/6 0 0 0
T^4/8 T^3/3 T^2/2 0 0 0
T^3/6 T^2/2 T 0 0 0
0 0 0 T^5/20 T^4/8 T^3/6
0 0 0 T^4/8 T^3/3 T^2/2
0 0 0 T^3/6 T^2/2 T ];
%CA模型
Q2=Q*20;
Q3=Q*80;
for i=2:length(z)
%a:进行交互
U=u(:,i);
miu(1,1)=Pt(1,1)*U(1)/(Pt(1,1)*U(1)+Pt(2,1)*U(2)+Pt(3,1)*U(3));
miu(1,2)=Pt(1,2)*U(1)/(Pt(1,2)*U(1)+Pt(2,2)*U(2)+Pt(3,2)*U(3));
miu(1,3)=Pt(1,3)*U(1)/(Pt(1,3)*U(1)+Pt(2,3)*U(2)+Pt(3,3)*U(3));
miu(2,1)=Pt(2,1)*U(2)/(Pt(1,1)*U(1)+Pt(2,1)*U(2)+Pt(3,1)*U(3));
miu(2,2)=Pt(2,2)*U(2)/(Pt(1,2)*U(1)+Pt(2,2)*U(2)+Pt(3,2)*U(3));
miu(2,3)=Pt(2,3)*U(2)/(Pt(1,3)*U(1)+Pt(2,3)*U(2)+Pt(3,3)*U(3));
miu(3,1)=Pt(3,1)*U(3)/(Pt(1,1)*U(1)+Pt(2,1)*U(2)+Pt(3,1)*U(3));
miu(3,2)=Pt(3,2)*U(3)/(Pt(1,2)*U(1)+Pt(2,2)*U(2)+Pt(3,2)*U(3));
miu(3,3)=Pt(3,3)*U(3)/(Pt(1,3)*U(1)+Pt(2,3)*U(2)+Pt(3,3)*U(3));
X01=miu(1,1)*X1(:,i-1)+miu(2,1)*X2(:,i-1)+miu(3,1)*X3(:,i-1); %交互后的第1个值 即 第1个滤波器输入值
X02=miu(1,2)*X1(:,i-1)+miu(2,2)*X2(:,i-1)+miu(3,2)*X3(:,i-1); %交互后的第2个值 即 第2个滤波器输入值
X03=miu(1,3)*X1(:,i-1)+miu(2,3)*X2(:,i-1)+miu(3,3)*X3(:,i-1); %交互后的第3个值 即 第3个滤波器输入值
P01=(P1+(X1(:,i-1)-X01)*(X1(:,i-1)-X01)')*miu(1,1)+(P2+(X2(:,i-1)-X01)*(X2(:,i-1)-X01)')*miu(2,1)+(P3+(X3(:,i-1)-X01)*(X3(:,i-1)-X01)')*miu(3,1); %相应于X01的协方差
P02=(P1+(X1(:,i-1)-X02)*(X1(:,i-1)-X02)')*miu(1,2)+(P2+(X2(:,i-1)-X02)*(X2(:,i-1)-X02)')*miu(2,2)+(P3+(X3(:,i-1)-X02)*(X3(:,i-1)-X02)')*miu(3,2); %相应于X02的协方差
P03=(P1+(X1(:,i-1)-X03)*(X1(:,i-1)-X03)')*miu(1,3)+(P2+(X2(:,i-1)-X03)*(X2(:,i-1)-X03)')*miu(2,3)+(P3+(X3(:,i-1)-X03)*(X3(:,i-1)-X03)')*miu(3,3); %相应于X03的协方差
%singer
X1(:,i)=F*X01;
P=F*P*F'+Q1;
H=[ 1, 0, 0, 0, 0, 0
0, 1, 0, 0, 0, 0
0, 0, 0, 1, 0, 0
0, 0, 0, 0, 1, 0];
S1=H*P1*H'+R;
K=P1*H'*inv(S1);% 增益K
V1=z(:,i)-H*X1(:,i);
X1(:,i)=X1(:,i)+K*(V1);
P=(eye(6)-K*H)*P1*(eye(6)+K*H)'-K*R*K';
%CA1
X2(:,i)=F*X02;
P2=F*P02*F'+Q2;
S2=H*P2*H'+R;
K=P2*H'*inv(S2);% 增益K
V2=z(:,i)-H*X2(:,i);
X2(:,i)=X2(:,i)+K*(V2);
P2=(eye(6)-K*H)*P2*(eye(6)+K*H)'-K*R*K';
%CA2
X3(:,i)=F*X03;
P3=F*P03*F'+Q3;
S3=H*P3*H'+R;
K=P3*H'*inv(S3);% 增益K
V3=z(:,i)-H*X3(:,i);
X3(:,i)=X3(:,i)+K*(V3);
P3=(eye(6)-K*H)*P3*(eye(6)+K*H)'-K*R*K';
q(1)=exp(-1/2*V1'*inv(S1)*V1)/sqrt(det(2*pi*S1));
q(2)=exp(-1/2*V2'*inv(S2)*V2)/sqrt(det(2*pi*S2));
q(3)=exp(-1/2*V3'*inv(S3)*V3)/sqrt(det(2*pi*S3));
%d:各滤波器的 模型概率更新（以前是U）
c=q(1)*(Pt(1,1)*U(1)+Pt(2,1)*U(2)+Pt(3,1)*U(3))+...
q(2)*(Pt(1,2)*U(1)+Pt(2,2)*U(2)+Pt(3,2)*U(3))+...
q(3)*(Pt(1,3)*U(1)+Pt(2,3)*U(2)+Pt(3,3)*U(3));
u(1,i+1)=(q(1)*(Pt(1,1)*U(1)+Pt(2,1)*U(2)+Pt(3,1)*U(3)))/c; % u 用来下一次交互前的 模型概率更新
u(2,i+1)=(q(2)*(Pt(1,2)*U(1)+Pt(2,2)*U(2)+Pt(3,2)*U(3)))/c;
u(3,i+1)=(q(3)*(Pt(1,3)*U(1)+Pt(2,3)*U(2)+Pt(3,3)*U(3)))/c;
%e:输出X、P
x(:,i)=X1(:,i)*u(1,i+1)+X2(:,i)*u(2,i+1)+X3(:,i)*u(3,i+1);
P=u(1,i+1)*(P1+(X1(:,i)-x(:,i))*(X1(:,i)-x(:,i))')+...
u(2,i+1)*(P2+(X2(:,i)-x(:,i))*(X2(:,i)-x(:,i))')+...
u(3,i+1)*(P3+(X3(:,i)-x(:,i))*(X3(:,i)-x(:,i))'); %此为IMM所需结果
end
%画图观察
% figure(3);
% plot(X(1,:),X(4,:),'b-'),hold on;
% plot(z(1,:),z(3,:),'k:')
% plot(x(1,:),x(4,:),'r:')
% title('XY平面上目标的飞行路径(IMM with EKF方法)');
% xlabel('x/（米）');ylabel('y/（米）');
% legend('真实轨迹','测量值','交互滤波结果');
%
% %将各模型的概率放在一张图上观察
% figure(4)
% subplot(2,2,1);
% plot((1:length(u))*T,u(1,:),':',(1:length(u))*T,u(2,:),'-',(1:length(u))*T,u(3,:),'-.');
% title('各模型概率总图(IMM with EKF方法)');
% xlabel('t/（秒）');ylabel('Probability');
% legend('singer','CA 1','CA 50');
%
% %将各模型的概率分别观察
% subplot(2,2,2);
% plot((1:length(u))*T,u(1,:),':')
% grid on;
% title('singer模型概率');
% xlabel('t/（秒）');ylabel('Probability');
% legend('singer');
%
% subplot(2,2,3);
% plot((1:length(u))*T,u(2,:),'-')
% grid on;
% title('CA 1模型概率');
% xlabel('t/（秒）');ylabel('Probability');
% legend('CA 1');
%
% subplot(2,2,4);
% plot((1:length(u))*T,u(3,:),'-.')
% grid on;
% title('CA 50模型概率');
% xlabel('t/（秒）');ylabel('Probability');
% legend('CA 50');
% figure(3);
% plot(X(1,:),'b-'),hold on;
% plot(z(1,:),'k:')
% plot(x(1,:),'r:')
% title('横坐标位移');
% xlabel('秒');ylabel('米');
% legend('真实轨迹','测量值','滤波值');
%
% figure(4);
% plot(X(2,:),'b-'),hold on;
% plot(z(2,:),'k:')
% plot(x(2,:),'r:')
% title('横坐标速度');
% xlabel('秒');ylabel('米/秒');
% legend('真实轨迹','测量值','滤波值');
%
% figure(5);
% plot(X(4,:),'b-'),hold on;
% plot(z(3,:),'k:')
% plot(x(4,:),'r:')
% title('纵坐标位移');
% xlabel('秒');ylabel('米');
% legend('真实轨迹','测量值','滤波值');
%
% figure(6);
% plot(X(5,:),'b-'),hold on;
% plot(z(4,:),'k:')
% plot(x(5,:),'r:')
% title('纵坐标速度');
% xlabel('秒');ylabel('米/秒');
% legend('真实轨迹','测量值','滤波值');⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -