AoA(Angle of Arrival) 스캐너 4대에서 얻은 방위각(Azimuth)과 고도각(Elevation) 정보를 사용하여 태그의 위치를 추정

이 코드는 AoA 스캐너 4대의 데이터를 사용하여 삼각측량(triangulation) 기법으로 태그의 위치를 추정합니다. 주요 원리는 다음과 같습니다:

1. 각 스캐너에서 측정된 방위각(Azimuth)과 고도각(Elevation)을 이용하여 3D 공간에서의 방향 벡터를 계산합니다.
2. 스캐너의 위치와 방향 벡터를 이용해 평면 방정식을 구성합니다.
3. 네 개의 스캐너에서 얻은 평면 방정식을 선형 시스템으로 구성합니다.
4. 최소 제곱법(least squares method)을 사용하여 네 개의 직선이 가장 가깝게 교차하는 지점을 찾아 태그의 위치를 추정합니다.

실제 사용 시 다음과 같이 데이터를 제공하면 됩니다:

• 각 스캐너의 3D 위치(x, y, z)
• 각 스캐너에서 측정된 방위각(azimuth)과 고도각(elevation)

이 코드는 최소 제곱법을 사용하여 네 개의 스캐너가 제공하는 방향 정보를 조합하므로, 측정 오차가 있어도 비교적 안정적인 위치 추정이 가능합니다.

/**
 * AoA 스캐너 4대의 데이터를 사용하여 태그 위치를 추정하는 함수
 * @param {Array} scanners - 각 스캐너의 위치와 측정 각도 정보 배열
 * @returns {Object} - 추정된 태그의 x, y, z 좌표
 */
function estimateTagPosition(scanners) {
  // 방정식 시스템을 준비하기 위한 행렬
  const A = [];
  const b = [];

  // 각 스캐너의 데이터를 사용하여 방정식 시스템 구성
  scanners.forEach(scanner => {
    const { position, azimuth, elevation } = scanner;
    
    // 방위각과 고도각을 라디안으로 변환
    const azimuthRad = azimuth * Math.PI / 180;
    const elevationRad = elevation * Math.PI / 180;
    
    // 스캐너에서 태그 방향의 단위 벡터 계산
    const directionVector = {
      x: Math.cos(elevationRad) * Math.sin(azimuthRad),
      y: Math.cos(elevationRad) * Math.cos(azimuthRad),
      z: Math.sin(elevationRad)
    };
    
    // 평면 방정식의 계수 구성
    // ax + by + cz + d = 0 형태에서, a, b, c는 방향 벡터이고 d는 -(a*x0 + b*y0 + c*z0)
    // 여기서 (x0, y0, z0)는 스캐너의 위치
    const a = directionVector.x;
    const b = directionVector.y;
    const c = directionVector.z;
    const d = -(a * position.x + b * position.y + c * position.z);
    
    // 행렬 A와 벡터 b에 추가
    A.push([a, b, c]);
    b.push(-d);
  });

  // 최소 제곱법을 사용하여 해 구하기
  // A^T * A * x = A^T * b
  const AT = transposeMatrix(A);
  const ATA = multiplyMatrices(AT, A);
  const ATb = multiplyMatrixVector(AT, b);
  
  // 역행렬을 사용하여 해 구하기
  const invATA = inverseMatrix3x3(ATA);
  const solution = multiplyMatrixVector(invATA, ATb);
  
  return {
    x: solution[0],
    y: solution[1],
    z: solution[2]
  };
}

/**
 * 행렬의 전치 계산
 * @param {Array} matrix - 원본 행렬
 * @returns {Array} - 전치 행렬
 */
function transposeMatrix(matrix) {
  const rows = matrix.length;
  const cols = matrix[0].length;
  const result = Array(cols).fill().map(() => Array(rows).fill(0));
  
  for (let i = 0; i < rows; i++) {
    for (let j = 0; j < cols; j++) {
      result[j][i] = matrix[i][j];
    }
  }
  
  return result;
}

/**
 * 두 행렬의 곱 계산
 * @param {Array} matrixA - 첫 번째 행렬
 * @param {Array} matrixB - 두 번째 행렬
 * @returns {Array} - 결과 행렬
 */
function multiplyMatrices(matrixA, matrixB) {
  const rowsA = matrixA.length;
  const colsA = matrixA[0].length;
  const rowsB = matrixB.length;
  const colsB = matrixB[0].length;
  
  if (colsA !== rowsB) {
    throw new Error('행렬 곱셈 불가: 차원이 맞지 않음');
  }
  
  const result = Array(rowsA).fill().map(() => Array(colsB).fill(0));
  
  for (let i = 0; i < rowsA; i++) {
    for (let j = 0; j < colsB; j++) {
      for (let k = 0; k < colsA; k++) {
        result[i][j] += matrixA[i][k] * matrixB[k][j];
      }
    }
  }
  
  return result;
}

/**
 * 행렬과 벡터의 곱 계산
 * @param {Array} matrix - 행렬
 * @param {Array} vector - 벡터
 * @returns {Array} - 결과 벡터
 */
function multiplyMatrixVector(matrix, vector) {
  const rows = matrix.length;
  const cols = matrix[0].length;
  
  if (cols !== vector.length) {
    throw new Error('행렬-벡터 곱셈 불가: 차원이 맞지 않음');
  }
  
  const result = Array(rows).fill(0);
  
  for (let i = 0; i < rows; i++) {
    for (let j = 0; j < cols; j++) {
      result[i] += matrix[i][j] * vector[j];
    }
  }
  
  return result;
}

/**
 * 3x3 행렬의 역행렬 계산
 * @param {Array} matrix - 3x3 행렬
 * @returns {Array} - 역행렬
 */
function inverseMatrix3x3(matrix) {
  // 행렬식 계산
  const det = matrix[0][0] * (matrix[1][1] * matrix[2][2] - matrix[1][2] * matrix[2][1]) -
              matrix[0][1] * (matrix[1][0] * matrix[2][2] - matrix[1][2] * matrix[2][0]) +
              matrix[0][2] * (matrix[1][0] * matrix[2][1] - matrix[1][1] * matrix[2][0]);
  
  if (Math.abs(det) < 1e-10) {
    throw new Error('행렬이 특이행렬(singular)입니다: 역행렬 계산 불가');
  }
  
  // 여인자 행렬 계산
  const adjugate = [
    [
      (matrix[1][1] * matrix[2][2] - matrix[1][2] * matrix[2][1]),
      (matrix[0][2] * matrix[2][1] - matrix[0][1] * matrix[2][2]),
      (matrix[0][1] * matrix[1][2] - matrix[0][2] * matrix[1][1])
    ],
    [
      (matrix[1][2] * matrix[2][0] - matrix[1][0] * matrix[2][2]),
      (matrix[0][0] * matrix[2][2] - matrix[0][2] * matrix[2][0]),
      (matrix[0][2] * matrix[1][0] - matrix[0][0] * matrix[1][2])
    ],
    [
      (matrix[1][0] * matrix[2][1] - matrix[1][1] * matrix[2][0]),
      (matrix[0][1] * matrix[2][0] - matrix[0][0] * matrix[2][1]),
      (matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0])
    ]
  ];
  
  // 역행렬 계산: 여인자 행렬 / 행렬식
  const inverse = adjugate.map(row => row.map(val => val / det));
  
  return inverse;
}

/**
 * 사용 예시
 */
function main() {
  // 스캐너 데이터 예시
  const scanners = [
    {
      position: { x: 0, y: 0, z: 0 },    // 스캐너 1 위치
      azimuth: 45,                        // 방위각 (도)
      elevation: 30                       // 고도각 (도)
    },
    {
      position: { x: 10, y: 0, z: 0 },    // 스캐너 2 위치
      azimuth: 135,                        // 방위각 (도)
      elevation: 30                       // 고도각 (도)
    },
    {
      position: { x: 10, y: 10, z: 0 },    // 스캐너 3 위치
      azimuth: 225,                        // 방위각 (도)
      elevation: 30                       // 고도각 (도)
    },
    {
      position: { x: 0, y: 10, z: 0 },    // 스캐너 4 위치
      azimuth: 315,                        // 방위각 (도)
      elevation: 30                       // 고도각 (도)
    }
  ];
  
  try {
    const tagPosition = estimateTagPosition(scanners);
    console.log('추정된 태그 위치:', tagPosition);
  } catch (error) {
    console.error('위치 추정 오류:', error.message);
  }
}

// 실행
main();