(57)【特許請求の範囲】
【請求項１】
電波航法装置と慣性航法装置の測定値から、カルマンフィルタにて慣性航法装置の測定誤差を推定し、補正するハイブリッド航法において、慣性航法装置の測定誤差の生成過程をランジュバン方程式でモデル化した、測定誤差を状態変数とするベクトルＸ、相関時定数により定まるシステム行列Ａおよびシステム雑音ｕによるシステム方程式を離散時間系に変換した式Ｘk ＝Ａk-1 Ｘk-1 ＋ｕk-1 （ｋは時間ステップ）を離散系システム方程式とし、
電波航法装置の有色雑音である測定誤差をマルコフ過程でモデル化して観測雑音を白色雑音とした、前記ベクトルＸ、観測行列Ｈおよび観測雑音ｗによる観測方程式を離散時間系に変換した式Ｚk ＝Ｈk Ｘk ＋ｗk （ｋは時間ステップ）を離散系観測方程式とし、
前記離散系システム方程式と前記離散系観測方程式とから構成したカルマンフィルタを用いたハイブリッド航法。
【請求項２】
電波航法装置と慣性航法装置の測定値をそれぞれ入力し、カルマンフィルタにて慣性航法装置の測定誤差を推定し、補正するハイブリッド航法装置において、
慣性航法装置の測定誤差の生成過程をランジュバン方程式でモデル化した、測定誤差を状態変数とするベクトルＸ、相関時定数により定まるシステム行列Ａおよびシステム雑音ｕによるシステム方程式を離散時間系に変換した式Ｘk ＝Ａk-1 Ｘk-1 ＋ｕk-1 （ｋは時間ステップ）を離散系システム方程式とし、
電波航法装置の有色雑音である測定誤差をマルコフ過程でモデル化して観測雑音を白色雑音とした、前記ベクトルＸ、観測行列Ｈおよび観測雑音ｗによる観測方程式を離散時間系に変換した式Ｚk ＝Ｈk Ｘk ＋ｗk （ｋは時間ステップ）を離散系観測方程式とし、
前記離散系システム方程式と前記離散系観測方程式とから前記カルマンフィルタを構成したハイブリッド航法装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、電波航法と慣性航法とを組み合わせたハイブリッド航法装置に関するものである。
【０００２】
【従来の技術】
従来、航空機や船舶の航法装置として、複数種の航法手段を組み合わせて、それぞれの欠点を補うことにより、位置や速度などの測定精度を高めたいわゆるハイブリッド航法装置が採用されている。また、測定された時系列データから現在の状態（位置や速度等の測定誤差）を推測するためにカルマンフィルタが用いられている。
【０００３】
例えば、「村田、新宮、小野、張替；日本宇宙技術研究所におけるＧＰＳ航法飛行実験について，日本航空宇宙学会誌，第41巻，第468 号，1991年 1月」には、全世界測位システム（ＧＰＳ）と慣性航法システム（ＩＮＳ）とのハイブリッド航法に関して報告されている。このようなＧＰＳとＩＮＳとのハイブリッド航法では、ＧＰＳとＩＮＳの位置情報または位置と速度の情報をカルマンフィルタに入力し、状態変数であるＩＮＳの位置（２または３箇）、速度（２または３箇）、加速度（２または３箇）、方位（１箇）、姿勢（２箇）、ジャイロ（３箇）の誤差を最適推定し、ＩＮＳの位置、速度、方位、姿勢を最適推定した誤差で補正して高精度の位置、速度、方位、姿勢の情報を出力するように構成されている。
【０００４】
【発明が解決しようとする課題】
図３はカルマンフィルタの誤差推定の３つのタイプについて、実際の誤差と推定誤差の変移の例を示している。ここで、縦軸は誤差の２乗平均値、横軸は経過時間である。ハイブリッド航法による位置や速度の精度はカルマンフィルタの誤差の推定精度に依存している。カルマンフィルタで誤差の最適推定値を得るためには、図３の（Ａ）に示すように、推定誤差を実際の誤差より少なく見積もるアンダーエスチメイトの状態となったり、（Ｂ）に示すように、推定誤差を実際の誤差より大きく見積もるオーバーエスチメイトの状態とならないように、すなわち（Ｃ）に示すように、推定誤差が実際の誤差に近似するようにカルマンフィルタを適切にチューニングする必要がある。
【０００５】
しかし、上述のとおり、ハイブリッド航法に使われる従来のカルマンフィルタの状態変数の箇数は１２以上もあり、しかも例えば速度誤差と方位および姿勢誤差との間、方位および姿勢誤差と位置誤差との間、方位と姿勢誤差との間などには相互依存の関係があるため、慣性航法装置の測定誤差の生成過程を表すシステム行列の複雑化に伴ってシステム方程式も複雑になる。
【０００６】
このように、従来のハイブリッド航法装置においては、システム方程式が複雑であるため、カルマンフィルタが誤差を最適推定できるように、カルマンフィルタをチューニングすることが煩雑であった。
【０００７】
また、一般に、ＧＰＳ受信機等の電波航法装置においては、信号処理に伴って測定誤差は自己相関を持つ有色雑音となる。したがって、観測方程式に含まれる観測雑音は有色雑音となってしまう。
【０００８】
しかし、カルマンフィルタの理論では、観測雑音が白色雑音であることと、正則（観測雑音の共分散行列の逆行列が存在する。）であることが前提になっている。そこで従来は、観測雑音が有色雑音である場合に、上記前提を満たすために、カルマンフィルタの更新周期を観測雑音の相関時定数より長く設定することによって、観測雑音が実質上白色雑音と見なせるようにしている。
【０００９】
ところが、カルマンフィルタの更新周期が長くなると、位置や速度の補正がその更新周期でしかできなくなってしまう。
【００１０】
このように、従来のハイブリッド航法装置においては、信号処理の結果、観測雑音が一般に有色雑音となるため、カルマンフィルタの更新周期は観測雑音の相関時定数に制限され、更新周期が長くなり、誤差推定の精度も劣化してしまう。
【００１１】
この発明の目的は、システム方程式の複雑化によるカルマンフィルタのチューニングの煩雑化の問題を解消し、また電波航法装置における信号処理による雑音の有色化に伴うカルマンフィルタの更新周期の制限の問題を解消したハイブリッド航法装置を提供することにある。
【００１２】
【課題を解決するための手段】
この発明は、電波航法装置と慣性航法装置による観測値をそれぞれ入力し、カルマンフィルタにて慣性航法装置の観測値を補正するハイブリッド航法装置であり、
慣性航法装置の測定誤差の生成過程をランジュバン方程式でモデル化した、測定誤差を状態変数とするベクトルＸ、相関時定数により定まるシステム行列Ａおよびシステム雑音ｕによるシステム方程式を離散時間系に変換した式Ｘk ＝Ａk-1 Ｘk-1 ＋ｕk-1 （ｋは時間ステップ）を離散系システム方程式とし、
電波航法装置の有色雑音である測定誤差をマルコフ過程でモデル化して観測雑音を白色雑音とした、前記ベクトルＸ、観測行列Ｈおよび観測雑音ｗによる観測方程式を離散時間系に変換した式Ｚk ＝Ｈk Ｘk ＋ｗk （ｋは時間ステップ）を離散系観測方程式とし、
前記離散系システム方程式と前記離散系観測方程式とから構成したカルマンフィルタを用いる。
【００１３】
このように、システム方程式をランジュバン方程式でモデル化し単純化することによりチューニングを簡単にする。また、観測方程式に含まれる雑音を白色雑音化することにより、カルマンフィルタの更新周期を短くできるようにする。
【００１４】
【発明の実施の形態】
図１はこの発明に実施形態であるハイブリッド航法装置の構成を示すブロック図である。ここで、ＩＮＳ１はジャイロと加速度計を用いて、それらが装備された航空機の位置を速度等を求める慣性航法装置、ＧＰＳ受信機２はＧＰＳ衛星からの電波を受信して航空機の位置と速度を測定する装置である。３はハイブリッド航法装置であり、ＩＮＳ１とＧＰＳ受信機２からそれぞれ位置と速度の情報を入力し、ＩＮＳの位置と速度の誤差を推定し、この推定した誤差でＩＮＳの位置と速度を補正することによって、高精度の位置と速度情報を出力する。
【００１５】
ハイブリッド航法装置３におけるカルマンフィルタ４は、ＩＮＳ航法装置１とＧＰＳ受信機２から入力された、それぞれの位置と速度の情報からＩＮＳの位置と速度の誤差を最適推定する。
【００１６】
説明を簡単にするために、２次元の場合について、従来の欠点を解消したハイブリッド航法装置について説明する。
【００１７】
〔１〕  システム方程式
前述のように、従来はシステム方程式が複雑であったため、カルマンフィルタのチューニングが煩雑となっていた。この発明ではまず、システム方程式を単純化することによりチューニングを簡単にする。
【００１８】
ＩＮＳの位置誤差はジャイロと加速度計の誤差が要因となって発生する。そのうちの７０％以上がジャイロの誤差に依存しており、加速度計の誤差の影響は少ない。このことは「早川、星野；慣性航法装置の慣性素子，日本航空宇宙学会誌，Vol.31，1983年 6月」に示されている。ＩＮＳの位置誤差の分散とジャイロの誤差の関係は〔数１〕で与えられる。この関係は「Diesel J.W；ＧＰＳ／ＩＮＳINTEGRATION BY FUNCTIONAL PARTITIONING」に示されている。
【００１９】
【数１】
0004145451.tif 000002
【００２０】
〔数１〕において、ＩＮＳの位置誤差の分散Ｑ(t) は、シューラ周期８４．４分の１／４周期以上の時間が経過すると、時間に比例する項が振動項よりも優勢になり、１シューラ周期以降は、時間に比例する項が振動項の５倍以上になって、振動項が無視できる。すなわち、１シューラ周期以降は、ＩＮＳの位置誤差の分散は時間に略比例して大きくなっていく。
【００２１】
このことは、ＩＮＳの位置誤差がランジュバン（Langevin）方程式でモデル化できることを示している。
【００２２】
ＩＮＳの誤差を状態変数とするベクトルを〔数２〕で表し、ランジュバン方程式でモデル化すると、Ａをシステム行列、ｕをシステム雑音としてシステム方程式は〔数３〕で表される。ただし、Ｔは転置を示す。
【００２３】
【数２】
0004145451.tif 000003
【００２４】
【数３】
0004145451.tif 000004
【００２５】
このように、ＩＮＳの位置誤差をランジュバン方程式でモデル化することにより、システム方程式は極めて単純化できた。相関時定数τ1 と北方向および東方向の加速度誤差ｕn ＝ｕe はシステム方程式の設計要素であり、システムの設計により定まる。なお、〔数３〕でＸ′と示したように、微分を意味する′（ドット）付き記号を以下２文字で表す。
【００２６】
〔２〕  観測方程式
観測値ベクトルを（ＩＮＳの位置）−（ＧＰＳの位置）とすると、観測方程式は〔数４〕で表すことができる。
【００２７】
【数４】
0004145451.tif 000005
【００２８】
前述のように、ＧＰＳ受信機の測位誤差は、受信信号に含まれるマルコフ性の電離層遅れにより、また、受信信号に含まれる擬似雑音符号を位相追尾して測定位置を更新する処理等を行うことにより、自己相関を持つ有色雑音になり、したがって、観測方程式に含まれる観測雑音は有色雑音となってしまう。
【００２９】
観測雑音が有色雑音である場合、従来なら、前述したように、カルマンフィルタの更新周期を相関時定数より短くすることはできなかったが、この発明ではこの制限を除くために、有色雑音を確率過程でモデル化することにより、カルマンフィルタの理論の前提を満たす表示形式に〔数４〕を変形する。
【００３０】
説明を簡単にするために、ＧＰＳの北方向および東方向の位置誤差（νn ，νe ）が、分散σ32、相関時定数τ2 をもつマルコフ過程（１次マルコフ過程）でモデル化した場合について説明する。
【００３１】
この場合、北方向および東方向の位置誤差νn ，νe の自己相関関数は
【００３２】
【数５】
0004145451.tif 000006
【００３３】
で表され、観測雑音νは、ウィナーヒンチンの自己相関関数とパワースペクトルとの関係より、〔数６〕のように、白色雑音ｗで駆動される方程式で表すことができる。
【００３４】
【数６】
0004145451.tif 000007
【００３５】
次に、〔数４〕を微分し、〔数３〕、〔数４〕、〔数６〕を使って整理すると、〔数７〕が得られる。
【００３６】
【数７】
0004145451.tif 000008
【００３７】
〔数７〕でＨ＝ＣＡ−ＦＣ，Ｚ＝Ｙ′−ＦＹとおくと、〔数７〕は〔数８〕になる。
【００３８】
【数８】
0004145451.tif 000009
【００３９】
上記Ｙ′はＩＮＳとＧＰＳの速度差から観測でき、新たにＺを観測値、Ｈを観測行列として、〔数８〕を観測方程式とすれば、〔数８〕は、観測雑音が白色雑音ｗであるので、カルマンフィルタの理論の前提を満たす表示形式に変形した観測方程式になる。ここで相関時定数τ2 と観測雑音ｗn ＝ｗe は観測方程式の設計要素であり、設計により定まる。例えば、ＧＰＳ受信機の測定位置の誤差を収集し、誤差の自己相関関数を求めることにより、τ2 ，ｗn ，ｗe を決定する。
【００４０】
〔３〕  カルマンフィルタ
〔数３〕のシステム方程式と〔数８〕の観測方程式を連続時間系から更新周期Δの離散時間系に変換すると、システム方程式は〔数９〕となり、観測方程式は〔数１０〕となる。ここで、ｋは０以上の整数であり、ｋ番目の時刻はΔｋで表される。この連続時間系から離散時間系の変換については「有本著；カルマン・フィルタ，産業図書，昭和52年」に示されている。
【００４１】
【数９】
0004145451.tif 000010
【００４２】
【数１０】
0004145451.tif 000011
【００４３】
システム方程式と観測方程式からカルマンフィルタの式のパラメータを決定する方法は周知技術であるので、ここではシステム方程式〔数９〕と観測方程式〔数１０〕から導いたカルマンフィルタの式のみを〔数１１〕〜〔数１５〕に示す。
【００４４】
伝播式
【００４５】
【数１１】
0004145451.tif 000012
【００４６】
このように、推定値を意味する^ （ハット）付き記号を以下２文字で表す。
【００４７】
【数１２】
0004145451.tif 000013
【００４８】
更新式
【００４９】
【数１３】
0004145451.tif 000014
【００５０】
【数１４】
0004145451.tif 000015
【００５１】
【数１５】
0004145451.tif 000016
【００５２】
以上に述べたように、このカルマンフィルタは、状態変数をランジュバン方程式でモデル化することにより、単純化したシステム方程式と、有色の観測雑音を１次マルコフ過程でモデル化することにより、白色の観測雑音をもつ表示形式に変形した観測方程式とを基にして構成されている。
【００５３】
なお、観測雑音が白色雑音と有色雑音を含む場合は、有色雑音をシステム方程式に組み込むことにより、前述のカルマンフィルタと同様のカルマンフィルタが構成できる。
【００５４】
次に、図１に示したハイブリッド航法装置３の処理手順の一例を、図２を参照して説明する。
先ず、上記システム方程式のＡk=0 ，Ｕk=0 、観測方程式のＨk=0 ，Ｗk=0 を設定する。Ａk=0 は〔数９〕で示したシステム行列であり、相関時定数τ1 で定まり、Ｕk=0 は同じく〔数９〕で示した加速度誤差ｕの共分散行列にΔを乗じたものであり、これらの値は設計要素である。また、Ｈk=0 は〔数１０〕に示した観測行列であり、相関時定数τ2 で定まり、Ｗk=0 は〔数６〕で示した白色雑音ｗの共分散行列に１／Δを乗じたものであり、これらの値も設計要素である。
【００５５】
次に、推定値の初期値Ｘ^ k=0 と共分散の初期値Ｐk=0 を設定する。
【００５６】
その後、更新タイミングとなれば、ＧＰＳ受信機２より北方向の位置ＸN ，東方向の位置ＸE ，北方向の速度ＶN ，東方向の速度ＶE をそれぞれ読み込む。また、ＩＮＳ航法装置１より北方向の位置ＸCN，東方向の位置ＸCE，北方向の速度ＶCN，東方向の速度ＶCEをそれぞれ読み込む。
【００５７】
続いて、
【００５８】
【数１６】
0004145451.tif 000017
【００５９】
をＺk として求める。また、事前推定値Ｘ^ k/k-1 を〔数１１〕から求める。ここで〔数１１〕のＡk は〔数９〕に示したように、ｋに係わらず一定であり、上記設定値Ａk=0 を用いる。
【００６０】
続いて、事前共分散Ｐk/k-1 を〔数１２〕から求める。ここでＵk は〔数９〕に示したように北方向および東方向の加速度誤差ｕn ，ｕe で決定されるので、ｋに係わらず一定であり、上記設定値Ｕk=0 を用いる。
【００６１】
続いて、フィルタゲインＧk を〔数１３〕から求める。ここでＨk は〔数１０〕に示したようにｋに係わらず一定であり、上記設定値Ｈk=0 を用いる。また、Ｗk も〔数１０〕に示したように、ｋに係わらず一定であり、上記設定値Ｗk=0 を用いる。
【００６２】
さらに、続いて、事後共分散Ｐk/k を〔数１５〕から求める。
そして、事後推定値Ｘk/k を〔数１４〕から求め、これを出力する。
【００６３】
以上の処理を更新タイミング毎に繰り返すことにより、推定誤差が急速に減少し、図１に示したハイブリッド航法装置３から、誤差分の補正された位置および速度のデータが出力される。
【００６４】
なお、〔数１１〕，〔数１２〕に示した伝播式の更新周期をΔ／Ｎ（Ｎ：２以上の整数）として、カルマンフィルタの更新周期（誤差推定周期）より短時間周期で、ＩＮＳの位置と速度の推定誤差を、その推移を補外することにより求め、ＩＮＳの位置と速度を補正してもよい。
【００６５】
また、以上に述べた例は、２次元の位置と速度を求めるものであったが、同様にして３次元の位置と速度を求めることもできる。
【００６６】
【発明の効果】
この発明によれば、高精度の位置と速度情報が得られるほか、次の利点がある。
(1) システム方程式をランジュバン方程式でモデル化することにより、システム方程式形式が単純となるため、設計要素が少なくなって、カルマンフィルタのチューニングが容易になる。また、システム方程式の形式が単純となるため、カルマンフィルタのチューニング時のシミュレーションと実際の環境とが少し異なっていても、実際の運用時に大きな推定誤差が生じることがなく、いわゆるロバスト性も高まる。
【００６７】
(2) 有色の観測雑音を１次マルコフ過程でモデル化し、観測方程式を白色の観測雑音をもつ表示形式に変形することにより、カルマンフィルタの更新周期は、観測雑音の相関時定数に制限されることなく、任意に設定できる。そのため、カルマンフィルタの更新周期を短くでき、誤差推定の精度が劣化することもない。
【００６８】
(3) 設計要素はプラットフォーム方式のＩＮＳと、ストラップダウン方式のＩＮＳとで異なるが、カルマンフィルタのアルゴリズムは同一となるので、カルマンフィルタの演算処理部分を変えることなく、いずれのタイプの慣性航法装置を用いたハイブリッド航法装置にも適用できる。
【図面の簡単な説明】
【図１】この発明の実施形態に係るハイブリッド航法装置の構成を示す図
【図２】同ハイブリッド航法装置における処理手順を示すフローチャート
【図３】カルマンフィルタのチューニング結果の例を示す図
【符号の説明】
１−ＩＮＳ航法装置
２−ＧＰＳ受信機
３−ハイブリッド航法装置
４−カルマンフィルタ
５−加算器
【図１】
0004145451.tif 000018
【図２】
0004145451.tif 000019
【図３】
0004145451.tif 000020