本節ではINS/GPSとして提案するアルゴリズムについて述べる。
まず、状態の初期値を設定するために、初期化を行う。この状態をInitialize状態とよぶことにす る。初期化が完了するとFlight可能なり、その状態をFlight時と称することにする。さらにFlight 時では、INSを利用し時間で更新を行うときと、GPSから情報を取得し状態量を修正するときの2 つの状態にわけることができる。前者をUpdate時、Correct時と称することにする。以下、これら 3つの状態ごとにアルゴリズムの詳細を述べる。
3.4.1 Initialize 状態
状態の初期値を設定するのがInitialize状態である。設定すべき値はINSの状態量である速度 (˙⃗ren)、位置( ˜qne)、姿勢( ˜qbn)である。
簡単のために、この状態においては静止状態におく、すなわち⃗r˙ne =⃗0になっているものとする。
また位置⃗r˙ne についてはGPSを利用して取得することにする。GPSは静止状態に数分間置くこと により、非常によい精度(数m以下のオーダー)で現在位置を取得可能であることが知られている。
姿勢については、静止状態であるから重力方向から3自由度のうち2自由度を決定することが可能 であり、残りの1自由度についてはInitialize時を経過しFilight時になった直後のGPSの速度方 向をもって決定することにする。
またINS/GPSでKalman Filterを行うために必要となる、INSの状態量と真値との誤差について
共分散をとった誤差共分散行列( ¯P)を適当な値の対角行列で初期化する。
3.4.2 Flight 状態 :Update 時
INSを時間更新する状態がUpdate時である。更新する状態量はINSの状態量である速度(˙⃗rne)、 位置( ˜qne)、姿勢( ˜qbn)、およびINS/GPSのKalman Filterにおける誤差共分散行列( ¯P)である。
• INSの状態量(˙⃗rne、q˜bn、q˜bn)の更新
INS用の慣性センサである加速度計とジャイロの出力を3.1.1節で導いた運動方程式を使用 し更新時間間隔で積分することにより、INSの状態量を更新する。
• 誤差共分散行列( ¯P)の更新
INSの状態量を用いて(2.4.8)で誤差共分散行列( ¯P)を更新する。式中のΦやΓは(2.5.4)、 (2.5.5)、(3.2.13)、(3.2.13)で計算される。
Strap-down方式 加速度計
&
ジャイロ
姿勢 運動方程式
b i
ϖrb/
ab
r
‑ +
+ +
gn
速度 位置 運動方程式
緯度・経度・高度 など h
~b*
qn q~enh
Pk k T Pk+1
T
k Q
PΦ +Γ Γ Φ
b i
ϖrn/
b n
ϖrb/
n
r&re
図3.2 Update時
3.4.3 Flight 状態 :Correct 時
GPSから情報が取得できたとき、GPSから得られた情報をもとにKalman Filterを用いて補正を 行うときがCorrect時である。このときはGPSの観測値とINSの状態量の差を用いて、誤差共分 散行列( ¯P)の更新、およびINSの状態量である速度(˙⃗rne)、位置( ˜qne)、姿勢( ˜qbn)の補正を行う。
• 誤差共分散行列( ¯P)の更新
誤差共分散行列( ¯P)を(2.4.17)で更新する。このときGPSの観測量とINSの状態量の差か ら3.3節で述べた観測方程式を導いて用いる。
• INSの状態量(˙⃗rne、q˜bn、q˜bn)の更新
GPSの観測量とINSの状態量の差、ならびに誤差共分散行列( ¯P)の更新の際に得られたカ ルマンゲイン、観測方程式用いて(2.4.9)でINSの状態量を更新する。
1
ˆ
+
Pk +1
Pk b
q~n n
qe
~
n
r&re
h
K
b
qn n ~ q~e n
rr&e
h
z K
H
+
‑ +
‑
図3.3 Correct時
第 4 章
数値シミュレーション
本章ではコンピュータを用いて行った数値シミュレーションについて述べる。
数値シミュレーションを行う意義は、実機試験の成否を論じるための対象としてだけでなく、実 機に近い数値モデルを獲得できたかどうか論じるためでもある。前者はこの数値シミュレーション という再現性がある安定した環境と外乱の極めて多い実機試験を行った環境の差について考察する ためである。後者は実機試験を行う前にシミュレーションという形で数値モデルが得られれば、今 後実機試験以前にある程度結果の先読みができることにつながるためである。
このような考えのもとに数値シミュレーションを行った。