📄 tdoa_smooth_cv_ct.m
字号:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% 平滑多模型估计在TDOA中的应用，包括两个模型：CV和CT
%%% 参考文献：《Interacting multiple model fixed-lag smoothing algorithm for Markovian switching systems》
%%% Zhaoting Liu 2007,12,21
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
True_trajectory1
All_time=300;
d=3;%delay
posit_radar0=[50 50]*10^3;
posit_radar1=[0 50]*10^3;
posit_radar2=[50 0]*10^3;
posit_radar3=[0 0]*10^3; % position of the 4 sensors
T=1;
F1 =[1 T 0 0
0 1 0 0
0 0 1 T
0 0 0 1 ];
F(:,:,1)=[F1 0*eye(4) 0*eye(4) 0*eye(4)
eye(4) 0*eye(4) 0*eye(4) 0*eye(4)
0*eye(4) eye(4) 0*eye(4) 0*eye(4)
0*eye(4) 0*eye(4) eye(4) 0*eye(4)];
G1=[T^2/2 0
T 0
0 T^2/2
0 T ];
G(:,:,1)=[G1;zeros(4,2);zeros(4,2);zeros(4,2)];
Q(:,:,1)=0.25*eye(2);
angle=pi*2/180;
F2 =[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) ];
F(:,:,2)=[F2 0*eye(4) 0*eye(4) 0*eye(4)
eye(4) 0*eye(4) 0*eye(4) 0*eye(4)
0*eye(4) eye(4) 0*eye(4) 0*eye(4)
0*eye(4) 0*eye(4) eye(4) 0*eye(4)];
G2=[T^2/2 0
T 0
0 T^2/2
0 T ];
G(:,:,2)=[G2;zeros(4,2);zeros(4,2);zeros(4,2)];
Q(:,:,2)=64*eye(2);
angle=-pi*3/180;
F3 =[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) ];
F(:,:,3)=[F3 0*eye(4) 0*eye(4) 0*eye(4)
eye(4) 0*eye(4) 0*eye(4) 0*eye(4)
0*eye(4) eye(4) 0*eye(4) 0*eye(4)
0*eye(4) 0*eye(4) eye(4) 0*eye(4)];
G3=[T^2/2 0
T 0
0 T^2/2
0 T ];
G(:,:,3)=[G3;zeros(4,2);zeros(4,2);zeros(4,2)];
Q(:,:,3)=64*eye(2);
w(:,1)=[0.5 0.5]';
w(:,2)=[8 8]';
w(:,3)=[8 8]';
Updated_covariance_P(:,:,1,1)=[eye(4) zeros(4,4) zeros(4,4) zeros(4,4)
zeros(4,4) zeros(4,4) zeros(4,4) zeros(4,4)
zeros(4,4) zeros(4,4) zeros(4,4) zeros(4,4)
zeros(4,4) zeros(4,4) zeros(4,4) zeros(4,4) ];
Updated_covariance_P(:,:,2,1)=[eye(4) zeros(4,4) zeros(4,4) zeros(4,4)
zeros(4,4) zeros(4,4) zeros(4,4) zeros(4,4)
zeros(4,4) zeros(4,4) zeros(4,4) zeros(4,4)
zeros(4,4) zeros(4,4) zeros(4,4) zeros(4,4) ];
Updated_covariance_P(:,:,3,1)=[eye(4) zeros(4,4) zeros(4,4) zeros(4,4)
zeros(4,4) zeros(4,4) zeros(4,4) zeros(4,4)
zeros(4,4) zeros(4,4) zeros(4,4) zeros(4,4)
zeros(4,4) zeros(4,4) zeros(4,4) zeros(4,4) ];
c=3*10^8; %光速度
deta_T=3*10^(-7); %观测时间的标准方差
deta_r=deta_T*c*2^0.5; %因为 r=c*gap_time 所以时间的方差可以转化成为距离差的方差
R=eye(3)*deta_r^2; % R：观测噪声V的协方差矩阵
%==========================================================================
%开始求非线性状态方程线性化的Jacobian矩阵
syms X VX Y VY
R0=((X-posit_radar0(1))^2+(Y-posit_radar0(2))^2)^(1/2);
R1=((X-posit_radar1(1))^2+(Y-posit_radar1(2))^2)^(1/2);
R2=((X-posit_radar2(1))^2+(Y-posit_radar2(2))^2)^(1/2);
R3=((X-posit_radar3(1))^2+(Y-posit_radar3(2))^2)^(1/2);
R10=R1-R0;
R20=R2-R0;
R30=R3-R0;
Jacobian_H=zeros(3,4);
Jacobian_H=jacobian([R10;R20;R30],[X VX Y VY ]);
%==========================================================================
PI=[0.9 0.05 0.05
0.10 0.80 0.10
0.05 0.05 0.9];
Mode_probability_u(:,1)=[0.8 0.1 0.1];
Model_number =3;
for n=1:100;%为求RMS，经过100 次蒙特卡洛实验
Overall_estimate_X(:,1)=[60.1*10^3 Velocity_x(1)+10*randn(1,1) 40.05*10^3 Velocity_y(1)+10*randn(1,1) zeros(1,4) zeros(1,4) zeros(1,4)]';
Updated_state_X(:,1,1) =[60.1*10^3 Velocity_x(1)+10*randn(1,1) 40.05*10^3 Velocity_y(1)+10*randn(1,1) zeros(1,4) zeros(1,4) zeros(1,4)]';
Updated_state_X(:,2,1) =[60.1*10^3 Velocity_x(1)+10*randn(1,1) 40.05*10^3 Velocity_y(1)+10*randn(1,1) zeros(1,4) zeros(1,4) zeros(1,4)]';
%======================================================================
%距离差测量
r0=((True_x-posit_radar0(1)).^2+(True_y-posit_radar0(2)).^2).^(1/2);
r1=((True_x-posit_radar1(1)).^2+(True_y-posit_radar1(2)).^2).^(1/2);
r2=((True_x-posit_radar2(1)).^2+(True_y-posit_radar2(2)).^2).^(1/2);
r3=((True_x-posit_radar3(1)).^2+(True_y-posit_radar3(2)).^2).^(1/2);
r10=r1-r0;
r20=r2-r0;
r30=r3-r0;
measure_r10=r10+deta_r*randn(1,All_time);
measure_r20=r20+deta_r*randn(1,All_time);
measure_r30=r30+deta_r*randn(1,All_time);
%======================================================================
for k=2:All_time %时间周期循环开始
%%%%%%%% 1. Model-conditioned reinitialization (for i = 1,2,: : : ,Model_number): %%%%%%%
for i=1:Model_number %单独循环
Predicted_mode_probability_u(i,k)= PI(1,i)*Mode_probability_u(1,k-1);
Predicted_mode_probability_u(i,k)=Predicted_mode_probability_u(i,k)+PI(2,i)*Mode_probability_u(2,k-1);
end
for i=1:Model_number %单独二重循环
for j=1:Model_number
Mixing_weight_u(j,i,k-1)=PI(j,i)*Mode_probability_u(j,k-1)/Predicted_mode_probability_u(i,k);
end
end
for i=1:Model_number
Mixing_estimate_X(:,i,k-1)= Updated_state_X(:,1,k-1).*Mixing_weight_u(1,i,k-1);
Mixing_estimate_X(:,i,k-1)=Mixing_estimate_X(:,i,k-1)+Updated_state_X(:,2,k-1).*Mixing_weight_u(2,i,k-1);
Mixing_covariance_P(:,:,i,k-1)= (Updated_covariance_P(:,:,1,k-1)+(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,1,k-1))*(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,1,k-1))')*Mixing_weight_u(1,i,k-1);
Mixing_covariance_P(:,:,i,k-1)=Mixing_covariance_P(:,:,i,k-1)+(Updated_covariance_P(:,:,2,k-1)+(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,2,k-1))*(Mixing_estimate_X(:,i,k-1)-Updated_state_X(:,2,k-1))')*Mixing_weight_u(2,i,k-1);
%%%%%%%% 2. Model-conditioned filtering (for i = 1,2,: : : ,Model_number): %%%%%%%%%%%
Predicted_state_X(:,i,k)=F(:,:,i)*Mixing_estimate_X(:,i,k-1)+G(:,:,i)*w(:,i);
Predicted_covariance_P(:,:,i,k)=F(:,:,i)*Mixing_covariance_P(:,:,i,k-1)*F(:,:,i)'+G(:,:,i)*Q(:,:,i)*G(:,:,i)';
%============================================================
%测量预测
rr0(i,k)=((Predicted_state_X(1,i,k)-posit_radar0(1))^2+(Predicted_state_X(3,i,k)-posit_radar0(2))^2)^(1/2);
rr1(i,k)=((Predicted_state_X(1,i,k)-posit_radar1(1))^2+(Predicted_state_X(3,i,k)-posit_radar1(2))^2)^(1/2);
rr2(i,k)=((Predicted_state_X(1,i,k)-posit_radar2(1))^2+(Predicted_state_X(3,i,k)-posit_radar2(2))^2)^(1/2);
rr3(i,k)=((Predicted_state_X(1,i,k)-posit_radar3(1))^2+(Predicted_state_X(3,i,k)-posit_radar3(2))^2)^(1/2);
rr10(i,k)=rr1(i,k)-rr0(i,k);
rr20(i,k)=rr2(i,k)-rr0(i,k);
rr30(i,k)=rr3(i,k)-rr0(i,k);
zz(:,i,k)=[rr10(i,k) rr20(i,k) rr30(i,k)]';
%============================================================
Measurement_z(:,k)=[measure_r10(k) measure_r20(k) measure_r30(k)]';%k时刻测量值
Measurement_residual_z(:,i,k)=Measurement_z(:,k)-zz(:,i,k);%测量残差
%============================================================
%计算k时刻的Jacobian矩阵
X =Predicted_state_X(1,i,k);
VX=Predicted_state_X(2,i,k);
AX=Predicted_state_X(3,i,k);
Y =Predicted_state_X(4,i,k);
VY=Predicted_state_X(5,i,k);
AX=Predicted_state_X(6,i,k);
Hh=eval(Jacobian_H);
H(:,:,i,k)=[Hh zeros(3,4) zeros(3,4) zeros(3,4)];
%============================================================
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);
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_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);
end %时间周期循环结束
%======================================================================
%平滑估计值的提取
%Position
X0=Overall_estimate_X(4*0+1,:);
Y0=Overall_estimate_X(4*0+3,:);
X1=Overall_estimate_X(4*1+1,:);
Y1=Overall_estimate_X(4*1+3,:);
X1=X1(2:All_time);
Y1=Y1(2:All_time);
X1=[X1 Overall_estimate_X(1,All_time)];
Y1=[Y1 Overall_estimate_X(3,All_time)];
X2=Overall_estimate_X(4*2+1,:);
Y2=Overall_estimate_X(4*2+3,:);
X2=X2(3:All_time);
Y2=Y2(3:All_time);
X2=[X2 Overall_estimate_X(1,All_time-1) Overall_estimate_X(1,All_time)];
Y2=[Y2 Overall_estimate_X(3,All_time-1) Overall_estimate_X(3,All_time)];
X3=Overall_estimate_X(4*3+1,:);
Y3=Overall_estimate_X(4*3+3,:);
X3=X3(4:All_time);
Y3=Y3(4:All_time);
X3=[X3 Overall_estimate_X(1,All_time-2) Overall_estimate_X(1,All_time-1) Overall_estimate_X(1,All_time)];
Y3=[Y3 Overall_estimate_X(3,All_time-2) Overall_estimate_X(3,All_time-1) Overall_estimate_X(3,All_time)];
%Velocity
VX0=Overall_estimate_X(4*0+2,:);
VY0=Overall_estimate_X(4*0+4,:);
VX1=Overall_estimate_X(4*1+2,:);
VY1=Overall_estimate_X(4*1+4,:);
VX1=VX1(2:All_time);
VY1=VY1(2:All_time);
VX1=[VX1 Overall_estimate_X(2,All_time)];
VY1=[VY1 Overall_estimate_X(4,All_time)];
VX2=Overall_estimate_X(4*2+2,:);
VY2=Overall_estimate_X(4*2+4,:);
VX2=VX2(3:All_time);
VY2=VY2(3:All_time);
VX2=[VX2 Overall_estimate_X(2,All_time-1) Overall_estimate_X(2,All_time)];
VY2=[VY2 Overall_estimate_X(4,All_time-1) Overall_estimate_X(4,All_time)];
VX3=Overall_estimate_X(4*3+2,:);
VY3=Overall_estimate_X(4*3+4,:);
VX3=VX3(4:All_time);
VY3=VY3(4:All_time);
VX3=[VX3 Overall_estimate_X(2,All_time-2) Overall_estimate_X(2,All_time-1) Overall_estimate_X(2,All_time)];
VY3=[VY3 Overall_estimate_X(4,All_time-2) Overall_estimate_X(4,All_time-1) Overall_estimate_X(4,All_time)];
%======================================================================
Position0_RMS(n,:) =((True_x-X0).^2+(True_y-Y0).^2);
Position1_RMS(n,:) =((True_x-X1).^2+(True_y-Y1).^2);
Position2_RMS(n,:) =((True_x-X2).^2+(True_y-Y2).^2);
Position3_RMS(n,:) =((True_x-X3).^2+(True_y-Y3).^2);
Velocity0_RMS(n,:) =((Velocity_x-VX0).^2+(Velocity_y-VY0).^2);
Velocity1_RMS(n,:) =((Velocity_x-VX1).^2+(Velocity_y-VY1).^2);
Velocity2_RMS(n,:) =((Velocity_x-VX2).^2+(Velocity_y-VY2).^2);
Velocity3_RMS(n,:) =((Velocity_x-VX3).^2+(Velocity_y-VY3).^2);
CV_Mode_probability_u(n,:)=Mode_probability_u(1,:);
CA_Mode_probability_u(n,:)=Mode_probability_u(2,:);
%======================================================================
end %100次蒙特卡洛实验结束
for k=1:All_time
Position0_Mean_RMS_IMM(k)=mean(Position0_RMS(:,k)).^0.5;
Position1_Mean_RMS_IMM(k)=mean(Position1_RMS(:,k)).^0.5;
Position2_Mean_RMS_IMM(k)=mean(Position2_RMS(:,k)).^0.5;
Position3_Mean_RMS_IMM(k)=mean(Position3_RMS(:,k)).^0.5;
Velocity0_Mean_RMS_IMM(k)=mean(Velocity0_RMS(:,k)).^0.5;
Velocity1_Mean_RMS_IMM(k)=mean(Velocity1_RMS(:,k)).^0.5;
Velocity2_Mean_RMS_IMM(k)=mean(Velocity2_RMS(:,k)).^0.5;
Velocity3_Mean_RMS_IMM(k)=mean(Velocity3_RMS(:,k)).^0.5;
forward_CV_Mode_probability(k)=mean(CV_Mode_probability_u(:,k));
end
figure(2)
k=1:All_time;
plot(k,Position0_Mean_RMS_IMM,k,Position1_Mean_RMS_IMM,k,Position2_Mean_RMS_IMM,'--',k,Position3_Mean_RMS_IMM,'.-')
xlabel('t (s)')
ylabel('Position RMS (m)')
figure(3)
plot(k,Velocity0_Mean_RMS_IMM,k,Velocity1_Mean_RMS_IMM,k,Velocity2_Mean_RMS_IMM,'--',k,Velocity3_Mean_RMS_IMM,'.-')
xlabel('t (s)')
ylabel('Velocity RMS (m)')
figure(5)
plot(k,forward_CV_Mode_probability)
xlabel('t (s)')
ylabel('Mode probability')
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -