JP2001174275A - ハイブリッド航法およびその装置 - Google Patents
ハイブリッド航法およびその装置Info
- Publication number
- JP2001174275A JP2001174275A JP35992899A JP35992899A JP2001174275A JP 2001174275 A JP2001174275 A JP 2001174275A JP 35992899 A JP35992899 A JP 35992899A JP 35992899 A JP35992899 A JP 35992899A JP 2001174275 A JP2001174275 A JP 2001174275A
- Authority
- JP
- Japan
- Prior art keywords
- equation
- noise
- observation
- kalman filter
- discrete
- 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.)
- Granted
Links
Landscapes
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
(57)【要約】
【課題】 電波航法と慣性航法のハイブリッド航法装置
において、システム方程式の複雑化によるカルマンフィ
ルタのチューニングの煩雑化の問題を解消し、また電波
航法装置における信号処理による雑音の有色化に伴うカ
ルマンフィルタの更新周期の制限の問題を解消した、ハ
イブリッド航法装置を得る。 【解決手段】 誤差の生成過程をランジュバン方程式で
モデル化することによりシステム方程式を単純化し、有
色の観測雑音をマルコフ過程でモデル化して白色雑音化
することにより、その有色雑音の相関時定数よりカルマ
ンフィルタの更新周期を短縮化できるようにする。
において、システム方程式の複雑化によるカルマンフィ
ルタのチューニングの煩雑化の問題を解消し、また電波
航法装置における信号処理による雑音の有色化に伴うカ
ルマンフィルタの更新周期の制限の問題を解消した、ハ
イブリッド航法装置を得る。 【解決手段】 誤差の生成過程をランジュバン方程式で
モデル化することによりシステム方程式を単純化し、有
色の観測雑音をマルコフ過程でモデル化して白色雑音化
することにより、その有色雑音の相関時定数よりカルマ
ンフィルタの更新周期を短縮化できるようにする。
Description
【０００１】
【発明の属する技術分野】この発明は、電波航法と慣性
航法とを組み合わせたハイブリッド航法装置に関するも
のである。
航法とを組み合わせたハイブリッド航法装置に関するも
のである。
【０００２】
【従来の技術】従来、航空機や船舶の航法装置として、
複数種の航法手段を組み合わせて、それぞれの欠点を補
うことにより、位置や速度などの測定精度を高めたいわ
ゆるハイブリッド航法装置が採用されている。また、測
定された時系列データから現在の状態（位置や速度等の
測定誤差）を推測するためにカルマンフィルタが用いら
れている。
複数種の航法手段を組み合わせて、それぞれの欠点を補
うことにより、位置や速度などの測定精度を高めたいわ
ゆるハイブリッド航法装置が採用されている。また、測
定された時系列データから現在の状態（位置や速度等の
測定誤差）を推測するためにカルマンフィルタが用いら
れている。
【０００３】例えば、「村田、新宮、小野、張替；日本
宇宙技術研究所におけるＧＰＳ航法飛行実験について，
日本航空宇宙学会誌，第41巻，第468 号，1991年 1月」
には、全世界測位システム（ＧＰＳ）と慣性航法システ
ム（ＩＮＳ）とのハイブリッド航法に関して報告されて
いる。このようなＧＰＳとＩＮＳとのハイブリッド航法
では、ＧＰＳとＩＮＳの位置情報または位置と速度の情
報をカルマンフィルタに入力し、状態変数であるＩＮＳ
の位置（２または３箇）、速度（２または３箇）、加速
度（２または３箇）、方位（１箇）、姿勢（２箇）、ジ
ャイロ（３箇）の誤差を最適推定し、ＩＮＳの位置、速
度、方位、姿勢を最適推定した誤差で補正して高精度の
位置、速度、方位、姿勢の情報を出力するように構成さ
れている。
宇宙技術研究所におけるＧＰＳ航法飛行実験について，
日本航空宇宙学会誌，第41巻，第468 号，1991年 1月」
には、全世界測位システム（ＧＰＳ）と慣性航法システ
ム（ＩＮＳ）とのハイブリッド航法に関して報告されて
いる。このようなＧＰＳとＩＮＳとのハイブリッド航法
では、ＧＰＳとＩＮＳの位置情報または位置と速度の情
報をカルマンフィルタに入力し、状態変数であるＩＮＳ
の位置（２または３箇）、速度（２または３箇）、加速
度（２または３箇）、方位（１箇）、姿勢（２箇）、ジ
ャイロ（３箇）の誤差を最適推定し、ＩＮＳの位置、速
度、方位、姿勢を最適推定した誤差で補正して高精度の
位置、速度、方位、姿勢の情報を出力するように構成さ
れている。
【０００４】
【発明が解決しようとする課題】図３はカルマンフィル
タの誤差推定の３つのタイプについて、実際の誤差と推
定誤差の変移の例を示している。ここで、縦軸は誤差の
２乗平均値、横軸は経過時間である。ハイブリッド航法
による位置や速度の精度はカルマンフィルタの誤差の推
定精度に依存している。カルマンフィルタで誤差の最適
推定値を得るためには、図３の（Ａ）に示すように、推
定誤差を実際の誤差より少なく見積もるアンダーエスチ
メイトの状態となったり、（Ｂ）に示すように、推定誤
差を実際の誤差より大きく見積もるオーバーエスチメイ
トの状態とならないように、すなわち（Ｃ）に示すよう
に、推定誤差が実際の誤差に近似するようにカルマンフ
ィルタを適切にチューニングする必要がある。
タの誤差推定の３つのタイプについて、実際の誤差と推
定誤差の変移の例を示している。ここで、縦軸は誤差の
２乗平均値、横軸は経過時間である。ハイブリッド航法
による位置や速度の精度はカルマンフィルタの誤差の推
定精度に依存している。カルマンフィルタで誤差の最適
推定値を得るためには、図３の（Ａ）に示すように、推
定誤差を実際の誤差より少なく見積もるアンダーエスチ
メイトの状態となったり、（Ｂ）に示すように、推定誤
差を実際の誤差より大きく見積もるオーバーエスチメイ
トの状態とならないように、すなわち（Ｃ）に示すよう
に、推定誤差が実際の誤差に近似するようにカルマンフ
ィルタを適切にチューニングする必要がある。
【０００５】しかし、上述のとおり、ハイブリッド航法
に使われる従来のカルマンフィルタの状態変数の箇数は
１２以上もあり、しかも例えば速度誤差と方位および姿
勢誤差との間、方位および姿勢誤差と位置誤差との間、
方位と姿勢誤差との間などには相互依存の関係があるた
め、慣性航法装置の測定誤差の生成過程を表すシステム
行列の複雑化に伴ってシステム方程式も複雑になる。
に使われる従来のカルマンフィルタの状態変数の箇数は
１２以上もあり、しかも例えば速度誤差と方位および姿
勢誤差との間、方位および姿勢誤差と位置誤差との間、
方位と姿勢誤差との間などには相互依存の関係があるた
め、慣性航法装置の測定誤差の生成過程を表すシステム
行列の複雑化に伴ってシステム方程式も複雑になる。
【０００６】このように、従来のハイブリッド航法装置
においては、システム方程式が複雑であるため、カルマ
ンフィルタが誤差を最適推定できるように、カルマンフ
ィルタをチューニングすることが煩雑であった。
においては、システム方程式が複雑であるため、カルマ
ンフィルタが誤差を最適推定できるように、カルマンフ
ィルタをチューニングすることが煩雑であった。
【０００７】また、一般に、ＧＰＳ受信機等の電波航法
装置においては、信号処理に伴って測定誤差は自己相関
を持つ有色雑音となる。したがって、観測方程式に含ま
れる観測雑音は有色雑音となってしまう。
装置においては、信号処理に伴って測定誤差は自己相関
を持つ有色雑音となる。したがって、観測方程式に含ま
れる観測雑音は有色雑音となってしまう。
【０００８】しかし、カルマンフィルタの理論では、観
測雑音が白色雑音であることと、正則（観測雑音の共分
散行列の逆行列が存在する。）であることが前提になっ
ている。そこで従来は、観測雑音が有色雑音である場合
に、上記前提を満たすために、カルマンフィルタの更新
周期を観測雑音の相関時定数より長く設定することによ
って、観測雑音が実質上白色雑音と見なせるようにして
いる。
測雑音が白色雑音であることと、正則（観測雑音の共分
散行列の逆行列が存在する。）であることが前提になっ
ている。そこで従来は、観測雑音が有色雑音である場合
に、上記前提を満たすために、カルマンフィルタの更新
周期を観測雑音の相関時定数より長く設定することによ
って、観測雑音が実質上白色雑音と見なせるようにして
いる。
【０００９】ところが、カルマンフィルタの更新周期が
長くなると、位置や速度の補正がその更新周期でしかで
きなくなってしまう。
長くなると、位置や速度の補正がその更新周期でしかで
きなくなってしまう。
【００１０】このように、従来のハイブリッド航法装置
においては、信号処理の結果、観測雑音が一般に有色雑
音となるため、カルマンフィルタの更新周期は観測雑音
の相関時定数に制限され、更新周期が長くなり、誤差推
定の精度も劣化してしまう。
においては、信号処理の結果、観測雑音が一般に有色雑
音となるため、カルマンフィルタの更新周期は観測雑音
の相関時定数に制限され、更新周期が長くなり、誤差推
定の精度も劣化してしまう。
【００１１】この発明の目的は、システム方程式の複雑
化によるカルマンフィルタのチューニングの煩雑化の問
題を解消し、また電波航法装置における信号処理による
雑音の有色化に伴うカルマンフィルタの更新周期の制限
の問題を解消したハイブリッド航法装置を提供すること
にある。
化によるカルマンフィルタのチューニングの煩雑化の問
題を解消し、また電波航法装置における信号処理による
雑音の有色化に伴うカルマンフィルタの更新周期の制限
の問題を解消したハイブリッド航法装置を提供すること
にある。
【００１２】
【課題を解決するための手段】この発明は、電波航法装
置と慣性航法装置による観測値をそれぞれ入力し、カル
マンフィルタにて慣性航法装置の観測値を補正するハイ
ブリッド航法装置であり、慣性航法装置の測定誤差の生
成過程をランジュバン方程式でモデル化した、測定誤差
を状態変数とするベクトルＸ、相関時定数により定まる
システム行列Ａおよびシステム雑音ｕによるシステム方
程式を離散時間系に変換した式Ｘk ＝Ａk- 1 Ｘk-1 ＋ｕ
k-1 （ｋは時間ステップ）を離散系システム方程式と
し、電波航法装置の有色雑音である測定誤差をマルコフ
過程でモデル化して観測雑音を白色雑音とした、前記ベ
クトルＸ、観測行列Ｈおよび観測雑音ｗによる観測方程
式を離散時間系に変換した式Ｚk ＝Ｈk Ｘk ＋ｗk （ｋ
は時間ステップ）を離散系観測方程式とし、前記離散系
システム方程式と前記離散系観測方程式とから構成した
カルマンフィルタを用いる。
置と慣性航法装置による観測値をそれぞれ入力し、カル
マンフィルタにて慣性航法装置の観測値を補正するハイ
ブリッド航法装置であり、慣性航法装置の測定誤差の生
成過程をランジュバン方程式でモデル化した、測定誤差
を状態変数とするベクトルＸ、相関時定数により定まる
システム行列Ａおよびシステム雑音ｕによるシステム方
程式を離散時間系に変換した式Ｘk ＝Ａk- 1 Ｘk-1 ＋ｕ
k-1 （ｋは時間ステップ）を離散系システム方程式と
し、電波航法装置の有色雑音である測定誤差をマルコフ
過程でモデル化して観測雑音を白色雑音とした、前記ベ
クトルＸ、観測行列Ｈおよび観測雑音ｗによる観測方程
式を離散時間系に変換した式Ｚk ＝Ｈk Ｘk ＋ｗk （ｋ
は時間ステップ）を離散系観測方程式とし、前記離散系
システム方程式と前記離散系観測方程式とから構成した
カルマンフィルタを用いる。
【００１３】このように、システム方程式をランジュバ
ン方程式でモデル化し単純化することによりチューニン
グを簡単にする。また、観測方程式に含まれる雑音を白
色雑音化することにより、カルマンフィルタの更新周期
を短くできるようにする。
ン方程式でモデル化し単純化することによりチューニン
グを簡単にする。また、観測方程式に含まれる雑音を白
色雑音化することにより、カルマンフィルタの更新周期
を短くできるようにする。
【００１４】
【発明の実施の形態】図１はこの発明に実施形態である
ハイブリッド航法装置の構成を示すブロック図である。
ここで、ＩＮＳ１はジャイロと加速度計を用いて、それ
らが装備された航空機の位置を速度等を求める慣性航法
装置、ＧＰＳ受信機２はＧＰＳ衛星からの電波を受信し
て航空機の位置と速度を測定する装置である。３はハイ
ブリッド航法装置であり、ＩＮＳ１とＧＰＳ受信機２か
らそれぞれ位置と速度の情報を入力し、ＩＮＳの位置と
速度の誤差を推定し、この推定した誤差でＩＮＳの位置
と速度を補正することによって、高精度の位置と速度情
報を出力する。
ハイブリッド航法装置の構成を示すブロック図である。
ここで、ＩＮＳ１はジャイロと加速度計を用いて、それ
らが装備された航空機の位置を速度等を求める慣性航法
装置、ＧＰＳ受信機２はＧＰＳ衛星からの電波を受信し
て航空機の位置と速度を測定する装置である。３はハイ
ブリッド航法装置であり、ＩＮＳ１とＧＰＳ受信機２か
らそれぞれ位置と速度の情報を入力し、ＩＮＳの位置と
速度の誤差を推定し、この推定した誤差でＩＮＳの位置
と速度を補正することによって、高精度の位置と速度情
報を出力する。
【００１５】ハイブリッド航法装置３におけるカルマン
フィルタ４は、ＩＮＳ航法装置１とＧＰＳ受信機２から
入力された、それぞれの位置と速度の情報からＩＮＳの
位置と速度の誤差を最適推定する。
フィルタ４は、ＩＮＳ航法装置１とＧＰＳ受信機２から
入力された、それぞれの位置と速度の情報からＩＮＳの
位置と速度の誤差を最適推定する。
【００１６】説明を簡単にするために、２次元の場合に
ついて、従来の欠点を解消したハイブリッド航法装置に
ついて説明する。
ついて、従来の欠点を解消したハイブリッド航法装置に
ついて説明する。
【００１７】〔１〕 システム方程式 前述のように、従来はシステム方程式が複雑であったた
め、カルマンフィルタのチューニングが煩雑となってい
た。この発明ではまず、システム方程式を単純化するこ
とによりチューニングを簡単にする。
め、カルマンフィルタのチューニングが煩雑となってい
た。この発明ではまず、システム方程式を単純化するこ
とによりチューニングを簡単にする。
【００１８】ＩＮＳの位置誤差はジャイロと加速度計の
誤差が要因となって発生する。そのうちの７０％以上が
ジャイロの誤差に依存しており、加速度計の誤差の影響
は少ない。このことは「早川、星野；慣性航法装置の慣
性素子，日本航空宇宙学会誌，Vol.31，1983年 6月」に
示されている。ＩＮＳの位置誤差の分散とジャイロの誤
差の関係は〔数１〕で与えられる。この関係は「Diesel
J.W；ＧＰＳ／ＩＮＳINTEGRATION BY FUNCTIONAL PART
ITIONING」に示されている。
誤差が要因となって発生する。そのうちの７０％以上が
ジャイロの誤差に依存しており、加速度計の誤差の影響
は少ない。このことは「早川、星野；慣性航法装置の慣
性素子，日本航空宇宙学会誌，Vol.31，1983年 6月」に
示されている。ＩＮＳの位置誤差の分散とジャイロの誤
差の関係は〔数１〕で与えられる。この関係は「Diesel
J.W；ＧＰＳ／ＩＮＳINTEGRATION BY FUNCTIONAL PART
ITIONING」に示されている。
【００１９】
【数１】
【００２０】〔数１〕において、ＩＮＳの位置誤差の分
散Ｑ(t) は、シューラ周期８４．４分の１／４周期以上
の時間が経過すると、時間に比例する項が振動項よりも
優勢になり、１シューラ周期以降は、時間に比例する項
が振動項の５倍以上になって、振動項が無視できる。す
なわち、１シューラ周期以降は、ＩＮＳの位置誤差の分
散は時間に略比例して大きくなっていく。
散Ｑ(t) は、シューラ周期８４．４分の１／４周期以上
の時間が経過すると、時間に比例する項が振動項よりも
優勢になり、１シューラ周期以降は、時間に比例する項
が振動項の５倍以上になって、振動項が無視できる。す
なわち、１シューラ周期以降は、ＩＮＳの位置誤差の分
散は時間に略比例して大きくなっていく。
【００２１】このことは、ＩＮＳの位置誤差がランジュ
バン（Langevin）方程式でモデル化できることを示して
いる。
バン（Langevin）方程式でモデル化できることを示して
いる。
【００２２】ＩＮＳの誤差を状態変数とするベクトルを
〔数２〕で表し、ランジュバン方程式でモデル化する
と、Ａをシステム行列、ｕをシステム雑音としてシステ
ム方程式は〔数３〕で表される。ただし、Ｔは転置を示
す。
〔数２〕で表し、ランジュバン方程式でモデル化する
と、Ａをシステム行列、ｕをシステム雑音としてシステ
ム方程式は〔数３〕で表される。ただし、Ｔは転置を示
す。
【００２３】
【数２】
【００２４】
【数３】
【００２５】このように、ＩＮＳの位置誤差をランジュ
バン方程式でモデル化することにより、システム方程式
は極めて単純化できた。相関時定数τ1 と北方向および
東方向の加速度誤差ｕn ＝ｕe はシステム方程式の設計
要素であり、システムの設計により定まる。なお、〔数
３〕でＸ′と示したように、微分を意味する′（ドッ
ト）付き記号を以下２文字で表す。
バン方程式でモデル化することにより、システム方程式
は極めて単純化できた。相関時定数τ1 と北方向および
東方向の加速度誤差ｕn ＝ｕe はシステム方程式の設計
要素であり、システムの設計により定まる。なお、〔数
３〕でＸ′と示したように、微分を意味する′（ドッ
ト）付き記号を以下２文字で表す。
【００２６】〔２〕 観測方程式 観測値ベクトルを（ＩＮＳの位置）−（ＧＰＳの位置）
とすると、観測方程式は〔数４〕で表すことができる。
とすると、観測方程式は〔数４〕で表すことができる。
【００２７】
【数４】
【００２８】前述のように、ＧＰＳ受信機の測位誤差
は、受信信号に含まれるマルコフ性の電離層遅れによ
り、また、受信信号に含まれる擬似雑音符号を位相追尾
して測定位置を更新する処理等を行うことにより、自己
相関を持つ有色雑音になり、したがって、観測方程式に
含まれる観測雑音は有色雑音となってしまう。
は、受信信号に含まれるマルコフ性の電離層遅れによ
り、また、受信信号に含まれる擬似雑音符号を位相追尾
して測定位置を更新する処理等を行うことにより、自己
相関を持つ有色雑音になり、したがって、観測方程式に
含まれる観測雑音は有色雑音となってしまう。
【００２９】観測雑音が有色雑音である場合、従来な
ら、前述したように、カルマンフィルタの更新周期を相
関時定数より短くすることはできなかったが、この発明
ではこの制限を除くために、有色雑音を確率過程でモデ
ル化することにより、カルマンフィルタの理論の前提を
満たす表示形式に〔数４〕を変形する。
ら、前述したように、カルマンフィルタの更新周期を相
関時定数より短くすることはできなかったが、この発明
ではこの制限を除くために、有色雑音を確率過程でモデ
ル化することにより、カルマンフィルタの理論の前提を
満たす表示形式に〔数４〕を変形する。
【００３０】説明を簡単にするために、ＧＰＳの北方向
および東方向の位置誤差（νn ，ν e ）が、分散σ3 2、
相関時定数τ2 をもつマルコフ過程（１次マルコフ過
程）でモデル化した場合について説明する。
および東方向の位置誤差（νn ，ν e ）が、分散σ3 2、
相関時定数τ2 をもつマルコフ過程（１次マルコフ過
程）でモデル化した場合について説明する。
【００３１】この場合、北方向および東方向の位置誤差
νn ，νe の自己相関関数は
νn ，νe の自己相関関数は
【００３２】
【数５】
【００３３】で表され、観測雑音νは、ウィナーヒンチ
ンの自己相関関数とパワースペクトルとの関係より、
〔数６〕のように、白色雑音ｗで駆動される方程式で表
すことができる。
ンの自己相関関数とパワースペクトルとの関係より、
〔数６〕のように、白色雑音ｗで駆動される方程式で表
すことができる。
【００３４】
【数６】
【００３５】次に、〔数４〕を微分し、〔数３〕、〔数
４〕、〔数６〕を使って整理すると、〔数７〕が得られ
る。
４〕、〔数６〕を使って整理すると、〔数７〕が得られ
る。
【００３６】
【数７】
【００３７】〔数７〕でＨ＝ＣＡ−ＦＣ，Ｚ＝Ｙ′−Ｆ
Ｙとおくと、〔数７〕は〔数８〕になる。
Ｙとおくと、〔数７〕は〔数８〕になる。
【００３８】
【数８】
【００３９】上記Ｙ′はＩＮＳとＧＰＳの速度差から観
測でき、新たにＺを観測値、Ｈを観測行列として、〔数
８〕を観測方程式とすれば、〔数８〕は、観測雑音が白
色雑音ｗであるので、カルマンフィルタの理論の前提を
満たす表示形式に変形した観測方程式になる。ここで相
関時定数τ2 と観測雑音ｗn ＝ｗe は観測方程式の設計
要素であり、設計により定まる。例えば、ＧＰＳ受信機
の測定位置の誤差を収集し、誤差の自己相関関数を求め
ることにより、τ2 ，ｗn ，ｗe を決定する。
測でき、新たにＺを観測値、Ｈを観測行列として、〔数
８〕を観測方程式とすれば、〔数８〕は、観測雑音が白
色雑音ｗであるので、カルマンフィルタの理論の前提を
満たす表示形式に変形した観測方程式になる。ここで相
関時定数τ2 と観測雑音ｗn ＝ｗe は観測方程式の設計
要素であり、設計により定まる。例えば、ＧＰＳ受信機
の測定位置の誤差を収集し、誤差の自己相関関数を求め
ることにより、τ2 ，ｗn ，ｗe を決定する。
【００４０】〔３〕 カルマンフィルタ 〔数３〕のシステム方程式と〔数８〕の観測方程式を連
続時間系から更新周期Δの離散時間系に変換すると、シ
ステム方程式は〔数９〕となり、観測方程式は〔数１
０〕となる。ここで、ｋは０以上の整数であり、ｋ番目
の時刻はΔｋで表される。この連続時間系から離散時間
系の変換については「有本著；カルマン・フィルタ，産
業図書，昭和52年」に示されている。
続時間系から更新周期Δの離散時間系に変換すると、シ
ステム方程式は〔数９〕となり、観測方程式は〔数１
０〕となる。ここで、ｋは０以上の整数であり、ｋ番目
の時刻はΔｋで表される。この連続時間系から離散時間
系の変換については「有本著；カルマン・フィルタ，産
業図書，昭和52年」に示されている。
【００４１】
【数９】
【００４２】
【数１０】
【００４３】システム方程式と観測方程式からカルマン
フィルタの式のパラメータを決定する方法は周知技術で
あるので、ここではシステム方程式〔数９〕と観測方程
式〔数１０〕から導いたカルマンフィルタの式のみを
〔数１１〕〜〔数１５〕に示す。
フィルタの式のパラメータを決定する方法は周知技術で
あるので、ここではシステム方程式〔数９〕と観測方程
式〔数１０〕から導いたカルマンフィルタの式のみを
〔数１１〕〜〔数１５〕に示す。
【００４４】伝播式
【００４５】
【数１１】
【００４６】このように、推定値を意味する^ （ハッ
ト）付き記号を以下２文字で表す。
ト）付き記号を以下２文字で表す。
【００４７】
【数１２】
【００４８】更新式
【００４９】
【数１３】
【００５０】
【数１４】
【００５１】
【数１５】
【００５２】以上に述べたように、このカルマンフィル
タは、状態変数をランジュバン方程式でモデル化するこ
とにより、単純化したシステム方程式と、有色の観測雑
音を１次マルコフ過程でモデル化することにより、白色
の観測雑音をもつ表示形式に変形した観測方程式とを基
にして構成されている。
タは、状態変数をランジュバン方程式でモデル化するこ
とにより、単純化したシステム方程式と、有色の観測雑
音を１次マルコフ過程でモデル化することにより、白色
の観測雑音をもつ表示形式に変形した観測方程式とを基
にして構成されている。
【００５３】なお、観測雑音が白色雑音と有色雑音を含
む場合は、有色雑音をシステム方程式に組み込むことに
より、前述のカルマンフィルタと同様のカルマンフィル
タが構成できる。
む場合は、有色雑音をシステム方程式に組み込むことに
より、前述のカルマンフィルタと同様のカルマンフィル
タが構成できる。
【００５４】次に、図１に示したハイブリッド航法装置
３の処理手順の一例を、図２を参照して説明する。先
ず、上記システム方程式のＡk=0 ，Ｕk=0 、観測方程式
のＨk=0 ，Ｗk=0 を設定する。Ａk=0 は〔数９〕で示し
たシステム行列であり、相関時定数τ1 で定まり、Ｕ
k=0 は同じく〔数９〕で示した加速度誤差ｕの共分散行
列にΔを乗じたものであり、これらの値は設計要素であ
る。また、Ｈk=0 は〔数１０〕に示した観測行列であ
り、相関時定数τ2 で定まり、Ｗk=0 は〔数６〕で示し
た白色雑音ｗの共分散行列に１／Δを乗じたものであ
り、これらの値も設計要素である。
３の処理手順の一例を、図２を参照して説明する。先
ず、上記システム方程式のＡk=0 ，Ｕk=0 、観測方程式
のＨk=0 ，Ｗk=0 を設定する。Ａk=0 は〔数９〕で示し
たシステム行列であり、相関時定数τ1 で定まり、Ｕ
k=0 は同じく〔数９〕で示した加速度誤差ｕの共分散行
列にΔを乗じたものであり、これらの値は設計要素であ
る。また、Ｈk=0 は〔数１０〕に示した観測行列であ
り、相関時定数τ2 で定まり、Ｗk=0 は〔数６〕で示し
た白色雑音ｗの共分散行列に１／Δを乗じたものであ
り、これらの値も設計要素である。
【００５５】次に、推定値の初期値Ｘ^ k=0 と共分散の
初期値Ｐk=0 を設定する。
初期値Ｐk=0 を設定する。
【００５６】その後、更新タイミングとなれば、ＧＰＳ
受信機２より北方向の位置ＸN ，東方向の位置ＸE ，北
方向の速度ＶN ，東方向の速度ＶE をそれぞれ読み込
む。また、ＩＮＳ航法装置１より北方向の位置ＸCN，東
方向の位置ＸCE，北方向の速度ＶCN，東方向の速度ＶCE
をそれぞれ読み込む。
受信機２より北方向の位置ＸN ，東方向の位置ＸE ，北
方向の速度ＶN ，東方向の速度ＶE をそれぞれ読み込
む。また、ＩＮＳ航法装置１より北方向の位置ＸCN，東
方向の位置ＸCE，北方向の速度ＶCN，東方向の速度ＶCE
をそれぞれ読み込む。
【００５７】続いて、
【００５８】
【数１６】
【００５９】をＺk として求める。また、事前推定値Ｘ
^ k/k-1 を〔数１１〕から求める。ここで〔数１１〕の
Ａk は〔数９〕に示したように、ｋに係わらず一定であ
り、上記設定値Ａk=0 を用いる。
^ k/k-1 を〔数１１〕から求める。ここで〔数１１〕の
Ａk は〔数９〕に示したように、ｋに係わらず一定であ
り、上記設定値Ａk=0 を用いる。
【００６０】続いて、事前共分散Ｐk/k-1 を〔数１２〕
から求める。ここでＵk は〔数９〕に示したように北方
向および東方向の加速度誤差ｕn ，ｕe で決定されるの
で、ｋに係わらず一定であり、上記設定値Ｕk=0 を用い
る。
から求める。ここでＵk は〔数９〕に示したように北方
向および東方向の加速度誤差ｕn ，ｕe で決定されるの
で、ｋに係わらず一定であり、上記設定値Ｕk=0 を用い
る。
【００６１】続いて、フィルタゲインＧk を〔数１３〕
から求める。ここでＨk は〔数１０〕に示したようにｋ
に係わらず一定であり、上記設定値Ｈk=0 を用いる。ま
た、Ｗk も〔数１０〕に示したように、ｋに係わらず一
定であり、上記設定値Ｗk=0を用いる。
から求める。ここでＨk は〔数１０〕に示したようにｋ
に係わらず一定であり、上記設定値Ｈk=0 を用いる。ま
た、Ｗk も〔数１０〕に示したように、ｋに係わらず一
定であり、上記設定値Ｗk=0を用いる。
【００６２】さらに、続いて、事後共分散Ｐk/k を〔数
１５〕から求める。そして、事後推定値Ｘk/k を〔数１
４〕から求め、これを出力する。
１５〕から求める。そして、事後推定値Ｘk/k を〔数１
４〕から求め、これを出力する。
【００６３】以上の処理を更新タイミング毎に繰り返す
ことにより、推定誤差が急速に減少し、図１に示したハ
イブリッド航法装置３から、誤差分の補正された位置お
よび速度のデータが出力される。
ことにより、推定誤差が急速に減少し、図１に示したハ
イブリッド航法装置３から、誤差分の補正された位置お
よび速度のデータが出力される。
【００６４】なお、〔数１１〕，〔数１２〕に示した伝
播式の更新周期をΔ／Ｎ（Ｎ：２以上の整数）として、
カルマンフィルタの更新周期（誤差推定周期）より短時
間周期で、ＩＮＳの位置と速度の推定誤差を、その推移
を補外することにより求め、ＩＮＳの位置と速度を補正
してもよい。
播式の更新周期をΔ／Ｎ（Ｎ：２以上の整数）として、
カルマンフィルタの更新周期（誤差推定周期）より短時
間周期で、ＩＮＳの位置と速度の推定誤差を、その推移
を補外することにより求め、ＩＮＳの位置と速度を補正
してもよい。
【００６５】また、以上に述べた例は、２次元の位置と
速度を求めるものであったが、同様にして３次元の位置
と速度を求めることもできる。
速度を求めるものであったが、同様にして３次元の位置
と速度を求めることもできる。
【００６６】
【発明の効果】この発明によれば、高精度の位置と速度
情報が得られるほか、次の利点がある。 (1) システム方程式をランジュバン方程式でモデル化す
ることにより、システム方程式形式が単純となるため、
設計要素が少なくなって、カルマンフィルタのチューニ
ングが容易になる。また、システム方程式の形式が単純
となるため、カルマンフィルタのチューニング時のシミ
ュレーションと実際の環境とが少し異なっていても、実
際の運用時に大きな推定誤差が生じることがなく、いわ
ゆるロバスト性も高まる。
情報が得られるほか、次の利点がある。 (1) システム方程式をランジュバン方程式でモデル化す
ることにより、システム方程式形式が単純となるため、
設計要素が少なくなって、カルマンフィルタのチューニ
ングが容易になる。また、システム方程式の形式が単純
となるため、カルマンフィルタのチューニング時のシミ
ュレーションと実際の環境とが少し異なっていても、実
際の運用時に大きな推定誤差が生じることがなく、いわ
ゆるロバスト性も高まる。
【００６７】(2) 有色の観測雑音を１次マルコフ過程で
モデル化し、観測方程式を白色の観測雑音をもつ表示形
式に変形することにより、カルマンフィルタの更新周期
は、観測雑音の相関時定数に制限されることなく、任意
に設定できる。そのため、カルマンフィルタの更新周期
を短くでき、誤差推定の精度が劣化することもない。
モデル化し、観測方程式を白色の観測雑音をもつ表示形
式に変形することにより、カルマンフィルタの更新周期
は、観測雑音の相関時定数に制限されることなく、任意
に設定できる。そのため、カルマンフィルタの更新周期
を短くでき、誤差推定の精度が劣化することもない。
【００６８】(3) 設計要素はプラットフォーム方式のＩ
ＮＳと、ストラップダウン方式のＩＮＳとで異なるが、
カルマンフィルタのアルゴリズムは同一となるので、カ
ルマンフィルタの演算処理部分を変えることなく、いず
れのタイプの慣性航法装置を用いたハイブリッド航法装
置にも適用できる。
ＮＳと、ストラップダウン方式のＩＮＳとで異なるが、
カルマンフィルタのアルゴリズムは同一となるので、カ
ルマンフィルタの演算処理部分を変えることなく、いず
れのタイプの慣性航法装置を用いたハイブリッド航法装
置にも適用できる。
【図１】この発明の実施形態に係るハイブリッド航法装
置の構成を示す図
置の構成を示す図
【図２】同ハイブリッド航法装置における処理手順を示
すフローチャート
すフローチャート
【図３】カルマンフィルタのチューニング結果の例を示
す図
す図
１−ＩＮＳ航法装置 ２−ＧＰＳ受信機 ３−ハイブリッド航法装置 ４−カルマンフィルタ ５−加算器
Claims (2)
- 【請求項１】 電波航法装置と慣性航法装置の測定値か
ら、カルマンフィルタにて慣性航法装置の測定誤差を推
定し、補正するハイブリッド航法において、 慣性航法装置の測定誤差の生成過程をランジュバン方程
式でモデル化した、測定誤差を状態変数とするベクトル
Ｘ、相関時定数により定まるシステム行列Ａおよびシス
テム雑音ｕによるシステム方程式を離散時間系に変換し
た式Ｘk ＝Ａk- 1 Ｘk-1 ＋ｕk-1 （ｋは時間ステップ）
を離散系システム方程式とし、 電波航法装置の有色雑音である測定誤差をマルコフ過程
でモデル化して観測雑音を白色雑音とした、前記ベクト
ルＸ、観測行列Ｈおよび観測雑音ｗによる観測方程式を
離散時間系に変換した式Ｚk ＝Ｈk Ｘk ＋ｗk （ｋは時
間ステップ）を離散系観測方程式とし、 前記離散系システム方程式と前記離散系観測方程式とか
ら構成したカルマンフィルタを用いたハイブリッド航
法。 - 【請求項２】 電波航法装置と慣性航法装置の測定値を
それぞれ入力し、カルマンフィルタにて慣性航法装置の
測定誤差を推定し、補正するハイブリッド航法装置にお
いて、 慣性航法装置の測定誤差の生成過程をランジュバン方程
式でモデル化した、測定誤差を状態変数とするベクトル
Ｘ、相関時定数により定まるシステム行列Ａおよびシス
テム雑音ｕによるシステム方程式を離散時間系に変換し
た式Ｘk ＝Ａk- 1 Ｘk-1 ＋ｕk-1 （ｋは時間ステップ）
を離散系システム方程式とし、 電波航法装置の有色雑音である測定誤差をマルコフ過程
でモデル化して観測雑音を白色雑音とした、前記ベクト
ルＸ、観測行列Ｈおよび観測雑音ｗによる観測方程式を
離散時間系に変換した式Ｚk ＝Ｈk Ｘk ＋ｗk （ｋは時
間ステップ）を離散系観測方程式とし、 前記離散系システム方程式と前記離散系観測方程式とか
ら前記カルマンフィルタを構成したハイブリッド航法装
置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP35992899A JP4145451B2 (ja) | 1999-12-17 | 1999-12-17 | ハイブリッド航法およびその装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP35992899A JP4145451B2 (ja) | 1999-12-17 | 1999-12-17 | ハイブリッド航法およびその装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2001174275A true JP2001174275A (ja) | 2001-06-29 |
| JP4145451B2 JP4145451B2 (ja) | 2008-09-03 |
Family
ID=18467028
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP35992899A Expired - Fee Related JP4145451B2 (ja) | 1999-12-17 | 1999-12-17 | ハイブリッド航法およびその装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP4145451B2 (ja) |
Cited By (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2007163335A (ja) * | 2005-12-15 | 2007-06-28 | Mitsubishi Electric Corp | 姿勢標定装置、姿勢標定方法および姿勢標定プログラム |
| JP2008039690A (ja) * | 2006-08-09 | 2008-02-21 | Toyota Motor Corp | 搬送波位相式測位装置 |
| WO2008117492A1 (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co., Ltd. | Gps複合航法装置 |
| JPWO2007105294A1 (ja) * | 2006-03-13 | 2009-07-23 | 中菱エンジニアリング株式会社 | ビークル走行軌跡計測装置 |
| JP2012042318A (ja) * | 2010-08-18 | 2012-03-01 | Ono Sokki Co Ltd | 移動体高精度速度計測装置及び方法 |
| JP2017156186A (ja) * | 2016-03-01 | 2017-09-07 | クラリオン株式会社 | 車載装置、及び、推定方法 |
| CN107228905A (zh) * | 2017-06-02 | 2017-10-03 | 东莞理工学院 | 基于双稳态系统的超声导波信号检测方法 |
| JP2017194302A (ja) * | 2016-04-19 | 2017-10-26 | クラリオン株式会社 | 位置推定装置、及び、推定方法 |
| US10288746B2 (en) | 2014-03-25 | 2019-05-14 | Seiko Epson Corporation | Error estimation method, motion analysis method, error estimation apparatus, and program |
| JP2019078596A (ja) * | 2017-10-23 | 2019-05-23 | 国立研究開発法人宇宙航空研究開発機構 | 飛行体用航法装置および飛行体制御方法 |
| JP2019532307A (ja) * | 2016-09-27 | 2019-11-07 | ビーエイイー・システムズ・インフォメーション・アンド・エレクトロニック・システムズ・インテグレイション・インコーポレーテッド | Ｇｐｓ妨害およびスプーフィングの検出 |
| CN112269200A (zh) * | 2020-10-14 | 2021-01-26 | 北京航空航天大学 | 一种基于可观测度的惯性/卫星系统自适应混合校正方法 |
| CN113551665A (zh) * | 2021-06-25 | 2021-10-26 | 中国科学院国家空间科学中心 | 一种用于运动载体的高动态运动状态感知系统及感知方法 |
| JP2023160046A (ja) * | 2022-04-21 | 2023-11-02 | ヤマハ発動機株式会社 | 船舶の速度制御方法及び船舶 |
| CN118091695A (zh) * | 2024-04-23 | 2024-05-28 | 山东哈工卓越智能有限公司 | 基于激光雷达的车厢边界预估方法、系统、装置及介质 |
-
1999
- 1999-12-17 JP JP35992899A patent/JP4145451B2/ja not_active Expired - Fee Related
Cited By (23)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2007163335A (ja) * | 2005-12-15 | 2007-06-28 | Mitsubishi Electric Corp | 姿勢標定装置、姿勢標定方法および姿勢標定プログラム |
| JPWO2007105294A1 (ja) * | 2006-03-13 | 2009-07-23 | 中菱エンジニアリング株式会社 | ビークル走行軌跡計測装置 |
| JP4846784B2 (ja) * | 2006-03-13 | 2011-12-28 | 中菱エンジニアリング株式会社 | ビークル走行軌跡計測装置 |
| JP2008039690A (ja) * | 2006-08-09 | 2008-02-21 | Toyota Motor Corp | 搬送波位相式測位装置 |
| WO2008117492A1 (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co., Ltd. | Gps複合航法装置 |
| US8649977B2 (en) | 2007-03-22 | 2014-02-11 | Furuno Electric Company Limited | GPS composite navigation apparatus |
| JP2012042318A (ja) * | 2010-08-18 | 2012-03-01 | Ono Sokki Co Ltd | 移動体高精度速度計測装置及び方法 |
| US10288746B2 (en) | 2014-03-25 | 2019-05-14 | Seiko Epson Corporation | Error estimation method, motion analysis method, error estimation apparatus, and program |
| JP2017156186A (ja) * | 2016-03-01 | 2017-09-07 | クラリオン株式会社 | 車載装置、及び、推定方法 |
| WO2017150106A1 (ja) * | 2016-03-01 | 2017-09-08 | クラリオン株式会社 | 車載装置、及び、推定方法 |
| US11036231B2 (en) | 2016-03-01 | 2021-06-15 | Clarion Co., Ltd. | In-vehicle device and estimation method |
| JP2017194302A (ja) * | 2016-04-19 | 2017-10-26 | クラリオン株式会社 | 位置推定装置、及び、推定方法 |
| US10620320B2 (en) | 2016-04-19 | 2020-04-14 | Clarion Co., Ltd. | Position estimation system and estimation method |
| JP2019532307A (ja) * | 2016-09-27 | 2019-11-07 | ビーエイイー・システムズ・インフォメーション・アンド・エレクトロニック・システムズ・インテグレイション・インコーポレーテッド | Ｇｐｓ妨害およびスプーフィングの検出 |
| CN107228905A (zh) * | 2017-06-02 | 2017-10-03 | 东莞理工学院 | 基于双稳态系统的超声导波信号检测方法 |
| JP2019078596A (ja) * | 2017-10-23 | 2019-05-23 | 国立研究開発法人宇宙航空研究開発機構 | 飛行体用航法装置および飛行体制御方法 |
| CN112269200A (zh) * | 2020-10-14 | 2021-01-26 | 北京航空航天大学 | 一种基于可观测度的惯性/卫星系统自适应混合校正方法 |
| CN112269200B (zh) * | 2020-10-14 | 2024-05-17 | 北京航空航天大学 | 一种基于可观测度的惯性/卫星系统自适应混合校正方法 |
| CN113551665A (zh) * | 2021-06-25 | 2021-10-26 | 中国科学院国家空间科学中心 | 一种用于运动载体的高动态运动状态感知系统及感知方法 |
| CN113551665B (zh) * | 2021-06-25 | 2023-08-11 | 中国科学院国家空间科学中心 | 一种用于运动载体的高动态运动状态感知系统及感知方法 |
| JP2023160046A (ja) * | 2022-04-21 | 2023-11-02 | ヤマハ発動機株式会社 | 船舶の速度制御方法及び船舶 |
| JP7439162B2 (ja) | 2022-04-21 | 2024-02-27 | ヤマハ発動機株式会社 | 船舶の速度制御方法及び船舶 |
| CN118091695A (zh) * | 2024-04-23 | 2024-05-28 | 山东哈工卓越智能有限公司 | 基于激光雷达的车厢边界预估方法、系统、装置及介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP4145451B2 (ja) | 2008-09-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4199553B2 (ja) | ハイブリッド航法装置 | |
| CN103630137B (zh) | 一种用于导航系统的姿态及航向角的校正方法 | |
| CN112097763B (zh) | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 | |
| JP4145451B2 (ja) | ハイブリッド航法およびその装置 | |
| US20180095159A1 (en) | Method for tracking the navigation of a mobile carrier with an extended kalman filter | |
| KR101209667B1 (ko) | 네비게이션 어플리케이션을 위한 개선된 지피에스 ａｄｒ처리 방법 | |
| CN110779518B (zh) | 一种具有全局收敛性的水下航行器单信标定位方法 | |
| US6711517B2 (en) | Hybrid inertial navigation method and device | |
| US8560234B2 (en) | System and method of navigation based on state estimation using a stepped filter | |
| CN103344260B (zh) | 基于rbckf的捷联惯导系统大方位失准角初始对准方法 | |
| EP2679954A1 (en) | Sequential estimation in a real-time positioning or navigation system using historical states | |
| CN107110650A (zh) | 在能观测性方面受约束的导航状态的估计方法 | |
| JP2009098125A (ja) | 動的に較正されるセンサデータと、ナビゲーションシステム内の繰り返し拡張カルマンフィルタとを使用する、ジャイロコンパスの整合用のシステム及び方法 | |
| JP2007537447A (ja) | 複数のナビゲーションコンポーネントの整合システム及び方法 | |
| CN112857398B (zh) | 一种系泊状态下舰船的快速初始对准方法和装置 | |
| CN113783652B (zh) | 一种组合导航系统的数据同步方法和装置 | |
| CN104713559B (zh) | 一种高精度sins模拟器的设计方法 | |
| CN110567455B (zh) | 一种求积更新容积卡尔曼滤波的紧组合导航方法 | |
| CN105091907A (zh) | Sins/dvl组合中dvl方位安装误差估计方法 | |
| CN103218482B (zh) | 一种动力学系统中不确定参数的估计方法 | |
| JP2020056701A (ja) | 航法装置、航行支援情報の生成方法、および、航行支援情報の生成プログラム | |
| JP5664059B2 (ja) | 車両用軌跡推定装置 | |
| Esswein et al. | GPS spoofing resilience via NMA/watermarks authentication and IMU prediction | |
| CN103901459B (zh) | 一种mems/gps组合导航系统中量测滞后的滤波方法 | |
| JP2010096647A (ja) | 航法装置及び推定方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20050408 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20080311 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20080603 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20080618 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 Ref document number: 4145451 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110627 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120627 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130627 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140627 Year of fee payment: 6 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| LAPS | Cancellation because of no payment of annual fees |