mock-gr/algorithms/fun9_tdoa1.cpp

//
// File: tdoa1.cpp
//
// MATLAB Coder version            : 5.2
// C/C++ source code generated on  : 24-Mar-2023 11:40:08
//

// Include Files
#include "fun9_tdoa1.h"
#include "fun9_pinv.h"
#include "rt_nonfinite.h"
#include "coder_array.h"
#include <cmath>
#include <iostream>

// Function Definitions
//
// TDOA Implementation
// 输入：Pos ：站的坐标
//      T ：站测得的到达时间
//      M ：总站数
// 输出：targetPos ：定位目标坐标
// parameters
//
// Arguments    : const coder::array<double, 2U> &Pos
//                const coder::array<double, 2U> &T
//                double M
//                double targetPos[3]
// Return Type  : void
//


static coder::array<double, 2U> argInit_Pos()
{
    coder::array<double, 2U> result;
    // Set the size of the array.
    // Change this size to the value that the application requires.
    result.set_size(3, 2);
    // Loop over the array to initialize each element.
    for (int idx0{0}; idx0 < result.size(0); idx0++) {
        for (int idx1{0}; idx1 < result.size(1); idx1++) {
            // Set the value of the array element.
            // Change this value to the value that the application requires.
            if (idx0 == 0 && idx1 == 0) {
                result[idx0 + result.size(0) * idx1] = (1.0e+06)*3.8304;
            } else if (idx0 == 0 && idx1 == 1) {
                result[idx0 + result.size(0) * idx1] = (1.0e+06)*3.7824;
            } else if (idx0 == 1 && idx1 == 0) {
                result[idx0 + result.size(0) * idx1] =  (1.0e+06)*5.0721;
            } else if (idx0 == 1 && idx1 == 1) {
                result[idx0 + result.size(0) * idx1] = (1.0e+06)*5.0957;
            } else if (idx0 == 2 && idx1 == 0) {
                result[idx0 + result.size(0) * idx1] = (1.0e+06)*2.0493;
            } else if (idx0 == 2 && idx1 == 1) {
                result[idx0 + result.size(0) * idx1] = (1.0e+06)*2.0795;
            }
        }
    }
    return result;
}

static coder::array<double, 2U> argInit_T()
{
    coder::array<double, 2U> result;
    // Set the size of the array.
    // Change this size to the value that the application requires.
    result.set_size(1, 2);
    // Loop over the array to initialize each element.
    for (int idx0{0}; idx0 < result.size(0); idx0++) {
        for (int idx1{0}; idx1 < result.size(1); idx1++) {
            // Set the value of the array element.
            // Change this value to the value that the application requires.
            if (idx0 == 0 && idx1 == 0) {
                result[idx0 + result.size(0) * idx1] = (1.0e-03)*0;
            } else if (idx0 == 0 && idx1 == 1) {
                result[idx0 + result.size(0) * idx1] = (1.0e-03)*-0.1496;
            }
        }
    }
    return result;
}


void tdoa1(const coder::array<double, 2U> &Pos,
           const coder::array<double, 2U> &T, double M, double targetPos[3])
{
  coder::array<double, 2U> A;
  coder::array<double, 2U> dummy;
  coder::array<double, 2U> r;
  coder::array<double, 2U> tdoa;
  coder::array<double, 2U> y;
  coder::array<double, 1U> b;
  coder::array<double, 1U> bsum;
  coder::array<double, 1U> varargin_3;
  double b_Pos;
  int k;
  int nblocks;
  int ncolx;
  int nrowx;
  int nxin;
  int nxout;
  signed char b_input_sizes_idx_1;
  signed char c_input_sizes_idx_1;
  signed char input_sizes_idx_1;
  boolean_T empty_non_axis_sizes;
  // the speed of light
  // number of sensors
  // TDOA method flag, 0 for linear 1 for Taylor
  // sensor positon vectors
  // includes all sensor positon vectors
  // includes all toa information

  tdoa.set_size(T.size(0), T.size(1));
  ncolx = T.size(0) * T.size(1);
  for (nblocks = 0; nblocks < ncolx; nblocks++) {
    tdoa[nblocks] = T[nblocks] - T[0];
  }
  nxin = tdoa.size(0) * tdoa.size(1) - 2;
  nrowx = tdoa.size(0);
  ncolx = tdoa.size(1);
  nxout = tdoa.size(0) * tdoa.size(1) - 1;
  for (k = 0; k < nxout; k++) {
    tdoa[k] = tdoa[k + 1];
  }
  if ((nrowx != 1) && (ncolx == 1)) {
    if (1 > nxout) {
      ncolx = 0;
    } else {
      ncolx = nxin + 1;
    }
    bsum.set_size(ncolx);
    for (nblocks = 0; nblocks < ncolx; nblocks++) {
      bsum[nblocks] = tdoa[nblocks];
    }
    tdoa.set_size(ncolx, 1);
    for (nblocks = 0; nblocks < ncolx; nblocks++) {
      tdoa[nblocks] = bsum[nblocks];
    }
  } else {
    if (1 > nxout) {
      ncolx = -1;
    } else {
      ncolx = nxin;
    }
    for (nblocks = 0; nblocks <= ncolx; nblocks++) {
      tdoa[nblocks] = tdoa[nblocks];
    }
    tdoa.set_size(1, ncolx + 1);
  }
  // tdoa为时差
  // %% Linear Solution
  if (2.0 > M) {
    nblocks = 0;
    nxout = 0;
  } else {
    nblocks = 1;
    nxout = static_cast<int>(M);
  }
  ncolx = Pos.size(0);
  nxin = nxout - nblocks;
  dummy.set_size(nxin, Pos.size(0));
  for (nxout = 0; nxout < ncolx; nxout++) {
    for (nrowx = 0; nrowx < nxin; nrowx++) {
      dummy[nrowx + dummy.size(0) * nxout] =
          Pos[nxout + Pos.size(0) * (nblocks + nrowx)];
    }
  }
  y.set_size(tdoa.size(0), tdoa.size(1));
  ncolx = tdoa.size(0) * tdoa.size(1);
  for (nblocks = 0; nblocks < ncolx; nblocks++) {
    y[nblocks] = -3.0E+8 * tdoa[nblocks];
  }
  b_Pos = Pos[0];
  ncolx = dummy.size(0);
  bsum.set_size(dummy.size(0));
  for (nblocks = 0; nblocks < ncolx; nblocks++) {
    bsum[nblocks] = b_Pos - dummy[nblocks];
  }
  b_Pos = Pos[1];
  ncolx = dummy.size(0);
  b.set_size(dummy.size(0));
  for (nblocks = 0; nblocks < ncolx; nblocks++) {
    b[nblocks] = b_Pos - dummy[nblocks + dummy.size(0)];
  }
  b_Pos = Pos[2];
  ncolx = dummy.size(0);
  varargin_3.set_size(dummy.size(0));
  for (nblocks = 0; nblocks < ncolx; nblocks++) {
    varargin_3[nblocks] = b_Pos - dummy[nblocks + dummy.size(0) * 2];
  }
  if (bsum.size(0) != 0) {
    nxin = bsum.size(0);
  } else if (b.size(0) != 0) {
    nxin = b.size(0);
  } else if (varargin_3.size(0) != 0) {
    nxin = varargin_3.size(0);
  } else if ((y.size(0) != 0) && (y.size(1) != 0)) {
    nxin = y.size(0);
  } else {
    nxin = 0;
    if (y.size(0) > 0) {
      nxin = y.size(0);
    }
  }
  empty_non_axis_sizes = (nxin == 0);
  if (empty_non_axis_sizes || (bsum.size(0) != 0)) {
    input_sizes_idx_1 = 1;
  } else {
    input_sizes_idx_1 = 0;
  }
  if (empty_non_axis_sizes || (b.size(0) != 0)) {
    b_input_sizes_idx_1 = 1;
  } else {
    b_input_sizes_idx_1 = 0;
  }
  if (empty_non_axis_sizes || (varargin_3.size(0) != 0)) {
    c_input_sizes_idx_1 = 1;
  } else {
    c_input_sizes_idx_1 = 0;
  }
  if (empty_non_axis_sizes || ((y.size(0) != 0) && (y.size(1) != 0))) {
    nrowx = y.size(1);
  } else {
    nrowx = 0;
  }
  A.set_size(nxin,
             ((input_sizes_idx_1 + b_input_sizes_idx_1) + c_input_sizes_idx_1) +
                 nrowx);
  ncolx = input_sizes_idx_1;
  for (nblocks = 0; nblocks < ncolx; nblocks++) {
    for (nxout = 0; nxout < nxin; nxout++) {
      A[nxout] = 2.0 * bsum[nxout];
    }
  }
  ncolx = b_input_sizes_idx_1;
  for (nblocks = 0; nblocks < ncolx; nblocks++) {
    for (nxout = 0; nxout < nxin; nxout++) {
      A[nxout + A.size(0) * input_sizes_idx_1] = 2.0 * b[nxout];
    }
  }
  ncolx = c_input_sizes_idx_1;
  for (nblocks = 0; nblocks < ncolx; nblocks++) {
    for (nxout = 0; nxout < nxin; nxout++) {
      A[nxout + A.size(0) * (input_sizes_idx_1 + b_input_sizes_idx_1)] =
          2.0 * varargin_3[nxout];
    }
  }
  for (nblocks = 0; nblocks < nrowx; nblocks++) {
    for (nxout = 0; nxout < nxin; nxout++) {
      A[nxout +
        A.size(0) * (((nblocks + input_sizes_idx_1) + b_input_sizes_idx_1) +
                     c_input_sizes_idx_1)] = 2.0 * y[nxout + nxin * nblocks];
    }
  }
  ncolx = tdoa.size(0) * tdoa.size(1);
  for (nblocks = 0; nblocks < ncolx; nblocks++) {
    tdoa[nblocks] = 3.0E+8 * tdoa[nblocks];
  }
  r.set_size(tdoa.size(0), tdoa.size(1));
  nxin = tdoa.size(0) * tdoa.size(1);
  for (k = 0; k < nxin; k++) {
    r[k] = tdoa[k] * tdoa[k];
  }
  if (Pos.size(0) == 0) {
    b_Pos = 0.0;
  } else {
    b_Pos = 0.0;
    if (Pos.size(0) == 1) {
      b_Pos = std::abs(Pos[0]);
    } else {
      double scale;
      scale = 3.3121686421112381E-170;
      nblocks = Pos.size(0) - 1;
      for (k = 0; k <= nblocks; k++) {
        double absxk;
        absxk = std::abs(Pos[k]);
        if (absxk > scale) {
          double t;
          t = scale / absxk;
          b_Pos = b_Pos * t * t + 1.0;
          scale = absxk;
        } else {
          double t;
          t = absxk / scale;
          b_Pos += t * t;
        }
      }
      b_Pos = scale * std::sqrt(b_Pos);
    }
  }
  b_Pos *= b_Pos;
  y.set_size(dummy.size(0), dummy.size(1));
  nxin = dummy.size(0) * dummy.size(1);
  for (k = 0; k < nxin; k++) {
    y[k] = dummy[k] * dummy[k];
  }
  if ((y.size(0) == 0) || (y.size(1) == 0)) {
    b.set_size(y.size(0));
    ncolx = y.size(0);
    for (nblocks = 0; nblocks < ncolx; nblocks++) {
      b[nblocks] = 0.0;
    }
  } else {
    int lastBlockLength;
    int xj;
    int xoffset;
    ncolx = y.size(0) - 1;
    nxout = y.size(0) << 10;
    b.set_size(y.size(0));
    bsum.set_size(y.size(0));
    if (y.size(1) <= 1024) {
      nxin = y.size(1);
      lastBlockLength = 0;
      nblocks = 1;
    } else {
      nxin = 1024;
      nblocks = y.size(1) / 1024;
      lastBlockLength = y.size(1) - (nblocks << 10);
      if (lastBlockLength > 0) {
        nblocks++;
      } else {
        lastBlockLength = 1024;
      }
    }
    for (xj = 0; xj <= ncolx; xj++) {
      b[xj] = y[xj];
      bsum[xj] = 0.0;
    }
    for (k = 2; k <= nxin; k++) {
      xoffset = (k - 1) * (ncolx + 1);
      for (xj = 0; xj <= ncolx; xj++) {
        b[xj] = b[xj] + y[xoffset + xj];
      }
    }
    for (int ib{2}; ib <= nblocks; ib++) {
      nxin = (ib - 1) * nxout;
      for (xj = 0; xj <= ncolx; xj++) {
        bsum[xj] = y[nxin + xj];
      }
      if (ib == nblocks) {
        nrowx = lastBlockLength;
      } else {
        nrowx = 1024;
      }
      for (k = 2; k <= nrowx; k++) {
        xoffset = nxin + (k - 1) * (ncolx + 1);
        for (xj = 0; xj <= ncolx; xj++) {
          bsum[xj] = bsum[xj] + y[xoffset + xj];
        }
      }
      for (xj = 0; xj <= ncolx; xj++) {
        b[xj] = b[xj] + bsum[xj];
      }
    }
  }
  ncolx = r.size(0);
  b.set_size(r.size(0));
  for (nblocks = 0; nblocks < ncolx; nblocks++) {
    b[nblocks] = (r[nblocks] + b_Pos) - b[nblocks];
  }
  if (A.size(0) < A.size(1)) {
    tdoa.set_size(A.size(1), A.size(0));
    ncolx = A.size(0);
    for (nblocks = 0; nblocks < ncolx; nblocks++) {
      nxin = A.size(1);
      for (nxout = 0; nxout < nxin; nxout++) {
        tdoa[nxout + tdoa.size(0) * nblocks] = A[nblocks + A.size(0) * nxout];
      }
    }
    coder::eml_pinv(tdoa, r);
    tdoa.set_size(r.size(1), r.size(0));
    ncolx = r.size(0);
    for (nblocks = 0; nblocks < ncolx; nblocks++) {
      nxin = r.size(1);
      for (nxout = 0; nxout < nxin; nxout++) {
        tdoa[nxout + tdoa.size(0) * nblocks] = r[nblocks + r.size(0) * nxout];
      }
    }
  } else {
    coder::eml_pinv(A, tdoa);
  }
  nxin = tdoa.size(0) - 1;
  nrowx = tdoa.size(1);
  bsum.set_size(tdoa.size(0));
  for (nxout = 0; nxout <= nxin; nxout++) {
    bsum[nxout] = 0.0;
  }
  for (k = 0; k < nrowx; k++) {
    ncolx = k * tdoa.size(0);
    for (nxout = 0; nxout <= nxin; nxout++) {
      bsum[nxout] = bsum[nxout] + tdoa[ncolx + nxout] * b[k];
    }
  }
  targetPos[0] = bsum[0];
  targetPos[1] = bsum[1];
  targetPos[2] = bsum[2];
}



//int main()
//{
//    //输入参数
//    coder::array<double, 2U> Pos;
//    coder::array<double, 2U> T;
//    double M;
//    //输出参数
//    double targetPos[3];

//    //给Pos赋初始值
//    Pos = argInit_Pos();
//    //给T赋初始值
//    T = argInit_T();
//    //给M赋初始值
//    M = 2;
//    tdoa1(Pos, T, M, targetPos);

//    for (int i = 0; i < 3; ++i) {
//        std::cout << targetPos[i] << " ";
//    }
//}