DOA/TDOA混合定位CRLB

clc; close all; clear all; % 测向站数量 M=6; % 目标数量 N=1; %观测站位置 s1=[1500 -1900 1400].'; s2=[-1600 2000 1700].'; s3=[1800 -1500 -1600].'; s4=[-1200 1400 -2000].'; s5=[1700 1600 2000].'; s6=[-1800 -1400 -1800].'; for i=1:1:M str_si=['s',mat2str(i)]; si_value=eval(str_si); s_true(3*(i-1)+1:3*(i-1)+3,1)=si_value; end % 目标位置 u1=[4000 4000 3000].'; for i=1:1:N str_ui=['u',mat2str(i)]; ui_value=eval(str_ui); u_true(3*(i-1)+1:3*(i-1)+3,1)=ui_value; end %% angle_noise_power_more=[0.1:0.1:1].*pi/180; r_noise_power=5; %% CRLB for i=1:1:M eval(['s',mat2str(i),'(:,1)',' = ','s_true(3*(i-1)+1:3*(i-1)+3,1)',';']); end for i=1:1:N eval(['u',mat2str(i),'(:,1)',' = ','u_true(3*(i-1)+1:3*(i-1)+3,1)',';']); end %% 对目标位置的导数 % 方位角、仰角对目标位置的导数 diff_theta_ui=zeros(M,3,N); diff_beta_ui=zeros(M,3,N); % 距离差对目标位置的导数 diff_r_ui=zeros(M-1,3,N); for j=1:1:N for i=1:1:M str_si=['s',mat2str(i)]; si_value=eval(str_si); str_ui=['u',mat2str(j)]; ui_value=eval(str_ui); ui_si=ui_value-si_value; ai2=sqrt(ui_si(1)^2+ui_si(2)^2); % 方位角对目标位置的导数 diff_theta_ui_1=-ui_si(2)/ai2^2; diff_theta_ui_2=ui_si(1)/ai2^2; diff_theta_ui(i,:,j)=[diff_theta_ui_1,diff_theta_ui_2,0]; % 仰角对目标位置的导数 diff_beta_ui_1=-ui_si(1)*ui_si(3)/ai2/(osjl(ui_value,si_value))^2; diff_beta_ui_2=-ui_si(2)*ui_si(3)/ai2/(osjl(ui_value,si_value))^2; diff_beta_ui_3=ai2/(osjl(ui_value,si_value))^2; diff_beta_ui(i,:,j)=[diff_beta_ui_1,diff_beta_ui_2,diff_beta_ui_3]; end end for j=1:1:N for i=2:1:M str_si=['s',mat2str(i)]; si_value=eval(str_si); str_ui=['u',mat2str(j)]; ui_value=eval(str_ui); ui_si=ui_value-si_value; % 距离差对目标位置的导数 ui_s1=ui_value-s1; ui_si=ui_value-si_value; diff_r_ui(i-1,:,j)=ui_si.'/osjl(ui_value,si_value)-ui_s1.'/osjl(ui_value,s1); end end F_u1=[diff_theta_ui(:,:,1);diff_beta_ui(:,:,1);diff_r_ui(:,:,1);]; F_u=blkdiag(F_u1); for big_big_loop=1:1:length(angle_noise_power_more) %方位角 deta_theta=angle_noise_power_more(big_big_loop); R_theta=deta_theta^2*eye((M),(M)); %仰角 deta_beta=angle_noise_power_more(big_big_loop); R_beta=deta_beta^2*eye((M),(M)); %距离差 deta_r=r_noise_power; R_t=(deta_r)^2*(eye(M-1,M-1)+ones(M-1,M-1))/2; Rn_a1=blkdiag(R_theta,R_beta,R_t); Rn_a=blkdiag(Rn_a1); % 观测站定位的先验均方根误差 prior_cov_s(big_big_loop,1)=0; F1=F_u.'*inv(Rn_a)*F_u; CRLB1=inv([diff_theta_ui(:,:,1);diff_beta_ui(:,:,1)].'*inv(blkdiag(R_theta,R_beta))*[diff_theta_ui(:,:,1);diff_beta_ui(:,:,1)]); CRLB2=inv([diff_r_ui(:,:,1)].'*inv(blkdiag(R_t))*[diff_r_ui(:,:,1)]); CRLB=inv([F1]); crb_source_DOA_TDOA(big_big_loop,1)=sqrt(trace(CRLB(1:3,1:3))); CRLB_DOA(big_big_loop,1)=sqrt(trace(CRLB1(1:3,1:3))); CRLB_TDOA(big_big_loop,1)=sqrt(trace(CRLB2(1:3,1:3))); end CRLB_u=[crb_source_DOA_TDOA]; figure(1) plot(angle_noise_power_more.*180/pi,CRLB_u,'r.-') hold on; plot(angle_noise_power_more.*180/pi,CRLB_DOA,'b^-') hold on; plot(angle_noise_power_more.*180/pi,CRLB_TDOA,'g*-') xlabel('角度测量误差') ylabel('RMSE(m)') legend('CRLB(DOA/TDOA)','CRLB(DOA)','CRLB(TDOA)') %% function [distance]=osjl(object,source) % 输入均为列向量 distance=sqrt(sum((object-source).^2)); end