GPSはアンテナと衛星までの距離を測るセンサーです。
４つ以上の衛星とアンテナ間の距離を測ることによりアンテナの３次元的な位置を割り出します。
NMEA0183のGPGGAセンテンスでは緯度、経度、高さ、ジオイド高等が得られます。（ジオイド高をどうやって得ているのかは今のところ解りません）
移動ロボットの姿勢（位置と向いている方向を含めて姿勢と言っています。）に関しては、緯度経度ではだいぶ使いにくいので、直交座標系で話ができると楽です。
そこで緯度経度から地球中心の直交座標へ変換して更に、地表のある点を原点として、地面が平面だと近似した地平座標に変換してやります。
ECEF座標へ
地球の中心を原点としたデカルト座標をECEF（Earth Center Earth Fixed）座標と言います。変換式はプログラム言語風に書くと以下の様になります。
Lat:緯度
Lon:経度
High:海抜高度（標高）
N：ジオイド高
PI：円周率
Xe,Ye,Ze:ECEF座標
f = 1 / 298.257223563;
a = 6378137;
e = Sqrt(f * (2 - f));
R = a / Sqrt(1 - e*e * Sin(Lat * PI / 180) * Sin(Lat * PI /180));
Xe = (R + High + N) * Cos(Lat * PI / 180) * Cos(Lon * PI / 180);
Ye = (R + High + N) * Cos(Lat * PI / 180) * Sin(Lon * PI / 180);
Ze = (R * (1 - e*e) + High + N) * Sin(Lat * PI / 180);
ENU座標（地平座標）へ
地平座標をENU座標と言います。Eは東西の東のEast、Nは南北の北のNorth、UはアップダウンのUpを表します。緯度経度からENUへはECEFに変換したものもつかいます。またENUでは地球平面上の何処が原点なのかあらかじめ決めておきECEFで保持しておかなければなりません。以下に変換式をプログラム言語風に書くと以下の様になります。
Xe,Ye,Ze:位置を変換したい点のECEF座標
x0,y0,z0:地平座標の原点としたい点のECEF座標
Lat,Lon:位置を変換したい点の緯度経度
E = -Sin(Lon * PI / 180) * (Xe - x0) + Cos(Lon * PI / 180) * (Ye - y0);
N = -Sin(Lat * PI / 180) * Cos(Lon * PI / 180) * (Xe - x0) - Sin(Lat * PI / 180) * Sin(Lon * PI / 180) * (Ye - y0) + Cos(Lat * PI / 180) * (Ze - z0);
U = Cos(Lat * PI / 180) * Cos(Lon * PI / 180) * (Xe - x0) + Cos(Lat) * Sin(Lon * PI / 180) * (Ye - y0) + Sin(Lat * PI / 180) * (Ze - z0);
参考文献：http://www.enri.go.jp/~fks442/K_MUSEN/
