JP2001264105A - 慣性航法装置、慣性航法装置の初期化方法及び記録媒体 - Google Patents
慣性航法装置、慣性航法装置の初期化方法及び記録媒体Info
- Publication number
- JP2001264105A JP2001264105A JP2000079871A JP2000079871A JP2001264105A JP 2001264105 A JP2001264105 A JP 2001264105A JP 2000079871 A JP2000079871 A JP 2000079871A JP 2000079871 A JP2000079871 A JP 2000079871A JP 2001264105 A JP2001264105 A JP 2001264105A
- Authority
- JP
- Japan
- Prior art keywords
- kalman filter
- diagonal element
- inertial navigation
- covariance matrix
- calculation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Gyroscopes (AREA)
- Navigation (AREA)
Abstract
(57)【要約】
【課題】カルマンフィルタの発散によるイニシャルアラ
イメントの精度不良の発生を未然に防止し、安定状態で
高速にイニシャルアライメントを行なう。 【解決手段】カルマンフィルタでの推定誤差共分散行列
の対角要素P(i)の正定性をモニタし(ステップS03〜
S07)、該正定性が喪失された場合に該対角要素P(i)
を充分小さい正の定数(const.)に置換して(ステップS
10)、演算を続行させる。
イメントの精度不良の発生を未然に防止し、安定状態で
高速にイニシャルアライメントを行なう。 【解決手段】カルマンフィルタでの推定誤差共分散行列
の対角要素P(i)の正定性をモニタし(ステップS03〜
S07)、該正定性が喪失された場合に該対角要素P(i)
を充分小さい正の定数(const.)に置換して(ステップS
10)、演算を続行させる。
Description
【0001】
【発明の属する技術分野】本発明は、イニシャルアライ
メント処理でカルマンフィルタを用いる慣性航法装置、
慣性航法装置の初期化方法及び記録媒体に関する。
メント処理でカルマンフィルタを用いる慣性航法装置、
慣性航法装置の初期化方法及び記録媒体に関する。
【0002】
【従来の技術】航空機等の飛翔体、船舶、車両などを含
めた航行体の航行時の位置、速度、姿勢、方位を計測す
るものとして慣性航法装置(以下「INS」と称する)
が多く用いられており、特に航空機等の飛翔体では、移
動速度が高く、航行距離が他に比較して大きいことか
ら、ほぼ必須の装置として使用されている。
めた航行体の航行時の位置、速度、姿勢、方位を計測す
るものとして慣性航法装置(以下「INS」と称する)
が多く用いられており、特に航空機等の飛翔体では、移
動速度が高く、航行距離が他に比較して大きいことか
ら、ほぼ必須の装置として使用されている。
【0003】しかるに、航空機で使用されるINSで
は、飛行前の静止状態において航法計算座標の初期化を
行ない、航法計算のための基準座標を決定することとな
っている。
は、飛行前の静止状態において航法計算座標の初期化を
行ない、航法計算のための基準座標を決定することとな
っている。
【0004】この基準座標は、真北基準、すなわちIN
Sの基準軸の真北方位角:ψと、水平基準、すなわちI
NSの基準軸のピッチ角:θ及び同ロール角:φとによ
って定義されるもので、これらψ,θ,φを求める航法
計算座標の初期値処理をイニシャルアライメントと称す
る。
Sの基準軸の真北方位角:ψと、水平基準、すなわちI
NSの基準軸のピッチ角:θ及び同ロール角:φとによ
って定義されるもので、これらψ,θ,φを求める航法
計算座標の初期値処理をイニシャルアライメントと称す
る。
【0005】このイニシャルアライメントを終了する
と、決定した航法計算座標上で実際に航空機の運動、す
なわち加速度や角速度などを検知して航法計算を開始す
ることができるようになる。
と、決定した航法計算座標上で実際に航空機の運動、す
なわち加速度や角速度などを検知して航法計算を開始す
ることができるようになる。
【0006】図3に現在広く採用されているイニシャル
アライメントの処理の流れを示す。イニシャルアライメ
ントは、コースアライメントとファインアライメントの
2つのフェーズに大別することができるもので、まずコ
ースアライメントにおいてジャイロやアクセロメータか
らなるセンサからのデータを累積して平均化し、大まか
な真北・水平基準(ψ0 ,θ0 ,φ0 )を求める。
アライメントの処理の流れを示す。イニシャルアライメ
ントは、コースアライメントとファインアライメントの
2つのフェーズに大別することができるもので、まずコ
ースアライメントにおいてジャイロやアクセロメータか
らなるセンサからのデータを累積して平均化し、大まか
な真北・水平基準(ψ0 ,θ0 ,φ0 )を求める。
【0007】次いで、ファインアライメントで、カルマ
ンフィルタなどのデジタルフィルタ手法により前のフェ
ーズのコースアライメントで得た真北・水平基準(ψ0
,θ0 ,φ0 )の誤差(ψ′,θ′,φ′)を推定
し、その差を求めることで精度の高い真北・水平基準
(ψ,θ,φ)、すなわち航法計算座標を決定する。
ンフィルタなどのデジタルフィルタ手法により前のフェ
ーズのコースアライメントで得た真北・水平基準(ψ0
,θ0 ,φ0 )の誤差(ψ′,θ′,φ′)を推定
し、その差を求めることで精度の高い真北・水平基準
(ψ,θ,φ)、すなわち航法計算座標を決定する。
【0008】
【発明が解決しようとする課題】上記したファインアラ
イメントを行なうためのカルマンフィルタは、例えば強
風等により動揺が大きいなどの悪環境下や、前のフェー
ズのコースアライメントの推定精度が悪い場合など、様
々な要因によって偶発的に数値が不安定な発散を生じる
ことがある。
イメントを行なうためのカルマンフィルタは、例えば強
風等により動揺が大きいなどの悪環境下や、前のフェー
ズのコースアライメントの推定精度が悪い場合など、様
々な要因によって偶発的に数値が不安定な発散を生じる
ことがある。
【0009】そして、カルマンフィルタで発散が発生す
ると、もはやその推定値は意味をなさなくなるため、カ
ルマンフィルタによる推定演算処理を初めからやり直さ
なくてはならなくなり、大きな時間の損失となる。
ると、もはやその推定値は意味をなさなくなるため、カ
ルマンフィルタによる推定演算処理を初めからやり直さ
なくてはならなくなり、大きな時間の損失となる。
【0010】上記のようなカルマンフィルタの発散のリ
スクを軽減するためには、カルマンフィルタの初期パラ
メータの設定によって小さめのゲインでゆっくりと収束
させる方法も考えられるが、このような方法を採ると、
必然的にカルマンフィルタが収束するまでに時間を要す
ることとなり、結果としてイニシャルアライメントに要
する時間が長くなってしまうという不具合が生じる。
スクを軽減するためには、カルマンフィルタの初期パラ
メータの設定によって小さめのゲインでゆっくりと収束
させる方法も考えられるが、このような方法を採ると、
必然的にカルマンフィルタが収束するまでに時間を要す
ることとなり、結果としてイニシャルアライメントに要
する時間が長くなってしまうという不具合が生じる。
【0011】本発明は上記のような実情に鑑みてなされ
たもので、その目的とするところは、カルマンフィルタ
の発散によるイニシャルアライメントの精度不良が発生
してしまうのを未然に防止し、安定した状態で且つ高速
にイニシャルアライメントを行なうことが可能な慣性航
法装置、この慣性航法装置の初期化方法及び慣性航法装
置に用いられる記録媒体を提供することにある。
たもので、その目的とするところは、カルマンフィルタ
の発散によるイニシャルアライメントの精度不良が発生
してしまうのを未然に防止し、安定した状態で且つ高速
にイニシャルアライメントを行なうことが可能な慣性航
法装置、この慣性航法装置の初期化方法及び慣性航法装
置に用いられる記録媒体を提供することにある。
【0012】
【課題を解決するための手段】請求項1記載の発明は、
イニシャルアライメント処理時にカルマンフィルタを用
いて基準座標に対する誤差を推定する演算を実行する慣
性航法装置において、上記カルマンフィルタでの推定誤
差共分散行列の対角要素(P(i))の正定性をモニタする
モニタ手段と、このモニタ手段により推定誤差共分散行
列の対角要素(P(i))の正定性が喪失された場合に該対
角要素を所定値に置換して演算を続行させる制御手段と
を具備したことを特徴とする。
イニシャルアライメント処理時にカルマンフィルタを用
いて基準座標に対する誤差を推定する演算を実行する慣
性航法装置において、上記カルマンフィルタでの推定誤
差共分散行列の対角要素(P(i))の正定性をモニタする
モニタ手段と、このモニタ手段により推定誤差共分散行
列の対角要素(P(i))の正定性が喪失された場合に該対
角要素を所定値に置換して演算を続行させる制御手段と
を具備したことを特徴とする。
【0013】このような構成とすれば、カルマンフィル
タに軽度の発散が生じても、推定誤差共分散行列の対角
要素(P(i))を充分ゲインの小さい所定値に置換して演
算を続行させることができるため、安定した状態で且つ
高速にイニシャルアライメントを行なうことが可能とな
る。
タに軽度の発散が生じても、推定誤差共分散行列の対角
要素(P(i))を充分ゲインの小さい所定値に置換して演
算を続行させることができるため、安定した状態で且つ
高速にイニシャルアライメントを行なうことが可能とな
る。
【0014】請求項2記載の発明は、上記請求項1記載
の発明において、上記制御手段は、該対角要素を所定値
に置換して演算を続行させる回数が制限値に達すると演
算を停止することを特徴とする。
の発明において、上記制御手段は、該対角要素を所定値
に置換して演算を続行させる回数が制限値に達すると演
算を停止することを特徴とする。
【0015】このような構成とすれば、上記請求項1記
載の発明の作用に加えて、カルマンフィルタに続行不可
能な発散が生じたと確実に判定した際には直ちにその演
算を停止させるようにしたため、続行しても意味のない
無駄な演算を継続することなく、次の処理に速やかに移
行することができる。
載の発明の作用に加えて、カルマンフィルタに続行不可
能な発散が生じたと確実に判定した際には直ちにその演
算を停止させるようにしたため、続行しても意味のない
無駄な演算を継続することなく、次の処理に速やかに移
行することができる。
【0016】請求項3記載の発明は、上記請求項2記載
の発明において、上記制御手段は、該対角要素を所定値
に置換して演算を続行させる回数が制限値に達して演算
を停止した後、カルマンフィルタを初期化して演算を再
実行させることを特徴とする。
の発明において、上記制御手段は、該対角要素を所定値
に置換して演算を続行させる回数が制限値に達して演算
を停止した後、カルマンフィルタを初期化して演算を再
実行させることを特徴とする。
【0017】このような構成とすれば、上記請求項2記
載の発明の作用に加えて、該カルマンフィルタによる推
定演算を即座に初めからやり直すようになるため、発散
が生じた場合であってもその時間的な損失を最小限に抑
えることが可能となる。
載の発明の作用に加えて、該カルマンフィルタによる推
定演算を即座に初めからやり直すようになるため、発散
が生じた場合であってもその時間的な損失を最小限に抑
えることが可能となる。
【0018】請求項4記載の発明は、上記請求項3記載
の発明において、上記制御手段により演算を停止してか
ら、次にカルマンフィルタを初期化して演算を再実行さ
せるまでの時間を設定する設定手段をさらに具備したこ
とを特徴とする。
の発明において、上記制御手段により演算を停止してか
ら、次にカルマンフィルタを初期化して演算を再実行さ
せるまでの時間を設定する設定手段をさらに具備したこ
とを特徴とする。
【0019】このような構成とすれば、上記請求項3記
載の発明の作用に加えて、カルマンフィルタに発散を生
じた原因に対応して次にカルマンフィルタによる推定演
算を初めからやり直すまでの待機時間を適宜設定できる
ようになるため、確実に発散を生じずにカルマンフィル
タによる推定演算を再度開始させることができるものと
なり、結果としてイニシャルアライメント全体に要する
時間を最小限に抑えることが可能となる。
載の発明の作用に加えて、カルマンフィルタに発散を生
じた原因に対応して次にカルマンフィルタによる推定演
算を初めからやり直すまでの待機時間を適宜設定できる
ようになるため、確実に発散を生じずにカルマンフィル
タによる推定演算を再度開始させることができるものと
なり、結果としてイニシャルアライメント全体に要する
時間を最小限に抑えることが可能となる。
【0020】請求項5記載の発明は、航行前のイニシャ
ルアライメント処理時にカルマンフィルタを用いた基準
座標に対する誤差を推定する演算を実行する慣性航法装
置の初期化方法であって、上記カルマンフィルタでの推
定誤差共分散行列の対角要素(P(i))の正定性をモニタ
し、推定誤差共分散行列の対角要素(P(i))の正定性が
喪失された場合に該対角要素を所定値に置換して演算を
続行させることを特徴とする。
ルアライメント処理時にカルマンフィルタを用いた基準
座標に対する誤差を推定する演算を実行する慣性航法装
置の初期化方法であって、上記カルマンフィルタでの推
定誤差共分散行列の対角要素(P(i))の正定性をモニタ
し、推定誤差共分散行列の対角要素(P(i))の正定性が
喪失された場合に該対角要素を所定値に置換して演算を
続行させることを特徴とする。
【0021】このような方法とすれば、カルマンフィル
タに軽度の発散が生じても、推定誤差共分散行列の対角
要素(P(i))を充分ゲインの小さい所定値に置換して演
算を続行させることができるため、安定した状態で且つ
高速にイニシャルアライメントを行なわせることが可能
となる。
タに軽度の発散が生じても、推定誤差共分散行列の対角
要素(P(i))を充分ゲインの小さい所定値に置換して演
算を続行させることができるため、安定した状態で且つ
高速にイニシャルアライメントを行なわせることが可能
となる。
【0022】請求項6記載の発明は、航行前のイニシャ
ルアライメント処理時にカルマンフィルタを用いた基準
座標に対する誤差を推定する演算を実行する慣性航法装
置で使用される記録媒体であって、上記カルマンフィル
タでの推定誤差共分散行列の対角要素(P(i))の正定性
をモニタするモニタ工程と、このモニタ工程で推定誤差
共分散行列の対角要素(P(i))の正定性が喪失された場
合に該対角要素を所定値に置換して演算を続行させる制
御工程とをコンピュータに実行させる命令を含む制御プ
ログラムを格納したことを特徴とする。
ルアライメント処理時にカルマンフィルタを用いた基準
座標に対する誤差を推定する演算を実行する慣性航法装
置で使用される記録媒体であって、上記カルマンフィル
タでの推定誤差共分散行列の対角要素(P(i))の正定性
をモニタするモニタ工程と、このモニタ工程で推定誤差
共分散行列の対角要素(P(i))の正定性が喪失された場
合に該対角要素を所定値に置換して演算を続行させる制
御工程とをコンピュータに実行させる命令を含む制御プ
ログラムを格納したことを特徴とする。
【0023】このような格納内容とすれば、カルマンフ
ィルタに軽度の発散が生じても、推定誤差共分散行列の
対角要素(P(i))を充分ゲインの小さい所定値に置換し
て演算を続行させることができるため、安定した状態で
且つ高速にイニシャルアライメントを行なわせることが
可能となる。
ィルタに軽度の発散が生じても、推定誤差共分散行列の
対角要素(P(i))を充分ゲインの小さい所定値に置換し
て演算を続行させることができるため、安定した状態で
且つ高速にイニシャルアライメントを行なわせることが
可能となる。
【0024】
【発明の実施の形態】以下本発明を航空機用のINSに
適用した場合の実施の一形態について図面を参照して説
明する。
適用した場合の実施の一形態について図面を参照して説
明する。
【0025】図1は同実施の形態に係るINS10の回
路構成を示すものである。同図で、11はジャイロセン
サ及びアクセロメータ(加速度計)からなるセンサとそ
れらセンサで得られるセンサデータの出力処理を行なう
センサ部であり、このセンサ部11が出力するセンサデ
ータが計算処理部12に送られる。
路構成を示すものである。同図で、11はジャイロセン
サ及びアクセロメータ(加速度計)からなるセンサとそ
れらセンサで得られるセンサデータの出力処理を行なう
センサ部であり、このセンサ部11が出力するセンサデ
ータが計算処理部12に送られる。
【0026】計算処理部12は、CPUとその制御プロ
グラムを記憶したROM、ワークエリアとして用いられ
るRAM等、小型のコンピュータシステムにより構成さ
れ、I/O部13を介して入力される制御指示にしたが
ってセンサ部11からのセンサデータを用いたイニシャ
ルアライメントや航法計算等の計算処理を行なうもの
で、その計算結果はI/O部13へ出力される。
グラムを記憶したROM、ワークエリアとして用いられ
るRAM等、小型のコンピュータシステムにより構成さ
れ、I/O部13を介して入力される制御指示にしたが
ってセンサ部11からのセンサデータを用いたイニシャ
ルアライメントや航法計算等の計算処理を行なうもの
で、その計算結果はI/O部13へ出力される。
【0027】I/O部13は、モード制御器14、制御
指示器15、及びこのINS10の外部の機器と接続さ
れ、計算処理部12で取り扱われる各種データ、制御指
示等の入出力処理を行なう。
指示器15、及びこのINS10の外部の機器と接続さ
れ、計算処理部12で取り扱われる各種データ、制御指
示等の入出力処理を行なう。
【0028】モード制御器14は、制御指示器15と共
にこのINS10のオペレータの近傍に設置されるもの
で、電源オン/オフ、イニシャルアライメントモード、
航法計算モードなどのモードの選択切換えを行なう。
にこのINS10のオペレータの近傍に設置されるもの
で、電源オン/オフ、イニシャルアライメントモード、
航法計算モードなどのモードの選択切換えを行なう。
【0029】制御指示器15は、数値入力や計算情報の
確認のための表示等を行なう。
確認のための表示等を行なう。
【0030】しかして、これらモード制御器14及び制
御指示器15を除く上記センサ部11、計算処理部1
2、及びI/O部13とこれらの各回路に動作電力を供
給する電源16とが慣性航法部17としてユニット化さ
れ、航空機内の所定位置に設置、搭載される。
御指示器15を除く上記センサ部11、計算処理部1
2、及びI/O部13とこれらの各回路に動作電力を供
給する電源16とが慣性航法部17としてユニット化さ
れ、航空機内の所定位置に設置、搭載される。
【0031】なお、上記計算処理部12が実行する計算
処理手順は、例えば予め図示しない記録媒体によりイン
ストールした制御プログラムにしたがってなされるもの
とする。
処理手順は、例えば予め図示しない記録媒体によりイン
ストールした制御プログラムにしたがってなされるもの
とする。
【0032】次に上記実施の形態の動作について説明す
る。
る。
【0033】図2は上記INS10により静止状態の航
空機で実行されるイニシャルアライメントの処理手順を
説明するものである。
空機で実行されるイニシャルアライメントの処理手順を
説明するものである。
【0034】その当初には、上記図3でも示した如くコ
ースアライメントにおいてセンサ部11のジャイロやア
クセロメータからのデータを累積して平均化し、大まか
な真北・水平基準(ψ0 ,θ0 ,φ0 )を求める(ステ
ップS01)。
ースアライメントにおいてセンサ部11のジャイロやア
クセロメータからのデータを累積して平均化し、大まか
な真北・水平基準(ψ0 ,θ0 ,φ0 )を求める(ステ
ップS01)。
【0035】次に、ファインアライメントに移行し、カ
ルマンフィルタの初期化を行なった後に(ステップS0
2)、カルマンフィルタによる推定演算の第1処理Aと
して推定誤差共分散行列の対角要素(P(i))を算出する
(ステップS03)。
ルマンフィルタの初期化を行なった後に(ステップS0
2)、カルマンフィルタによる推定演算の第1処理Aと
して推定誤差共分散行列の対角要素(P(i))を算出する
(ステップS03)。
【0036】この推定誤差共分散行列の対角要素(P
(i))は、カルマンフィルタの基本システム方程式を次の
ように規定した場合に得られるものである。すなわち、 (フィルタ方程式) <観測直前> Xt+1/t=Φt・Xt/t …(1) <観測直後> Xt/t=Xt/t-1+gt (Yt−Yt′) …(2) (観測方程式) Yt′=Mt・Xt/t-1 …(3) (但し、X :状態変数(推定値)、 Y′:観測値(理論値)、 添字のt/t-1は観測直前のパラメータ値、 t/tは観測直後のパラメータ値。) (カルマンゲイン) gt=(Pt/t-1・Mt T ・(Mt・Pt/t-1・Mt T・+Vt) …(4) (推定誤差共分散行列) <観測直前> Pt+1/t=Φt・Pt/t・Φt T+Ht …(5) <観測直後> Pn =Pn′−gn・Qn・gn T …(6) (但し、Ht :システム誤差共分散行列、 Vt :観測誤差共分散行列。) として、その対角要素(P(i))(iはカルマンフィルタ
の状態変数で、n次のカルマンフィルタであればP
(1),P(2),‥‥,P(n)を求める)が得られるよう
になる。
(i))は、カルマンフィルタの基本システム方程式を次の
ように規定した場合に得られるものである。すなわち、 (フィルタ方程式) <観測直前> Xt+1/t=Φt・Xt/t …(1) <観測直後> Xt/t=Xt/t-1+gt (Yt−Yt′) …(2) (観測方程式) Yt′=Mt・Xt/t-1 …(3) (但し、X :状態変数(推定値)、 Y′:観測値(理論値)、 添字のt/t-1は観測直前のパラメータ値、 t/tは観測直後のパラメータ値。) (カルマンゲイン) gt=(Pt/t-1・Mt T ・(Mt・Pt/t-1・Mt T・+Vt) …(4) (推定誤差共分散行列) <観測直前> Pt+1/t=Φt・Pt/t・Φt T+Ht …(5) <観測直後> Pn =Pn′−gn・Qn・gn T …(6) (但し、Ht :システム誤差共分散行列、 Vt :観測誤差共分散行列。) として、その対角要素(P(i))(iはカルマンフィルタ
の状態変数で、n次のカルマンフィルタであればP
(1),P(2),‥‥,P(n)を求める)が得られるよう
になる。
【0037】しかして、この対角要素(P(i))の算出
後、該カルマンフィルタの状態変数を表わす変数iに初
期値「0」を代入する(ステップS04)。
後、該カルマンフィルタの状態変数を表わす変数iに初
期値「0」を代入する(ステップS04)。
【0038】続いて、この変数iを「+1」更新設定し
(ステップS05)、該対角要素(P(i))で表わされる
値が負数であるか否かによりカルマンフィルタで発散を
生じている慮があるか否かを判断し(ステップS0
6)、該対角要素(P(i))が負数でなければなんら問題
ないものと判断し、次いでこの時の変数iがカルマンフ
ィルタの次数nとはなっていないことを確認して(ステ
ップS07)、再び上記ステップS05からの処理に戻
る。
(ステップS05)、該対角要素(P(i))で表わされる
値が負数であるか否かによりカルマンフィルタで発散を
生じている慮があるか否かを判断し(ステップS0
6)、該対角要素(P(i))が負数でなければなんら問題
ないものと判断し、次いでこの時の変数iがカルマンフ
ィルタの次数nとはなっていないことを確認して(ステ
ップS07)、再び上記ステップS05からの処理に戻
る。
【0039】また、上記ステップS06で推定誤差共分
散行列の対角要素(P(i))が負数となっているものと判
断した場合には、その時点での変数iで表わされるi次
でのエラーカウント数Flag(i)の値を「+1」更
新設定し、その更新設定した値が予め定めた所定値、例
えば「5」となっていないことを確認した上で(ステッ
プS09)、算出した推定誤差共分散行列の対角要素
(P(i))に代えて、充分小さいゲインを表わす予め用意
した正の定数(const.)を代入した上で(ステッ
プS10)、上記ステップS07に進む。
散行列の対角要素(P(i))が負数となっているものと判
断した場合には、その時点での変数iで表わされるi次
でのエラーカウント数Flag(i)の値を「+1」更
新設定し、その更新設定した値が予め定めた所定値、例
えば「5」となっていないことを確認した上で(ステッ
プS09)、算出した推定誤差共分散行列の対角要素
(P(i))に代えて、充分小さいゲインを表わす予め用意
した正の定数(const.)を代入した上で(ステッ
プS10)、上記ステップS07に進む。
【0040】しかして、ステップS07で変数iがカル
マンフィルタの次数nとなったと判断すると、カルマン
フィルタによる推定演算の第2処理Bとして上記コース
アライメントで得た真北・水平基準(ψ0 ,θ0 ,φ0
)の誤差(ψ′,θ′,φ′)を推定演算し(ステッ
プS12)、同様にして前回得た同誤差との差分の絶対
値が予め用意したしきい値Th以下となったか否か、す
なわちこのカルマンフィルタにより算出した推定誤差が
充分収束してきたか否かを判断するもので(ステップS
13)、上記差分の絶対値がしきい値Thより大きく、
まだ算出した推定誤差が充分には収束していないと判断
すると、再び上記ステップS03の推定誤差共分散行列
の対角要素(P(i))の算出を行なう処理に戻る。
マンフィルタの次数nとなったと判断すると、カルマン
フィルタによる推定演算の第2処理Bとして上記コース
アライメントで得た真北・水平基準(ψ0 ,θ0 ,φ0
)の誤差(ψ′,θ′,φ′)を推定演算し(ステッ
プS12)、同様にして前回得た同誤差との差分の絶対
値が予め用意したしきい値Th以下となったか否か、す
なわちこのカルマンフィルタにより算出した推定誤差が
充分収束してきたか否かを判断するもので(ステップS
13)、上記差分の絶対値がしきい値Thより大きく、
まだ算出した推定誤差が充分には収束していないと判断
すると、再び上記ステップS03の推定誤差共分散行列
の対角要素(P(i))の算出を行なう処理に戻る。
【0041】こうしてステップS03からの処理を、コ
ースアライメントで得た真北・水平基準(ψ0 ,θ0 ,
φ0 )の誤差(ψ′,θ′,φ′)が収束するまで繰返
し実行していく。
ースアライメントで得た真北・水平基準(ψ0 ,θ0 ,
φ0 )の誤差(ψ′,θ′,φ′)が収束するまで繰返
し実行していく。
【0042】しかして、上記ステップS09においてi
次でのエラーカウント数Flag(i)の値が予め定め
た所定値、例えば「5」となったと判断した場合には、
同一の次数iで推定誤差共分散行列の対角要素(P(i))
が5回繰返して負数となったこととなり、カルマンフィ
ルタが完全に発散してしまっており、このカルマンフィ
ルタの状態で推定演算を続行する意味が全くないものと
判断して、その時点での推定誤差値によって推定対象と
なるイニシャルアライメントの真北・水平基準(ψ0 ,
θ0 ,φ0 )に対する誤差補正をカルマンフィルタの終
了処理として実行し(ステップS11)、それから再び
上記ステップS02に進んで、カルマンフィルタの初期
化処理からやり直す。
次でのエラーカウント数Flag(i)の値が予め定め
た所定値、例えば「5」となったと判断した場合には、
同一の次数iで推定誤差共分散行列の対角要素(P(i))
が5回繰返して負数となったこととなり、カルマンフィ
ルタが完全に発散してしまっており、このカルマンフィ
ルタの状態で推定演算を続行する意味が全くないものと
判断して、その時点での推定誤差値によって推定対象と
なるイニシャルアライメントの真北・水平基準(ψ0 ,
θ0 ,φ0 )に対する誤差補正をカルマンフィルタの終
了処理として実行し(ステップS11)、それから再び
上記ステップS02に進んで、カルマンフィルタの初期
化処理からやり直す。
【0043】しかして、1次乃至n次に至る各次数iで
エラーカウント数Flag(i)の値が予め定めた所定
値「5」とならないままに、ステップS13でカルマン
フィルタによる推定演算の第2処理Bとして上記コース
アライメントで算出した真北・水平基準(ψ0 ,θ0 ,
φ0 )の誤差(ψ′,θ′,φ′)と前回の同誤差との
差分の絶対値が予め用意したしきい値Th以下となった
と判断した場合、すなわちこのカルマンフィルタにより
算出した推定誤差が充分収束してきたと判断した場合に
は、このとき最終的に算出した誤差誤差(ψ′,θ′,
φ′)を用いてコースアライメントで得た真北・水平基
準(ψ0 ,θ0 ,φ0 )との差を求めて精度の高い真北
・水平基準(ψ,θ,φ)、すなわち航法計算座標を決
定し(ステップS14)、以上でこの図2によるイニシ
ャルアライメント処理を終了して、実際の航法計算モー
ドに移行する。
エラーカウント数Flag(i)の値が予め定めた所定
値「5」とならないままに、ステップS13でカルマン
フィルタによる推定演算の第2処理Bとして上記コース
アライメントで算出した真北・水平基準(ψ0 ,θ0 ,
φ0 )の誤差(ψ′,θ′,φ′)と前回の同誤差との
差分の絶対値が予め用意したしきい値Th以下となった
と判断した場合、すなわちこのカルマンフィルタにより
算出した推定誤差が充分収束してきたと判断した場合に
は、このとき最終的に算出した誤差誤差(ψ′,θ′,
φ′)を用いてコースアライメントで得た真北・水平基
準(ψ0 ,θ0 ,φ0 )との差を求めて精度の高い真北
・水平基準(ψ,θ,φ)、すなわち航法計算座標を決
定し(ステップS14)、以上でこの図2によるイニシ
ャルアライメント処理を終了して、実際の航法計算モー
ドに移行する。
【0044】このように、すべての状態変数に対応する
推定誤差共分散行列の対角要素(P(i))が正定性を喪失
したかどうかを負数となったか否かにより常時モニタし
ながらカルマンフィルタの算出する推定誤差が収束する
のを待つものとした。
推定誤差共分散行列の対角要素(P(i))が正定性を喪失
したかどうかを負数となったか否かにより常時モニタし
ながらカルマンフィルタの算出する推定誤差が収束する
のを待つものとした。
【0045】この場合、推定誤差共分散行列の対角要素
(P(i))が複数となったことで即座にカルマンフィルタ
が発散したと判断するものではなく、ごく小さな正の定
数値に置換して上記モニタを継続する。
(P(i))が複数となったことで即座にカルマンフィルタ
が発散したと判断するものではなく、ごく小さな正の定
数値に置換して上記モニタを継続する。
【0046】このため、推定誤差共分散行列の対角要素
(P(i))が偶発的に負数となっても、上記置換処理によ
りカルマンフィルタがそのまま正常の処理動作に復帰す
ることも多々あるもので、安易な発散判定によりイニシ
ャルアライメント処理に要する時間が頻繁に長くするこ
とを確実に防止している。
(P(i))が偶発的に負数となっても、上記置換処理によ
りカルマンフィルタがそのまま正常の処理動作に復帰す
ることも多々あるもので、安易な発散判定によりイニシ
ャルアライメント処理に要する時間が頻繁に長くするこ
とを確実に防止している。
【0047】しかしながら、推定誤差共分散行列の対角
要素(P(i))の同一要素(次数)で負数であると判断さ
れることが連続ではなくとも所定回数、例えば5回繰返
された場合には、このカルマンフィルタの状態が全体で
バランスを失っている可能性が高く、正常な状態に復帰
できずに発散を生じる可能性が高いものと判定して、そ
の時点でカルマンフィルタで得ている推定値を用いて推
定対象の補正を終了処理として実行し、それから再びカ
ルマンフィルタの初期化を行なって推定演算をやり直
す。
要素(P(i))の同一要素(次数)で負数であると判断さ
れることが連続ではなくとも所定回数、例えば5回繰返
された場合には、このカルマンフィルタの状態が全体で
バランスを失っている可能性が高く、正常な状態に復帰
できずに発散を生じる可能性が高いものと判定して、そ
の時点でカルマンフィルタで得ている推定値を用いて推
定対象の補正を終了処理として実行し、それから再びカ
ルマンフィルタの初期化を行なって推定演算をやり直
す。
【0048】この回数値「5」は経験則等により任意に
可変設定することが可能であり、その回数に基づいて、
カルマンフィルタに続行不可能な発散が生じたと判定し
た際には直ちにその演算を停止させるため、それ以後続
行しても意味のない無駄な推定演算を継続することな
く、次の処理に確実に且つ速やかに移行することができ
る。
可変設定することが可能であり、その回数に基づいて、
カルマンフィルタに続行不可能な発散が生じたと判定し
た際には直ちにその演算を停止させるため、それ以後続
行しても意味のない無駄な推定演算を継続することな
く、次の処理に確実に且つ速やかに移行することができ
る。
【0049】なお、上記図2で示した本発明の実施の一
形態の動作では、発散と判定すると直ちにステップS0
2におけるカルマンフィルタの初期化処理から始めるも
のとしたが、本発明はこれに限ることなく、発散と判断
した時点でカルマンフィルタの終了を行なってから、次
にこのINS10のオペレータにより制御指示器15で
再度のファインアライメントの開始が指示されるまで待
機するものとしても良い。
形態の動作では、発散と判定すると直ちにステップS0
2におけるカルマンフィルタの初期化処理から始めるも
のとしたが、本発明はこれに限ることなく、発散と判断
した時点でカルマンフィルタの終了を行なってから、次
にこのINS10のオペレータにより制御指示器15で
再度のファインアライメントの開始が指示されるまで待
機するものとしても良い。
【0050】さらには、カルマンフィルタに発散が生じ
たからには、例えばこのINS10を搭載した航空機が
静止している飛行場が強風下にあって、航空機の機体に
大きな動揺が生じているなど、なんらかの要因が考えら
れるため、上記ステップS11でカルマンフィルタの終
了処理を行なってから、再度ステップS02でその初期
化処理を行なうまでの待機時間をその要因等に応じて制
御指示器15で任意に可変設定できるものとしてもよ
い。
たからには、例えばこのINS10を搭載した航空機が
静止している飛行場が強風下にあって、航空機の機体に
大きな動揺が生じているなど、なんらかの要因が考えら
れるため、上記ステップS11でカルマンフィルタの終
了処理を行なってから、再度ステップS02でその初期
化処理を行なうまでの待機時間をその要因等に応じて制
御指示器15で任意に可変設定できるものとしてもよ
い。
【0051】その場合、カルマンフィルタに発散を生じ
た原因に対応して、発散を生じずにカルマンフィルタに
よる推定演算を確実に再度開始させることができるもの
となり、結果としてイニシャルアライメント全体に要す
る時間を最小限に抑えることが可能となる。
た原因に対応して、発散を生じずにカルマンフィルタに
よる推定演算を確実に再度開始させることができるもの
となり、結果としてイニシャルアライメント全体に要す
る時間を最小限に抑えることが可能となる。
【0052】なお、上記実施の形態では、計算処理部1
2が実行する計算処理手順は予め図示しない記録媒体に
よりインストールした制御プログラムにしたがってなさ
れるものとして説明したが、本発明はこれに限らず、例
えばなんらかの通信媒体を介してINS10外部の通信
装置からI/O部13により入力し、計算処理部12に
インストールした制御プログラムにしたがってなされる
ものとしてもよい。
2が実行する計算処理手順は予め図示しない記録媒体に
よりインストールした制御プログラムにしたがってなさ
れるものとして説明したが、本発明はこれに限らず、例
えばなんらかの通信媒体を介してINS10外部の通信
装置からI/O部13により入力し、計算処理部12に
インストールした制御プログラムにしたがってなされる
ものとしてもよい。
【0053】その他、本発明はその要旨を逸脱しない範
囲内で種々変形して実施することが可能であるものとす
る。
囲内で種々変形して実施することが可能であるものとす
る。
【0054】
【発明の効果】請求項1記載の発明によれば、カルマン
フィルタに軽度の発散が生じても、推定誤差共分散行列
の対角要素(P(i))を充分ゲインの小さい所定値に置換
して演算を続行させることができるため、安定した状態
で且つ高速にイニシャルアライメントを行なうことが可
能となる。
フィルタに軽度の発散が生じても、推定誤差共分散行列
の対角要素(P(i))を充分ゲインの小さい所定値に置換
して演算を続行させることができるため、安定した状態
で且つ高速にイニシャルアライメントを行なうことが可
能となる。
【0055】請求項2記載の発明によれば、上記請求項
1記載の発明の効果に加えて、カルマンフィルタに続行
不可能な発散が生じたと確実に判定した際には直ちにそ
の演算を停止させるようにしたため、続行しても意味の
ない無駄な演算を継続することなく、次の処理に速やか
に移行することができる。
1記載の発明の効果に加えて、カルマンフィルタに続行
不可能な発散が生じたと確実に判定した際には直ちにそ
の演算を停止させるようにしたため、続行しても意味の
ない無駄な演算を継続することなく、次の処理に速やか
に移行することができる。
【0056】請求項3記載の発明によれば、上記請求項
2記載の発明の効果に加えて、該カルマンフィルタによ
る推定演算を即座に始めからやり直すようになるため、
発散が生じた場合であってもその時間的な損失を最小限
に抑えることが可能となる。
2記載の発明の効果に加えて、該カルマンフィルタによ
る推定演算を即座に始めからやり直すようになるため、
発散が生じた場合であってもその時間的な損失を最小限
に抑えることが可能となる。
【0057】請求項4記載の発明によれば、上記請求項
3記載の発明の効果に加えて、カルマンフィルタに発散
を生じた原因に対応して次にカルマンフィルタによる推
定演算を始めからやり直すまでの待機時間を適宜設定で
きるようになるため、確実に発散を生じずにカルマンフ
ィルタによる推定演算を再度開始させることができるも
のとなり、結果としてイニシャルアライメント全体に要
する時間を最小限に抑えることが可能となる。
3記載の発明の効果に加えて、カルマンフィルタに発散
を生じた原因に対応して次にカルマンフィルタによる推
定演算を始めからやり直すまでの待機時間を適宜設定で
きるようになるため、確実に発散を生じずにカルマンフ
ィルタによる推定演算を再度開始させることができるも
のとなり、結果としてイニシャルアライメント全体に要
する時間を最小限に抑えることが可能となる。
【0058】請求項5記載の発明によれば、カルマンフ
ィルタに軽度の発散が生じても、推定誤差共分散行列の
対角要素(P(i))を充分ゲインの小さい所定値に置換し
て演算を続行させることができるため、安定した状態で
且つ高速にイニシャルアライメントを行なわせることが
可能となる。
ィルタに軽度の発散が生じても、推定誤差共分散行列の
対角要素(P(i))を充分ゲインの小さい所定値に置換し
て演算を続行させることができるため、安定した状態で
且つ高速にイニシャルアライメントを行なわせることが
可能となる。
【0059】請求項6記載の発明によれば、カルマンフ
ィルタに軽度の発散が生じても、推定誤差共分散行列の
対角要素(P(i))を充分ゲインの小さい所定値に置換し
て演算を続行させることができるため、安定した状態で
且つ高速にイニシャルアライメントを行なわせることが
可能となる。
ィルタに軽度の発散が生じても、推定誤差共分散行列の
対角要素(P(i))を充分ゲインの小さい所定値に置換し
て演算を続行させることができるため、安定した状態で
且つ高速にイニシャルアライメントを行なわせることが
可能となる。
【図1】本発明の実施の一形態に係るINSの回路構成
を示すブロック図。
を示すブロック図。
【図2】同実施の形態に係るINSのイニシャルアライ
メント処理の内容を示すフローチャート。
メント処理の内容を示すフローチャート。
【図3】一般的なイニシャルアライメントの処理の流れ
を示す図。
を示す図。
10…慣性航法装置(INS) 11…センサ部 12…計算処理部 13…I/O部 14…モード制御器 15…制御指示器 16…電源 17…慣性航法部
Claims (6)
- 【請求項1】イニシャルアライメント処理時にカルマン
フィルタを用いて基準座標に対する誤差を推定する演算
を実行する慣性航法装置において、 上記カルマンフィルタでの推定誤差共分散行列の対角要
素(P(i))の正定性をモニタするモニタ手段と、 このモニタ手段により推定誤差共分散行列の対角要素
(P(i))の正定性が喪失された場合に該対角要素を所定
値に置換して演算を続行させる制御手段とを具備したこ
とを特徴とする慣性航法装置。 - 【請求項2】上記制御手段は、該対角要素を所定値に置
換して演算を続行させる回数が制限値に達すると演算を
停止することを特徴とする請求項1記載の慣性航法装
置。 - 【請求項3】上記制御手段は、該対角要素を所定値に置
換して演算を続行させる回数が制限値に達して演算を停
止した後、カルマンフィルタを初期化して演算を再実行
させることを特徴とする請求項2記載の慣性航法装置。 - 【請求項4】上記制御手段により演算を停止してから、
次にカルマンフィルタを初期化して演算を再実行させる
までの時間を設定する設定手段をさらに具備したことを
特徴とする請求項3記載の慣性航法装置。 - 【請求項5】航行前のイニシャルアライメント処理時に
カルマンフィルタを用いた基準座標に対する誤差を推定
する演算を実行する慣性航法装置の初期化方法であっ
て、 上記カルマンフィルタでの推定誤差共分散行列の対角要
素(P(i))の正定性をモニタし、 推定誤差共分散行列の対角要素(P(i))の正定性が喪失
された場合に該対角要素を所定値に置換して演算を続行
させることを特徴とする慣性航法装置の初期化方法。 - 【請求項6】航行前のイニシャルアライメント処理時に
カルマンフィルタを用いた基準座標に対する誤差を推定
する演算を実行する慣性航法装置で使用される記録媒体
であって、 上記カルマンフィルタでの推定誤差共分散行列の対角要
素(P(i))の正定性をモニタするモニタ工程と、 このモニタ工程で推定誤差共分散行列の対角要素(P
(i))の正定性が喪失された場合に該対角要素を所定値に
置換して演算を続行させる制御工程とをコンピュータに
実行させる命令を含む制御プログラムを格納したことを
特徴とする、コンピュータが読取り可能な記録媒体。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2000079871A JP2001264105A (ja) | 2000-03-22 | 2000-03-22 | 慣性航法装置、慣性航法装置の初期化方法及び記録媒体 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2000079871A JP2001264105A (ja) | 2000-03-22 | 2000-03-22 | 慣性航法装置、慣性航法装置の初期化方法及び記録媒体 |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2001264105A true JP2001264105A (ja) | 2001-09-26 |
Family
ID=18597059
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2000079871A Pending JP2001264105A (ja) | 2000-03-22 | 2000-03-22 | 慣性航法装置、慣性航法装置の初期化方法及び記録媒体 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2001264105A (ja) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004229282A (ja) * | 2003-01-21 | 2004-08-12 | Samsung Electronics Co Ltd | タップ係数更新方法及びタップ係数更新回路 |
KR101080831B1 (ko) | 2009-09-02 | 2011-11-07 | 국방과학연구소 | 지상항법장치에서의 방위각 정보 제공방법 |
CN108663040A (zh) * | 2017-03-28 | 2018-10-16 | 精工爱普生株式会社 | 信号处理装置、检测装置、测定装置、电子设备及移动体 |
JP2018173067A (ja) * | 2017-03-31 | 2018-11-08 | ダイハツ工業株式会社 | 内燃機関の制御装置 |
JP2021167727A (ja) * | 2020-04-08 | 2021-10-21 | 株式会社デンソー | 車両用測位装置および車両用測位装置用のプログラム |
CN113916222A (zh) * | 2021-09-15 | 2022-01-11 | 北京自动化控制设备研究所 | 基于卡尔曼滤波估计方差约束的组合导航方法 |
-
2000
- 2000-03-22 JP JP2000079871A patent/JP2001264105A/ja active Pending
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004229282A (ja) * | 2003-01-21 | 2004-08-12 | Samsung Electronics Co Ltd | タップ係数更新方法及びタップ係数更新回路 |
JP4681813B2 (ja) * | 2003-01-21 | 2011-05-11 | 三星電子株式会社 | タップ係数更新方法及びタップ係数更新回路 |
KR101080831B1 (ko) | 2009-09-02 | 2011-11-07 | 국방과학연구소 | 지상항법장치에서의 방위각 정보 제공방법 |
CN108663040A (zh) * | 2017-03-28 | 2018-10-16 | 精工爱普生株式会社 | 信号处理装置、检测装置、测定装置、电子设备及移动体 |
JP2018173067A (ja) * | 2017-03-31 | 2018-11-08 | ダイハツ工業株式会社 | 内燃機関の制御装置 |
JP2021167727A (ja) * | 2020-04-08 | 2021-10-21 | 株式会社デンソー | 車両用測位装置および車両用測位装置用のプログラム |
JP7363651B2 (ja) | 2020-04-08 | 2023-10-18 | 株式会社デンソー | 車両用測位装置および車両用測位装置用のプログラム |
CN113916222A (zh) * | 2021-09-15 | 2022-01-11 | 北京自动化控制设备研究所 | 基于卡尔曼滤波估计方差约束的组合导航方法 |
CN113916222B (zh) * | 2021-09-15 | 2023-10-13 | 北京自动化控制设备研究所 | 基于卡尔曼滤波估计方差约束的组合导航方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5982213B2 (ja) | 航空機の飛行パラメータを決定する方法およびシステム | |
US8412389B2 (en) | Integrity monitoring of inertial reference unit | |
US11120562B2 (en) | Posture estimation method, posture estimation apparatus and computer readable storage medium | |
WO2022063120A1 (zh) | 组合导航***初始化方法、装置、介质及电子设备 | |
WO2022007602A1 (zh) | 确定车辆位置的方法和装置 | |
CN108592917B (zh) | 一种基于失准角的卡尔曼滤波姿态估计方法 | |
JP3726884B2 (ja) | 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム | |
CN112946681B (zh) | 融合组合导航信息的激光雷达定位方法 | |
JP2001264105A (ja) | 慣性航法装置、慣性航法装置の初期化方法及び記録媒体 | |
WO2017141469A1 (ja) | 位置推定装置 | |
CN116301058B (zh) | 一种无人飞行反馈非线性偏航控制方法、***和设备 | |
CN110209185B (zh) | 一种利用星敏信息姿态重置的航天器姿态机动稳定方法 | |
JP2008143398A (ja) | 飛翔体制御システムおよび飛翔体の飛行制御方法 | |
JP2001264106A (ja) | 慣性航法装置、慣性航法装置の初期化方法及び記録媒体 | |
JP2004305805A (ja) | 散布制御装置及び散布装置の制御方法 | |
EP4227214A1 (en) | Aircraft control system, aircraft, aircraft control method, and program | |
Laupré et al. | Reliable strategies for implementing model-based navigation on fixed-wing drones | |
JP2009294009A (ja) | 航法装置、航法装置の姿勢角計算方法および姿勢角計算プログラム | |
US11209457B2 (en) | Abnormality detection device, abnormality detection method, and non-transitory tangible computer readable medium | |
JPH0597096A (ja) | 航空機の校正対気速度制御と航空機のマツハ数制御との間の遷移をスムーズにする方法及び装置 | |
JP2001264104A (ja) | 慣性航法装置、慣性航法装置の初期化方法及び記録媒体 | |
CN114019954A (zh) | 航向安装角标定方法、装置、计算机设备和存储介质 | |
JP3919911B2 (ja) | 方位姿勢基準装置 | |
US7576684B2 (en) | Integrated attitude altimeter | |
EP4379322A1 (en) | Utilization of sensor error flags for data estimation and data reliability estimation in inertial navigation |