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代码如下所示：
module position_function(
i_clk,
i_rst,
time_dur1,
time_dur2,
time_dur3,
time_dur4,
px,
py,
pz,
//test
B1,
B2,
B3,
X1,
X2,
X3,
Y1,
Y2,
Y3,
Z1,
Z2,
Z3
);
//根据实际BS的位置需要调整这几个参数
parameter BS0x = 32'd0;
parameter BS0y = 32'd0;
parameter BS0z = 32'd0;
parameter BS1x = 32'd500;
parameter BS1y = 32'd500;
parameter BS1z = 32'd0;
parameter BS2x = 32'd0;
parameter BS2y = 32'd500;
parameter BS2z = 32'd300;
parameter BS3x = 32'd500;
parameter BS3y = 32'd0;
parameter BS3z = 32'd300;
input i_clk;
input i_rst;
input signed[31:0]time_dur1;
input signed[31:0]time_dur2;
input signed[31:0]time_dur3;
input signed[31:0]time_dur4;
output signed[31:0]px;
output signed[31:0]py;
output signed[31:0]pz;
//test
output signed[31:0]B1;
output signed[31:0]B2;
output signed[31:0]B3;
output signed[63:0]X1;
output signed[63:0]X2;
output signed[63:0]X3;
output signed[63:0]Y1;
output signed[63:0]Y2;
output signed[63:0]Y3;
output signed[63:0]Z1;
output signed[63:0]Z2;
output signed[63:0]Z3;
//L = time_dur .* light_speed;
wire signed[63:0]L1tmp;
wire signed[63:0]L2tmp;
wire signed[63:0]L3tmp;
wire signed[63:0]L4tmp;
wire signed[31:0]L1;
wire signed[31:0]L2;
wire signed[31:0]L3;
wire signed[31:0]L4;
multi_tc multi_tc_u1(
.clk (i_clk),
.a (time_dur1),
.b (32'd300000000), //light speed
.sclr(i_rst),
.p (L1tmp)
);
multi_tc multi_tc_u2(
.clk (i_clk),
.a (time_dur2),
.b (32'd300000000), //light speed
.sclr(i_rst),
.p (L2tmp)
);
multi_tc multi_tc_u3(
.clk (i_clk),
.a (time_dur3),
.b (32'd300000000), //light speed
.sclr(i_rst),
.p (L3tmp)
);
multi_tc multi_tc_u4(
.clk (i_clk),
.a (time_dur4),
.b (32'd300000000), //light speed
.sclr(i_rst),
.p (L4tmp)
);
//L=[L1,L2,L3,L4]
assign L1 = L1tmp[63:32];
assign L2 = L2tmp[63:32];
assign L3 = L3tmp[63:32];
assign L4 = L4tmp[63:32];
reg signed[31:0]P1x_d1;
reg signed[31:0]P1y_d1;
reg signed[31:0]P1z_d1;
reg signed[31:0]P2x_d1;
reg signed[31:0]P2y_d1;
reg signed[31:0]P2z_d1;
reg signed[31:0]P3x_d1;
reg signed[31:0]P3y_d1;
reg signed[31:0]P3z_d1;
reg signed[31:0]P1x_d2;
reg signed[31:0]P1y_d2;
reg signed[31:0]P1z_d2;
reg signed[31:0]P2x_d2;
reg signed[31:0]P2y_d2;
reg signed[31:0]P2z_d2;
reg signed[31:0]P3x_d2;
reg signed[31:0]P3y_d2;
reg signed[31:0]P3z_d2;
reg signed[31:0]P1x_d3;
reg signed[31:0]P1y_d3;
reg signed[31:0]P1z_d3;
reg signed[31:0]P2x_d3;
reg signed[31:0]P2y_d3;
reg signed[31:0]P2z_d3;
reg signed[31:0]P3x_d3;
reg signed[31:0]P3y_d3;
reg signed[31:0]P3z_d3;
reg signed[31:0]P1x;
reg signed[31:0]P1y;
reg signed[31:0]P1z;
reg signed[31:0]P2x;
reg signed[31:0]P2y;
reg signed[31:0]P2z;
reg signed[31:0]P3x;
reg signed[31:0]P3y;
reg signed[31:0]P3z;
//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
//在matlab中计算完了直接用到FPGA中
//inv(P'*P)*P'//在matlab中计算完了直接用到FPGA中
// 1.0e+06 *
// 4.2950 -4.2950 4.2950
// 4.2950 4.2950 -4.2950
// -7.1583 7.1583 7.1583
always @(posedge i_clk or posedge i_rst)
begin
if(i_rst)
begin
P1x_d1 <= 32'd0;
P1y_d1 <= 32'd0;
P1z_d1 <= 32'd0;
P2x_d1 <= 32'd0;
P2y_d1 <= 32'd0;
P2z_d1 <= 32'd0;
P3x_d1 <= 32'd0;
P3y_d1 <= 32'd0;
P3z_d1 <= 32'd0;
P1x_d2 <= 32'd0;
P1y_d2 <= 32'd0;
P1z_d2 <= 32'd0;
P2x_d2 <= 32'd0;
P2y_d2 <= 32'd0;
P2z_d2 <= 32'd0;
P3x_d2 <= 32'd0;
P3y_d2 <= 32'd0;
P3z_d2 <= 32'd0;
P1x_d3 <= 32'd0;
P1y_d3 <= 32'd0;
P1z_d3 <= 32'd0;
P2x_d3 <= 32'd0;
P2y_d3 <= 32'd0;
P2z_d3 <= 32'd0;
P3x_d3 <= 32'd0;
P3y_d3 <= 32'd0;
P3z_d3 <= 32'd0;
P1x <= 32'd0;
P1y <= 32'd0;
P1z <= 32'd0;
P2x <= 32'd0;
P2y <= 32'd0;
P2z <= 32'd0;
P3x <= 32'd0;
P3y <= 32'd0;
P3z <= 32'd0;
end
else begin
P1x_d3 <= 32'd4295000;
P1y_d3 <= 32'd4295000;
P1z_d3 <= 32'd4295000;
P2x_d3 <= 32'd4295000;
P2y_d3 <= 32'd4295000;
P2z_d3 <= 32'd4295000;
P3x_d3 <= 32'd7158300;
P3y_d3 <= 32'd7158300;
P3z_d3 <= 32'd7158300;
P1x_d2 <= P1x_d3;
P1y_d2 <= P1y_d3;
P1z_d2 <= P1z_d3;
P2x_d2 <= P2x_d3;
P2y_d2 <= P2y_d3;
P2z_d2 <= P2z_d3;
P3x_d2 <= P3x_d3;
P3y_d2 <= P3y_d3;
P3z_d2 <= P3z_d3;
P1x_d1 <= P1x_d2;
P1y_d1 <= P1y_d2;
P1z_d1 <= P1z_d2;
P2x_d1 <= P2x_d2;
P2y_d1 <= P2y_d2;
P2z_d1 <= P2z_d2;
P3x_d1 <= P3x_d2;
P3y_d1 <= P3y_d2;
P3z_d1 <= P3z_d2;
P1x <= P1x_d1;
P1y <= P1y_d1;
P1z <= P1z_d1;
P2x <= P2x_d1;
P2y <= P2y_d1;
P2z <= P2z_d1;
P3x <= P3x_d1;
P3y <= P3y_d1;
P3z <= P3z_d1;
end
end
//parameter BS1x = 32'd500;
//parameter BS1y = 32'd500;
//parameter BS1z = 32'd0;
//
//parameter BS2x = 32'd0;
//parameter BS2y = 32'd500;
//parameter BS2z = 32'd300;
//
//parameter BS3x = 32'd500;
//parameter BS3y = 32'd0;
//parameter BS3z = 32'd300;
//B(1,:) = (BS(2,1)^2 + BS(2,2)^2 + BS(2,3)^2) - L(2)^2 + L(1)^2;
wire signed[63:0]BS1x2;
wire signed[63:0]BS1y2;
wire signed[63:0]BS1z2;
wire signed[63:0]L22;
wire signed[63:0]L12;
multi_tc multi_tc_u11(
.clk (i_clk),
.a (BS1x),
.b (BS1x), //light speed
.sclr(i_rst),
.p (BS1x2)
);
multi_tc multi_tc_u12(
.clk (i_clk),
.a (BS1y),
.b (BS1y), //light speed
.sclr(i_rst),
.p (BS1y2)
);
multi_tc multi_tc_u13(
.clk (i_clk),
.a (BS1z),
.b (BS1z), //light speed
.sclr(i_rst),
.p (BS1z2)
);
multi_tc multi_tc_u14(
.clk (i_clk),
.a (L2),
.b (L2), //light speed
.sclr(i_rst),
.p (L22)
);
multi_tc multi_tc_u15(
.clk (i_clk),
.a (L1),
.b (L1), //light speed
.sclr(i_rst),
.p (L12)
);
reg signed[31:0]r11;
reg signed[31:0]r12;
reg signed[31:0]B1;
always @(posedge i_clk or posedge i_rst)
begin
if(i_rst)
begin
r11 <= 32'd0;
r12 <= 32'd0;
B1 <= 32'd0;
end
else begin
r11 <= BS1x2[31:0] + BS1y2[31:0] + BS1z2[31:0];
r12 <= L12[31:0] - L22[31:0];
B1 <= r11 + r12;
end
end
//B(2,:) = (BS(3,1)^2 + BS(3,2)^2 + BS(3,3)^2) - L(3)^2 + L(1)^2;
wire signed[63:0]BS2x2;
wire signed[63:0]BS2y2;
wire signed[63:0]BS2z2;
wire signed[63:0]L32;
multi_tc multi_tc_u21(
.clk (i_clk),
.a (BS2x),
.b (BS2x), //light speed
.sclr(i_rst),
.p (BS2x2)
);
multi_tc multi_tc_u22(
.clk (i_clk),
.a (BS2y),
.b (BS2y), //light speed
.sclr(i_rst),
.p (BS2y2)
);
multi_tc multi_tc_u23(
.clk (i_clk),
.a (BS2z),
.b (BS2z), //light speed
.sclr(i_rst),
.p (BS2z2)
);
multi_tc multi_tc_u24(
.clk (i_clk),
.a (L3),
.b (L3), //light speed
.sclr(i_rst),
.p (L32)
);
reg signed[31:0]r21;
reg signed[31:0]r22;
reg signed[31:0]B2;
always @(posedge i_clk or posedge i_rst)
begin
if(i_rst)
begin
r21 <= 32'd0;
r22 <= 32'd0;
B2 <= 32'd0;
end
else begin
r21 <= BS2x2[31:0] + BS2y2[31:0] + BS2z2[31:0];
r22 <= L12[31:0] - L32[31:0];
B2 <= r21 + r22;
end
end
//B(3,:) = (BS(4,1)^2 + BS(4,2)^2 + BS(4,3)^2) - L(4)^2 + L(1)^2;
wire signed[63:0]BS3x2;
wire signed[63:0]BS3y2;
wire signed[63:0]BS3z2;
wire signed[63:0]L42;
multi_tc multi_tc_u31(
.clk (i_clk),
.a (BS3x),
.b (BS3x), //light speed
.sclr(i_rst),
.p (BS3x2)
);
multi_tc multi_tc_u32(
.clk (i_clk),
.a (BS3y),
.b (BS3y), //light speed
.sclr(i_rst),
.p (BS3y2)
);
multi_tc multi_tc_u33(
.clk (i_clk),
.a (BS3z),
.b (BS3z), //light speed
.sclr(i_rst),
.p (BS3z2)
);
multi_tc multi_tc_u34(
.clk (i_clk),
.a (L4),
.b (L4), //light speed
.sclr(i_rst),
.p (L42)
);
reg signed[31:0]r31;
reg signed[31:0]r32;
reg signed[31:0]B3;
always @(posedge i_clk or posedge i_rst)
begin
if(i_rst)
begin
r31 <= 32'd0;
r32 <= 32'd0;
B3 <= 32'd0;
end
else begin
r31 <= BS3x2[31:0] + BS3y2[31:0] + BS3z2[31:0];
r32 <= L12[31:0] - L42[31:0];
B3 <= r31 + r32;
end
end
//k = inv(P'*P)*P'*B/2;
//X = P1x*B1/2 + P1y*B2/2 + P1z*B3/2;
wire signed[63:0]X1;
wire signed[63:0]X2;
wire signed[63:0]X3;
multi_tc multi_tc_uX1(
.clk (i_clk),
.a (P1x),
.b ({B1[31],B1[31:1]}), //light speed
.sclr(i_rst),
.p (X1)
);
multi_tc multi_tc_uX2(
.clk (i_clk),
.a (P1y),
.b ({B2[31],B2[31:1]}), //light speed
.sclr(i_rst),
.p (X2)
);
multi_tc multi_tc_uX3(
.clk (i_clk),
.a (P1z),
.b ({B3[31],B3[31:1]}), //light speed
.sclr(i_rst),
.p (X3)
);
//Y = P2x*B1/2 + P2y*B2/2 + P2z*B3/2;
wire signed[63:0]Y1;
wire signed[63:0]Y2;
wire signed[63:0]Y3;
multi_tc multi_tc_uY1(
.clk (i_clk),
.a (P2x),
.b ({B1[31],B1[31:1]}), //light speed
.sclr(i_rst),
.p (Y1)
);
multi_tc multi_tc_uY2(
.clk (i_clk),
.a (P2y),
.b ({B2[31],B2[31:1]}), //light speed
.sclr(i_rst),
.p (Y2)
);
multi_tc multi_tc_uY3(
.clk (i_clk),
.a (P2z),
.b ({B3[31],B3[31:1]}), //light speed
.sclr(i_rst),
.p (Y3)
);
//Z = P3x*B1/2 + P3y*B2/2 + P3z*B3/2;
wire signed[63:0]Z1;
wire signed[63:0]Z2;
wire signed[63:0]Z3;
multi_tc multi_tc_uZ1(
.clk (i_clk),
.a (P3x),
.b ({B1[31],B1[31:1]}), //light speed
.sclr(i_rst),
.p (Z1)
);
multi_tc multi_tc_uZ2(
.clk (i_clk),
.a (P3y),
.b ({B2[31],B2[31:1]}), //light speed
.sclr(i_rst),
.p (Z2)
);
multi_tc multi_tc_uZ3(
.clk (i_clk),
.a (P3z),
.b ({B3[31],B3[31:1]}), //light speed
.sclr(i_rst),
.p (Z3)
);
reg signed[31:0]px;
reg signed[31:0]py;
reg signed[31:0]pz;
always @(posedge i_clk or posedge i_rst)
begin
if(i_rst)
begin
px <= 32'd0;
py <= 32'd0;
pz <= 32'd0;
end
else begin
px <= X1[63:32] - X2[63:32] + X3[63:32];
py <= Y1[63:32] + Y2[63:32] - Y3[63:32];
pz <= Z2[63:32] + Z3[63:32] - Z1[63:32];
end
end
endmodule
