(57)【要約】【課題】単一のGPSアンテナからの受信信号に基づいて算出される位置及び速度と、加速度センサ及び角速度センサで構成される慣性センサ出力を用いて、リアルタイムに移動体のロール、ピッチ及び方位を算出できる姿勢及び方位検出装置を提供することを目的とする。また、GPSの支援が得られない場合においても、ロール、ピッチが提供できる姿勢検出装置装置を提供できる。【解決手段】GPSから得られる位置及び速度と慣性センサ出力から、移動体のロール、ピッチ及び擬似方位を算定する。この算定において、角速度センサを用いて移動体の回転時に生じる横滑り角を検出することによって、擬似方位を補正要望の真方位を求め、誤差最小規範に基づいて、フィルタのパラメータ及びウェートを決定する構造のフィルタによって、ロール、ピッチの雑音を低減する技法を用いて高精度のロール及びピッチを得ている。【選択図】  図２
2008232867.tif 000002
【特許請求の範囲】
【請求項１】
単一GPSアンテナを有するGPS受信機と慣性センサとで構成される移動体の姿勢及び方位検出装置において、
前記GPS受信機出力と前記慣性センサ観測量を用いて、移動体の姿勢及び方位を算定する姿勢演算部と、
前記姿勢演算部出力のロール及び/或いはピッチの雑音を低減するための、誤差最小規範に基づいてフィルタのパラメータ及びウェートを決定する構造のフィルタ部と、
移動体の旋回時の横滑り角を推定し、該横滑り角推定値で前記姿勢演算部出力の方位を補正する横滑り補正部と、
位置、速度、姿勢及び方位を算定する慣性航法演算処理部と、
前記フィルタ部出力の姿勢と慣性航法演算処理部出力の姿勢を比較し、前記横滑り補正部出力の方位と前記慣性航法演算処理部出力の方位を比較する姿勢比較部と、
前記GPS受信機出力の少なくとも速度と前記慣性航法演算処理部出力の少なくとも速度をそれぞれ比較するＰＶ比較部と、
前記姿勢比較部及び前記ＰＶ比較部の出力に基づいて、前記慣性センサ出力誤差及び前記慣性航法演算処理部出力誤差を推定する誤差推定部を備え、
前記誤差推定部で推定された誤差量で前記慣性センサ出力及び慣性航法演算処理部出力を補正することを特徴とする移動体の姿勢及び方位検出装置。
【請求項２】
請求項１に記載の移動体の姿勢及び方位検出装置において、
前記姿勢演算部は、移動体の進行方向が前記ヘディング軸と一致するとの仮定に基づいて前記GPS受信機出力の速度から算定した前記ヘディング軸速度と、該ヘディング軸に直交する成分の前記慣性センサ出力の加速度及び角速度とから移動体のロールを算定し、
前記ヘディング軸成分速度の所定期間における変化から算定した前記ヘディング軸成分加速度と、該ヘディング軸成分の前記慣性センサ出力の加速度と、ローカル重力を計算するための前記GPS受信機出力の位置から移動体のピッチを算定することを特徴とする移動体の姿勢及び方位検出装置。
【請求項３】
請求項１に記載の移動体の姿勢及び方位検出装置において、
前記横滑り角推定値は、前記ヘディング軸に直交する垂直軸回りの角速度の比例値で算出するか、または前記垂直軸回りの角速度が予め決められた閾値を超えたときの前記垂直軸回りの角速度を積分して得た方位と前記姿勢演算部出力の方位との差から算出するかのいずれか、若しくは併用で算出することを特徴とする移動体の姿勢及び方位検出装置。
【請求項４】
請求項２に記載の移動体の姿勢及び方位検出装置において、
前記姿勢演算部のピッチ算定は、前記ヘディング軸成分速度の所定期間における変化から算定した前記ヘディング軸成分加速度を無視して、前記移動体のピッチを算定することを特徴とする移動体の姿勢及び方位検出装置。
【請求項５】
請求項２に記載の移動体の姿勢及び方位検出装置において、
前記姿勢演算部のピッチ算定は、前記ローカル重力を計算するための位置を外部設定入力するか、または無視して、前記移動体のピッチを算定することを特徴とする移動体の姿勢及び方位検出装置。
【請求項６】
請求項２に記載の移動体の姿勢及び方位検出装置において、
前記GPS受信機出力が得られないときは、前記姿勢演算部出力のロール及び/或いはピッチは前記フィルタ部ロール及び/或いはピッチを出力することを特徴とする移動体の姿勢及び方位検出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は単一のGPSアンテナと慣性センサを備えた姿勢及び方位検出装置に関し、特にGPSから得た方位を移動体の回転時に生じる横滑り角を検出することによって要望の真方位を求める装置に関する。
【背景技術】
【０００２】
GPSを用いて姿勢及び方位を決定する手法は、例えば、非特許文献１、非特許文献２等に報告されている。ここで記載されている複数のGPSアンテナを用いたGPS／INS統合化技法はGPS受信機からの得られた姿勢及び方位（ＧＰＳ姿勢及び方位）、位置（GPS位置）、速度（GPS速度）と、慣性センサ出力を用いて慣性航法演算装置（INS）で得られた姿勢及び方位、位置、速度（以下、INS姿勢及び方位、INS位置、INS速度と呼ぶ））を融合することによって、よりロバスト且つ高精度な姿勢及び方位、速度、位置等を得るための技法である。
【０００３】
複数のアンテナで構成される姿勢及び方位検出装置は、基準アンテナ位置に対する他のアンテナ位置で形成される複数の位置ベクトル（基線ベクトル）をキャリア位相差による相対測位技法を用いて決定することにより、姿勢及び方位を算定している。
【０００４】
２個のGPSアンテナ構成ではピッチまたはロールのいずれかと方位が得られ、３個以上のGPSアンテナ構成ではロール、ピッチ、方位が得られる。GPS受信機から得られた姿勢及び方位、位置、速度はINSから得られたそれらと、夫々、比較し、これらの差情報に基づいて、慣性センサ（IMU）観測量誤差及びINS誤差を高精度に推定することができるために、高精度の姿勢及び方位、速度、位置が得られる。
【０００５】
また、非特許文献３は単一のGPSアンテナのみを用いて姿勢及び方位を決定する方法が記述されている。
また、特許文献１記載の技法は単一のGPSアンテナと慣性センサを融合する技法が提唱されている。
【非特許文献１】Cohen, C.E., Attitude determination using GPS: PhD thesis, Stanford University, Department of Aeronautics and  Astronautics, 1992.
【非特許文献２】Gang  Lu,Development of a GPS Multi-Antenna System for Attitude Determination, Gang Lu (Ph.D.thesis), December 1994, UCGE Report No. 20073.
【特許文献１】米国特許第５,４０６,４８９号
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、前記の複数のGPSアンテナを用いたGPS／INS統合化技法は高精度の姿勢及び方位と位置と速度が得られる代わりに、下記の代償を払わなければならない。第一に、姿勢及び方位演算に先立って、キャリア位相差の整数バイアス（Integer ambiguity）を高速且つ正確に決定する必要がある。
第二に、移動体座標系における複数のGPSアンテナ位置が既知であることが要求されるが、現場で計測することは現実的に難しい。第三に、複数のアンテナ構成は装置の小型化、軽量化、低価格化に対して障害となるために用途が限定される。
【０００７】
小型、軽量化、低価格の観点から、単一のGPSアンテナ構成が望まれるが、単一のGPSアンテナを用いたGPS／INS統合化技法はGPS受信機から姿勢及び方位が利用できないために、一部のIMU誤差及びINS誤差誤差しか推定できなく、高精度且つ信頼できる姿勢及び方位が得られない。
【０００８】
特許文献１はGPS受信機から得られた位置変化と、移動体座標系の各軸に装備された加速度センサ出力の二重積分で得た位置変化が等しくなるような座標変換係数を推定し、該推定値からGPS姿勢及び方位を求める方式である。
【０００９】
この技法において、座標変換係数は位置変化ベクトル算定期間内における平均値となるため、高ダイナミックスを被る移動体の場合に姿勢及び方位精度が劣化し、また、加速度センサの２重積分で得られた位置変化はセンサの誤差のために時間経過で大きな誤差を持つ欠点を有する。
【００１０】
本発明はかかる問題点を鑑みてなされたものであり、本発明の第一の目的は単一のGPSアンテナと安価な慣性センサを用いて、小型、軽量且つ安価な移動体の、少なくとも、姿勢及び方位を検出する装置を提供することを目的とする。  
【００１１】
また、本発明の第二の目的はGPSの支援が得られない場合においても、ロール、ピッチが提供できる装置を提供するものである。
【課題を解決するための手段】
【００１２】
前述の問題を解決するために、本発明は、単一GPSアンテナを有するGPS受信機と慣性センサで構成される移動体の姿勢及び方位検出装置において、前記GPS受信機出力と前記慣性センサ観測量を用いて、移動体の姿勢及び方位を算定する姿勢演算部と、前記姿勢演算部出力のロール及び/或いはピッチの雑音を低減するための、誤差最小規範に基づいてフィルタのパラメータ及びウェートを決定する構造のフィルタ部と、移動体の旋回時の横滑り角を推定し、該横滑り角推定値で前記姿勢演算部出力の方位を補正する横滑り補正部と、位置、速度、姿勢及び方位を算定する慣性航法演算処理部と、前記フィルタ部出力の姿勢と慣性航法演算処理部出力の姿勢を比較し、前記横滑り補正部出力の方位と前記慣性航法演算処理部出力の方位とを比較する姿勢比較部と、前記GPS受信機出力の少なくとも速度と前記慣性航法演算処理部出力の少なくとも速度をそれぞれ比較するＰＶ比較部と、前記姿勢比較部及び前記ＰＶ比較部の出力に基づいて、前記慣性センサ出力誤差及び前記慣性航法演算処理部出力誤差を推定する誤差推定部を備え、前記誤差推定部で推定された誤差量で前記慣性センサ出力及び慣性航法演算処理部出力を補正することを特徴とする。
【００１３】
また、本発明は、
前記姿勢演算部が、移動体の進行方向は前記ヘディング軸と一致するとの仮定に基づいて前記GPS受信機出力の速度から算定した前記ヘディング軸速度と、該ヘディング軸に直交する成分の前記慣性センサ出力の加速度及び角速度から移動体のロールを算定し、前記ヘディング軸成分速度の所定期間における変化から算定した前記ヘディング軸成分加速度と、該ヘディング軸成分の前記慣性センサ出力の加速度と、ローカル重力を計算するための前記GPS受信機出力の位置から移動体のピッチを算定することを特徴とする。
【００１４】
また、本発明は、前記横滑り角推定値を前記ヘディング軸に直交する垂直軸回りの角速度の比例値で算出するか、または前記垂直軸回りの角速度が予め決められた閾値を超えたときの前記垂直軸回りの角速度を積分して得た方位と前記姿勢演算部出力の方位との差から算出するかのいずれか、若しくは併用で算出することを特徴とする。
【００１５】
また、本発明は、前記姿勢演算部のピッチ算定が、前記ヘディング軸成分速度の所定期間における変化から算定した前記ヘディング軸成分加速度を無視して、前記移動体のピッチを算定することを特徴とする。
【００１６】
また、本発明は、前記姿勢演算部のピッチ算定が、前記ローカル重力を計算するための位置を外部設定入力するか、または無視して、前記移動体のピッチを算定することを特徴とする。
【００１７】
また、本発明は、前記GPS受信機出力が得られないときは、前記姿勢演算部出力のロール及び/或いはピッチは前記フィルタ部ロール及び/或いはピッチを出力することを特徴とする。
【発明の効果】
【００１８】
本発明によれば、単一のGPSアンテナ構成では得られない姿勢及び方位をGPS速度と慣性センサから得ることによって、単一のGPSアンテナ構成であっても、あたかも複数のアンテナを用いたGPS/INS統合化方式と同じ技法が適用できるため、小型で安価に姿勢及び方位が得られる利点を有する。  
【００１９】
また、本発明によれば、移動体の旋回時における横滑りによって生じるGPS速度から算定した方位（進路方位または擬似方位と呼ぶ）と移動体で定義したヘディング軸の方位（以下、ヘディング方位まはた真方位と呼ぶ）との差異を推定し補正するため、GPS速度から算定した方位を用いても、複数のアンテナ構成で得られる方位と同じ移動体の要望のヘディング方位が得られる利点を有する。
【００２０】
また、本発明によれば、GPS速度を用いて方位を算定しているために、複数のアンテナ構成におけるキャリア測位技法による基線ベクトル決定に要求されるキャリア位相Ambiguityを求める必要がないので、計算負荷の小さく、且つ電源投入後、即座に姿勢及び方位が求まる利点を有する。
【００２１】
また、本発明によれば、GPS受信機出力が得られない場合においても、方位は得られないが、慣性センサ観測量からロール及び/或いはピッチを算定し、此等の誤差を最小にするような最適フィルタで処理することにより、通常の低域通過フィルタ処理をしたときに生じるような位相遅れのない、且つ約３〜５倍の精度向上が期待できるロール、ピッチが得られる利点を有する。
【発明を実施するための最良の形態】
【００２２】
以下、本発明の実施形態１による単一GPSアンテナによる姿勢及び方位検出装置について図面を参照しながら説明する。
【００２３】
図1は本発明に関係する航法座標系と移動体座標系の関係及び、IMU及びGPSアンテナの位置を示す図である。ここで、航法座標系(Xn-Yn-Zn)は、北方向をXn軸の正方向、東方向をYn軸の正方向とする右手系の直交座標系であり、GPS受信機の位置及び速度はこの座標系で出力される。また、移動体座標系(Xb-Yb-Zb)は、移動体で定義したヘディング軸において、移動体の前進方向をXb軸の正方向とし、右手方向をYb軸の正方向とする右手系の直交座標系である。前記ヘディング軸の定義は任意でよいが、例えば、船舶の場合、通常、船首尾線で定義される。
【００２４】
尚、両座標系の原点は必ずしも一致させる必要はないが、図１では理解を助けるために一致させている。また、GPSアンテナは移動体座標系における既知の位置に固定して設置すればよい。
【００２５】
移動体座標系の各軸成分の加速度センサ、及びヘディング軸に直交する軸の角速度を計測するための角速度センサを含むIMUが固定設置されている。尚、図１では、便宜上、慣性センサの設定位置は、移動体座標系の原点としているが、原点に拘るものではない。
【００２６】
図２は本発明の最良の実施形態によるGPS／INS統合化技法による姿勢及び方位装置の構成図であり、単一のGPSアンテナ１に接続され、位置、速度、ＧＰＳ時刻などを出力するGPS受信機２と、３軸加速度センサ及び２軸角速度センサを内蔵した慣性センサ（IMU）３と、演算処理部４で構成される。
【００２７】
演算処理部４はロール、ピッチ、方位を生成する姿勢演算部５と、横滑り補正部６と、フィルタ部７と、ＰＶ比較部８と、姿勢比較部９と、誤差推定部１０と、慣性航法演算部１１とから構成される。以下、各部の説明をする。
【００２８】
姿勢演算部５は移動体の姿勢（ロール及びピッチ）及びＧＰＳ方位を算出する。航法座標系に対する移動体のロール及びピッチは、地球自転角速度を無視すると、移動体位置、移動体座標系における速度及び加速度、移動体座標軸の角速度が与えられれば、  [数１]、  [数２]で求めることができる。
ここで、右肩添字bは移動体座標系換算値であることを示し、右下添字は各軸成分を示す。また、aは加速度観測量、ωは角速度観測量であり、vは移動体の速度、αは移動体の運動加速度（vの時間微分）、gは移動体の位置で決まるローカル重力である。
【数１】
2008232867.tif 000003
【数２】
2008232867.tif 000004
【００２９】
しかしながら、[数１]、  [数２]に基づいて、ロール、ピッチを算定するために必要な移動体座標系における速度や運動加速度はＧＰＳ位置及びＧＰＳ速度から得られない。従って、本発明では、以下の方法でロール、ピッチ、方位の近似値を算定する。
【００３０】
移動体座標系の各軸成分速度は以下のようにして求める。例えば、移動体が船、車両等の場合、横、垂直速度成分は微小と考えられる。この仮定より、移動体の速度ベクトルの方向は移動体座標系のヘディング軸（即ち、Xb軸）に一致すると見なすことができるから、速度ベクトルvbは航法座標系におけるGPS速度ベクトルvnを用いて  [数３]で求める。
【数３】
2008232867.tif 000005
【００３１】
ヘディング軸成分運動加速度αはヘディング軸成分速度の所定期間における変化から求める。ここで、所定期間は例えば、GPS受信機出力の更新周期とすることもできる。ヘディング軸に直交する（即ち、移動体座標系Yb軸、 Zb軸）成分の移動体の運動加速度は進行方向成分に比べて微小であるので無視できる。
【００３２】
ローカル重力はGPS位置から公知の計算式で、随時、計算できるが、GPS位置が得られない場合は、外部より位置を入力するか、または代表的なローカル重力値を設定してもよい。
【００３３】
移動体の進路方位（ＧＰＳ方位）はGPS速度水平速度成分を用いて  [数４]式で求める。
【数４】
2008232867.tif 000006
【００３４】
[数４]で算定した進路方位は擬似方位であり、例えば、移動体の旋回で横滑りが生じた場合はヘディング軸が指す要望のヘディング方位と差が生じる。通常、要望の方位はヘディング軸を参照とする方位（ヘディング方位）であるから、横滑り角を推定し、進路方位を補正する必要がある。図３は船を例にしたときの進路方位とヘディング方位の差異を示している。船の進路方位Ψpとヘディング方位θyの関係は  [数５]で表せる。
【数５】
2008232867.tif 000007
ここで、Θsが船の旋回で生じた横滑り角である。
【００３５】
横滑り補正部６は横滑り角を推定し、補正する処理部である。本発明による横滑り角の推定は下記の横滑り角推定方法１及び/或いは横滑り角推定方法２で行うことを特徴とする。
横滑り角の推定方法１は、横滑り角Θsは舵角μに比例し、舵角は船の旋回角速度Ωに比例するとの考えに基づいた方法であり、[数６]を用いて推定する。
【数６】
2008232867.tif 000008
【００３６】
ここで、比例係数ksは船の形状、舵の形状や等によって決まる定数とする。船の旋回角速度Ωは航法座標系における回転角速度のz軸成分角速度に等しいから、IMU３からの角速度観測量と、上述で算出したロール及びピッチ、または該ロール及びピッチの後述のフィルタ処理後のロール及びピッチを用いて  [数７]で算定できる。
【数７】
2008232867.tif 000009
【００３７】
図４に横滑り角推定方法２に係わる概念図を示している。図４の下図は移動体の旋回角速度Ωを示し、上図は旋回時における横滑り角Θsを示している。船の旋回開始後の旋回角は、[数７]で算定された旋回角速度を用いて、[数８]で近似できる。
【数８】
2008232867.tif 000010
【００３８】
ここで、ΨΩ(to)は旋回開始時刻ｔoにおける方位の初期値である。初期値ΨΩ(to)として、旋回開始時刻ｔoにおける慣性航法演算部１１で推定された方位θyを設定すると、旋回角ΨΩはヘディング方位θyに一致する。また、初期値ΨΩ(to)は、時刻ｔoにおけるΨΩ(to)は横ずれ角が零で近似できるから、GPS速度から算定された進路方位を設定することもできる。
【００３９】
ｔoのタイミングは予め設定された角速度閾値Ωoに対して、|Ω|≧|Ωo |から知ることができる。ｔsは旋回時間（|Ω|>|Ωo |）である。船が連続して一定角速度で回転している場合における積分時間ｔsは角速度Ωの誤差が無視できれば、船の旋回期間とすることができる。しかし、実際は誤差があるから、ΨΩの誤差が時間経過につれて増加する。この不具合を避けるために、通常、ｔsはΩが定常値に到達するまでの時間のＫ倍（１≦K≦５）程度に設定すればよい。この結果、横滑り角は、  [数４]、  [数５]、[数８]を用いて、[数９]で推定できる。
【数９】
2008232867.tif 000011
尚、|Ω|<|Ωo |では横滑り角の推定、補正は行わないように処理する。
【００４０】
次に図２におけるフィルタ部７について説明する。姿勢演算部５で生成されたロール及び/或いはピッチは慣性センサ観測量の観測雑音や算定式の近似誤差のために誤差を含んでいる。この誤差は移動体の動きと同じ周波数帯域を有するために、低域通過フィルタ及び/或いは高域通過フィルタではこの誤差を低減できない。フィルタ部７は最小２乗規範に基づいて誤差を最少にする条件下で、フィルタのパラメータ及びウェートが決定される最適フィルタによる処理を行う。
【００４１】
この結果、位相遅れのなく、且つロール、ピッチ誤差の周波数成分に関係なく、該誤差が低減できるために、フィルタ処理を行わないときに比較して、約３〜５倍の精度向上が期待できるロール、ピッチが得られる。前記フィルタ７処理によって、GPS受信機の出力が得られないときでも、高精度のロール及び/或いはピッチを得ることができる。
【００４２】
図５は前記フィルタ７の構成例を示している。ロール、ピッチ共に同じ構成で実施できる。図５において、観測モデル１は図２の姿勢演算部５で生成されるロール或いはピッチの観測モデル、即ち、観測モデル１の出力が図２の姿勢演算部５で生成されるロール或いはピッチ観測量である。
【００４３】
前記のロール或いはピッチ観測量は最適フィルタモデル２に入力され、最適フィルタモデル２出力は比較部４において参照値と比較され、その差はシステム同定部３へ出力される。システム同定部３は最小２乗規範に基づいて、ロール或いはピッチ誤差が最少になるように、最適フィルタモデル２のフィルタのパラメータ及びウェートを公知のシステム同定技法を用いて推定する。
【００４４】
前記参照値は、たとえば、工場または実際のフィールド実験で評価するときに評価基準装置として用いるロール、ピッチ計測装置で得られる値を使用できる。また、評価システム同定部３の処理は前記の工場または実際のフィールド実験時のテストデータに基づいてオフラインで一度、決定しておけばよい。
【００４５】
以下に、Li(z)(i=1,2)の伝達関数を有する一次の低域通過フィルタと、H1(z)、H2(z)の伝達関数を有する一次の高域通過フィルタを縦続接続したときの構成例による前記最適フィルタモデル２の実現方法を説明する。一次の低域及び高域通過フィルタのパルス伝達関数は[数１０]、[数１１]で表せる。
【数１０】
2008232867.tif 000012
【数１１】
2008232867.tif 000013
今、前記最適フィルタ２のk時刻における入力をYm(z)とすると、各フィルタの出力は次式で得られる。
【数１２】
2008232867.tif 000014
2008232867.tif 000015
2008232867.tif 000016
2008232867.tif 000017
ここで、Pl1、Pl2、Ph1、Ph2はフィルタパラメータであり、Δｔは演算周期、τl1、τl2はL1(z)、L2(z)の時定数、τh1、τh2はH1(z)、H2(z)の時定数とすると、[数１３]で表せる。
【数１３】
2008232867.tif 000018
最適フィルタモデル２の出力は[数１４]で得られる。
【数１４】
2008232867.tif 000019
上述のように、最適フィルタのパラメータ及びウェートの推定は[数１４]の出力と参照値の差から、前記システム同定部３で決定できる。
【００４６】
図６は小型船搭載時の実データを用いて、ロールフィルタのパラメータ及びウェート推定値を決定し(上段図)、該パラメータ及びウェート推定値を用いた最適フィルタで処理したときの特性（下段図）である。該最適フィルタ処理により、ロール誤差が約1/3〜1/5に低減できることが分かる。尚、この例ではi=1,2であるが、最小２乗規範に基づいて決定されるフィルタであれば、この構成例やLi(z)、Hi(z)の数に限定するものではない。
【００４７】
次に図２の慣性航法演算部１１について説明する。該慣性航法演算部１１はIMU３の慣性センサ観測量やGPS位置とGPS速度を入力として、通常の公知の方法によりINS姿勢、ＩＮＳ方位、INS位置、INS速度を演算する処理部である。尚、図２には示していないが、姿勢角の初期値は姿勢演算部５から得た姿勢及び方位を用いる。
【００４８】
前記ＩＮＳ姿勢及びＩＮＳ方位は、フィルタ７より得られた姿勢、及び横滑り補正部６より得られる方位と姿勢比較部でそれぞれ比較され、また、ＩＮＳ位置、ＩＮＳ速度はGPS位置、ＧＰＳ速度とＰＶ比較部でそれぞれ比較される。前期姿勢比較部及びＰＶ比較部出力の各比較差は誤差推定部１０に入力される。
【００４９】
誤差推定部１０は慣性航法演算部１１出力の位置、速度、姿勢及び方位などの誤差や慣性センサ観測量誤差などを推定する。誤差推定部１０における該諸誤差の推定は、通常、カルマン・フィルタ技法が適用される。この場合、前期姿勢比較部及びPV比較部の出力情報はカルマン・フィルタの観測更新の観測量として用いられる。
【００５０】
尚、誤差推定部１０におけて該諸誤差を推定するためには、図２では示していないが、ＧＰＳ位置、ＧＰＳ速度、慣性センサ３の観測量、姿勢及び方位などの全て或いは一部の情報が必要となる。これらの情報はダイナミックモデル式や観測モデル式の係数の算定に使用される。誤差推定部１０で推定された各推定誤差は、ＧＰＳ受信機の出力更新周期毎に、負帰還され、補正される。
【００５１】
尚、前記慣性航法演算部１１で位置の演算を削除し、ＰＶ比較部の位置の比較を削除し、前記誤差推定部１０における位置誤差の推定を削除することもできる。
尚、図２において、GPS速度、位置も利用するものとして説明したが、速度のみ使用することも可能である。
【００５２】
尚、GPS速度が得られない場合は、方位は算定できないが、[数１]、[数２]で移動体の速度をゼロにして、ロール、ピッチを算出し、前記の最適フィルタ処理することで実用可能な精度で求まる。
【図面の簡単な説明】
【００５３】
【図１】本発明に係わる座標系の説明図
【図２】本発明に係わる単一GPSアンテナを用いた航法装置の構成説明図
【図３】本発明に係わる方式で算定した方位とヘディングを説明する図
【図４】本発明に係わる横滑り角を推定する説明する図
【図５】本発明に係わる最適フィルタの構成実施例図
【図６】本発明に係わる最適フィルタ処理効果を示す図
【符号の説明】
【００５４】
１    ＧＰＳアンテナ
２    GPS受信機
３    慣性センサ
４    演算処理部
５    姿勢演算部
６    横滑り補正部
７    フィルタ部
８    PV比較部
９    姿勢比較部
１０  誤差推定部
１１  慣性航法演算部
５１  観測モデル
５２  最適フィルタモデル
５３  システム同定部
５４  比較部
【図１】
2008232867.tif 000020
【図２】
2008232867.tif 000021
【図３】
2008232867.tif 000022
【図４】
2008232867.tif 000023
【図５】
2008232867.tif 000024
【図６】
2008232867.tif 000025