【MATLAB代码，带TDOA数据导入】TDOA求三维下的位置(1主锚点、3副锚点)，多个时间点、输出位置图像

% TDOA求三维下的位置(1主锚点、3副锚点) % 多个时间点、输出位置图像 clear;clc;close all; rng(0); data_1 = readmatrix('data.xlsx'); %锚点位置 data_2 = readmatrix('data.xlsx','Sheet',2); %天基无源定位时差 %% 主程序 % range_err = 1e-2; %时钟与时间计算误差 % point1 = [3,1,1]; %待定位点的坐标设置 for i1 = 1:100 baseP = [ data_1(i1,1:3); data_1(i1,4:6); data_1(i1,7:9); data_1(i1,10:12);]; %设置基站的坐标 % r_err = 0.2; %设置测距误差（标准差） % R_real = sqrt(diag((point1-baseP)*(point1'-baseP'))); % T = R_real/c; %含噪声的传播时间 % 计算距离差值 d12 = data_2(i1,1)*3e8; d13 = data_2(i1,2)*3e8; d14 = data_2(i1,3)*3e8; % 设置方程 syms x y z eq1 = ((x - baseP(1,1))^2 + (y - baseP(1,2))^2 + (z - baseP(1,3))^2)^0.5 - ... ((x - baseP(2,1))^2 + (y - baseP(2,2))^2 + (z - baseP(2,3))^2)^0.5 == d12; eq2 = ((x - baseP(1,1))^2 + (y - baseP(1,2))^2 + (z - baseP(1,3))^2)^0.5 - ... ((x - baseP(3,1))^2 + (y - baseP(3,2))^2 + (z - baseP(3,3))^2)^0.5 == d13; eq3 = ((x - baseP(1,1))^2 + (y - baseP(1,2))^2 + (z - baseP(1,3))^2)^0.5 - ... ((x - baseP(4,1))^2 + (y - baseP(4,2))^2 + (z - baseP(4,3))^2)^0.5 == d14; % 求解方程 solution = solve([eq1, eq2, eq3], [x, y, z]); p_out = [double(solution.x),double(solution.y),double(solution.z)]; p_out_(i1,:) = p_out; end %% 绘图 figure; plot3(baseP(:,1),baseP(:,2),baseP(:,3),'*r'); xlabel('x/m');ylabel('y/m');zlabel('z/m'); title('锚点初始位置'); figure; plot3(p_out_(:,1),p_out_(:,2),p_out_(:,3),'.-'); xlabel('x/m');ylabel('y/m');zlabel('z/m'); title('待定位点位置解算'); %% 结果输出 % fprintf('输入的坐标为(%f,%f,%f)\n',point1(1),point1(2),point1(3)); fprintf('输出的x轴坐标为：'); disp(p_out_(:,1)'); fprintf('输出的y轴坐标为：'); disp(p_out_(:,2)'); fprintf('输出的z轴坐标为：'); disp(p_out_(:,3)');