📄 agimm.m
字号:
Measurement_z(:,k)=[Measurement_r(k) Measurement_rv(k) Measurement_beta(k)]';%k时刻测量值
Rr=(Predicted_state_X(1,i,k)^2+Predicted_state_X(3,i,k)^2).^0.5;
Rrv=(Predicted_state_X(1,i,k)*Predicted_state_X(2,i,k)+Predicted_state_X(3,i,k)*Predicted_state_X(4,i,k))/Rr;
Bbeta=atan(Predicted_state_X(1,i,k)/Predicted_state_X(3,i,k));
zz(:,i,k)=[Rr Rrv Bbeta]';
Measurement_residual_z(:,i,k)=Measurement_z(:,k)-zz(:,i,k);%测量残差
X=Predicted_state_X(1,i,k);
Y=Predicted_state_X(3,i,k);
VX=Predicted_state_X(2,i,k);
VY=Predicted_state_X(4,i,k);
H(:,:,i,k)=eval(Jacobian_H);
Residual_covariance_S(:,:,i,k)=H(:,:,i,k)*Predicted_covariance_P(:,:,i,k)*H(:,:,i,k)'+R;
Filter_gain_K(:,:,i,k)=Predicted_covariance_P(:,:,i,k)*H(:,:,i,k)'*Residual_covariance_S(:,:,i,k)^(-1);
Updated_state_X(:,i,k)=Predicted_state_X(:,i,k)+Filter_gain_K(:,:,i,k)*Measurement_residual_z(:,i,k);
Updated_covariance_P(:,:,i,k)=Predicted_covariance_P(:,:,i,k)-Filter_gain_K(:,:,i,k)*Residual_covariance_S(:,:,i,k)*Filter_gain_K(:,:,i,k)';
%%%%%%%% 3. Mode probability update (for i = 1,2,: : : ,Model_number): %%%%%%%%%%%
Model_likelihood_L(i,k)=exp(-Measurement_residual_z(:,i,k)'*Residual_covariance_S(:,:,i,k)^(-1)*Measurement_residual_z(:,i,k)/2)/(det(2*pi*Residual_covariance_S(:,:,i,k)))^0.5 ;
end
for i=1:Model_number %单独循环，因为要用到全部的 Model_likelihood_L(i,k)
mu= Predicted_mode_probability_u(1,k)*Model_likelihood_L(1,k);
mu=mu+Predicted_mode_probability_u(2,k)*Model_likelihood_L(2,k);
mu=mu+Predicted_mode_probability_u(3,k)*Model_likelihood_L(3,k);
Mode_probability_u(i,k)=Predicted_mode_probability_u(i,k)*Model_likelihood_L(i,k)/mu;
end
%%%%%%%% 4. Estimate fusion: %%%%%%%%%%%
Overall_estimate_X(:,k)= Updated_state_X(:,1,k)*Mode_probability_u(1,k);
Overall_estimate_X(:,k)=Overall_estimate_X(:,k)+Updated_state_X(:,2,k)*Mode_probability_u(2,k);
Overall_estimate_X(:,k)=Overall_estimate_X(:,k)+Updated_state_X(:,3,k)*Mode_probability_u(3,k);
Overall_covariance_P(:,:,k)= ( Updated_covariance_P(:,:,1,k)+(Overall_estimate_X(:,k)-Updated_state_X(:,1,k))*(Overall_estimate_X(:,k)-Updated_state_X(:,1,k))' )*Mode_probability_u(1,k);
Overall_covariance_P(:,:,k)=Overall_covariance_P(:,:,k)+( Updated_covariance_P(:,:,2,k)+(Overall_estimate_X(:,k)-Updated_state_X(:,2,k))*(Overall_estimate_X(:,k)-Updated_state_X(:,2,k))' )*Mode_probability_u(2,k);
Overall_covariance_P(:,:,k)=Overall_covariance_P(:,:,k)+( Updated_covariance_P(:,:,3,k)+(Overall_estimate_X(:,k)-Updated_state_X(:,3,k))*(Overall_estimate_X(:,k)-Updated_state_X(:,3,k))' )*Mode_probability_u(3,k);
%=====================================================================================================================
%模型集转移
angle_center=Mode_probability_u(1,k)*angle_left+Mode_probability_u(2,k)*angle_center+Mode_probability_u(3,k)*angle_right;
if angle_center==max([angle_left,angle_center,angle_right])
angle_left=(angle_center-distance_L/2)*(Mode_probability_u(1,k)<t1)+(angle_center-distance_L)*(Mode_probability_u(1,k)>=t1);
angle_right=(angle_center+distance_R/2)*(Mode_probability_u(3,k)<t1)+(angle_center+distance_R)*(Mode_probability_u(3,k)>=t1);
elseif angle_left==max([angle_left,angle_center,angle_right])
angle_left=(angle_center-2*distance_L)*(Mode_probability_u(1,k)>t2)+(angle_center-distance_L)*(Mode_probability_u(1,k)<=t2);
angle_right=(angle_center+distance_R);
else
angle_left=(angle_center-distance_L);
angle_right=(angle_center+distance_R*2)*(Mode_probability_u(3,k)>t2)+(angle_center+distance_R)*(Mode_probability_u(3,k)<=t2);
end
%====================================================================================================================================================
%===================================
%记录CV概率的变化情况
if angle_center==0
CV_Mode_probability(k) =Mode_probability_u(2,k);
elseif angle_left==0
CV_Mode_probability(k) =Mode_probability_u(1,k);
elseif angle_right==0
CV_Mode_probability(k) =Mode_probability_u(3,k);
else
CV_Mode_probability(k)=0;
end
%===================================
angle=angle_left;
if angle~=0
F(:,:,1)=[1 sin(angle*T)/angle 0 -(1-cos(angle*T))/angle
0 cos(angle*T) 0 -sin(angle*T)
0 (1-cos(angle*T))/angle 1 sin(angle*T)/angle
0 sin(angle*T) 0 cos(angle*T) ];
else
F(:,:,1)=[1 T 0 0
0 1 0 0
0 0 1 T
0 0 0 1 ];
end
angle=angle_center;
if angle~=0
F(:,:,2)=[1 sin(angle*T)/angle 0 -(1-cos(angle*T))/angle
0 cos(angle*T) 0 -sin(angle*T)
0 (1-cos(angle*T))/angle 1 sin(angle*T)/angle
0 sin(angle*T) 0 cos(angle*T) ];
else
F(:,:,2)=[1 T 0 0
0 1 0 0
0 0 1 T
0 0 0 1 ];
end
angle=angle_right;
if angle~=0
F(:,:,3)=[1 sin(angle*T)/angle 0 -(1-cos(angle*T))/angle
0 cos(angle*T) 0 -sin(angle*T)
0 (1-cos(angle*T))/angle 1 sin(angle*T)/angle
0 sin(angle*T) 0 cos(angle*T) ];
else
F(:,:,3)=[1 T 0 0
0 1 0 0
0 0 1 T
0 0 0 1 ];
end
%=====================================================================================================================================================
end %时间周期循环结束
Posit_RMS(n,:)=(True_x-Overall_estimate_X(1,:)).^2+(True_y-Overall_estimate_X(3,:)).^2;
Veloc_RMS(n,:)=(Velocity_x-Overall_estimate_X(2,:)).^2+(Velocity_y-Overall_estimate_X(4,:)).^2;
CV_Mode_probability_u(n,:)=CV_Mode_probability;
%为了计算两种模型的使用概率
end %100次蒙特卡洛实验结束
for k=1:All_time
Posit_Mean_RMS_AGIMM(k)=mean(Posit_RMS(:,k)).^(1/2);
Veloc_Mean_RMS_AGIMM(k)=mean(Veloc_RMS(:,k)).^(1/2);
CV_AGIMM_Mode_probability(k)=mean(CV_Mode_probability_u(:,k));
end
figure(3)
plot(Posit_Mean_RMS_AGIMM)
xlabel('采样时间 /s')
ylabel('RMS(位置均方根误差)')
figure(4)
plot(Veloc_Mean_RMS_AGIMM)
xlabel('采样时间 /s')
ylabel('RMS(速度均方根误差)')
figure(5)
plot(CV_AGIMM_Mode_probability)
xlabel('采样时间 /s')
ylabel('CV模型概率')
figure(6)
plot(Overall_estimate_X(1,:),Overall_estimate_X(3,:))⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -