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

• 각 스캐너의 (x, y) 위치는 알고 있음
• 각 스캐너에서 태그까지의 방향은 azimuth (방위각, θ)만 사용
• elevation (고도각)은 무시

그럼 각 스캐너에서 나오는 건 XY 평면상의 직선이 됩니다.
우리는 4개의 직선의 교점을 최소제곱으로 구해 태그 위치를 추정할 수 있습니다.

---

✨ 2D 자바스크립트 코드

function degreesToRadians(deg) {
    return deg * Math.PI / 180;
}

// azimuth θ (deg) → 2D 방향 벡터 [x, y]
function azimuthToDirection(azimuthDeg) {
    const azimuth = degreesToRadians(azimuthDeg);
    const x = Math.cos(azimuth);
    const y = Math.sin(azimuth);
    return [x, y];
}

// 벡터 연산
function dot(a, b) {
    return a[0]*b[0] + a[1]*b[1];
}

function subtract(a, b) {
    return [a[0]-b[0], a[1]-b[1]];
}

function add(a, b) {
    return [a[0]+b[0], a[1]+b[1]];
}

function scale(v, s) {
    return [v[0]*s, v[1]*s];
}

function norm(v) {
    return Math.sqrt(dot(v, v));
}

// 직선들에서 최소거리점 추정 (2D)
function estimatePosition2D(scannerPositions, azimuths) {
    const N = scannerPositions.length;
    let A = [[0,0],[0,0]];
    let b = [0,0];

    for (let i = 0; i < N; i++) {
        const p = scannerPositions[i];  // [x, y]
        const d = azimuthToDirection(azimuths[i]);
        const dNorm = scale(d, 1 / norm(d));

        const I = [[1,0],[0,1]];
        const ddt = [
            [dNorm[0]*dNorm[0], dNorm[0]*dNorm[1]],
            [dNorm[1]*dNorm[0], dNorm[1]*dNorm[1]],
        ];

        // I - d * d^T
        const Ai = I.map((row, r) => row.map((val, c) => val - ddt[r][c]));
        const bi = Ai.map(row => dot(row, p));

        // A += Ai
        for (let r = 0; r < 2; r++) {
            for (let c = 0; c < 2; c++) {
                A[r][c] += Ai[r][c];
            }
            b[r] += bi[r];
        }
    }

    // Ax = b → 풀기 (2x2 행렬 역행렬)
    const detA = A[0][0]*A[1][1] - A[0][1]*A[1][0];
    if (Math.abs(detA) < 1e-6) {
        throw new Error("Singular matrix, can't solve!");
    }

    const invA = [
        [ A[1][1]/detA, -A[0][1]/detA],
        [-A[1][0]/detA,  A[0][0]/detA]
    ];

    const x = [
        invA[0][0]*b[0] + invA[0][1]*b[1],
        invA[1][0]*b[0] + invA[1][1]*b[1]
    ];

    return x;
}

// 예시
const scannerPositions = [
    [0, 0],
    [10, 0],
    [0, 10],
    [10, 10]
];

const azimuths = [45, 135, -45, -135];   // degrees

const estimatedTag = estimatePosition2D(scannerPositions, azimuths);
console.log("Estimated Tag Position (XY):", estimatedTag);

---

📊 설명

✅ Azimuth → XY 평면상의 방향 벡터
✅ 각 직선의 최소제곱 교점 계산
✅ A * x = b 꼴로 풀어 (x, y) 좌표 추정

---

💡 추가로 할 수 있는 것

• 실시간 데이터 시각화 (예: D3.js, Three.js로 그리기)
• 가중치 추가 (스캐너마다 신뢰도 다를 때)
• 오차/신뢰구간 계산