您现在的位置:首页 >> 技术文章 >> MATLAB技术 >> 内容

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

 

 

作者:TOA定位算法的FPGA实现 来源:TOA定位算法的FPGA实现
  • 您是如何找到本站的?
  • 百度搜索
  • Google搜索
  • 查阅资料过程中
  • 论坛发现
  • 百度贴吧发现
  • 朋友介绍
本站最新成功开发工程项目案例
  • 上一篇:基于基因数据的神经网络模式分类研究
  • 下一篇:没有了
  • 相关文章
    • 没有相关文章
    相关评论
    发表我的评论
    • 大名:
    • 内容:
  • matlab代做|matlab专业代做|matlab淘宝代做|FPGA项目合作(www.hslogic.com) © 2023 版权所有 All Rights Reserved.
  • Email:highspeed_logic@163.com 站长QQ: 1224848052

    专业代做/代写/承接、MATLAB、SIMULINK、FPGA项目、博士/硕士/本科课题、Coursework、Eassy、Assignment