MATLAB仿真TOA定位算法：
function [x1,y1,z1] = func_TOA(BS,time_dur,light_speed);
L = time_dur .* light_speed;
n = length(BS);
for i = 2:n
P(i-1,1) = BS(i,1);
P(i-1,2) = BS(i,2);
P(i-1,3) = BS(i,3);
end
B(1,:) = (BS(2,1)^2 + BS(2,2)^2 + BS(2,3)^2) - L(2)^2 + L(1)^2;
B(2,:) = (BS(3,1)^2 + BS(3,2)^2 + BS(3,3)^2) - L(3)^2 + L(1)^2;
B(3,:) = (BS(4,1)^2 + BS(4,2)^2 + BS(4,3)^2) - L(4)^2 + L(1)^2;
k = inv(P'*P)*P'*B/2;
x1 = k(1);
y1 = k(2);
z1 = k(3);
FPGA的实现verilog代码如下所示：</
