TOA定位算法的FPGA实现

时间：2023/3/7 2:18:19 点击：

核心提示：TOA定位算法的FPGA实现...

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

• 您是如何找到本站的？
• 百度搜索
• 查阅资料过程中
• 论坛发现
• 百度贴吧发现
• 朋友介绍
• 上一篇：基于基因数据的神经网络模式分类研究
• 下一篇：没有了
• 没有相关文章
• 大名：
• 内容：