functionimm_extend2()%交互式多模型初步改进clearall;closeall;timenum=75;t=1;transfer_matric=[0.980.02;%马尔科夫转移矩阵矩阵为j行i列0.020.98];u_10=0.5;%目标1在模型i在k-1时刻的概率u_20=0.5;%目标1在模型i在k-1时刻的概率u=[u_10u_20];Q1=100;%目标1的状态噪声协方差阵Q2=4000;%目标1的状态噪声协方差阵R=10000;%观测噪声协方差阵%R2=700;x00_1=[000]";%模型1估计的初始值x00_2=[000]";%模型2估计的初始值p00_1=[10000;0100;000];p00_2=[10000;0100;0010];dis=1000;%位移vel=450;%速度acc=50;%加速度xx01=[disvel0]";%状态的初始值xx02=[disvelacc]";%状态的初始值%xx2=0;f1=[1t0;010;000];%状态转移阵f2=[1t0.5*t^2;01t;001];l1=[0.5*t^2t0]";%状态干扰阵l2=[0.5*t^2t1]";h=[100];%观测转移阵I=[100;010;001];num=1;XX1=zeros(3,25);XX2=XX1;XX3=XX1;whilenum<=timenumifnum<=25xx1=f1*xx01+l1*sqrt(Q1);%目标的状态值XX1(:,num)=xx1;z(num)=h*xx1+sqrt(R)*randn(1);%目标的观测值elseif25<num<=50xx2=f2*xx02+l2*sqrt(Q2);%目标的状态值XX2(:,num-25)=xx2;z(num)=h*xx2+sqrt(R)*randn(1);%目标的观测值elseif50<num<=timenumxx3=f1*xx01+l1*sqrt(Q1);%目标的状态值XX3(:,num-50)=xx3;z(num)=h*xx3+sqrt(R)*randn(1);%目标的观测值end;end;end;%================inputalternation==============================tran_11=transfer_matric(1,1)*u(1);tran_12=transfer_matric(1,2)*u(1);tran_21=transfer_matric(2,1)*u(2);tran_22=transfer_matric(2,2)*u(2);sum_tran_j1=tran_11+tran_21;sum_tran_j2=tran_12+tran_22;sum_tran_j=[sum_tran_j1sum_tran_j2];u_mix(1,1)=tran_11/sum_tran_j1;u_mix(1,2)=tran_12/sum_tran_j2;u_mix(2,1)=tran_21/sum_tran_j1;u_mix(2,2)=tran_22/sum_tran_j2;%fori=1:2%x00_estimate_j(i)=sum_tran_j(i)*x00(i);%p00_estimate_j(i)=sum_tran_j(i)*(p00+(x00-x00_estimate_j(i))*(x00-x00_estimate_j(i))");%p00_estimate以行存储%end;x00_estimate_j(:,1)=x00_1*u_mix(1,1)+x00_2*u_mix(2,1);x00_estimate_j(:,2)=x00_1*u_mix(1,2)+x00_2*u_mix(2,2);p00_estimate_j1=(p00_1+(x00_1-x00_estimate_j(:,1))*(x00_1-x00_estimate_j(:,1))")*u_mix(1,1)+...(p00_2+(x00_2-x00_estimate_j(:,1))*(x00_2-x00_estimate_j(:,1))")*u_mix(2,1);p00_estimate_j2=(p00_1+(x00_1-x00_estimate_j(:,2))*(x00_1-x00_estimate_j(:,2))")*u_mix(1,2)+...(p00_2+(x00_2-x00_estimate_j(:,2))*(x00_2-x00_estimate_j(:,2))")*u_mix(2,2);%==================filtercalculate=================================%================model1===================================x1_pre=f1*x00_estimate_j(:,1);p1_pre=f1*p00_estimate_j1*f1"+l1*Q1*l1";s1=h*p1_pre*h"+R;k_gain1=p1_pre*h"*inv(s1);p1_estimate=(I-k_gain1*h)*p1_pre;v1=z(num)-h*x1_pre;x1_estimate=x1_pre+k_gain1*v1;X1_estimate(:,num)=x1_estimate;%================model2===================================x2_pre=f2*x00_estimate_j(:,2);p2_pre=f2*p00_estimate_j2*f2"+l2*Q2*l2";s2=h*p2_pre*h"+R;k_gain2=p2_pre*h"*inv(s2);p2_estimate=(I-k_gain2*h)*p2_pre;v2=z(num)-h*x2_pre;x2_estimate=x2_pre+k_gain2*v2;X2_estimate(:,num)=x2_estimate;%===============================================================%===================modelprobabilityalternate=================p_pre=[p1_pre;p2_pre];x_pre=[x1_pre"x2_pre"];s=[s1(1)s2(1)];v=[v1v2];fori=1:2model_fun(i)=(1/sqrt(2*pi*s(i)))*exp(-v(i)*v(i)"/(2*abs(s(i))));end;sum_tran=model_fun(1)*sum_tran_j1+model_fun(2)*sum_tran_j2;u_j1=model_fun(1)*sum_tran_j1/sum_tran;u_j2=model_fun(2)*sum_tran_j2/sum_tran;u_j=[u_j1u_j2];%==============================================================%====================outputalternation=========================x_sum=x1_estimate*u_j(1)+x2_estimate*u_j(2)X_sum(:,num)=x_sum;p_sum=(p1_estimate+(x_sum-x1_estimate)*(x_sum-x1_estimate)")*u_j1+(p2_estimate+(x_sum-x2_estimate)*(x_sum-x2_estimate)")*u_j2;P_sum(3*num-2:3*num,1:3)=p_sum;%x00=x_sum(k);%x00=[x1_estimate(k)x2_estimate(k)];x00_1=x1_estimate;x00_2=x2_estimate;%p00=[p1_estimate(k)p2_estimate(k)];p00_1=p1_estimate;p00_2=p2_estimate;if(num<=25|25<num<=75)xx01=xx1;elseif25<num<=50xx02=xx2;%else%xx01=xx3;end;end;%xx2=xx_2(k);u=[u_j1u_j2];num=num+1;end;XX=[XX1XX2XX3];figure;plot(XX(1,:)","b");holdon;plot(X_sum(1,:),"m")legend("状态","融合输出的估计");plot(z,"r")xlabel("采样次数");ylabel("相应值");grid;holdoff;%figure;%plot(p1_estimate,"b");%holdon;%plot(p2_estimate,"m");%plot(p_sum,"r");%holdoff;%legend("模型1的协方差","模型2的协方差","融合输出的协方差");%xlabel("采样次数");%ylabel("相应值");%grid;figure;plot(XX(2,:)","b");holdon;plot(X_sum(2,:)","r");legend("状态","融合输出的估计");xlabel("采样次数");ylabel("相应值");grid;holdoff;figure;plot(XX(3,:),"b");holdon;plot(X_sum(3,:)","r");legend("状态","融合输出的估计");xlabel("采样次数");ylabel("相应值");grid;holdoff;X_sumXX
2023-06-26 05:11:181