三种模型交互

  • locust_dpw
    了解作者
  • matlab
    开发工具
  • 15.2KB
    文件大小
  • zip
    文件格式
  • 0
    收藏次数
  • 1 积分
    下载积分
  • 0
    下载次数
  • 2022-08-14 20:42
    上传日期
交互多模型源码,包含几种常用的模型,供初学者参考。
IMM_CS-CA-MCT.zip
  • 20 交互多模型(CS CA MCT)
  • track_51.asv
    5.2KB
  • IMM.m
    13.7KB
  • CSCT.m
    6.1KB
  • CSCT.asv
    6.1KB
  • IMM.asv
    13.7KB
  • track_51.m
    5.2KB
  • IMM_yuanshi.m
    8.2KB
内容介绍
%%% 程序中的算法可以对照硕士论文《一种改进的机动目标算法》;假定模型集为: CV CS MCT %%%%%%%%%%%%%%%%%% close all; clc; clear all; %%%%%%%%% 公共初始参数 %%%%%%%%%%%%%%%%%%% N=1000; T=1; M=10; r1=100; r2=100; a=1/60; p=[ 0.95 0.025 0.025 % 马尔科夫概率矩阵,模型转移概率 0.025 0.95 0.025 0.025 0.025 0.95 ]; u=[ 0.25 0.5 0.25 ]'; % 初始时刻模型概率 y=track_51(); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 产生运动轨迹 Z=[y(1,:)+r1*randn(1,N);y(4,:)+r2*randn(1,N)];%%%%%%%%%%%%% 产生真实航迹 %%%%%模型初始权值%%%%%%%% X=zeros(6,N); X1=zeros(6,N); X2=zeros(6,N); X33=zeros(6,N); X_SE=zeros(6,N); X_MSE=zeros(6,N); X_hangji=zeros(6,N); X1_mix=zeros(6,N); X2_mix=zeros(6,N); X3=zeros(5,N);%x位置、x速度、y位置、y速度、角速度w(t) X_P3=zeros(5,N); w_av=zeros(1,N); wX_hangji=zeros(5,N); %%%%%%%%%%%% 模型一参数 CV %%%%%%%%% F1=[1 T 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 T 0 0 0 0 0 1 0 0 0 0 0 0 1]; Q1=[T^3/3 T^2/2 0 0 0 0 T^2/2 T 0 0 0 0 0 0 0 0 0 0 0 0 0 T^3/3 T^2/2 0 0 0 0 T^2/2 T 0 0 0 0 0 0 0]; H1=[1 0 0 0 0 0 0 0 0 1 0 0 ]; %%%%%%%%%%%% 模型二参数 CS %%%%%%%%% a_max=100; R=[ r1^2 0 0 r2^2 ]; q11=(1-exp(-2*a*T)+2*a*T+2*a^3*T^3/3-2*a^2*T^2-4*a*T*exp(-a*T))/(2*a^5); q12=(exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2)/(2*a^4); q13=(1-exp(-2*a*T)-2*a*T*exp(-a*T))/(2*a^3); q21=q12; q22=(4*exp(-a*T)-3-exp(-2*a*T)+2*a*T)/(2*a^3); q23=(exp(-2*a*T)+1-2*exp(-a*T))/(2*a^2); q31=q13; q32=q23; q33=(1-exp(-2*a*T))/(2*a); q2=[q11 q12 q13 q21 q22 q23 q31 q32 q33 ]; f2=[1 T (a*T-1+exp(-a*T))/a^2 0 1 (1-exp(-a*T))/a 0 0 exp(-a*T) ]; F2=[f2 zeros(3,3) zeros(3,3) f2 ]; U2=[T^2/2-(a*T-1+exp(-a*T))/a^2;T-(1-exp(-a*T))/a;1-exp(-a*T)];%%%%% H2=[1 0 0 0 0 0 0 0 0 1 0 0 ]; %%%%%%%%%%%% 模型三参数 MCT %%%%%%%%% H3=[1 0 0 0 0 0 0 1 0 0]; w_max=2; delta=50; U3=[0 0 0 0 1-exp(-a*T)]'; %%%%%%%%%滤波%%%%%%%%%%%%%%%%% tic % 计时开始 for j=1:M for k=1:N if k==1 %%%%%%%%%%%%%%%%%%%%%%%滤波初始化%%%%%%%%%%%%%%%%%%%%%%% X(:,1)=[Z(1,1);0;0;Z(2,1);0;0]; % 第一点 X1(:,1)=X(:,1); X2(:,1)=X(:,1); X(:,2)=[Z(1,2);(Z(1,2)-Z(1,1))/T;0;Z(2,2);(Z(2,2)-Z(2,1))/T;0]; % 第二点 X1(:,2)=X(:,2); X2(:,2)=X(:,2); P01=[ r1^2 r1^2/T 0 r1^2/T 2*r1^2/T^2 0 0 0 0 ]; P02=[ r2^2 r2^2/T 0 r2^2/T 2*r2^2/T^2 0 0 0 0 ]; P=[ P01 zeros(3,3) zeros(3,3) P02 ]; P1=P; P2=P; P33=P; w=(atan(Z(1,1)/Z(2,1))-atan(Z(1,2)/Z(2,2)))/T; X3(:,1)=[Z(1,1);0;Z(2,1);0;0]; X33(:,1)=[Z(1,1);0;0;Z(2,1);0;0]; X_P3(:,1)=X3(:,1); X3(:,2)=[Z(1,2);(Z(1,2)-Z(1,1))/T;Z(2,2);(Z(2,2)-Z(2,1))/T;w]; X33(:,2)=[Z(1,2);(Z(1,2)-Z(1,1))/T;0;Z(2,2);(Z(2,2)-Z(2,1))/T;0]; X_P3(:,2)=X3(:,2); P3=[r1^2 r1^2/T 0 0 0 r1^2/T 2*r1^2/T^2 0 0 0 0 0 r2^2 r2^2/T 0 0 0 r2^2/T 2*r2^2/T^2 0 0 0 0 0 0]; w_av(2)=w; else if k>2 %%%%%%%%%%%%%%%%%%%%%% 输入交互 %%%%%%%%%%%%% for j=1:3 c(j)=dot(p(:,j),u); end for i=1:3 for j=1:3 uu(i,j)=p(i,j)*u(i)/c(j); end end c_av=p'*u; %%%%预测权重%%%%%%%%%%% %uu=p.*(u*ones(1,3))./(ones(3,1)*c_av');%%%%%混合权重%%% X1_mix(:,k-1)=X1(:,k-1)*uu(1,1)+X2(:,k)*uu(2,1)+X33(:,k)*uu(3,1); X2_mix(:,k-1)=X1(:,k-1)*uu(1,2)+X2(:,k-1)*uu(2,2)+X33(:,k)*uu(3,2); X3_mix(:,k-1)=X1(:,k-1)*uu(1,3)+X2(:,k-1)*uu(2,3)+X33(:,k)*uu(3,3); P1_mix=uu(1,1)*(P1+(X1(:,k-1)-X1_mix(:,k-1))*(X1(:,k-1)-X1_mix(:,k-1))')+uu(2,1)*(P2+(X2(:,k-1)-X1_mix(:,k-1))*(X2(:,k-1)-X1_mix(:,k-1))')+uu(3,1)*(P33+(X33(:,k-1)-X3_mix(:,k-1))*(X33(:,k-1)-X3_mix(:,k-1))');%混合估计协方差 P2_mix=uu(1,2)*(P1+(X1(:,k-1)-X2_mix(:,k-1))*(X1(:,k-1)-X2_mix(:,k-1))')+uu(2,2)*(P2+(X2(:,k-1)-X2_mix(:,k-1))*(X2(:,k-1)-X2_mix(:,k-1))')+uu(3,2)*(P33+(X33(:,k-1)-X3_mix(:,k-1))*(X33(:,k-1)-X3_mix(:,k-1))'); P3_mix=uu(1,3)*(P1+(X1(:,k-1)-X2_mix(:,k-1))*(X1(:,k-1)-X2_mix(:,k-1))')+uu(2,3)*(P2+(X2(:,k-1)-X2_mix(:,k-1))*(X2(:,k-1)-X2_mix(:,k-1))')+uu(3,3)*(P33+(X33(:,k-1)-X3_mix(:,k-1))*(X33(:,k-1)-X3_mix(:,k-1))'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 输入交互后,模型分别进行滤波 %%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%模型一进行滤波 CV %%%%%%%%%%%%%% X_P1(:,k)=F1*X1_mix(:,k-1); P_P1=F1*P1_mix*F1'+Q1; gamma1(:,k)=Z(:,k)-H1*X_P1(:,k);%%%%%%%% 新息向量 S1=H1*P_P1*H1'+R; K1=P_P1*H1'*inv(S1); X1(:,k)=X_P1(:,k)+K1*gamma1(:,k); P1=(eye(6)-K1*H1)*P_P1; lamda1(k)=1/(sqrt(2*pi*det(S1)))*exp(-0.5*gamma1(:,k)'*inv(S1)*gamma1(:,k));%%%似然函数 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%模型二进行滤波 CS %%%%%%%%%%%%%% if X2_mix(3,k-1)>0 % x轴加速度 ca_x=(4-pi)/pi*(a_max-X2_mix(3,k-1))^2; else ca_x=(4-pi)/pi*(-a_max+X2_mix(3,k-1))^2; end if X2_mix(6,k-1)>0 % y轴加速度 ca_y=(4-pi)/pi*(a_max-X2_mix(6,k-1))^2; else ca_y=(4-pi)/pi*(-a_max+X2_mix(6,k-1))^2; end Q2=2*a*[ca_x*q2 zeros(3,3) zeros(3,3) ca_y*q2]; X_P2(:,k)=F2*X2_mix(:,k-1)+[U2*X2_mix(3,k-1);U2*X2_mix(6,k-1)]; P_P2=F2*P2_mix*F2'+Q2; gamma2(:,k)=Z(:,k)-H2*X_P2(:,k);%%%%%%%%%% 新息向量 S2=H2*P_P2*H2'+R; K2=P_P2*H2'*inv(S2); X2(:,k)=X_P2(:,k)+K2*gamma2(:,k); P2=(eye(6)-K2*H2)*P_P2; lamda2(k)=1/(sqrt(2*pi*det(S2)))*exp(-0.5*gamma2(:,k)'*inv(S2)*gamma2(:,k));%%%似然函数 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%模型三进行滤波 MCT %%%%%%%%%%%%%% f3=[1 sin(X3(5,k-1)*T)/X3(5,k-1) 0 -(1-cos(X3(5,k-1)*T))/X3(5,k-1) 0 0 cos(X3(5,k-1)*T) 0 -sin(X3(5,k-1)*T) 0 0 (1-cos(X3(5,k-1)*T))/X3(5,k-1) 1 sin(X3(5,k-1)*T)/X3(5,k-1) 0 0 sin(X3(5,k-1)*T) 0 cos(X3(5,k-1)*T) 0 0 0 0 0 exp(-a*T)]; q3=[(2*(X3(5,k-1)*T-sin(X3(5,k-1)*T)))/X3(5,k-1)^3 (1-cos(X3(5,k-1)*T))/X3(5,k-1)^2 0 (X3(5,k-1)*T-sin(X3(5,k-1)*T))/X3(5,k-1)^2 (1-cos(X3(5,k-1)*T))/X3(5,k-1)^2 T -(X3(5,k-1)*T-sin(X3(5,k-1)*T))/X3(5,k-1)^2 0 0 -(X3(5,k-1)*T-sin(X3(5,k-1)*T))/X3(5,k-1)^2 (2*(X3(5,k-1)*T-sin(X3(5,k-1)*T)))/X3(5,k-1)^3 (1-cos(X3(5,k-1)*T))/X3(5,k-1)^2 (X3(5,k-1)*T-sin(X3(5,k-1)*T))/X3(5,k-1)^2 0 (1-cos(X3(5,k-1)*T))/X3(5,k-1)^2 T]; F3=[1 sin(X3(5,k-1)*T)/X3(5,k-1) 0 -(1-cos(X3(5,k-1)*T))/X3(5,k-1) (cos(X3(5
评论
    相关推荐