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