前回の記事 をXにポストしたところ、多くの方々からアドバイスをいただきました。ありがとうございます。

おかげさまで目的地に近づくと直線距離表示がバグるのはArduinoの計算桁数が不足しているために値が丸められてしまっているのが原因っぽいということが分かりました。たしかに目的地までの距離が近くなるとacos(x)のxの値が限りなく1に近づくので相当な小数点以下の桁数が必要になります。double型にしておけば良いというわけでもないんですね。（Arduinoの場合、double型にしても計算精度はfloatと変わらないそうです）

---

ハードウェアを計算能力の高いラズパイpicoWに

---

さて、じゃあどうする？ということでラズパイpico Wを入手。これなら計算能力は充分ある上、Arduino IDEでスケッチをそのまま書き込んで使えるらしい。

初期設定を済ませ、早速書き込み！さあどうだ！？

・・・

・・・・・・

・・・・・・・・・

待てど暮らせどOLEDには一向に距離表示が出てこない。いったんGPSはおいといてOLED単独で動作確認 → 問題なし。GPS単独で通信確認 → 衛星から情報を受信しているものの、一部しか受信（復調？）できておらず、NMEAセンテンスとして成り立っていない。。。シリアル通信は一応できているし、モジュールの受信状態を示すLEDも点滅しているのになんでじゃー

---

Arduinoに戻る

---

いったんは心が折れかけたものの、どうしても諦めきれずに調べていると、地球上の２点間の距離を計算する方法が他にもいくつか見つかりました。

１）ハーバーサインの公式

計算式だけでなくC言語のコード例もあったので実装してみましたがうまく動作せず。自分には勉強不足で理解できないところも多々あったので断念。

２）geographicLibというC言語のライブラリ

ナノメートルオーダーという凄まじい計算精度を持つらしい。これがライブラリにあるなんてありがたい！と実装しようとしたのですが、コンパイルが上手くいかず断念。

３）Hubenyの公式

三角関数（sin、cosのみ）を用いた公式なのでやりやすい。式を見る限り、日本国内の緯度経度なら桁数が少なくても計算が破綻するようなことはなさそう。そしてこの手法は無線家にはお馴染み、あの カシミール3D でも使われている手法らしい。ということでコレを実装してみました。

---

Hubenyの公式を実装

---

指数の計算等、慣れていないところでミスらないよう回りくどい書き方をしているので非常にダサいと思いますが、以下のように書いてみました。

    double Rx = 6378.137;          // km

    double Ry = 6356.752314;   // km

    double x1 = gps.location.lng() * (M_PI/180);  // 現在地の経度

    double y1 = gps.location.lat() * (M_PI/180);   // 現在地の緯度

    double x2 = 135.00000 * (M_PI/180);             // 目的地の経度

    double y2 = 35.00000   * (M_PI/180);             // 目的地の緯度

    double E = sqrt(((Rx * Rx)-(Ry * Ry))/(Rx * Rx));

    double Dx = x1 - x2;

    double Dy = y1 - y2;

    double P = (y1 + y2)/2;

    double W = sqrt(1-((E * E)*sin(P*(M_PI/180)) * sin(P*(M_PI/180))));

    double N = Rx / W;

    double M = Rx * ((1 - (E * E)) / (pow(W,3)));

    double kyori = sqrt(((Dy*M)*(Dy*M))+(Dx*N*cos(P*(M_PI/180)))*(Dx*N*cos(P*(M_PI/180))));  // 目的地までの直線距離

さて結果は如何に？

おぉぉぉぉ！距離が近くても破綻せずちゃんと表示してる！

試しに近所に目的地を設定してクルマで走ってみると、ちゃんと近づいていく様子が分かる。最後、 数メートル単位まで減っていった ときはシビれました。

よっしゃこれで完成！・・・と思ったのですが、目的地を日本各地に設定してみると場所によっては18%もの誤差。これはいくらなんでも大きすぎる。

目的地までの距離と誤差をプロットしてみるとこんな感じ。この他にも緯度・経度と誤差とか、現在地の座標に対し目的地がプラス側なのかマイナス側なのかとか、いろんな見方をしてみましたが規則性は見出せませんでした。この誤差も桁数まるめの影響なのかなぁ。

---

三平方の定理

---

ダメもとで三平方の定理も試してみることにしました。本来は地球の曲率も考慮した距離にしないといけないのですが無視します。地球内部を突っ切って最短で２点間を結ぶ直線の距離になります。地表をなぞった場合と地中を突っ切った場合とで誤差は出ますが、前述のHubenyの公式で発生した 18%の誤差よりはマシ なんじゃないだろうかということで。

三平方の定理を使うにはもう一つ課題があります。それは 緯度経度が直交座標ではない ということ。地球は球体（正確には楕円体）なので、経度１度あたりの距離が北にいくほど短くなります。例えば北海道（北緯45度）では経度1度あたりの距離はおよそ79km、沖縄（北緯24度）ではおよそ102kmです。これを直交座標として扱ってしまうと誤差がかなり大きくなります。

そこで、赤道直下（北緯0度）における経度1度あたりの距離：111.321kmに cos（緯度）を掛けることで緯度に応じた距離を算出する補正 をしてみました。赤道直下ならcos(0°)=1なので111.321km、北極点ならcos(90°)=1なので0kmという具合です。

    double x1 = gps.location.lng();  // 現在地の経度

    double y1 = gps.location.lat();   // 現在地の緯度

    double x2 = 135.00000;             // 目的地の経度

    double y2 = 35.00000;              // 目的地の緯度

    double Dx = x1 - x2;                   //経度の差分（度）

    double Dxkyori = Dx * 110;         //経度の差分（km）

    double Dy = y1 - y2;                   //緯度の差分（度）

    double Dykyori = Dy * cos(((y1 + y2) / 2) * M_PI / 180) * 111.321;  //緯度の差分（km）

    double kyori = sqrt((Dxkyori * Dxkyori) + (Dykyori * Dykyori));      //目的地までの距離

Dykyoriの項で補正を行なっています。目的地と現在地の2地点の緯度の平均値を用いて補正しているので、２点の距離が離れるほど誤差は大きくなります。

さあどうだ！？

Hubenyより誤差が大きい。。。

こちらも誤差の大小に規則性は見出せませんでした。シンプルな計算なので桁数少なくてもなんとかなるかと思ったのですが。

---

暫定仕様

---

とりあえず ” 目的地に近づくと表示がバグる ” という最大の欠点は解消できたので一応は使えそうです。誤差が残っているのが気持ち悪いですが、そもそもルートの残距離ではなく直線距離を表示という時点で実用性は無い遊びアイテムなので、暫定仕様としてあちこち遠出するときに使ってみようと思います。気が向いたら誤差改善やラズパイのリベンジをしたいと思います。

ゴールデンウィークに初めて作ってみてバグが発覚してから１ヶ月以上。だいぶ苦しみましたが、おかげでほんの少しだけスキルが上がったように思います。アドバイスをいただいた皆さん、ありがとうございました！