単独測位を試す

今日、昼にラーメンを食いに行こうと思ったら、前から行こうと思っていた店が休み、２番目に行った店は開店３０分前にもかかわらず行列が・・・。ラーメンは諦め、結局行きつけの店でカツ丼を食べました。女将さんからは「いつものね？」と言われた。相変わらず、おいしかったです。満足。


で、帰ると微妙な時間だったので、今日は単独測位プログラムを書いてみた。
観測方程式はこんな感じ。

 f(x,y,z,dt)=sqrt(dx^2+dy^2+dz^2)+C*dt

fは擬似距離から時計誤差とかを処理したもの。
x,y,zが線形じゃないので、これをテイラー展開して、非線形最小二乗法にする。
数式はノートに書いてみたら２ページになった上に、計算ミスもあったので今度メモっておきたいなあ。

さて、使ったデータは、GEONET「3018」観測点で、答えは(-3.9695074060E+06,3.3278898661E+06,3.7090383081E+06)　(2012/8/16のF3解)。

で、初期値を(0,0,0,0)から計算してみると、

satnum, x(sat), y(sat), z(sat), dt(sat)
SATPOS:  32,    8075372.3045,   22569776.5255,   12147617.4769, -0.0004770960
SATPOS:  84,  -23318319.2847,   18586114.8063,  -25160339.5912,  0.0000130641
SATPOS:  30,   -3332934.0086,   24125709.4567,   10248378.3800, -0.0003655502
SATPOS:  31,   -9053121.7708,   11965903.1817,   22118916.5717,  0.0002678009
SATPOS:  29,  -21467145.5567,     171477.0652,   15665776.8263,  0.0003446830
SATPOS:  16,     563918.4785,   26380546.3717,    -191508.9968, -0.0002438091
SATPOS:  25,  -15055889.6059,   -4018044.5479,   21474952.5222,  0.0000398373
SATPOS:  14,  -21625311.0310,   14935548.3151,    3395049.7157,  0.0002101171
itrnum, time, x(sta), y(sta), z(sta), dt(sta)
POS(000): 2012/08/16 00:00:00,-4755659.5459, 3686366.5401, 4073518.8387,0.003110526
POS(001): 2012/08/16 00:00:00,-4152400.3866, 3342603.6460, 3754318.7889,0.003189483
POS(002): 2012/08/16 00:00:00,-4143561.5152, 3338589.6516, 3752314.5547,0.003225822
POS(003): 2012/08/16 00:00:00,-4143559.3173, 3338587.5856, 3752310.6861,0.003262148
POS(004): 2012/08/16 00:00:00,-4143559.3125, 3338587.5793, 3752310.6798,0.003298474
POS(005): 2012/08/16 00:00:00,-4143559.3125, 3338587.5793, 3752310.6798,0.003334800

となった。５回くらいiterationかければ収束するのかぁ。

しかし座標値が２００kmとかずれている・・・。一体何なんだ。それなりに正解に近いというのがいやらしい。

ということで試行錯誤の末。
・・・衛星時計誤差を補正していないためだった。忘れていたよ・・・。
しかし、たった1ミリ秒以下じゃん、とたかをくくっていたけども、こんなに効くんだなぁ。へー。

ということで、こちらが修正後。

（衛星時計誤差を考慮）
satnum, x(sat), y(sat), z(sat), dt(sat)
SATPOS:  32,    8075372.3045,   22569776.5255,   12147617.4769, -0.0004770960
SATPOS:  84,  -23318319.2847,   18586114.8063,  -25160339.5912,  0.0000130641
SATPOS:  30,   -3332934.0086,   24125709.4567,   10248378.3800, -0.0003655502
SATPOS:  31,   -9053121.7708,   11965903.1817,   22118916.5717,  0.0002678009
SATPOS:  29,  -21467145.5567,     171477.0652,   15665776.8263,  0.0003446830
SATPOS:  16,     563918.4785,   26380546.3717,    -191508.9968, -0.0002438091
SATPOS:  25,  -15055889.6059,   -4018044.5479,   21474952.5222,  0.0000398373
SATPOS:  14,  -21625311.0310,   14935548.3151,    3395049.7157,  0.0002101171
itrnum, time, x(sta), y(sta), z(sta), dt(sta)
POS(000): 2012/08/16 00:00:00,-4546577.0224, 3674293.9179, 4025366.1831,0.002756903
POS(001): 2012/08/16 00:00:00,-3977504.4200, 3331504.2787, 3710605.1526,-0.000159663
POS(002): 2012/08/16 00:00:00,-3969535.1170, 3327893.6137, 3709067.7009,-0.000198327
POS(003): 2012/08/16 00:00:00,-3969533.8351, 3327893.1039, 3709067.8222,-0.000198333
POS(004): 2012/08/16 00:00:00,-3969533.8351, 3327893.1039, 3709067.8222,-0.000198333

ということで、大体数十メートルくらいの精度になったみたいだ。
まだ相対論効果、電離層遅延、対流圏遅延を考慮していないけど、これくらいにはなるんだね。
次はこれらを入れたらどう変わるかを見てみようかな。

今日は、単独測位プログラム程度とは言ってもそれなりに計算しなきゃいけないんだな、ということが分かった。相対測位とか面倒くさそうだな、こりゃ。