最小二乘法和加权最小二乘法的非视距传播下的3个基站的TDOA定位性能对比，并给出完整matlab代码

最小二乘法和加权最小二乘法的非视距传播下的3个基站的TDOA定位性能对比：

在非视距条件下，定位误差会变大，因此需要采用加权最小二乘法来提高定位精度。加权最小二乘法是在最小二乘法的基础上，对每个测量值进行加权，使得测量误差小的值拥有更大的权重。

下面是完整的MATLAB代码：

clc
clear all

%% 三个基站的坐标（单位：米）
BS1 = [0,0,0];
BS2 = [25,0,0];
BS3 = [0,50,0];

%% 目标点的真实坐标（单位：米）
target = [15, 20, 5];

%% 生成TDOA测量值
TDOA1 = norm(target-BS1) - norm(target-BS2);
TDOA2 = norm(target-BS1) - norm(target-BS3);
TDOA3 = norm(target-BS2) - norm(target-BS3);

%% 添加噪声
TDOA1 = TDOA1 + normrnd(0,1);
TDOA2 = TDOA2 + normrnd(0,1);
TDOA3 = TDOA3 + normrnd(0,1);

%% 非视距TDOA定位
syms x y z;
f1 = sqrt((x-BS1(1))^2 + (y-BS1(2))^2 + (z-BS1(3))^2) - sqrt((x-BS2(1))^2 + (y-BS2(2))^2 + (z-BS2(3))^2) - TDOA1;
f2 = sqrt((x-BS1(1))^2 + (y-BS1(2))^2 + (z-BS1(3))^2) - sqrt((x-BS3(1))^2 + (y-BS3(2))^2 + (z-BS3(3))^2) - TDOA2;
f3 = sqrt((x-BS2(1))^2 + (y-BS2(2))^2 + (z-BS2(3))^2) - sqrt((x-BS3(1))^2 + (y-BS3(2))^2 + (z-BS3(3))^2) - TDOA3;

F = [f1; f2; f3];

X = [x; y; z];

J = jacobian(F,X);

f = matlabFunction(F,'Vars',{X});
j = matlabFunction(J,'Vars',{X});

x0 = [0;0;0];
[x,~,~,~,~] = lsqnonlin(f,x0,[],[],optimset('Display','off'));

%% 最小二乘法TDOA定位
syms x y z;
f1 = (sqrt((x-BS1(1))^2 + (y-BS1(2))^2 + (z-BS1(3))^2) - sqrt((x-BS2(1))^2 + (y-BS2(2))^2 + (z-BS2(3))^2))^2 - TDOA1^2;
f2 = (sqrt((x-BS1(1))^2 + (y-BS1(2))^2 + (z-BS1(3))^2) - sqrt((x-BS3(1))^2 + (y-BS3(2))^2 + (z-BS3(3))^2))^2 - TDOA2^2;
f3 = (sqrt((x-BS2(1))^2 + (y-BS2(2))^2 + (z-BS2(3))^2) - sqrt((x-BS3(1))^2 + (y-BS3(2))^2 + (z-BS3(3))^2))^2 - TDOA3^2;

F = [f1; f2; f3];

X = [x; y; z];

J = jacobian(F,X);

f = matlabFunction(F,'Vars',{X});
j = matlabFunction(J,'Vars',{X});

x0 = [0;0;0];
[x1,~,~,~,~] = lsqnonlin(f,x0,[],[],optimset('Display','off'));

%% 加权最小二乘法TDOA定位
syms x y z;
f1 = (sqrt((x-BS1(1))^2 + (y-BS1(2))^2 + (z-BS1(3))^2) - sqrt((x-BS2(1))^2 + (y-BS2(2))^2 + (z-BS2(3))^2))^2 - TDOA1^2;
f2 = (sqrt((x-BS1(1))^2 + (y-BS1(2))^2 + (z-BS1(3))^2) - sqrt((x-BS3(1))^2 + (y-BS3(2))^2 + (z-BS3(3))^2))^2 - TDOA2^2;
f3 = (sqrt((x-BS2(1))^2 + (y-BS2(2))^2 + (z-BS2(3))^2) - sqrt((x-BS3(1))^2 + (y-BS3(2))^2 + (z-BS3(3))^2))^2 - TDOA3^2;

F = [f1; f2; f3];

X = [x; y; z];

J = jacobian(F,X);

f = matlabFunction(F,'Vars',{X});
j = matlabFunction(J,'Vars',{X});

x0 = [0;0;0];
W = diag([1/1^2, 1/1^2, 1/1^2]);
[x2,~,~,~,~] = lsqnonlin(@(x) W*f(x),x0,[],[],optimset('Display','off'));

%% 输出结果
disp("非视距TDOA定位结果：");
disp("x="+num2str(x(1))+", y="+num2str(x(2))+", z="+num2str(x(3)));
disp("最小二乘法TDOA定位结果：");
disp("x="+num2str(x1(1))+", y="+num2str(x1(2))+", z="+num2str(x1(3)));
disp("加权最小二乘法TDOA定位结果：");
disp("x="+num2str(x2(1))+", y="+num2str(x2(2))+", z="+num2str(x2(3)));
disp("真实坐标：");
disp("x="+num2str(target(1))+", y="+num2str(target(2))+", z="+num2str(target(3)));

代码中使用了MATLAB自带的lsqnonlin函数进行最小二乘法求解，其中W是加权矩阵，可以根据实际情况进行调整。