【Swift】Accelerateライブラリの行列計算を使った三辺測量の関数

三辺測量

3つの受信機が取得した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))")
    }

注意

iPhoneでGPSを使って緯度経度を取得して使う場合、
緯度経度の値[°(degree)]と送信機と受信機の距離[m]をそのまま使うとおかしな値が出ます。
引数の値は単位を揃えましょう。