Edited at

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

More than 1 year has passed since last update.


三辺測量

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]をそのまま使うとおかしな値が出ます。

引数の値は単位を揃えましょう。