삼각측량

세 개의 수신기에서 얻은 RSSI를 토대로 송신기의 거리를 추정하고, 삼각 측정을 사용하여 송신기의 위치를 추정한다
수신기 A(x1, y1), B(x2, y2), C(x3, y3)
송신기 X (X, Y)
수신기에서 송신기까지의 거리 A-X:d1, B-X:d2, C-X:d3

소스 코드

calculatePosition.swift

import Accelerate

func calculatePosition(x1: Double, y1: Double, x2: Double, y2: Double, x3: Double, y3: Double, d1: Double, d2: Double, d3: Double) {
        let A: [Double] = [2*(x1-x3), 2*(y1-y3),
                           2*(x2-x3), 2*(y2-y3)]
        print("matrixA = \(A)")
        let B: [Double] = [(pow(x1, 2)-pow(x3, 2))+(pow(y1, 2)-pow(y3, 2))+(pow(d3, 2)-pow(d1, 2)),
                           (pow(x2, 2)-pow(x3, 2))+(pow(y2, 2)-pow(y3, 2))+(pow(d3, 2)-pow(d2, 2))]
        print("matrixB = \(B)")

        let matrixA: la_object_t = la_matrix_from_double_buffer(A, 2, 2, 2, la_hint_t(LA_NO_HINT), la_attribute_t(LA_DEFAULT_ATTRIBUTES))

        let matrixB: la_object_t = la_matrix_from_double_buffer(B, 2, 1, 1, la_hint_t(LA_NO_HINT), la_attribute_t(LA_DEFAULT_ATTRIBUTES))

        let matrixX: la_object_t = la_solve(matrixA, matrixB)

        var bufferA: [Double] = [Double](repeating: 0.0, count: 4)
        la_matrix_to_double_buffer(&bufferA, 2, matrixA)
        var bufferB: [Double] = [Double](repeating: 0.0, count: 2)
        la_matrix_to_double_buffer(&bufferB, 1, matrixB)
        var bufferX: [Double] = [Double](repeating: 0.0, count: 2)
        la_matrix_to_double_buffer(&bufferX, 1, matrixX)

        print("matrixX = \(bufferX)")

        let X = String(bufferX[0])
        let Y = String(bufferX[1])
        print("Target is (\(X), \(Y))")
    }

주의

아이폰에 GPS를 사용해 위도 경도를 얻은 후 사용하는 경우
위도경도치[°(degree)]와 발사기와 수신기의 거리[m]를 직접 사용하면 이상한 값이 나타날 수 있다.
매개변수의 값이 단위를 정렬합니다.