TOA定位算法的FPGA实现

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代码如下所示：</