论文仿真实现（matlab）

clc; close all; clear all; % An Approximately Efficient TDOA Localization Algorithm in Closed-Form for Locating Multiple ... % Disjoint Sources With Erroneous Sensor Positions % 测向站数量 M=6; % 目标数量 N=1; s1=[300 100 150].'; s2=[400 150 100].'; s3=[300 500 200].'; s4=[350 200 100].'; s5=[-100 -100 -100].'; s6=[200 -300 -200].'; % 目标位置 u=[250 -550 -550].'; for i=1:M % 组合字符串，得到接收站的变量名，以便循环 str_si=['s',mat2str(i)]; % 返回变量名对应的值 si_value=eval(str_si); % 观测站真实位置向量 s_true(3*(i-1)+1:3*i,1)=si_value; end for i=2:M % 组合字符串，得到接收站的变量名，以便循环 str_si=['s',mat2str(i)]; % 返回变量名对应的值 si_value=eval(str_si); % 距离差真实值 r_true(i-1,1)=osjl(u,si_value)-osjl(s1,u); end deta_s_more=10.^[-1.5:0.25:1]; big_loop_number=length(deta_s_more); small_loop_number=2000; for big_loop=1:1:big_loop_number for small_loop=1:1:small_loop_number deta_r=sqrt(0.001); Qr=deta_r^2*(ones(M-1,M-1)+eye(M-1,M-1))/2; r=r_true+deta_r*randn(M-1,1); B11=2*diag(r_true); deta_s=deta_s_more(big_loop); Qs=deta_s^2*eye(3*M,3*M); s=s_true+deta_s*randn(3*M,1); for i=2:M G11(i-1,:)=2*[s(3*(i-1)+1:3*i).'-s(1:3).',-r(i-1)]; u_s1=(u-s(1:3))/osjl(u,s(1:3)); u_si=(u-s(3*(i-1)+1:3*i)); D1(i-1,:)=2*[-r(i-1)*u_s1.'-(u-s(1:3)).',zeros(1,3*(i-2)),u_si.',zeros(1,3*(M-i))]; end yita=zeros(M-1,1); s1_false=s(1:3); for i=2:M si=s(3*(i-1)+1:3*i); yita(i-1,1)=r(i-1)^2-si.'*si+s(1:3).'*s(1:3)+2*(si-s(1:3)).'*u; end G1=[G11,D1;zeros(3*M,4),eye(3*M,3*M)]; W1=[inv(B11*Qr*B11.'),zeros(M-1,3*M);zeros(3*M,M-1),inv(Qs)]; h1=[yita.',zeros(1,3*M)].'; fai1=inv(G1.'*W1*G1)*G1.'*W1*h1; H=G11; aaa=inv(B11*Qr*B11.'); R=inv(Qs)+D1.'*aaa*D1; cov_theta1=inv(H.'*aaa*H-H.'*aaa*D1*inv(R)*D1.'*aaa*H); u_estimate1=u-fai1(1:3); u_estimate1_s1=u_estimate1-s(1:3); h2=[u_estimate1_s1.'.*u_estimate1_s1.',(fai1(4))^2].'; P=[eye(3,3),ones(3,1)].'; G2=P; B21=2*diag([-(u-s(1:3)).',osjl(s(1:3),u)]); W2=inv(B21*cov_theta1*B21.'); fai2=inv(G2.'*W2*G2)*G2.'*W2*h2; u_estimate2=-sqrt(abs(fai2))+s(1:3); u_mse(big_loop,small_loop)=(sum((u_estimate2-u).^2)); [big_loop,small_loop] end end RMSE=10*log10(sum(u_mse,2)/small_loop_number); plot(10*log10(deta_s_more.^2./deta_r^2),RMSE,'b*') %% CRLB % 测向站数量 M=6; % 目标数量 N=1; s1=[300 100 150].'; s2=[400 150 100].'; s3=[300 500 200].'; s4=[350 200 100].'; s5=[-100 -100 -100].'; s6=[200 -300 -200].'; u=[250 -550 -550].'; % u=[-200 -250 -250].'; r_diff_u=zeros(M-1,3); r_diff_s=zeros(M-1,3*M); for i=2:M % 组合字符串，得到接收站的变量名，以便循环 str_si=['s',mat2str(i)]; % 返回变量名对应的值 si_value=eval(str_si); pi=(u-si_value)/osjl(u,si_value); p1=(u-s1)/osjl(u,s1); r_diff_u(i-1,:)=pi-p1; r_diff_s(i-1,:)=[p1.',zeros(1,3*(i-2)),-pi.',zeros(1,3*(M-i))]; end deta_s_more=10.^[-1.5:0.25:1]; for loop=1:1:length(deta_s_more) deta_s=deta_s_more(loop); Qs=deta_s^2*eye(3*M,3*M); deta_r=sqrt(0.001); Qr=deta_r^2*(ones(M-1,M-1)+eye(M-1,M-1))/2; X=r_diff_u.'*inv(Qr)*r_diff_u; Y=r_diff_u.'*inv(Qr)*r_diff_s; Z=r_diff_s.'*inv(Qr)*r_diff_s+inv(Qs); CRLB=inv(X-Y*inv(Z)*Y.'); crb_trace(loop)=10*log10(trace(CRLB)); end hold on; plot(10*log10(deta_s_more.^2./deta_r^2),crb_trace,'r.-') xlabel('{\it\sigma}^2_s/{\it\sigma}^2_r(dB)') ylabel('10log(MSE(m^2))') legend('定位精度','CRLB')