📄 imm3_imm2_singel_kalman.m
字号:
if cbar_IMM3_1<10^(-8) % prevent cbar_IMM3_1 from blowing up
cbar_IMM3_1=10^(-8);
else
cbar_IMM3_1=cbar_IMM3_1;
end
%mix probabilities
mu_IMM3_11(2)=rho11_IMM3/(3*cbar_IMM3_1);
mu_IMM3_21(2)=rho21_IMM3/(3*cbar_IMM3_1);
mu_IMM3_31(2)=rho31_IMM3/(3*cbar_IMM3_1);
mu_IMM3_12(2)=rho12_IMM3/(3*cbar_IMM3_2);
mu_IMM3_22(2)=rho22_IMM3/(3*cbar_IMM3_2);
mu_IMM3_32(2)=rho32_IMM3/(3*cbar_IMM3_2);
mu_IMM3_13(2)=rho13_IMM3/(3*cbar_IMM3_3);
mu_IMM3_23(2)=rho13_IMM3/(3*cbar_IMM3_3);
mu_IMM3_33(2)=rho33_IMM3/(3*cbar_IMM3_3);
xhat_IMM2_01(:,2)=mu_IMM2_11(2)*xup_IMM2_1(:,2)+mu_IMM2_21(2)*xup_IMM2_2(:,2);
xhat_IMM2_02(:,2)=mu_IMM2_12(2)*xup_IMM2_1(:,2)+mu_IMM2_22(2)*xup_IMM2_2(:,2);
Phat_IMM2_01(:,:,2)=mu_IMM2_11(2)*(Pup_IMM2_1(:,:,2)+(xup_IMM2_1(:,2)-xhat_IMM2_01(:,2))*(xup_IMM2_1(:,2)-xhat_IMM2_01(:,2))')+...
mu_IMM2_21(2)*(Pup_IMM2_2(:,:,2)+(xup_IMM2_2(:,2)-xhat_IMM2_01(:,2))*(xup_IMM2_2(:,2)-xhat_IMM2_01(:,2))');
Phat_IMM2_02(:,:,2)=mu_IMM2_12(2)*(Pup_IMM2_1(:,:,2)+(xup_IMM2_1(:,2)-xhat_IMM2_02(:,2))*(xup_IMM2_1(:,2)-xhat_IMM2_02(:,2))')+...
mu_IMM2_22(2)*(Pup_IMM2_2(:,:,2)+(xup_IMM2_2(:,2)-xhat_IMM2_02(:,2))*(xup_IMM2_2(:,2)-xhat_IMM2_02(:,2))');
xhat_IMM3_01(:,2)=mu_IMM3_11(2)*xup_IMM3_1(:,2)+mu_IMM3_21(2)*xup_IMM3_2(:,2)+mu_IMM3_31(2)*xup_IMM3_3(:,2);
xhat_IMM3_02(:,2)=mu_IMM3_12(2)*xup_IMM3_1(:,2)+mu_IMM3_22(2)*xup_IMM3_2(:,2)+mu_IMM3_32(2)*xup_IMM3_3(:,2);
xhat_IMM3_03(:,2)=mu_IMM3_13(2)*xup_IMM3_1(:,2)+mu_IMM3_23(2)*xup_IMM3_2(:,2)+mu_IMM3_33(2)*xup_IMM3_3(:,2);
Phat_IMM3_01(:,:,2)=mu_IMM3_11(2)*(Pup_IMM3_1(:,:,2)+(xup_IMM3_1(:,2)-xhat_IMM3_01(:,2))*(xup_IMM3_1(:,2)-xhat_IMM3_01(:,2))')+...
mu_IMM3_21(2)*(Pup_IMM3_2(:,:,2)+(xup_IMM3_2(:,2)-xhat_IMM3_01(:,2))*(xup_IMM3_2(:,2)-xhat_IMM3_01(:,2))')+...
mu_IMM3_31(2)*(Pup_IMM3_3(:,:,2)+(xup_IMM3_3(:,2)-xhat_IMM3_01(:,2))*(xup_IMM3_3(:,2)-xhat_IMM3_01(:,2))');
Phat_IMM3_02(:,:,2)=mu_IMM3_12(2)*(Pup_IMM3_1(:,:,2)+(xup_IMM3_1(:,2)-xhat_IMM3_02(:,2))*(xup_IMM3_1(:,2)-xhat_IMM3_02(:,2))')+...
mu_IMM3_22(2)*(Pup_IMM3_2(:,:,2)+(xup_IMM3_2(:,2)-xhat_IMM3_02(:,2))*(xup_IMM3_2(:,2)-xhat_IMM3_02(:,2))')+...
mu_IMM3_32(2)*(Pup_IMM3_3(:,:,2)+(xup_IMM3_3(:,2)-xhat_IMM3_02(:,2))*(xup_IMM3_3(:,2)-xhat_IMM3_02(:,2))');
Phat_IMM3_03(:,:,2)=mu_IMM3_13(2)*(Pup_IMM3_1(:,:,2)+(xup_IMM3_1(:,2)-xhat_IMM3_03(:,2))*(xup_IMM3_1(:,2)-xhat_IMM3_03(:,2))')+...
mu_IMM3_23(2)*(Pup_IMM3_2(:,:,2)+(xup_IMM3_2(:,2)-xhat_IMM3_03(:,2))*(xup_IMM3_2(:,2)-xhat_IMM3_03(:,2))')+...
mu_IMM3_33(2)*(Pup_IMM3_3(:,:,2)+(xup_IMM3_3(:,2)-xhat_IMM3_03(:,2))*(xup_IMM3_3(:,2)-xhat_IMM3_03(:,2))');
%*********************************************
% single-Kalman-model
%*******************************************
X_up_K=zeros(nx1,nsample); % 状态更新阵的初始化
X_pre_K=zeros(nx1,nsample); % 状态预计
P_up_K=zeros(nx1,nx1,nsample); %更新协方差
P_pre_K=zeros(nx1,nx1,nsample); %预计协方差
Z_tilde_K=zeros(nz,nsample); %新息矩阵初始化
S_K=zeros(nz,nz,nsample); %新息方差矩阵初始化
Z_pre_K=zeros(nz,nsample); %测量值预测值
xerror_K=zeros(4,nsample);
K_K=zeros(nx1,nz,nsample); % kalman filter gain
xup_K(:,2)=[zobs(1,2) (zobs(1,2)-zobs(1,1))/delta zobs(2,2) (zobs(2,2)-zobs(2,1))/delta]';
r1=[r r/delta;r/delta 2*r/delta^2];
Pup_K(:,:,2)=[r1,zeros(2);zeros(2),r1];
% --------------kalman filter process-----------------------------
for ii=3:nsample
%--------------------IMM2---------------------------------------
% IMM2 滤波器1
xpre_IMM2_1(:,ii)=F_ncv*xhat_IMM2_01(:,ii-1);
Ppre_IMM2_1(:,:,ii)=F_ncv*Phat_IMM2_01(:,:,ii-1)*F_ncv'+G2*q1*eye(2)*G2';
zpre_IMM2_1(:,ii)=H_ncv*xpre_IMM2_1(:,ii);
ztilde_IMM2_1(:,ii)=zobs(:,ii)-zpre_IMM2_1(:,ii);
S_IMM2_1(:,:,ii)=H_ncv*Ppre_IMM2_1(:,:,ii)*H_ncv'+r*eye(2);
K_IMM2_1(:,:,ii)=Ppre_IMM2_1(:,:,ii)*H_ncv'*inv(S_IMM2_1(:,:,ii));
xup_IMM2_1(:,ii)=xpre_IMM2_1(:,ii)+K_IMM2_1(:,:,ii)*ztilde_IMM2_1(:,ii);
Pup_IMM2_1(:,:,ii)=Ppre_IMM2_1(:,:,ii)-K_IMM2_1(:,:,ii)*S_IMM2_1(:,:,ii)*K_IMM2_1(:,:,ii)';
lamda_IMM2_1=exp(-0.5*ztilde_IMM2_1(:,ii)'*inv(S_IMM2_1(:,:,ii))*ztilde_IMM2_1(:,ii))/sqrt(det(2*pi*S_IMM2_1(:,:,ii)));
% IMM2 滤波器2
F2_IMM2=[1,sin(ww1(ii)*delta)/ww1(ii),0,-(1-cos(ww1(ii)*delta))/ww1(ii);
0,cos(ww1(ii)*delta),0,-sin(ww1(ii)*delta);
0,(1-cos(ww1(ii)*delta))/ww1(ii),1,sin(ww1(ii)*delta)/ww1(ii);
0,sin(ww1(ii)*delta),0,cos(ww1(ii)*delta)];
xpre_IMM2_2(:,ii)=F2_IMM2*xhat_IMM2_02(:,ii-1);
Ppre_IMM2_2(:,:,ii)=F2_IMM2*Phat_IMM2_02(:,:,ii-1)*F2_IMM2'+G2*q2*eye(2)*G2';
zpre_IMM2_2(:,ii)=H_notr*xpre_IMM2_2(:,ii);
ztilde_IMM2_2(:,ii)=zobs(:,ii)-zpre_IMM2_2(:,ii);
S_IMM2_2(:,:,ii)=H_notr*Ppre_IMM2_2(:,:,ii)*H_notr'+r*eye(2);
K_IMM2_2(:,:,ii)=Ppre_IMM2_2(:,:,ii)*H_notr'*inv(S_IMM2_2(:,:,ii));
xup_IMM2_2(:,ii)=xpre_IMM2_2(:,ii)+K_IMM2_2(:,:,ii)*ztilde_IMM2_2(:,ii);
Pup_IMM2_2(:,:,ii)=Ppre_IMM2_2(:,:,ii)-K_IMM2_2(:,:,ii)*S_IMM2_2(:,:,ii)*K_IMM2_2(:,:,ii)';
lamda_IMM2_2=exp(-0.5*ztilde_IMM2_2(:,ii)'*inv(S_IMM2_2(:,:,ii))*ztilde_IMM2_2(:,ii))/sqrt(det(2*pi*S_IMM2_2(:,:,ii)));
% 计算模型概率
c_IMM2=lamda_IMM2_1*cbar_IMM2_1+lamda_IMM2_2*cbar_IMM2_2;
mu_IMM2(1,ii)=lamda_IMM2_1*cbar_IMM2_1/c_IMM2;
mu_IMM2(2,ii)=lamda_IMM2_2*cbar_IMM2_2/c_IMM2;
%滤波器输出
xout_IMM2(:,ii)=mu_IMM2(1,ii)*xup_IMM2_1(:,ii)+mu_IMM2(2,ii)*xup_IMM2_2(:,ii);
Pout_IMM2(:,:,ii)=mu_IMM2(1,ii)*(Pup_IMM2_1(:,:,ii)+(xup_IMM2_1(:,ii)-xout_IMM2(:,ii))*(xup_IMM2_1(:,ii)-xout_IMM2(:,ii))')+...
mu_IMM2(2,ii)*(Pup_IMM2_2(:,:,ii)+(xup_IMM2_2(:,ii)-xout_IMM2(:,ii))*(xup_IMM2_2(:,ii)-xout_IMM2(:,ii))');
xerror_IMM2(:,ii)=xtrue(:,ii)-xout_IMM2(:,ii);
%计算模型混合概率
cbar_IMM2_1=rho11_IMM2*mu_IMM2(1,ii)+rho21_IMM2*mu_IMM2(2,ii);
cbar_IMM2_2=rho12_IMM2*mu_IMM2(1,ii)+rho22_IMM2*mu_IMM2(2,ii);
mu_IMM2_11(ii)=rho11_IMM2*mu_IMM2(1,ii)/cbar_IMM2_1;
mu_IMM2_12(ii)=rho12_IMM2*mu_IMM2(1,ii)/cbar_IMM2_2;
mu_IMM2_21(ii)=rho21_IMM2*mu_IMM2(2,ii)/cbar_IMM2_1;
mu_IMM2_22(ii)=rho22_IMM2*mu_IMM2(2,ii)/cbar_IMM2_2;
%mixing
xhat_IMM2_01(:,ii)=mu_IMM2_11(ii)*xup_IMM2_1(:,ii)+mu_IMM2_21(ii)*xup_IMM2_2(:,ii);
xhat_IMM2_02(:,ii)=mu_IMM2_12(ii)*xup_IMM2_1(:,ii)+mu_IMM2_22(ii)*xup_IMM2_2(:,ii);
Phat_IMM2_01(:,:,ii)=mu_IMM2_11(ii)*(Pup_IMM2_1(:,:,ii)+(xup_IMM2_1(:,ii)-xhat_IMM2_01(:,ii))*(xup_IMM2_1(:,ii)-xhat_IMM2_01(:,ii))')+...
mu_IMM2_21(ii)*(Pup_IMM2_2(:,:,ii)+(xup_IMM2_2(:,ii)-xhat_IMM2_01(:,ii))*(xup_IMM2_2(:,ii)-xhat_IMM2_01(:,ii))');
Phat_IMM2_02(:,:,ii)=mu_IMM2_12(ii)*(Pup_IMM2_1(:,:,ii)+(xup_IMM2_1(:,ii)-xhat_IMM2_02(:,ii))*(xup_IMM2_1(:,ii)-xhat_IMM2_02(:,ii))')+...
mu_IMM2_22(ii)*(Pup_IMM2_2(:,:,ii)+(xup_IMM2_2(:,ii)-xhat_IMM2_02(:,ii))*(xup_IMM2_2(:,ii)-xhat_IMM2_02(:,ii))');
%-------------------------------IMM3---------------------------------
%-IMM3 滤波器1滤波----------------------
xpre_IMM3_1(:,ii)=F_ncv*xhat_IMM3_01(:,ii-1);
Ppre_IMM3_1(:,:,ii)=F_ncv*Phat_IMM3_01(:,:,ii-1)*F_ncv'+G2*q1*eye(2)*G2';
zpre_IMM3_1(:,ii)=H_notr*xpre_IMM3_1(:,ii);
ztilde_IMM3_1(:,ii)=zobs(:,ii)-zpre_IMM3_1(:,ii);
S_IMM3_1(:,:,ii)=H_notr*Ppre_IMM3_1(:,:,ii)*H_notr'+r*eye(2);
K_IMM3_1(:,:,ii)=Ppre_IMM3_1(:,:,ii)*H_notr'*inv(S_IMM3_1(:,:,ii));
xup_IMM3_1(:,ii)=xpre_IMM3_1(:,ii)+K_IMM3_1(:,:,ii)*ztilde_IMM3_1(:,ii);
Pup_IMM3_1(:,:,ii)=Ppre_IMM3_1(:,:,ii)-K_IMM3_1(:,:,ii)*S_IMM3_1(:,:,ii)*K_IMM3_1(:,:,ii)';
lamda_IMM3_1=exp(-0.5*ztilde_IMM3_1(:,ii)'*inv(S_IMM3_1(:,:,ii))*ztilde_IMM3_1(:,ii))/sqrt(det(2*pi*S_IMM3_1(:,:,ii)));
% IMM3 滤波器2 滤波
F2_IMM3=[1,sin(ww2(ii)*delta)/ww2(ii),0,-(1-cos(ww2(ii)*delta))/ww2(ii);
0,cos(ww2(ii)*delta),0,-sin(ww2(ii)*delta);
0,(1-cos(ww2(ii)*delta))/ww2(ii),1,sin(ww2(ii)*delta)/ww2(ii);
0,sin(ww2(ii)*delta),0,cos(ww2(ii)*delta)];
xpre_IMM3_2(:,ii)=F2_IMM3*xhat_IMM3_02(:,ii-1);
Ppre_IMM3_2(:,:,ii)=F2_IMM3*Phat_IMM3_02(:,:,ii-1)*F2_IMM3'+G2*q2*eye(2)*G2';
zpre_IMM3_2(:,ii)=H_notr*xpre_IMM3_2(:,ii);
ztilde_IMM3_2(:,ii)=zobs(:,ii)-zpre_IMM3_2(:,ii);
S_IMM3_2(:,:,ii)=H_notr*Ppre_IMM3_2(:,:,ii)*H_notr'+r*eye(2);
K_IMM3_2(:,:,ii)=Ppre_IMM3_2(:,:,ii)*H_notr'*inv(S_IMM3_2(:,:,ii));
xup_IMM3_2(:,ii)=xpre_IMM3_2(:,ii)+K_IMM3_2(:,:,ii)*ztilde_IMM3_2(:,ii);
Pup_IMM3_2(:,:,ii)=Ppre_IMM3_2(:,:,ii)-K_IMM3_2(:,:,ii)*S_IMM3_2(:,:,ii)*K_IMM3_2(:,:,ii)';
lamda_IMM3_2=exp(-0.5*ztilde_IMM3_2(:,ii)'*inv(S_IMM3_2(:,:,ii))*ztilde_IMM3_2(:,ii))/sqrt(det(2*pi*S_IMM3_2(:,:,ii)));
%IMM3 滤波器3滤波
F3_IMM3=[1,sin(ww3(ii)*delta)/ww3(ii),0,-(1-cos(ww3(ii)*delta))/ww3(ii);
0,cos(ww3(ii)*delta),0,-sin(ww3(ii)*delta);
0,(1-cos(ww3(ii)*delta))/ww3(ii),1,sin(ww3(ii)*delta)/ww3(ii);
0,sin(ww3(ii)*delta),0,cos(ww3(ii)*delta)];
xpre_IMM3_3(:,ii)=F3_IMM3*xhat_IMM3_03(:,ii-1);
Ppre_IMM3_3(:,:,ii)=F3_IMM3*Phat_IMM3_03(:,:,ii-1)*F3_IMM3'+G2*q2*eye(2)*G2';
zpre_IMM3_3(:,ii)=H_notr*xpre_IMM3_3(:,ii);
ztilde_IMM3_3(:,ii)=zobs(:,ii)-zpre_IMM3_3(:,ii);
S_IMM3_3(:,:,ii)=H_notr*Ppre_IMM3_3(:,:,ii)*H_notr'+r*eye(2);
K_IMM3_3(:,:,ii)=Ppre_IMM3_3(:,:,ii)*H_notr'*inv(S_IMM3_3(:,:,ii));
xup_IMM3_3(:,ii)=xpre_IMM3_3(:,ii)+K_IMM3_3(:,:,ii)*ztilde_IMM3_3(:,ii);
Pup_IMM3_3(:,:,ii)=Ppre_IMM3_3(:,:,ii)-K_IMM3_3(:,:,ii)*S_IMM3_3(:,:,ii)*K_IMM3_3(:,:,ii)';
lamda_IMM3_3=exp(-0.5*ztilde_IMM3_3(:,ii)'*inv(S_IMM3_3(:,:,ii))*ztilde_IMM3_3(:,ii))/sqrt(det(2*pi*S_IMM3_3(:,:,ii)));
%计算模型概率
c_IMM3=lamda_IMM3_1*cbar_IMM3_1+lamda_IMM3_2*cbar_IMM3_2+lamda_IMM3_3*cbar_IMM3_3;
mu_IMM3(1,ii)=lamda_IMM3_1*cbar_IMM3_1/c_IMM3;
mu_IMM3(2,ii)=lamda_IMM3_2*cbar_IMM3_2/c_IMM3;
mu_IMM3(3,ii)=lamda_IMM3_3*cbar_IMM3_3/c_IMM3;
%滤波输出
xout_IMM3(:,ii)=mu_IMM3(1,ii)*xup_IMM3_1(:,ii)+mu_IMM3(2,ii)*xup_IMM3_2(:,ii)+mu_IMM3(3,ii)*xup_IMM3_3(:,ii);
Pout_IMM3(:,:,ii)=mu_IMM3(1,ii)*(Pup_IMM3_1(:,:,ii)+(xup_IMM3_1(:,ii)-xout_IMM3(:,ii))*(xup_IMM3_1(:,ii)-xout_IMM3(:,ii))')+...
mu_IMM3(2,ii)*(Pup_IMM3_2(:,:,ii)+(xup_IMM3_2(:,ii)-xout_IMM3(:,ii))*(xup_IMM3_2(:,ii)-xout_IMM3(:,ii))')+...
mu_IMM3(3,ii)*(Pup_IMM3_3(:,:,ii)+(xup_IMM3_3(:,ii)-xout_IMM3(:,ii))*(xup_IMM3_3(:,ii)-xout_IMM3(:,ii))');
xerror_IMM3(:,ii)=xtrue(:,ii)-xout_IMM3(:,ii);
%计算混合概率
cbar_IMM3_1=rho11_IMM3*mu_IMM3(1,ii)+rho21_IMM3*mu_IMM3(2,ii)+rho31_IMM3*mu_IMM3(3,ii);
cbar_IMM3_2=rho12_IMM3*mu_IMM3(1,ii)+rho22_IMM3*mu_IMM3(2,ii)+rho32_IMM3*mu_IMM3(3,ii);
cbar_IMM3_3=rho13_IMM3*mu_IMM3(1,ii)+rho23_IMM3*mu_IMM3(2,ii)+rho33_IMM3*mu_IMM3(3,ii);
mu_IMM3_11(ii)=rho11_IMM3*mu_IMM3(1,ii)/cbar_IMM3_1;
mu_IMM3_12(ii)=rho12_IMM3*mu_IMM3(1,ii)/cbar_IMM3_2;
mu_IMM3_13(ii)=rho13_IMM3*mu_IMM3(1,ii)/cbar_IMM3_3;
mu_IMM3_21(ii)=rho21_IMM3*mu_IMM3(2,ii)/cbar_IMM3_1;
mu_IMM3_22(ii)=rho22_IMM3*mu_IMM3(2,ii)/cbar_IMM3_2;
mu_IMM3_23(ii)=rho23_IMM3*mu_IMM3(2,ii)/cbar_IMM3_3;
mu_IMM3_31(ii)=rho31_IMM3*mu_IMM3(3,ii)/cbar_IMM3_1;
mu_IMM3_32(ii)=rho32_IMM3*mu_IMM3(3,ii)/cbar_IMM3_2;
mu_IMM3_33(ii)=rho33_IMM3*mu_IMM3(3,ii)/cbar_IMM3_3;
xhat_IMM3_01(:,ii)=mu_IMM3_11(ii)*xup_IMM3_1(:,ii)+mu_IMM3_21(ii)*xup_IMM3_2(:,ii)+mu_IMM3_31(ii)*xup_IMM3_3(:,ii);
xhat_IMM3_02(:,ii)=mu_IMM3_12(ii)*xup_IMM3_1(:,ii)+mu_IMM3_22(ii)*xup_IMM3_2(:,ii)+mu_IMM3_32(ii)*xup_IMM3_3(:,ii);
xhat_IMM3_03(:,ii)=mu_IMM3_13(ii)*xup_IMM3_1(:,ii)+mu_IMM3_23(ii)*xup_IMM3_2(:,ii)+mu_IMM3_33(ii)*xup_IMM3_3(:,ii);
Phat_IMM3_01(:,:,ii)=mu_IMM3_11(ii)*(Pup_IMM3_1(:,:,ii)+(xup_IMM3_1(:,ii)-xhat_IMM3_01(:,ii))*(xup_IMM3_1(:,ii)-xhat_IMM3_01(:,ii))')+...
mu_IMM3_21(ii)*(Pup_IMM3_2(:,:,ii)+(xup_IMM3_2(:,ii)-xhat_IMM3_01(:,ii))*(xup_IMM3_2(:,ii)-xhat_IMM3_01(:,ii))')+...
mu_IMM3_31(ii)*(Pup_IMM3_3(:,:,ii)+(xup_IMM3_3(:,ii)-xhat_IMM3_01(:,ii))*(xup_IMM3_3(:,ii)-xhat_IMM3_01(:,ii))');
Phat_IMM3_02(:,:,ii)=mu_IMM3_12(ii)*(Pup_IMM3_1(:,:,ii)+(xup_IMM3_1(:,ii)-xhat_IMM3_02(:,ii))*(xup_IMM3_1(:,ii)-xhat_IMM3_02(:,ii))')+...
mu_IMM3_22(ii)*(Pup_IMM3_2(:,:,ii)+(xup_IMM3_2(:,ii)-xhat_IMM3_02(:,ii))*(xup_IMM3_2(:,ii)-xhat_IMM3_02(:,ii))')+...
mu_IMM3_32(ii)*(Pup_IMM3_3(:,:,ii)+(xup_IMM3_3(:,ii)-xhat_IMM3_02(:,ii))*(xup_IMM3_3(:,ii)-xhat_IMM3_02(:,ii))');
Phat_IMM3_03(:,:,ii)=mu_IMM3_13(ii)*(Pup_IMM3_1(:,:,ii)+(xup_IMM3_1(:,ii)-xhat_IMM3_03(:,ii))*(xup_IMM3_1(:,ii)-xhat_IMM3_03(:,ii))')+...
mu_IMM3_23(ii)*(Pup_IMM3_2(:,:,ii)+(xup_IMM3_2(:,ii)-xhat_IMM3_03(:,ii))*(xup_IMM3_2(:,ii)-xhat_IMM3_03(:,ii))')+...
mu_IMM3_33(ii)*(Pup_IMM3_3(:,:,ii)+(xup_IMM3_3(:,ii)-xhat_IMM3_03(:,ii))*(xup_IMM3_3(:,ii)-xhat_IMM3_03(:,ii))');
%------------------------single Kalman------------------------------
X_pre_K(:,ii)=F_ncv*X_up_K(:,ii-1);
Z_pre_K(:,ii)=H_ncv*X_pre_K(:,ii);
Z_tilde_K(:,ii)=zobs(:,ii)-Z_pre_K(:,ii);
P_pre_K(:,:,ii)=F_ncv*P_up_K(:,:,ii-1)*F_ncv'+G2*q4*G2';
S_K(:,:,ii)=H_ncv* P_pre_K(:,:,ii)*H_ncv'+r*eye(nz);
K_K(:,:,ii)=P_pre_K(:,:,ii)*H_ncv'*inv(S_K(:,:,ii));
X_up_K(:,ii)=X_pre_K(:,ii)+K_K(:,:,ii)*Z_tilde_K(:,ii);
P_up(:,:,ii)=(eye(nx1)-K_K(:,:,ii)*H_ncv)*P_pre_K(:,:,ii);
xerror_K(:,ii)=xtrue(:,ii)-X_up_K(:,ii);
end %ii=3:nsample
for k=3:nsample
positionRMS_IMM2(k)=positionRMS_IMM2(k)+xerror_IMM2(1,k)^2+xerror_IMM2(3,k)^2;
positionRMS_IMM3(k)=positionRMS_IMM3(k)+xerror_IMM3(1,k)^2+xerror_IMM3(3,k)^2;
positionRMS_K(k)=positionRMS_K(k)+xerror_K(1,k)^2+xerror_K(3,k)^2;
end
toc
end %1:nloops
for k=3:nsample
positionRMS_IMM2(k)=sqrt(positionRMS_IMM2(k)/nloops);
positionRMS_IMM3(k)=sqrt(positionRMS_IMM3(k)/nloops);
positionRMS_K(k)=sqrt(positionRMS_K(k)/nloops);
end
figure(2)
plot(3:nsample,positionRMS_IMM2(3:nsample),'b-'),grid on;
hold on;
plot(3:nsample,positionRMS_IMM3(3:nsample),'r-');
hold on;
plot(5:nsample,positionRMS_K(5:nsample),'r:');
hold on;
plot(141*ones(1,30),'m');
hold on;
axis([0 30 70 160]);
X=8*ones(1,91);
Y=70:160;
plot(X,Y);
xlabel('Time(Sampling periods)');
ylabel('RMS(meter)');
title(' RMS位置估计误差(转弯速率3度/s)');
axis([0 30 70 160]);
X=8*ones(1,90);
Y=70:160;
% % title(' RMS位置估计误差(转弯速率6度/s)');
% title(' RMS位置估计误差(转弯速率10度/s)');
legend('IMM2','IMM3','Kalman');
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -