(57)【特許請求の範囲】
【請求項１】
衛星航法装置により得られる測定値と、慣性航法装置により得られる測定値とから、カルマンフィルタにて慣性航法装置の測定誤差を推定して、補正するハイブリッド航法装置において、
前記衛星航法装置により得られる位置、速度および加速度と、前記慣性航法装置により得られる風速および風向とを用いて前記衛星航法装置により得られる速度のベクトル成分を、前記慣性航法装置により得られる風速及び風向から算出される風速ベクトルの各成分で差分補正し、該差分補正した前記速度の各ベクトル成分に基づいて前記衛星航法装置により得られる姿勢角および方位角を算出するとともに、前記衛星航法装置により測定および算出された位置、速度、加速度、姿勢角、および方位角と、前記慣性航法装置より得られる位置、速度、加速度、姿勢角、および方位角との各差分値を算出して観測値として出力する観測値演算手段と、
該観測値演算手段で出力された前記観測値に基づいて、前記慣性航法装置により得られる位置、速度、姿勢角、方位角の推定誤差を出力するカルマンフィルタと、
該カルマンフィルタにより出力された推定誤差に基づいて、前記慣性航法装置から出力された位置、速度、姿勢角、および方位角を補正して出力する補正手段と、を備えたことを特徴とするハイブリッド航法装置。
【請求項２】
前記衛星航法装置には、少なくとも二つのアンテナが設置されており、該衛星航法装置にて、姿勢角および方位角を算出し、
前記観測値演算手段は、少なくとも、前記衛星航法装置により得られる姿勢角および方位角と前記慣性航法装置により得られる姿勢角および方位角とを入力し、前記衛星航法装置により得れらる測定値と、前記慣性航法装置より得られる測定値との差を算出して観測値として出力し、
前記カルマンフィルタは、前記観測値演算手段で出力された前記観測値に基づいて、前記慣性航法装置により得られる姿勢角、および方位角の推定誤差を出力し、
前記補正手段は、前記カルマンフィルタにより出力された推定誤差に基づいて、前記慣性航法装置から出力された姿勢角、および方位角を補正して出力することを特徴とする請求項１に記載のハイブリッド航法装置。
【請求項３】
前記カルマンフィルタは、前記姿勢角の推定誤差および前記方位角の推定誤差を前記慣性航法装置にフィードバックし、
該慣性航法装置は、前記姿勢角の推定誤差および前記方位角の推定誤差に基づいて自身の姿勢を補正する請求項１または請求項２に記載のハイブリッド航法装置。
【請求項４】
前記カルマンフィルタは、前記位置の推定誤差の生成過程にランジュバン方程式を用いた請求項１〜請求項３のいずれかに記載のハイブリッド航法装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、ＧＰＳ受信機等の衛星航法装置を用いた電波航法と慣性航法装置（ＩＮＳ）を用いた慣性航法のそれぞれより得られる測定値に基づいて、機体の位置、速度、姿勢、方位等を得るハイブリッド航法を用いたハイブリッド航法装置に関するものである。
【０００２】
【従来の技術】
従来、航空機や船舶等の航法装置として、複数種類の航法手段を備え、これらを組み合わせることで機体や船体の位置や速度の測定精度を高めるハイブリッド航法装置が各種考案されている。このようなハイブリッド航法装置には、測定された時系列データから現在の位置や速度などの状態を推定するカルマンフィルタが用いられている航法装置がある。
【０００３】
このような従来のハイブリッド航法装置について、図を参照して説明する。
なお、衛星航法装置として、ＧＰＳ受信機を例に説明する。
図３は、ＧＰＳ受信機とＩＮＳ（慣性航法装置）とを組み合わせた従来のハイブリッド航法装置の主要部を示すブロック図である。
図３に示すように、ＧＰＳ受信機２で測定された位置情報および速度情報と、ＩＮＳ１で測定された位置情報および速度情報とをカルマンフィルタ３に入力し、ＩＮＳ１から得られる、位置、速度、方位角および姿勢角等の誤差を最適推定する。そして、ハイブリッド航法装置１０は、最適推定された位置、速度、方位角、および姿勢角等の誤差に基づいてＩＮＳ１より入力される位置、速度、方位角、および姿勢角を補正して出力する。（例えば、特許文献１参照。）。
【０００４】
【特許文献１】
特開２００１−１７４２７５号公報
【０００５】
【発明が解決しようとする課題】
上述のようなハイブリッド航法装置に備えられたカルマンフィルタにおいては、ＩＮＳから入力される測定値とＧＰＳ受信機等の衛星航法装置から入力される測定値との差に基づいて、誤差推定値を算出する。
【０００６】
しかしながら、従来のハイブリッド航法装置では、衛星航法装置から入力される測定値が位置および速度のみであるため、ＩＮＳから入力される測定値が位置、速度、姿勢角、および方位角であっても、カルマンフィルタに入力可能な測定値の差としては、位置と速度のみであった。このため、位置と速度との誤差推定は高精度に行うことができても、姿勢角や方位角については高精度に誤差推定することができなかった。つまり、ハイブリッド航法装置は、ＩＮＳから出力される姿勢角および方位角を高精度に補正し、正確な姿勢角および方位角を提供することができなかった。
【０００７】
この発明の目的は、衛星航法装置とＩＮＳとから入力される測定値に基づいて、姿勢角および方位角を高精度に推定することができるハイブリッド航法装置を提供することにある。
【０００８】
【課題を解決するための手段】
この発明は、衛星航法装置により得られる測定値と、慣性航法装置により得られる測定値とから、カルマンフィルタにて慣性航法装置の測定誤差を推定して、補正するハイブリッド航法装置において、衛星航法装置により得られる位置、速度および加速度と、慣性航法装置により得られる風速および風向とを用いて衛星航法装置により得られる速度のベクトル成分を、慣性航法装置により得られる風速及び風向から算出される風速ベクトルの各成分で差分補正し、該差分補正した速度の各ベクトル成分に基づいて前衛星航法装置により得られる姿勢角および方位角を算出するとともに、衛星航法装置により測定および算出された位置、速度、加速度、姿勢角、および方位角と、慣性航法装置より得られる位置、速度、加速度、姿勢角、および方位角との各差分値を算出して観測値として出力する観測値演算手段と、観測値演算手段で出力された観測値に基づいて、前記慣性航法装置により得られる位置、速度、姿勢角、および方位角の推定誤差を出力するカルマンフィルタと、カルマンフィルタにより出力された推定誤差に基づいて、慣性航法装置から出力された位置、速度、姿勢角、および方位角を補正して出力する補正手段と、を備えたことを特徴としている。
【０００９】
この構成では、観測値演算手段で、衛星航法装置から入力された速度および加速度に基づいて姿勢角および方位角を算出する。この際、慣性航法装置から入力される風速および風向を用いることで算出する。このように衛星航法装置の測定値に基づく姿勢角および方位角と、慣性航法装置により測定された姿勢角および方位角とを用いて、姿勢角および方位角の差分値（観測値）を算出し、これらを他の観測値である位置の差分値、速度の差分値、加速度の差分値とともにカルマンフィルタに出力する。カルマンフィルタは、入力されたこれらの観測値に基づいて誤差推定を行い、補正手段である加算器に出力する。加算器は、ＩＮＳから入力された、位置、速度、加速度、姿勢角、および方位角の各測定値と、カルマンフィルタにより推定された誤差とを加算することで、ＩＮＳからの各測定値の補正を行って出力する。
【００１０】
また、この発明は、衛星航法装置には少なくとも二つのアンテナが設置されており、該衛星航法装置にて姿勢角および方位角を算出し、観測値演算手段は、少なくとも衛星航法装置により得られる姿勢角および方位角と慣性航法装置により得られる姿勢角および方位角とを入力し、衛星航法装置により得れらる測定値と、慣性航法装置より得られる測定値との差を算出して観測値として出力し、カルマンフィルタは、観測値演算手段で出力された観測値に基づいて、慣性航法装置により得られる姿勢角、および方位角の推定誤差を出力し、補正手段は、カルマンフィルタにより出力された推定誤差に基づいて、慣性航法装置から出力された姿勢角、および方位角を補正して出力することを特徴としている。
【００１１】
この構成では、観測値演算手段で、衛星航法装置により測定算出された姿勢角および方位角と、慣性航法装置により測定算出された姿勢角および方位角とを用いて、姿勢角および方位角の差分値（観測値）を算出し、カルマンフィルタに出力する。カルマンフィルタは、入力されたこれらの観測値に基づいて誤差推定を行い、補正手段である加算器に出力する。加算器は、ＩＮＳから入力された姿勢角、および方位角の各測定値と、カルマンフィルタにより推定された誤差とを加算することで、ＩＮＳからの各測定値の補正を行って出力する。
【００１２】
また、この発明は、カルマンフィルタで、姿勢角の推定誤差および方位角の推定誤差を慣性航法装置にフィードバックし、該慣性航法装置は、姿勢角の推定誤差および方位角の推定誤差に基づいて自身の姿勢を補正することを特徴としている。
【００１３】
この構成では、カルマンフィルタで推定された誤差をＩＮＳにフィードバックすることで、ＩＮＳは、フィードバックされた推定誤差に基づいて、プラットホームのアライメントを調整する。そして、観測値演算手段は、前述のような観測値の算出を行い、カルマンフィルタに出力する。カルマンフィルタはこの観測値に基づき、誤差を推定し、加算器に出力するとともに、再度ＩＮＳにも出力する。
【００１４】
また、この発明は、カルマンフィルタで、位置の推定誤差の生成過程にランジュバン方程式を用いたことを特徴としている。
【００１５】
この構成では、ＩＮＳにおける位置誤差が加速度計の誤差に殆ど影響されないことに基づいて、カルマンフィルタにおける演算で加速度に関する誤差を省略する。これにより、カルマンフィルタにおいては、加速度の誤差推定が削減され、位置、速度、姿勢角、および方位角の誤差推定のみを行うこととなる。
【００１６】
【発明の実施の形態】
本発明の実施形態に係るハイブリッド航法装置について図を参照して説明する。なお、本実施形態では、衛星航法装置としてＧＰＳ受信機を例に説明する。
図１は、本発明の実施形態に係るハイブリッド航法装置の主要部を示すブロック図である。
図１において、１はジャイロと加速度計とを備え、位置、速度、加速度、姿勢角、方位角、風速、および風向を計測して出力する、本発明の慣性航法装置に相当するＩＮＳであり、２は受信したＧＰＳ信号に基づいて位置、速度、加速度を出力するＧＰＳ受信機である。
【００１７】
また、ハイブリッド航法装置１０は、ＩＮＳ１およびＧＰＳ受信機２から入力された位置等の各測定値に基づいて、差分値である観測値を演算する観測値演算部４と、入力された観測値に基づいて、ＩＮＳ１で計測された各測定値に誤差を推定するカルマンフィルタ３と、ＩＮＳ１から出力された各測定値とカルマンフィルタ３から出力された各測定値の推定誤差とを入力して、該推定誤差に基づいて前記各測定値を補正して出力する加算器５とを備える。
【００１８】
これらＩＮＳ１、ＧＰＳ受信機２、およびハイブリッド航法装置１０が、航空機等の機体に搭載されている。
【００１９】
（１）システム方程式
本発明に係るハイブリッド航法装置で用いるシステム方程式は、次のように表すことができる。
【００２０】
【数１】
0004199553.tif 000002
【００２１】
数１において、ｘはＩＮＳ１における誤差ベクトル、Ａはシステム行列、ｕは駆動雑音ベクトルであり、各測定値は、それぞれに直交する北方向（Ｎ方向）、東方向（Ｅ方向）、垂直下方向（Ｄ方向）からなるＮＥＤ座標系を用いる。
また、数１において、ａN ，ａE ，ａD はそれぞれＩＮＳで計測されたＮ軸方向、Ｅ軸方向、Ｄ軸方向の加速度、ｖN ，ｖE ，ｖD はそれぞれＩＮＳで計測されたＮ軸方向、Ｅ軸方向、Ｄ軸方向の速度、ΩE は地球の角速度、Ｒは地球の平均半径、λは機体の緯度、τg ，τa は相関時定数である。
【００２２】
ここで、誤差ベクトルｘの各要素は、
【００２３】
【数２】
0004199553.tif 000003
【００２４】
であり、駆動雑音ベクトルの各要素は、
【００２５】
【数３】
0004199553.tif 000004
【００２６】
である。
【００２７】
なお、駆動雑音ベクトルｕおよび相関時定数τg ，τa は設計パラメータであり、システムの設計により定まる。
【００２８】
（２）観測方程式
本発明に係るハイブリッド航法装置で用いる観測方程式は、次のように表すことができる。
【００２９】
【数４】
0004199553.tif 000005
【００３０】
（４）式において、ｙはＩＮＳ１とＧＰＳ受信機２でそれぞれ測定した位置の差、姿勢角の差から計算したチルトの差、および方位角の差を観測値とする観測ベクトル、Ｃは観測行列、ｗは観測雑音ベクトルである。
【００３１】
ここで、観測ベクトルｙの各要素は、
【００３２】
【数５】
0004199553.tif 000006
【００３３】
であり、観測雑音ベクトルｗの各要素は、
【００３４】
【数６】
0004199553.tif 000007
【００３５】
である。
なお、観測雑音ベクトルｗは設計パラメータであり、設計により定まる。
【００３６】
（３）観測値演算部
観測演算部４では、ＧＰＳ受信機２から入力された速度ベクトルｖg と加速度ベクトルａg とに基づいて、機体軸座標系の姿勢角（ロール角φg 、ピッチ角θg ）と方位角ψg とを、次に示す方法で算出する。
【００３７】
前記ＧＰＳ受信機２により測定された地球軸座標系（ＮＥＤ座標系）の速度ベクトルｖg および加速度ベクトルａg は（７）式で表される。
【００３８】
【数７】
0004199553.tif 000008
【００３９】
一方、ＩＮＳ１で測定された風速をｖs 、風向をｖd とすると、風速ベクトルｖw は（８）式で表される。
【００４０】
【数８】
0004199553.tif 000009
【００４１】
安定軸座標系の速度をベクトルｖs とすると、
【００４２】
【数９】
0004199553.tif 000010
【００４３】
と表され、ＧＰＳ受信機２により計測された地球軸座標系の速度ベクトルｖg と風速ベクトルｖw との差で表すことができるので、（１０）式のように表される。
【００４４】
【数１０】
0004199553.tif 000011
【００４５】
安定軸座標系の加速度ベクトルａs は、
【００４６】
【数１１】
0004199553.tif 000012
【００４７】
と表され、速度ベクトルｖs の時間微分であり、風速がほぼ定常であるとすると、加速度ベクトルａs は、（１２）式で表される。
【００４８】
【数１２】
0004199553.tif 000013
【００４９】
次に、安定軸座標系の姿勢角であるピッチ角θs と方位角ψs は（１０）式を用いて、（１３）式および（１４）式で表すことができる。
【００５０】
【数１３】
0004199553.tif 000014
【００５１】
【数１４】
0004199553.tif 000015
【００５２】
一方、ロール角φs は（１２）式を用いて、次に示す方法で求められる。
加速度ベクトルａs の速度ベクトルｖs に対する直交成分をａs n とすると、
【００５３】
【数１５】
0004199553.tif 000016
【００５４】
となり、同様に、重力加速度ベクトルｇの速度ベクトルｖs に対する直交成分をｇn とすると、
【００５５】
【数１６】
0004199553.tif 000017
【００５６】
となる。ここで、揚力加速度ベクトルをＩとし、水平基準ベクトルをｐすると、揚力加速度ベクトルＩと水平基準ベクトルｐのそれぞれは、次のように表される。
【００５７】
【数１７】
0004199553.tif 000018
【００５８】
【数１８】
0004199553.tif 000019
【００５９】
これら揚力加速度ベクトルＩおよび水平基準ベクトルｐを用いると、ロール角φs は次の式により求められる。
【００６０】
【数１９】
0004199553.tif 000020
【００６１】
次に、安定軸座標系で求めた姿勢角θs ，φs および方位角ψs から、ＩＮＳで測定された姿勢角θ，φおよび方位角ψと同じ座標系にするため、機体軸座標系の姿勢角および方位角に変換する。
機体軸座標系のオイラー角を（ψg ，θg ，φg ）とすると、地球軸座標系から機体軸座標系への変換行列Ｒ（ψg ，θg ，φg ）は、次式で表すことができる。
【００６２】
【数２０】
0004199553.tif 000021
【００６３】
地球軸座標系から安定軸座標系への変換行列はＲ（ψs ，θs ，φs ）であり、迎角計からの迎角をαとすると安定軸座標系から機体軸座標系への変換行列はＲ（０，α，０）であるので、変換行列Ｒ（ψg ，θg ，φg ），Ｒ（ψs ，θs ，φs ），Ｒ（０，α，０）には次式の関係が成り立つ。
【００６４】
【数２１】
0004199553.tif 000022
【００６５】
したがって、機体軸座標系のロール角θg 、ピッチ角φg 、方位角ψg はそれぞれ次のように表される。
【００６６】
【数２２】
0004199553.tif 000023
【００６７】
【数２３】
0004199553.tif 000024
【００６８】
【数２４】
0004199553.tif 000025
【００６９】
このようにＧＰＳ受信機１の測定値により求められた姿勢角θg ，φg および方位角ψg と、ＩＮＳにより測定された姿勢角θ，φおよび方位角ψとの差に基づいて、Ｎ軸回りのチルトｙ3 とＥ軸回りのチルトｙ4 とＤ軸回りの方位角ｙ5 とが次式により求められる。
【００７０】
【数２５】
0004199553.tif 000026
【００７１】
また、観測値演算部４は、ＩＮＳ１により測定された位置とＧＰＳ受信機２により測定された位置とに基づき、Ｎ軸方向の位置の差ｙ1 とＥ軸方向の位置の差ｙ2 とを求める。このようにして、観測方程式の観測値ベクトルｙを求める。
【００７２】
上述のＧＰＳ受信機２で測定した速度および加速度から機体の姿勢角（ピッチ角、ロール角）および方位角を求める方法は、「RICHARD P.KORNFIELD, R.JOHN HAHSMAN and JOHN J.DEYST:Single-Antenna GPS-Based Aircraft Attitude Determinetion, Navigation Journal of The Institute of Navigation, Vol.45, No.1, Spring,1998 」に示されている。
【００７３】
（４）カルマンフィルタ
上述のシステム方程式と観測方程式とを離散時間系に変換すると、次式のように表される。
【００７４】
【数２６】
0004199553.tif 000027
【００７５】
【数２７】
0004199553.tif 000028
【００７６】
この離散時間系システム方程式と観測方程式とを用いて周知方法で次に示すカルマンフィルタの各式を導く。
【００７７】
伝播式
【００７８】
【数２８】
0004199553.tif 000029
【００７９】
このように、推定値を意味する＾（ハット）付き記号を以下２文字で表す。
【００８０】
【数２９】
0004199553.tif 000030
【００８１】
更新式
【００８２】
【数３０】
0004199553.tif 000031
【００８３】
【数３１】
0004199553.tif 000032
【００８４】
【数３２】
0004199553.tif 000033
【００８５】
カルマンフィルタ３は、（２８）式〜（３０）式に示したカルマンフィルタの式を用いて、更新周期Ｔｓ毎に観測値演算部４から入力される各観測値に基づいて、誤差推定を行い、加算器５に出力する。ここで、カルマンフィルタ３は、上述のカルマンフィルタの式により求められたＮ軸回りのチルト誤差ｘ5 とＥ軸回りのチルト誤差ｘ6 とに基づいて、次の変換式を用いて姿勢角誤差を算出した後に、加算器５に出力する。
【００８６】
【数３３】
0004199553.tif 000034
【００８７】
（５）加算器
加算器５はＩＮＳ１から直接入力された位置、速度、姿勢角、方位角に、カルマンフィルタ３から入力された位置誤差、速度誤差、姿勢角誤差、方位角誤差をそれぞれ加算する。すなわち、ＩＮＳ１から入力された位置、速度、姿勢角、方位角を、それぞれ位置誤差、速度誤差、姿勢角誤差、方位角誤差で補正し、表示器（図示せず）等に出力する。
【００８８】
このような構成とすることで、カルマンフィルタで姿勢角および方位角の誤差推定を正確に行うことができるので、従来のように位置と速度のみを高精度に得られるだけでなく、姿勢角や方位角についても高精度に得ることができる。
【００８９】
なお、本実施形態では、カルマンフィルタ３で推定した誤差に基づいて、加算器５で、ＩＮＳ１から出力された各測定値を補正する方法を示したが、図２に示すように、カルマンフィルタ３で推定した誤差をＩＮＳ１にフィードバックし、プラットホームのアライメントを調整する。このような構成とすることで、特にジャイロバイアス誤差が１ｄｅｇ／ｈ程度のような、低精度のＩＮＳを用いても誤差が発散することなく、高精度に誤差推定を行うことができる。これにより、低精度であるが安価なＩＮＳを使用できるので、装置全体のコストダウンを行うことができる。
【００９０】
また、本実施形態に示したカルマンフィルタにランジュバン方程式を適用することができる。これは、ＩＮＳの位置誤差はジャイロの誤差と加速度計の誤差が要因となっているが、その殆どがジャイロの誤差であるため、加速度計の誤差を省略することができるからである。これを利用し、ランジュバン方程式で位置誤差の生成過程をモデル化する。
このようにランジュバン方程式で位置誤差の生成過程をモデル化することで、システム方程式の誤差ベクトルから加速度誤差を省略することができる。これにより、この誤差ベクトルをｘ”とすると、システム行列Ａ”、駆動雑音ベクトルｕ”は次式となる。
【００９１】
【数３４】
0004199553.tif 000035
【００９２】
【数３５】
0004199553.tif 000036
【００９３】
この時、システム方程式は、
【００９４】
【数３６】
0004199553.tif 000037
【００９５】
となる。
【００９６】
このシステム方程式と上述の観測方程式（（２）式）とを用いることで、位置誤差、姿勢角誤差、方位角誤差をカルマンフィルタで推定することができる。
【００９７】
このような構成とすることで、ＩＮＳのジャイロの誤差における加速度要因を省略することができるので、位置誤差および速度誤差とジャイロのバイアス誤差とを分離することができる。このため、カルマンフィルタのチューニングは容易になり、位置誤差、姿勢角誤差、方位角誤差等のカルマンフィルタによる誤差推定を容易にすることができる。
【００９８】
なお、上述の実施形態では、ＧＰＳ受信機で位置、速度、および加速度を測定して、ＩＮＳの位置、速度、姿勢角、および方位角の誤差推定に利用する方法を示したが、ＧＰＳ受信機で姿勢角、および方位角を直接算出して、ＩＮＳの姿勢角、および方位角の誤差推定に利用してもよい。
【００９９】
この場合、ＧＰＳ受信機には少なくとも二つのＧＰＳアンテナが備えられており、これらＧＰＳアンテナ間の相対測位を行い、この結果から姿勢角および方位角を算出する。そして、ＧＰＳ受信機で算出された姿勢角および方位角と、ＩＮＳで測定された姿勢角および方位角との差分を求め、カルマンフィルタで誤差推定を行う。
【０１００】
このような構成とすることで、ＧＰＳ受信機およびＩＮＳでそれぞれ直接求められた姿勢角および方位角を用いて、これらの誤差推定を行うことができる。
【０１０１】
また、ＧＰＳ受信機で測定、算出された位置、速度、加速度、姿勢角、および方位角のうちの必要な測定値と、ＩＮＳで測定した位置、速度、加速度、姿勢角、方位角、風向、および風速のうちの必要な測定値とを用いて、これらを組み合わせ、誤差推定を行ってもよい。
【０１０２】
また、上述の実施形態では、プラットホーム方式のＩＮＳについて説明したが、ストラップダウン方式のＩＮＳやＩＲＵ（慣性基準ユニット）についても上述の構成を適用することができる。
【０１０３】
【発明の効果】
この発明によれば、カルマンフィルタで位置誤差、速度誤差とともに、姿勢角誤差および方位角誤差をそれぞれの観測値に基づいて推定するため、高精度に誤差推定を行うことができる。
【０１０４】
また、衛星航法装置および慣性航法装置のそれぞれで少なくとも姿勢角および方位角を直接、測定算出し、これらを用いて各測定値の差分を算出してカルマンフィルタで誤差推定することで、高精度に誤差推定することができる。
【０１０５】
このため、これらの推定誤差を用いてＩＮＳから出力される位置、姿勢角、方位角を補正することで、機体の正確な位置、姿勢、方位を得ることができる。
【０１０６】
また、この発明によれば、カルマンフィルタで推定した位置誤差、速度誤差、姿勢角誤差、および方位角誤差をＩＮＳにフィードバックすることにより、ＧＰＳ受信機から得られる位置、速度の測定値や速度及び加速度から得られる姿勢角、方位角と、ＩＮＳで予め補正された位置、速度、加速度、姿勢角、方位角との差が徐々に小さくなるため、観測演算部からカルマンフィルタに出力される観測値を徐々に小さくすることができる。このように徐々に小さくなる観測値をカルマンフィルタに入力していくことで、誤差が収束しやすくなり、さらに高精度に誤差推定を行うことができる。
【０１０７】
また、この発明によれば、カルマンフィルタにランジュバン方程式を適用することで、加速度誤差を省略することができ、位置誤差および速度誤差とジャイロのバイアス誤差とを分離することができるので、誤差推定を容易にすることができる。
【図面の簡単な説明】
【図１】本発明の実施形態に係るハイブリッド航法装置の主要部を示すブロック図
【図２】他の構成のハイブリッド航法装置の主要部を示すブロック図
【図３】従来のハイブリッド航法装置の主要部を示すブロック図
【符号の説明】
１−ＩＮＳ
２−ＧＰＳ受信機
３−カルマンフィルタ
４−観測値演算部
５−加算器
１０−ハイブリッド航法装置
【図１】
0004199553.tif 000038
【図２】
0004199553.tif 000039
【図３】
0004199553.tif 000040