おはよう！ よい子のみんな、きょうはお姉さんとカーナビを作ろう☆ （注： 実際にはハンダ付けやファームウェアづくりがかなりめんどくさいので作りません。）

みんな、GPSって知ってるよね。御殿場プリティー・シスターズじゃないぞぉ。そう、人工衛星を使って居場所を調べる仕組みのことだよ。

GPS衛星はめっちゃ正確な時計を積んでて、その時刻を電波で地上に送ってくるの。電波って速そうに見えて実はかなりトロいから、お家のくるまに届いた時刻はちょっと遅れてる。

もし、くるまにすんごい正確な時計があれば、自分の時計の時刻と衛星から届いた時刻を引き算して光の速度で割り算すれば、GPSからくるままでの距離（この距離をrって書くことにするよ）がわかっちゃう。

GPS衛星の場所はアメリカかどこかのエライ人が測ってくれてるからわかるでしょ、すると、お家のくるまはGPS衛星を中心とした半径rの球面（←サッカーボールの皮みたいな感じね♥）の上のどこかにあることがわかるんだ。でも、球面のどこかはわかんない。

そこで、2機めのGPSで同じようにすると、くるまがあるはずの場所の球面がもう1個描ける。二つの球面の交わったとこにくるまがあるんだけど…。二つの球面が交わると円になるから、まだ場所がはっきり決まらない。

さらに、3機めのGPSで同じようにすると、三つめの球面が描ける。球面1と球面2の交わりの円が、球面3と交わる場所は2箇所。1箇所は地上のそば、もう1箇所は地上から離れた場所で交わるんだ。くるまはふつう空を飛んだりしないので、地上に近い側で交わった場所がキミのいる場所だ。

そういう原理なのですが、お家に原子時計を持っている人は稀なので、実際には手元の時計がナイものとして計算したりします。

イメージ的には…。まず、二つの衛星から届いた時刻情報t₁とt₂の差t₁-t₂をとります。これを光速cで割り算すると、衛星2と車の距離r₂と、衛星1と車の距離r₁の距離差r₂-r₁の値が決まります。

距離差が一定の面は双曲面となります。同様に別の衛星とペアを組んで別の双曲面を求めます。三つの双曲面があれば、それらの交わりは点となりますから、車の位置qが特定できます。よって、4機のGPS衛星が見えることが必要条件となります。

GPS受信機やカーナビによっては3機だけでもなんとなく位置がわかるものもありますが、例えば海抜0mにいるといった適当な仮定をおいて解いているようなものなので、精度は劣ります。

さぁ、いぐぞー。未知の時刻tにおける車の3次元座標をq(t)、その時間微分（3次元速度）をq'(t)とおく。i番めのGPS衛星から受信した時刻情報をτ_i(t)、GPS衛星の軌道情報から得られる衛星iの3次元座標をp_i(t)とおく。

お姉さん、車速パルスも使っちゃうぞ。車の車速パルスで計測した1次元速度をβ(t)とおく。

電波を受信できた衛星の機体数をNとし、観測ベクトルをy=[τ₁ τ₂ ... τ_N | β]^T、状態ベクトルをx=[t | q^T | q'^T]^Tとおく。ここに、右上付きのTは転置を表す。

以下めんどくさいので、時刻のインデックスを省略する。

観測雑音をwとしたとき、観測方程式 y = h(x)+wの関数hは
τ_i = t-||q-p_i||/c
β = ||q'||
により表わされる。

一方、プロセス雑音をvとおいたとき、状態遷移方程式 x' = f(x)+vの関数fを、
f(x) = [1 | q'^T | 0^T]^T
により定義する。なお、0は3次元の零ベクトルとする。また、今回は、等速運動により状態遷移をモデル化した。

あとは、関数hと関数fを状態ベクトルxで偏微分し、状態遷移の方は適当にRunge-Kuttaかナニかで数値積分しつつも （積分するまでもないや…） 拡張カルマンフィルタに放りこんでやれば、おうちのくるまの場所がわかります。

さぁて次回は、光ファイバジャイロも積んじゃうぞ。こんどはサニャック効果 … 一般相対論が入ってくるよぉ。（もう、やりませんて！）

ごめんなさい。欲求不満が募っているようです。