三点測位をやってみた~TriThingPulse~
随分と放置してしまったけど、そろそろ何か書かないとと思ったので、特に有用性は無い事でも書いてみようと……
三点測位って?
GPS等で位置検知に使われる三点測位という手法がある。
センサからの電波強度を持って距離を測定し、緯度経度に直すらしい。
センサからの電波強度を求める式はわりと直ぐ見つかる↓(参考URL:
iBeaconのRSSIでiPhoneの二次元座標をとれたらいいな - tsujimotterのノートブック
)
※RSSI(r)……測定地点での電波強度
A……センサから1m付近での電波強度
B……電波の減衰を表す定数(一般には2らしい)
r……センサから観測地点までの距離
でも意外と直交座標系に直すようなツールやプログラムが特に無い。
ちょっと作ってみた。(先に言うと柔軟性はあまり考えていなかった(;´ω`))
というわけで
プロジェクト「TriThingPulse」開始
※特に意味は無い
どうすれば出来る?
まずは上の電波強度を求める式を変換して、距離を求める式にする。
ここで久々の対数→指数で戸惑う。そもそもここで間違ってたら終わり。(;・ω・)
これを使うと↓みたいな形で三点からの距離が求まる。
だから、距離を半径とした円の方程式を作って交点を求めれば良い。
つまり↓を計算して、を求めれば良いはず。
※はそれぞれ円の中心のx座標/y座標つまりセンサの位置。
この式に定数を設定する。決めなければならない定数は
① 三点の座標(どこかを原点にしないといけない→センサのどれかを原点にするのが多分楽?)
② 三点それぞれの1m付近で観測される電波強度
③ 電波の減衰率
④ 観測点での電波強度
コードに落としこむ
○ざっくり設計
<画面(windowsフォームアプリ)>
① 定数の固定値のB、各センサの座標、A値、RSSI(r)値をそれぞれ入力出来る
② 計算結果が出力される(ついでに①の情報とかも出しとく?)
<計算>
① 計算については事前計算はC#、交点の計算はpythonに任せる。(理由:めんどくさいから)
② 事前計算とは各センサからの距離の計算。
<フレームワークとか>
① C#(.Net4.0)
② python3.5.1
③ sympy-1.0.win32.exe(交点計算用:http://www.sympy.org/en/index.html)
○作ってみた
C#側(事前計算部分とpythonを実行。表示。)
using System; using System.Collections.Generic; using System.ComponentModel; using System.Data; using System.Drawing; using System.Linq; using System.Text; using System.Windows.Forms; using System.Net; using System.Diagnostics; namespace TriThingPulse { public partial class Form1 : Form { public Form1() { InitializeComponent(); } private void button1_Click(object sender, EventArgs e) { //3つのWi-Fiのアクセスポイント(AP)からの距離を三点測位の計算式より算出 //各APの座標を与える事で、端末の位置を直交座標系で算出する。 //3つの波という事でTriThingPulse。他意はない。 //まぁ長く使うもんでもないし……動けばよかろ。 //AとBとRSSI(r)からAPからの距離を計算する double distApA = 0.0; double distApB = 0.0; double distApC = 0.0; try { distApA = Math.Pow(10.0, (double.Parse(AApA.Text) - double.Parse(RrApA.Text)) / (10.0 * double.Parse(B.Text))); distApB = Math.Pow(10.0, (double.Parse(AApB.Text) - double.Parse(RrApB.Text)) / (10.0 * double.Parse(B.Text))); distApC = Math.Pow(10.0, (double.Parse(AApC.Text) - double.Parse(RrApC.Text)) / (10.0 * double.Parse(B.Text))); } catch (Exception ex) { resultBox.Text = ex.Message + "\r\n"; } //デバック時(1.0, 0.0)が交点になる #if DEBUG XApA.Text = Convert.ToString(0); YApA.Text = Convert.ToString(0); XApB.Text = Convert.ToString(-3); YApB.Text = Convert.ToString(0); XApC.Text = Convert.ToString(2); YApC.Text = Convert.ToString(0); distApA = 1.0; distApB = 2.0; distApC = 3.0; #endif //結果のboxに表示 resultBox.Text = "各APからの距離は\r\n"; resultBox.Text += "AP-A:" + distApA + "\r\n"; resultBox.Text += "AP-B:" + distApB + "\r\n"; resultBox.Text += "AP-C:" + distApC + "\r\n"; //フォームの入力内容と共に、pythonのスクリプトを実行。 //引数はそれぞれのAPのx,y,それぞれの距離(xA yA rA xB yB rB xC yC rC) Process p = new Process(); p.StartInfo.FileName = Environment.GetEnvironmentVariable("ComSpec"); p.StartInfo.UseShellExecute = false; p.StartInfo.RedirectStandardOutput = true; p.StartInfo.RedirectStandardInput = false; p.StartInfo.CreateNoWindow = true; p.StartInfo.Arguments = @"/c python [f:id:wannabehuman:20160728001516p:plain]script/CalcGridPosition.py " + string.Join(" ", XApA.Text, YApA.Text, distApA, XApB.Text, YApB.Text, distApB, XApC.Text, YApC.Text, distApC); resultBox.Text += "execute cmd: " + p.StartInfo.Arguments.ToString() + "\r\n"; //起動 p.Start(); //出力を読み取る string results = p.StandardOutput.ReadToEnd(); p.WaitForExit(); p.Close(); //結果を取得 resultBox.Text += "result: " + results; } } }
import sys from sympy import * param = sys.argv try: p1 = float(param[1]) q1 = float(param[2]) r1 = float(param[3]) p2 = float(param[4]) q2 = float(param[5]) r2 = float(param[6]) p3 = float(param[7]) q3 = float(param[8]) r3 = float(param[9]) x,y,p,q,r = symbols('x y p q r') print(str(p1) + " " + str(q1) + " " + str(r1)) print(str(p2) + " " + str(q2) + " " + str(r2)) print(str(p3) + " " + str(q3) + " " + str(r3)) circle = (x -(-1 * p))**2 + (y-(-1 * q))**2 - r**2 c1 = circle.subs({p:p1, q:q1, r:r1}) c2 = circle.subs({p:p2, q:q2, r:r2}) c3 = circle.subs({p:p3, q:q3, r:r3}) print(solve({c1,c2,c3},{x,y})) except Exception as e: print(e.message)
※コードの綺麗さはあまり考えてない
で、こんな感じのが出来上がった。※デバッグモードで実行してる(いい感じの電波強度とかワカラン)