JP2023136095A - 移動体姿勢計測装置及び移動体姿勢計測プログラム - Google Patents

Abstract

【課題】本開示は、ジャイロセンサの角速度情報及び衛星航法システムの観測情報に基づいて、移動体の姿勢情報を状態空間モデルの状態ベクトルとして計測するにあたり、衛星信号の受信環境が劣化しているときでも、移動体の姿勢が動揺しているときでも、移動体の姿勢情報を高精度に計測することを目的とする。【解決手段】本開示は、ジャイロセンサＧの角速度情報に基づいて、状態空間モデルの状態方程式を用いて、移動体の姿勢情報の予測値を計算する姿勢予測部と、衛星航法システムの観測情報に基づいて、状態空間モデルの観測方程式を用いて、移動体の姿勢情報の補正値を計算する姿勢補正部と、ジャイロセンサＧの角速度情報に基づいて、状態空間モデルを用いずに、ジャイロセンサＧの角速度情報の誤差分散を推定する角速度誤差分散推定部２と、を備えることを特徴とする移動体姿勢計測装置Ｄである。【選択図】図２

Description

本開示は、ジャイロセンサの角速度情報及び衛星航法システムの観測情報に基づいて、移動体の姿勢情報を状態空間モデルの状態ベクトルとして計測する技術に関する。

衛星航法システムの観測情報に基づいて、移動体の姿勢情報を複数アンテナ間の基線ベクトルとして計測する技術が、特許文献１に開示されている。特許文献１では、衛星信号の観測値のみを使用するため、衛星信号の受信環境が劣化しているときには、移動体の姿勢情報を計測することができない。

ジャイロセンサの角速度情報及び衛星航法システムの観測情報に基づいて、移動体の姿勢情報を状態空間モデルの状態ベクトルとして計測する技術が、特許文献２に開示されている。特許文献２では、衛星信号の受信環境が劣化しているときでも、ジャイロセンサの角速度情報を用いて、移動体の姿勢情報を計測することができる。

特許第３５０２００７号公報 米国特許第７２９２１８５号明細書 特許第５０５２８４５号公報

特許文献２では、ジャイロセンサの角速度情報のバイアス誤差を低減するために、衛星航法システムの観測情報に基づく状態空間モデルを用いて、移動体の姿勢情報を推定するとともに、ジャイロセンサの角速度情報のバイアス誤差を推定している。しかし、衛星信号の受信環境が劣化しているときには、ジャイロセンサの角速度情報のバイアス誤差を高精度に推定することができず、ジャイロセンサのバイアス誤差除去後の角速度情報を高精度に出力することができず、移動体の姿勢情報を高精度に計測することができない。

そこで、前記課題を解決するために、本開示は、ジャイロセンサの角速度情報及び衛星航法システムの観測情報に基づいて、移動体の姿勢情報を状態空間モデルの状態ベクトルとして計測するにあたり、衛星信号の受信環境が劣化しているときでも、移動体の姿勢が動揺しているときでも、移動体の姿勢情報を高精度に計測することを目的とする。

前記課題を解決するために、衛星航法システムの観測情報に基づく状態空間モデルを用いずに、現実に得られたジャイロセンサの角速度情報に基づいて、ジャイロセンサの角速度情報の誤差分散を現実に即して推定する。そして、ジャイロセンサの角速度情報に基づいて、状態空間モデルの状態方程式を用いて、移動体の姿勢情報の予測値を計算する。さらに、衛星航法システムの観測情報に基づいて、状態空間モデルの観測方程式を用いて、移動体の姿勢情報の補正値を計算する。ここで、状態空間モデルの状態方程式において、現実に即したジャイロセンサの角速度情報の誤差分散が大きい又は小さいほど、状態空間モデルの状態方程式のプロセス雑音を現実に即してそれぞれ大きく又は小さく推定する。

具体的には、本開示は、ジャイロセンサの角速度情報及び衛星航法システムの観測情報に基づいて、移動体の姿勢情報を状態空間モデルの状態ベクトルとして計測する移動体姿勢計測装置であって、前記ジャイロセンサの角速度情報に基づいて、前記状態空間モデルの状態方程式を用いて、前記移動体の姿勢情報の予測値を計算する姿勢予測部と、前記衛星航法システムの観測情報に基づいて、前記状態空間モデルの観測方程式を用いて、前記移動体の姿勢情報の補正値を計算する姿勢補正部と、前記ジャイロセンサの角速度情報に基づいて、前記状態空間モデルを用いずに、前記ジャイロセンサの角速度情報の誤差分散を推定する角速度誤差分散推定部と、を備え、前記姿勢予測部は、前記ジャイロセンサの角速度情報の誤差分散が大きい又は小さいほど、前記姿勢予測部のプロセス雑音をそれぞれ大きく又は小さく推定することを特徴とする移動体姿勢計測装置である。

この構成によれば、移動体の姿勢が動揺しているときには、衛星信号の受信環境の劣化時でも、ジャイロセンサの角速度情報の誤差分散を大きく推定したうえで、状態空間モデルの状態方程式のプロセス雑音を大きく推定する。一方で、移動体の姿勢が静止しているときには、衛星信号の受信環境の劣化時でも、ジャイロセンサの角速度情報の誤差分散を小さく推定したうえで、状態空間モデルの状態方程式のプロセス雑音を小さく推定する。

そして、移動体の姿勢が動揺しているときには、状態空間モデルの観測方程式の姿勢補正ゲインを大きく計算したうえで、ジャイロセンサの角速度情報と比べて衛星航法システムの観測情報を重視して、移動体の姿勢情報を高精度に計測することができる。一方で、移動体の姿勢が静止しているときには、状態空間モデルの観測方程式の姿勢補正ゲインを小さく計算したうえで、衛星航法システムの観測情報と比べてジャイロセンサの角速度情報を重視して、移動体の姿勢情報を高精度に計測することができる。

つまり、移動体の姿勢情報の補正値は、推定したプロセス雑音及び衛星航法システムの観測情報の観測雑音に基づいた姿勢補正ゲインによって、ジャイロセンサの角速度情報及び衛星航法システムの観測情報が最適に重み付け合成されるようなかたちで計算される。このように、ジャイロセンサの角速度情報及び衛星航法システムの観測情報に基づいて、移動体の姿勢情報を状態空間モデルの状態ベクトルとして計測するにあたり、衛星信号の受信環境が劣化しているときでも、移動体の姿勢が動揺しているときでも、ジャイロセンサの角速度情報及び衛星航法システムの観測情報の重視バランスが状況変化に応じて適切に調整されるため、移動体の姿勢情報を高精度に計測することができる。

また、本開示は、前記角速度誤差分散推定部は、前記ジャイロセンサの角速度情報を、異なる通過特性を有する複数のフィルタに入力し、前記複数のフィルタの出力の差分が大きい又は小さいほど、前記ジャイロセンサの角速度情報の誤差分散をそれぞれ大きく又は小さく推定することを特徴とする移動体姿勢計測装置である。

この構成によれば、移動体の姿勢が動揺しているときには、ジャイロセンサの角速度情報の誤差分散を大きく推定することができる。一方で、移動体の姿勢が静止しているときには、ジャイロセンサの角速度情報の誤差分散を小さく推定することができる。

また、本開示は、前記移動体の姿勢情報の予測値と、前記移動体の姿勢情報の補正値と、に基づいて、前記移動体の姿勢情報の誤差分散を推定する姿勢誤差分散推定部、をさらに備え、前記姿勢予測部は、前記移動体の姿勢情報の誤差分散が大きい又は小さいほど、前記姿勢予測部のプロセス雑音をそれぞれ小さく又は大きく推定することを特徴とする移動体姿勢計測装置である。

この構成によれば、衛星信号の受信環境が劣化しているときには、移動体の姿勢情報の誤差分散を大きく推定したうえで、トレードオフパラメータとして、状態空間モデルの状態方程式のプロセス雑音を小さく推定する。一方で、衛星航法システムの受信環境が良好であるときには、移動体の姿勢情報の誤差分散を小さく推定したうえで、トレードオフパラメータとして、状態空間モデルの状態方程式のプロセス雑音を大きく推定する。

そして、衛星信号の受信環境が劣化しているときには、状態空間モデルの観測方程式の姿勢補正ゲインを小さく計算したうえで、衛星航法システムの観測情報と比べてジャイロセンサの角速度情報を重視して、移動体の姿勢情報を高精度に計測することができる。一方で、衛星航法システムの受信環境が良好であるときには、状態空間モデルの観測方程式の姿勢補正ゲインを大きく計算したうえで、ジャイロセンサの角速度情報と比べて衛星航法システムの観測情報を重視して、移動体の姿勢情報を高精度に計測することができる。

また、本開示は、前記姿勢予測部は、前記移動体の姿勢情報の誤差分散の逆数と、前記ジャイロセンサの角速度情報の誤差分散と、の和を、前記姿勢予測部のプロセス雑音として推定することを特徴とする移動体姿勢計測装置である。

この構成によれば、衛星信号の受信環境が劣化している、又は、移動体の姿勢が静止しているときには、状態空間モデルの状態方程式のプロセス雑音を小さく推定することができる。一方で、衛星信号の受信環境が良好である、又は、移動体の姿勢が動揺しているときには、状態空間モデルの状態方程式のプロセス雑音を大きく推定することができる。

また、本開示は、前記衛星航法システムの観測情報が、取得不能になったときに、前記姿勢補正部が、前記移動体の姿勢情報の補正ゲインを０に維持したうえで、前記姿勢予測部は、前記移動体の姿勢情報の予測値の誤差分散に、前記姿勢予測部のプロセス雑音を加算し続け、前記衛星航法システムの観測情報が、取得可能になったときに、前記姿勢補正部は、前記移動体の姿勢情報の予測値の誤差分散に基づいて、前記移動体の姿勢情報の補正ゲインを計算し始めたうえで、前記移動体の姿勢情報の補正値を計算し始めることを特徴とする移動体姿勢計測装置である。

この構成によれば、衛星信号の受信環境が中断した後には、衛星信号の中断時間及び移動体の動揺状態に応じて、移動体の姿勢情報の予測値の誤差分散を大きく推定することができる。そして、衛星信号の受信環境が回復した直後には、衛星信号の中断時間及び移動体の動揺状態に応じて、状態空間モデルの観測方程式の姿勢補正ゲインを大きく計算することができる。つまり、衛星信号の受信環境が回復した直後には、姿勢補正ゲインが大きくなることで、衛星信号の中断時間及び移動体の動揺状態に応じて蓄積する移動体の姿勢情報の予測値の誤差を是正し、移動体の姿勢情報の予測値の誤差が是正された後には、移動体の姿勢情報の補正値の誤差分散を早めに収束させることで、衛星信号の受信環境が回復した後に素早く移動体の姿勢情報を高精度に計測することができる。

また、本開示は、前記ジャイロセンサの角速度情報は、より高いレートで更新されており、前記衛星航法システムの観測情報は、より低いレートで更新されており、前記姿勢予測部は、前記ジャイロセンサの角速度情報のより短い更新間隔で処理を実行し、前記姿勢補正部は、前記衛星航法システムの観測情報のより長い更新間隔でのみ処理を実行することを特徴とする移動体姿勢計測装置である。

この構成によれば、ジャイロセンサの角速度情報が衛星航法システムの観測情報と比べて高いレートで更新されたとしても、逆行列計算を含まず処理負荷の軽い姿勢予測のみを実行し、逆行列計算を含み処理負荷の重い姿勢補正を実行しない。よって、組み込み用の非力なＣＰＵを用いるとしても、処理負荷オーバの問題に対処したうえで、最新の移動体の姿勢情報の予測値及び最適な移動体の姿勢情報の補正値を提供することができる。

また、本開示は、以上に記載の移動体姿勢計測装置が備える、各処理部が行なう各処理ステップを、コンピュータに実行させるための移動体姿勢計測プログラムである。

この構成によれば、以上に記載の効果を有するプログラムを提供することができる。

このように、本開示は、ジャイロセンサの角速度情報及び衛星航法システムの観測情報に基づいて、移動体の姿勢情報を状態空間モデルの状態ベクトルとして計測するにあたり、衛星信号の受信環境が劣化しているときでも、移動体の姿勢が動揺しているときでも、移動体の姿勢情報を高精度に計測することができる。

本開示の移動体姿勢計測装置の搭載を示す図である。 本開示の移動体姿勢計測装置の構成を示す図である。 本開示の姿勢計算部の構成を示す図である。 本開示の姿勢予測部の処理を示す図である。 本開示の角速度誤差分散推定部の構成を示す図である。 本開示の角速度誤差分散推定部の処理を示す図である。 本開示の姿勢補正部の処理を示す図である。 本開示の受信環境に応じた姿勢補正ゲインの調整を示す図である。 本開示の動揺状態に応じた姿勢補正ゲインの調整を示す図である。 本開示のＧＮＳＳ情報の取得可否に応じた処理を示す図である。 本開示のＧＮＳＳ情報の取得可否に応じた処理を示す図である。 本開示の各々の情報の更新間隔に応じた処理を示す図である。 本開示の各々の情報の更新間隔に応じた処理を示す図である。

添付の図面を参照して本開示の実施形態を説明する。以下に説明する実施形態は本開示の実施の例であり、本開示は以下の実施形態に制限されるものではない。

（本開示の移動体姿勢計測装置の構成）
本開示の移動体姿勢計測装置の搭載を図１に示す。移動体姿勢計測装置Ｄは、船舶Ｓ等の移動体に搭載され、ジャイロセンサＧ及び受信アンテナＡ１、Ａ２、Ａ３を備える。受信アンテナＡ１は、基準アンテナであり、基線ベクトルＢ１は、受信アンテナＡ１、Ａ２間ベクトルであり、基線ベクトルＢ２は、受信アンテナＡ１、Ａ３間ベクトルである。

本開示の移動体姿勢計測装置の構成を図２に示す。移動体姿勢計測装置Ｄは、ＧＮＳＳ受信機Ｒ１、Ｒ２、Ｒ３、角速度バイアス除去部１、角速度誤差分散推定部２、姿勢計算部３、搬送波位相差計算部４及びアンビギュイティ推定部５をさらに備える。本開示の姿勢計算部の構成を図３に示す。姿勢計算部３は、姿勢予測部３１、姿勢補正部３２及び姿勢誤差分散推定部３３を備える。角速度バイアス除去部１、角速度誤差分散推定部２、姿勢計算部３、搬送波位相差計算部４及びアンビギュイティ推定部５は、図４、７、１０、１２に示すプログラムをコンピュータにインストールし実現することができる。

移動体姿勢計測装置Ｄは、ジャイロセンサＧの角速度情報及び衛星航法システムの観測情報に基づいて、船舶Ｓ等の移動体の姿勢情報を状態空間モデルの状態ベクトルとして計測する。ここで、衛星航法システムは、ＧＰＳ、Ｇａｌｉｌｅｏ、ＧＬＯＮＡＳＳ、ＢｅｉＤｏｕ又はＱＺＳＳ等を用いればよい。本開示では、非力なＣＰＵが組み込まれることを考慮して、状態空間モデルは線形性を仮定したカルマンフィルタで解いている。変形例では、高性能なＣＰＵが組み込まれることを考慮して、状態空間モデルは非線形性を許容したｕｎｓｃｅｎｔｅｄカルマンフィルタ又はパーティクルフィルタ等で解いてもよい。

姿勢予測部３１は、ジャイロセンサＧの角速度情報に基づいて、状態空間モデルの状態方程式を用いて、船舶Ｓ等の移動体の姿勢情報の予測値を計算する。姿勢補正部３２は、衛星航法システムの観測情報に基づいて、状態空間モデルの観測方程式を用いて、船舶Ｓ等の移動体の姿勢情報の補正値を計算する。つまり、姿勢計算部３は、受信アンテナＡ１、Ａ２、Ａ３の幾何学配置を拘束したうえで、ジャイロセンサＧの角速度情報及び衛星航法システムの観測情報をカルマンフィルタにおいてタイトカップリングしている。

以下に示す実施形態では、最初に、姿勢予測部３１の処理について説明する。次に、姿勢補正部３２の処理について説明する。次に、ＧＮＳＳ情報の取得可否に応じた処理について説明する。最後に、各々の情報の更新間隔に応じた処理について説明する。

（本開示の姿勢予測部の処理）
本開示の姿勢予測部の処理を図４に示す。姿勢予測部３１は、ジャイロセンサＧの角速度情報に基づいて、状態空間モデルの状態方程式を用いて、船舶Ｓ等の移動体の姿勢情報の予測値を計算する。カルマンフィルタの処理は、数式１、２のように表される。

ｘ_ｎ ^－はタイミングｎでの移動体の姿勢情報の予測値であり、ｘ_ｎ－１はタイミングｎ－１での移動体の姿勢情報の補正値であり、Ｆは時間遷移行列である。Ｐ_ｎ ^－はタイミングｎでの移動体の姿勢情報の予測値の誤差共分散行列であり、Ｐ_ｎ－１はタイミングｎ－１での移動体の姿勢情報の補正値の誤差共分散行列であり、Ｑ_ｎはプロセス雑音である。

ジャイロセンサＧは、移動体の姿勢変化に応じて、３軸（ジャイロセンサＧの座標系のｘ軸、ｙ軸及びｚ軸）の角速度情報を出力する。角速度バイアス除去部１及び角速度誤差分散推定部２は、ジャイロセンサＧの角速度情報を取得する（ステップＳ１）。

角速度バイアス除去部１は、衛星航法システムの観測情報に基づく状態空間モデルを用いずに、現実に得られたジャイロセンサＧの角速度情報に基づいて、ジャイロセンサＧの角速度情報のバイアス誤差を現実に即して除去する（ステップＳ２）。

ジャイロセンサＧの角速度情報のバイアス誤差は、ジャイロセンサＧの角速度情報に重畳するオフセット誤差である。角速度バイアス除去部１は、ジャイロセンサＧの角速度情報の静的なバイアス誤差を除去するために、移動体の静止中でのジャイロセンサＧの角速度情報のオフセット誤差を測定してもよい。角速度バイアス除去部１は、ジャイロセンサＧの角速度情報の動的なバイアス誤差を除去するために、ジャイロセンサＧの角速度情報の低周波成分を相補フィルタ又はハイパスフィルタ等により除去してもよい。

角速度誤差分散推定部２は、衛星航法システムの観測情報に基づく状態空間モデルを用いずに、現実に得られたジャイロセンサＧのバイアス誤差除去後の角速度情報に基づいて、ジャイロセンサＧの角速度情報の誤差分散を現実に即して推定する（ステップＳ３）。

本開示の角速度誤差分散推定部の構成を図５に示す。角速度誤差分散推定部２は、フィルタ部２１Ａ、２１Ｂ及び角速度誤差分散計算部２２を備える。

フィルタ部２１Ａ、２１Ｂは、ジャイロセンサＧのバイアス誤差除去後の角速度情報を入力する。フィルタ部２１Ａ、２１Ｂは、異なる通過特性を有するような、ローパスフィルタ又はメディアンフィルタ等である。フィルタ部２１Ａの時定数は、移動体の動揺周期と比べて長く、フィルタ部２１Ｂの時定数は、移動体の動揺周期と比べて短い。

角速度誤差分散計算部２２は、フィルタ部２１Ａ、２１Ｂの出力を入力する。角速度誤差分散計算部２２は、フィルタ部２１Ａ、２１Ｂの出力の差分が大きい又は小さいほど、ジャイロセンサＧの角速度情報の誤差分散をそれぞれ大きく又は小さく推定する。

本開示の角速度誤差分散推定部の処理を図６に示す。移動体の姿勢が静止しているときには、フィルタ部２１Ａ、２１Ｂの出力の差分が小さいため、角速度誤差分散計算部２２は、ジャイロセンサＧの角速度情報の誤差分散を小さく推定する。移動体の姿勢が動揺しているときには、フィルタ部２１Ａ、２１Ｂの出力の差分が大きいため、角速度誤差分散計算部２２は、ジャイロセンサＧの角速度情報の誤差分散を大きく推定する。

移動体の姿勢が動揺しているときには、移動体の姿勢が静止しているときと比べて、ジャイロセンサＧの角速度情報の誤差分散が大きい理由として、以下の理由が挙げられる。第１の理由として、ジャイロセンサＧの角速度情報のバイアス誤差は、ゆっくりした変動であるので、ジャイロセンサＧの角速度情報の周波数領域における低周波成分に相当する。ここで、ジャイロセンサＧの角速度情報の周波数領域における低周波成分は、移動体の動揺によって生じる低周波成分も重畳する。よって、ジャイロセンサＧの角速度情報の誤差分散は、移動体が動揺しているときは、バイアス誤差除去後（低周波成分除去後）であっても、大きくなるのである。第２の理由として、ジャイロセンサＧの角速度情報は、ジャイロセンサＧのスケールファクタ誤差が重畳するため、ジャイロセンサＧの角速度情報が大きいほど、ジャイロセンサＧのスケールファクタ誤差が大きいためである。

角速度誤差分散計算部２２は、ジャイロセンサＧの角速度情報の誤差分散を数式３～６により計算する。σ_ｇ＿ｘ ^２、σ_ｇ＿ｙ ^２、σ_ｇ＿ｚ ^２は、ジャイロセンサＧの角速度情報の誤差分散のｘ軸成分、ｙ軸成分、ｚ軸成分であり、σ_ｇ＿ｘ０ ^２、σ_ｇ＿ｙ０ ^２、σ_ｇ＿ｚ０ ^２は、ジャイロセンサＧの角速度情報の誤差分散のｘ軸成分、ｙ軸成分、ｚ軸成分の初期設定値である。

α_ｇ＿ｘ、α_ｇ＿ｙ、α_ｇ＿ｚは、数式４により計算される。ｂ_ｘ＿ａ、ｂ_ｙ＿ａ、ｂ_ｚ＿ａは、フィルタ部２１Ａの出力のｘ軸成分、ｙ軸成分、ｚ軸成分であり、ｂ_ｘ＿ｂ、ｂ_ｙ＿ｂ、ｂ_ｚ＿ｂは、フィルタ部２１Ｂの出力のｘ軸成分、ｙ軸成分、ｚ軸成分であり、ＴＨ_ｇ＿ｘ、ＴＨ_ｇ＿ｙ、ＴＨ_ｇ＿ｚは、所定閾値のｘ軸成分、ｙ軸成分、ｚ軸成分である。フィルタ部２１Ａ、２１Ｂの出力の差分が所定閾値と比べて大きいときには、α_ｇ＿ｘ、α_ｇ＿ｙ、α_ｇ＿ｚが１より大きく計算され、ジャイロセンサＧの角速度情報の誤差分散が大きく計算される。フィルタ部２１Ａ、２１Ｂの出力の差分が所定閾値と比べて小さいときには、α_ｇ＿ｘ、α_ｇ＿ｙ、α_ｇ＿ｚが１に等しく計算され、ジャイロセンサＧの角速度情報の誤差分散が小さく計算される。

α_ｇ＿ｘ、α_ｇ＿ｙ、α_ｇ＿ｚは、数式５によっても計算される。ｂ_ｘ＿ａ、ｂ_ｙ＿ａ、ｂ_ｚ＿ａは、フィルタ部２１Ａの出力のｘ軸成分、ｙ軸成分、ｚ軸成分であり、ｂ_ｘ＿ｂ、ｂ_ｙ＿ｂ、ｂ_ｚ＿ｂは、フィルタ部２１Ｂの出力のｘ軸成分、ｙ軸成分、ｚ軸成分である。フィルタ部２１Ａ、２１Ｂの出力の差分に等しく、α_ｇ＿ｘ、α_ｇ＿ｙ、α_ｇ＿ｚが計算され、フィルタ部２１Ａ、２１Ｂの出力の差分に比例して、ジャイロセンサＧの角速度情報の誤差分散が計算される。

α_ｇ＿ｘ、α_ｇ＿ｙ、α_ｇ＿ｚは、数式６によっても計算される。ｂ_ｘ、ｂ_ｙ、ｂ_ｚは、角速度バイアス除去部１の出力のｘ軸成分、ｙ軸成分、ｚ軸成分であり、ＳＦは、ジャイロセンサＧのスケールファクタである。角速度バイアス除去部１の出力にジャイロセンサＧのスケールファクタが乗算され、ジャイロセンサＧの角速度情報の誤差分散が計算される。

姿勢誤差分散推定部３３は、移動体の姿勢情報の予測値と、移動体の姿勢情報の補正値と、に基づいて、移動体の姿勢情報の誤差分散を推定する（ステップＳ４）。

姿勢誤差分散推定部３３は、移動体の姿勢情報の誤差分散を数式７、８により計算する。σ_ｅ ^２は、移動体の姿勢情報の誤差分散であり、ｘ_ｎ ^－は、タイミングｎでの移動体の姿勢情報の予測値であり、ｘ_ｎは、タイミングｎでの移動体の姿勢情報の補正値であり、Ｐ_ｎは、タイミングｎでの移動体の姿勢情報の補正値の誤差共分散行列である。

移動体の姿勢情報の誤差分散は、数式８をさらに用いて計算される。数式７の移動体の姿勢情報の誤差分散が所定閾値ＴＨ_ｅと比べて大きいときには、数式８の移動体の姿勢情報の誤差分散が存在すると計算される。数式７の移動体の姿勢情報の誤差分散が所定閾値ＴＨ_ｅと比べて小さいときには、数式８の移動体の姿勢情報の誤差分散が存在しないと計算される。このように、移動体の姿勢情報の予測値と補正値との差分が大きい又は小さいほど、移動体の姿勢情報の誤差分散がそれぞれ大きく又は小さく計算される。

姿勢誤差分散推定部３３は、移動体の姿勢情報の誤差分散を、数式７、８のみならず、数式９～１１によっても、計算することができる。σ_ｅ ^２は、移動体の姿勢情報の誤差分散であり、σ_ｅ０ ^２は、移動体の姿勢情報の誤差分散の初期設定値である。

α_ｅは、数式１０により計算される。ｘ_ｎ ^－は、タイミングｎでの移動体の姿勢情報の予測値であり、ｘ_ｎは、タイミングｎでの移動体の姿勢情報の補正値であり、ＴＨ_ｅは、所定閾値である。移動体の姿勢情報の予測値と補正値との差分が所定閾値と比べて大きいときには、α_ｅが１より大きく計算され、移動体の姿勢情報の誤差分散が大きく計算される。移動体の姿勢情報の予測値と補正値との差分が所定閾値と比べて小さいときには、α_ｅが１に等しく計算され、移動体の姿勢情報の誤差分散が小さく計算される。

α_ｅは、数式１１によっても計算される。ｘ_ｎ ^－は、タイミングｎでの移動体の姿勢情報の予測値であり、ｘ_ｎは、タイミングｎでの移動体の姿勢情報の補正値である。移動体の姿勢情報の予測値と補正値との差分に等しく、α_ｅが計算され、移動体の姿勢情報の予測値と補正値との差分に比例して、移動体の姿勢情報の誤差分散が計算される。

姿勢予測部３１は、移動体の姿勢情報の予測値を数式１により計算する（ステップＳ５）。ここで、時間遷移行列Ｆは、数式１２～１６により計算される。

ψ_ｎ－１、θ_ｎ－１、φ_ｎ－１は、タイミングｎ－１での移動体の姿勢情報の補正値のヨー軸成分、ピッチ軸成分、ロール軸成分である。^Ｓω_ｘ、^Ｓω_ｙ、^Ｓω_ｚは、ジャイロセンサＧのバイアス誤差除去後の角速度情報のｘ軸成分、ｙ軸成分、ｚ軸成分である。^ＳＧ（ｘ）は、ジャイロセンサＧの座標系での角速度情報を、Ｅａｒｔｈ座標系での角速度情報にオイラー変換する行列である。Δｔは、隣接タイミング間の経過時間である。

数式１２の右辺は非線形関数であるところ、数式１２の右辺をテイラー展開したうえで、数式１２の右辺を線形関数に近似すれば、時間遷移行列Ｆは数式１６により計算される。Ｉ_３×３は、３行３列の単位行列であり、О_３×１は、３行１列の零行列である。

姿勢予測部３１は、移動体の姿勢情報の予測値の誤差共分散行列を数式２により計算する（ステップＳ６）。ここで、プロセス雑音Ｑ_ｎは、数式１７、１８により計算される。

σ_ｇ＿ｘ ^２、σ_ｇ＿ｙ ^２、σ_ｇ＿ｚ ^２は、ジャイロセンサＧの角速度情報の誤差分散のｘ軸成分、ｙ軸成分、ｚ軸成分である。σ_ｅ ^２は、移動体の姿勢情報の誤差分散である。α_ｅは、^ＳＧ（ｘ_ｎ－１）・^ＳＱ_ｇ・^ＳＧ^Ｔ（ｘ_ｎ－１）に対して、Ｉ_３×３／σ_ｅ ^２を加算する強さを示す定数である。

つまり、姿勢予測部３１は、移動体の姿勢情報の誤差分散の逆数と、ジャイロセンサＧの角速度情報の誤差分散と、の和を、プロセス雑音Ｑ_ｎとして推定する。よって、姿勢予測部３１は、ジャイロセンサＧの角速度情報の誤差分散が大きい又は小さいほど、プロセス雑音Ｑ_ｎをそれぞれ大きく又は小さく推定する。一方で、姿勢予測部３１は、移動体の姿勢情報の誤差分散が大きい又は小さいほど、プロセス雑音Ｑ_ｎをそれぞれ小さく又は大きく推定する。このようなプロセス雑音Ｑ_ｎの効果については、図８、９で説明する。

（本開示の姿勢補正部の処理）
本開示の姿勢補正部の処理を図７に示す。姿勢補正部３２は、衛星航法システムの観測情報に基づいて、状態空間モデルの観測方程式を用いて、船舶Ｓ等の移動体の姿勢情報の補正値を計算する。カルマンフィルタの処理は、数式１９～２１のように表される。

Ｋ_ｎはタイミングｎでの姿勢補正ゲイン（カルマンゲイン）であり、Ｐ_ｎ ^－はタイミングｎでの移動体の姿勢情報の予測値の誤差共分散行列であり、Ｈは空間写像行列であり、Ｒ_ｎは観測雑音の誤差共分散行列である。ｘ_ｎはタイミングｎでの移動体の姿勢情報の補正値であり、ｘ_ｎ ^－はタイミングｎでの移動体の姿勢情報の予測値であり、ｚ_ｎはタイミングｎでの観測ベクトルである。Ｐ_ｎはタイミングｎでの移動体の姿勢情報の補正値の誤差共分散行列である。

ＧＮＳＳ受信機Ｒ１、Ｒ２、Ｒ３は、衛星航法システムの観測情報（例えば、搬送波情報（キャリア信号）、衛星情報（仰角、方位角及び信号強度等）、信号品質情報（ＰＬＬ又はＦＬＬ等の信号制御状態、サイクルスリップ状態、ハーフサイクル状態及び追尾継続時間等）、並びに、受信情報（アンテナ位置及び現在時刻等）等）を出力する。搬送波位相差計算部４は、衛星航法システムの観測情報を取得する（ステップＳ１１）。

搬送波位相差計算部４は、ＧＮＳＳ受信機Ｒ１、Ｒ２、Ｒ３における搬送波位相差を計算し、これらの搬送波位相差に対する衛星情報を出力する（ステップＳ１２）。

搬送波位相差計算部４は、ＧＮＳＳ受信機Ｒ１、Ｒ２、Ｒ３における搬送波位相の一重位相差を数式２２により計算する。Φ^ｊ _ｒｂは、基準アンテナｂとサブアンテナｒとの間の衛星ｊの搬送波位相の一重位相差であり、Φ^ｊ _ｂは、基準アンテナｂで観測した衛星ｊの搬送波位相であり、Φ^ｊ _ｒは、サブアンテナｒで観測した衛星ｊの搬送波位相である。

搬送波位相差計算部４は、ＧＮＳＳ受信機Ｒ１、Ｒ２、Ｒ３における搬送波位相の二重位相差を数式２３により計算する。Φ^ｊｋ _ｒｂは、基準アンテナｂとサブアンテナｒとの間及び衛星ｊと衛星ｋとの間の搬送波位相の二重位相差であり、Φ^ｋ _ｒｂは、基準アンテナｂとサブアンテナｒとの間の衛星ｋの搬送波位相の一重位相差である。

ここで、ＧＮＳＳ受信機Ｒ１、Ｒ２、Ｒ３における搬送波位相の一重位相差は、受信機時計誤差が重畳するため、受信機時計誤差を推定しなければ使用できないが、数式２２にて要素となる搬送波位相の数は２個であるため、位相差結果に重畳しているガウス白色雑音量は、数式２３による搬送波位相の二重位相差より小さい。

一方で、ＧＮＳＳ受信機Ｒ１、Ｒ２、Ｒ３における搬送波位相の二重位相差は、受信機時計誤差を相殺するため、受信機時計誤差を推定しなくても使用できるが、数式２３にて要素となる搬送波位相の数は４個であるため、位相差結果に重畳しているガウス白色雑音量は、数式２２による搬送波位相の一重位相差より大きい。

そこで、搬送波位相差計算部４は、移動体の姿勢情報の予測値又は補正値が未知である状態では、ＧＮＳＳ受信機Ｒ１、Ｒ２、Ｒ３における搬送波位相の二重位相差を計算することが望ましく、移動体の姿勢情報の予測値又は補正値が確定した状態では、移動体の姿勢情報に基づいて受信機時計誤差を推定できるので、ＧＮＳＳ受信機Ｒ１、Ｒ２、Ｒ３における搬送波位相の一重位相差を計算することが望ましい。

アンビギュイティ推定部５は、ＧＮＳＳ受信機Ｒ１、Ｒ２、Ｒ３における搬送波位相差に含まれる、アンビギュイティ（波長数の倍数）を推定する（ステップＳ１３）。

アンビギュイティ推定部５は、ＬＡＭＢＤＡ法等の古典手法又は特許文献３の開示方法等を用いて、移動体の姿勢情報の予測値又は補正値の正確な計算を可能にする。

姿勢補正部３２は、姿勢補正ゲインを数式１９により計算し（ステップＳ１４）、移動体の姿勢情報の補正値を数式２０により計算し（ステップＳ１５）、移動体の姿勢情報の補正値の誤差共分散行列を数式２１により計算する（ステップＳ１６）。

以下に、数式１９～２１において、空間写像行列Ｈ、観測ベクトルｚ_ｎ及び観測雑音の誤差共分散行列Ｒ_ｎを計算する方法を示す。ここで、アンテナ数は３台であり、基線ベクトル数は２本であり、搬送波位相の二重位相差が計算され、搬送波周波数帯は単一種類である。なお、搬送波位相の一重位相差が計算されるときには、受信機時計誤差が加味される。また、搬送波周波数帯が複数種類であるときには、搬送波波長が区別される。また、異なる衛星航法システムが利用されるときには、衛星航法システム間の時計誤差が加味される。

任意の基線ベクトルｂについて、基準アンテナｂとサブアンテナｒとの間及び衛星１と衛星ｍとの間の搬送波位相の二重位相差は、数式２３で計算され、数式２４の左辺で使用される。数式２４の中辺において、ρ^１ｍ _ｒｂは、基準アンテナｂとサブアンテナｒとの間及び衛星１と衛星ｍとの間の幾何学距離の二重距離差であり、λは、搬送波波長であり、Ｎ^１ｍ _ｒｂは、基準アンテナｂとサブアンテナｒとの間及び衛星１と衛星ｍとの間の搬送波位相のアンビギュイティの二重位相差である。

任意の基線ベクトルｂについて、基準アンテナｂとサブアンテナｒとの間及び衛星１と衛星ｍとの間の搬送波位相の二重位相差は、数式２４の右辺により書き換えられる。ρ^ｍ _ｒｂ（ρ^１ _ｒｂ）は、基準アンテナｂとサブアンテナｒとの間の衛星ｍ（衛星１）の幾何学距離の一重距離差であり、Ｎ^ｍ _ｒｂ（Ｎ^１ _ｒｂ）は、基準アンテナｂとサブアンテナｒとの間の衛星ｍ（衛星１）の搬送波位相のアンビギュイティの一重位相差である。

任意の基線ベクトルｂについて、基準アンテナｂとサブアンテナｒとの間の衛星ｍの幾何学距離の一重距離差は、移動体の姿勢情報の概略値（＝移動体の姿勢情報の予測値）の周りでテイラー展開されたときに、数式２５～３０により計算される。

ρ^ｍ _ｒｂ、０は、移動体の姿勢情報の概略値（右下添え字０）における、基準アンテナｂとサブアンテナｒとの間の衛星ｍの幾何学距離の一重距離差である。ｅ^ｍ _ｒは、サブアンテナｒから衛星ｍへの視線方向ベクトル（方向余弦ベクトル）である。Ａ^ｍ _ｒ及びＥ^ｍ _ｒは、サブアンテナｒから衛星ｍへの方位角及び仰角である。^ＳＲ_０は、センサ座標系（左上添え字Ｓ）における移動体の姿勢情報の概略値（右下添え字０）をＥａｒｔｈ座標系に変換する回転行列である。ψ_０、θ_０、φ_０は、移動体の姿勢情報の概略値（右下添え字０）である。ψ_ｎ、θ_ｎ、φ_ｎは、移動体の姿勢情報の補正値である。^Ｓｒ_ｒｂは、センサ座標系（左上添え字Ｓ）における基線ベクトルであり、移動体に固定され既知の情報である。

基線ベクトルｂ１、ｂ２により構成される観測方程式は、数式２４～３０に基づいて数式３１～３５により計算される。空間写像行列Ｈは、数式３３により計算される。観測ベクトルｚ_ｎは、数式３２により計算される。観測雑音の誤差共分散行列Ｒ_ｎは、衛星情報（仰角、方位角及び信号強度等）等に基づいて、観測誤差モデルにより計算される。

ｙ_ｂｉは、任意の基線ベクトルｂｉについて、（１）と（２）との間の差を示すゼロ残差ベクトルである：（１）基準アンテナｂとサブアンテナｒ_ｉとの間及び衛星１と衛星ｍとの間の搬送波位相の二重位相差Φ^１ｍ _ｒｉｂ、（２）移動体の姿勢情報の概略値における、基準アンテナｂとサブアンテナｒ_ｉとの間及び衛星１と衛星ｍとの間の幾何学距離の二重距離差ρ^１ｍ _{ｒｉｂ、０}と、アンビギュイティ推定部５が推定した、アンビギュイティＮ^１ｍ _ｒｉｂと、の和。

本開示の受信環境に応じた姿勢補正ゲインの調整を図８に示す。図８の左欄では、衛星航法システムの受信環境が良好であるときの姿勢補正ゲインの調整を示し、図８の右欄では、衛星信号の受信環境が劣化しているときの姿勢補正ゲインの調整を示す。

衛星航法システムの受信環境が良好であるときには、姿勢誤差分散推定部３３は、移動体の姿勢情報の誤差分散を小さく推定する。すると、姿勢予測部３１は、トレードオフパラメータとして、プロセス雑音Ｑ_ｎ（＝角速度誤差分散＋１／姿勢誤差分散、数式１７を参照）を大きく推定する。そして、姿勢補正部３２は、姿勢補正ゲインＫ_ｎを大きく計算したうえで、ジャイロセンサＧの角速度情報と比べて衛星航法システムの観測情報を重視して、移動体の姿勢情報の補正値を高精度に計測することができる。

衛星航法システムの受信環境が劣化しているときには、姿勢誤差分散推定部３３は、移動体の姿勢情報の誤差分散を大きく推定する。すると、姿勢予測部３１は、トレードオフパラメータとして、プロセス雑音Ｑ_ｎ（＝角速度誤差分散＋１／姿勢誤差分散、数式１７を参照）を小さく推定する。そして、姿勢補正部３２は、姿勢補正ゲインＫ_ｎを小さく計算したうえで、衛星航法システムの観測情報と比べてジャイロセンサＧの角速度情報を重視して、移動体の姿勢情報の補正値を高精度に計測することができる。

本開示の動揺状態に応じた姿勢補正ゲインの調整を図９に示す。図９の左欄では、船舶Ｓ等の移動体の姿勢が静止しているときの姿勢補正ゲインの調整を示し、図９の右欄では、船舶Ｓ等の移動体の姿勢が動揺しているときの姿勢補正ゲインの調整を示す。

船舶Ｓ等の移動体の姿勢が静止しているときには、衛星信号の受信環境の劣化時でも、角速度誤差分散推定部２は、ジャイロセンサＧの角速度情報の誤差分散を小さく推定する。すると、姿勢予測部３１は、プロセス雑音Ｑ_ｎ（＝角速度誤差分散＋１／姿勢誤差分散、数式１７を参照）を小さく推定する。そして、姿勢補正部３２は、姿勢補正ゲインＫ_ｎを小さく計算したうえで、衛星航法システムの観測情報と比べてジャイロセンサＧの角速度情報を重視して、移動体の姿勢情報の補正値を高精度に計測することができる。

船舶Ｓ等の移動体の姿勢が動揺しているときには、衛星信号の受信環境の劣化時でも、角速度誤差分散推定部２は、ジャイロセンサＧの角速度情報の誤差分散を大きく推定する。すると、姿勢予測部３１は、プロセス雑音Ｑ_ｎ（＝角速度誤差分散＋１／姿勢誤差分散、数式１７を参照）を大きく推定する。そして、姿勢補正部３２は、姿勢補正ゲインＫ_ｎを大きく計算したうえで、ジャイロセンサＧの角速度情報と比べて衛星航法システムの観測情報を重視して、移動体の姿勢情報の補正値を高精度に計測することができる。

つまり、移動体の姿勢情報の補正値は、推定したプロセス雑音（角速度誤差分散及び姿勢誤差分散を構成要素とする。）及び衛星航法システムの観測情報の観測雑音に基づいた姿勢補正ゲインによって、ジャイロセンサＧの角速度情報及び衛星航法システムの観測情報が最適に重み付け合成されるようなかたちで計算される。このように、ジャイロセンサＧの角速度情報及び衛星航法システムの観測情報に基づいて、移動体の姿勢情報を状態空間モデルの状態ベクトルとして計測するにあたり、衛星信号の受信環境が劣化しているときでも、移動体の姿勢が動揺しているときでも、ジャイロセンサＧの角速度情報及び衛星航法システムの観測情報の重視バランスが状況変化に応じて適切に調整されるため、移動体の姿勢情報を高精度に計測することができる。そして、ジャイロセンサＧの角速度情報のバイアス誤差は、状態空間モデルを用いずに角速度バイアス除去部１を用いて推定されるため、状態空間モデルの状態ベクトルの要素数が必要最低限になり、状態空間モデルの観測方程式の冗長性が増し、移動体の姿勢情報を高精度に計測することができる。

（本開示のＧＮＳＳ情報の取得可否に応じた処理）
本開示のＧＮＳＳ情報の取得可否に応じた処理を図１０、１１に示す。図１１の左欄では、衛星航法システムの観測情報が受信されているときを示し、図１１の中欄では、衛星航法システムの観測情報が一時受信されなくなったときを示し、図１１の右欄では、衛星航法システムの観測情報が再度受信されるようになったときを示す。

まず、衛星航法システムの観測情報が、取得不能になったときについて説明する（ステップＳ２１、ＮＯ）。姿勢補正部３２は、ステップＳ１４において移動体の姿勢情報の補正ゲインＫ_ｎを０に維持する（ステップＳ２２）。そして、姿勢補正部３２は、ステップＳ１５、Ｓ１６を実行するにあたり、移動体の姿勢情報の補正値ｘ_ｎを移動体の姿勢情報の予測値ｘ_ｎ ^－に等しく設定し、移動体の姿勢情報の補正値の誤差共分散行列Ｐ_ｎを移動体の姿勢情報の予測値の誤差共分散行列Ｐ_ｎ ^－に等しく設定する（ステップＳ２３）。

姿勢予測部３１は、ステップＳ１～Ｓ６を実行するにあたり、移動体の姿勢情報の予測値の誤差共分散行列Ｐ_ｎ ^－に、プロセス雑音Ｑ_ｎを加算し続ける（ステップＳ２４）。このように、衛星信号の受信環境が中断した後には、衛星信号の中断時間及び移動体の動揺状態に応じて、移動体の姿勢情報の予測値の誤差分散を大きく推定することができる。

次に、衛星航法システムの観測情報が、取得可能になったときについて説明する（ステップＳ２１、ＹＥＳ）。姿勢補正部３２は、ステップＳ１１～Ｓ１３を実行する（ステップＳ２５）。そして、姿勢補正部３２は、ステップＳ１４～Ｓ１６を実行するにあたり、ステップＳ２４における移動体の姿勢情報の予測値の誤差共分散行列Ｐ_ｎ ^－に基づいて、移動体の姿勢情報の補正ゲインＫ_ｎ及び補正値ｘ_ｎを計算し始める（ステップＳ２６）。

姿勢予測部３１は、ステップＳ１～Ｓ６を実行する（ステップＳ２７）。このように、衛星信号の受信環境が回復した直後には、衛星信号の中断時間及び移動体の動揺状態に応じて、状態空間モデルの観測方程式の姿勢補正ゲインを大きく計算することができる。つまり、衛星信号の受信環境が回復した直後には、姿勢補正ゲインが大きくなることで、衛星信号の中断時間及び移動体の動揺状態に応じて蓄積する移動体の姿勢情報の予測値の誤差を是正し、移動体の姿勢情報の予測値の誤差が是正された後には、移動体の姿勢情報の補正値の誤差分散を早めに収束させることで、衛星信号の受信環境が回復した後に素早く移動体の姿勢情報を高精度に計測することができる。

（本開示の各々の情報の更新間隔に応じた処理）
本開示の各々の情報の更新間隔に応じた処理を図１２、１３に示す。図１３の上段では、ジャイロセンサＧの角速度情報は、より高いレートで更新されることが一般的である（時刻ｔ_１、ｔ_２、ｔ_３、ｔ_４、ｔ_５、・・・）。図１３の下段では、衛星航法システムの観測情報は、より低いレートで更新されることが一般的である（時刻ｔ_１、ｔ_３、ｔ_５、・・・）。

姿勢予測部３１は、ジャイロセンサＧの角速度情報のより短い更新間隔で処理を実行する（ステップＳ３１、Ｓ３２）。つまり、姿勢予測部３１は、ジャイロセンサＧの角速度情報が更新されたときのみ（ステップＳ３１、ＹＥＳ）、ステップＳ１～Ｓ６を実行する（ステップＳ３２）。ただし、姿勢予測部３１は、移動体の姿勢情報の前回予測値ｘ_ｎ ^－及び前回補正値ｘ_ｎの情報があるときのみ、ステップＳ４を実行する（ステップＳ３２）。

つまり、姿勢予測部３１は、ジャイロセンサＧの角速度情報が衛星航法システムの観測情報と比べて高いレートで更新されたとしても、逆行列計算（数式１９を参照）を含まず処理負荷の軽い姿勢予測のみを実行する。そして、姿勢予測部３１は、ジャイロセンサＧの角速度情報のより短い更新間隔内に、１回分の姿勢予測を完了させればよい。

姿勢補正部３２は、衛星航法システムの観測情報のより長い更新間隔でのみ処理を実行する（ステップＳ３３、Ｓ３４）。つまり、姿勢補正部３２は、衛星航法システムの観測情報が更新されたときのみ（ステップＳ３３、ＹＥＳ）、ステップＳ１１～Ｓ１６を実行する（ステップＳ３４）。ここで、姿勢補正部３２がステップＳ１１～Ｓ１６を実行しないときには、姿勢予測部３１のみがステップＳ１～Ｓ６を実行したうえで、移動体の姿勢情報の補正値ｘ_ｎの情報に代えて、移動体の姿勢情報の予測値ｘ_ｎ ^－の情報を出力する。

つまり、姿勢補正部３２は、ジャイロセンサＧの角速度情報が衛星航法システムの観測情報と比べて高いレートで更新されたとしても、逆行列計算を含み（数式１９を参照）処理負荷の重い姿勢補正を実行しない。そして、姿勢補正部３２は、衛星航法システムの観測情報のより長い更新間隔内に、１回分の姿勢補正を完了させればよい。

このように、組み込み用の非力なＣＰＵを用いるとしても、逆行列計算処理（数式１９を参照）による処理負荷オーバの問題に対処したうえで、最新の移動体の姿勢情報の予測値及び最適な移動体の姿勢情報の補正値を提供することができる。

本開示の移動体姿勢計測装置及び移動体姿勢計測プログラムは、国際海事機関（ＩＭＯ）が定義した船首方位伝達装置（ＴＨＤ）の性能要件に準拠したＧＮＳＳコンパス等に適用することができるが、船舶以外の移動体にも適用することができる。

Ｓ：船舶
Ｄ：移動体姿勢計測装置
Ｇ：ジャイロセンサ
Ａ１、Ａ２、Ａ３：受信アンテナ
Ｂ１、Ｂ２：基線ベクトル
Ｒ１、Ｒ２、Ｒ３：ＧＮＳＳ受信機
１：角速度バイアス除去部
２：角速度誤差分散推定部
３：姿勢計算部
４：搬送波位相差計算部
５：アンビギュイティ推定部
２１Ａ、２１Ｂ：フィルタ部
２２：角速度誤差分散計算部
３１：姿勢予測部
３２：姿勢補正部
３３：姿勢誤差分散推定部

Claims (7)

1. ジャイロセンサの角速度情報及び衛星航法システムの観測情報に基づいて、移動体の姿勢情報を状態空間モデルの状態ベクトルとして計測する移動体姿勢計測装置であって、
   前記ジャイロセンサの角速度情報に基づいて、前記状態空間モデルの状態方程式を用いて、前記移動体の姿勢情報の予測値を計算する姿勢予測部と、
   前記衛星航法システムの観測情報に基づいて、前記状態空間モデルの観測方程式を用いて、前記移動体の姿勢情報の補正値を計算する姿勢補正部と、
   前記ジャイロセンサの角速度情報に基づいて、前記状態空間モデルを用いずに、前記ジャイロセンサの角速度情報の誤差分散を推定する角速度誤差分散推定部と、を備え、
   前記姿勢予測部は、前記ジャイロセンサの角速度情報の誤差分散が大きい又は小さいほど、前記姿勢予測部のプロセス雑音をそれぞれ大きく又は小さく推定する
   ことを特徴とする移動体姿勢計測装置。
2. 前記角速度誤差分散推定部は、前記ジャイロセンサの角速度情報を、異なる通過特性を有する複数のフィルタに入力し、前記複数のフィルタの出力の差分が大きい又は小さいほど、前記ジャイロセンサの角速度情報の誤差分散をそれぞれ大きく又は小さく推定する
   ことを特徴とする、請求項１に記載の移動体姿勢計測装置。
3. 前記移動体の姿勢情報の予測値と、前記移動体の姿勢情報の補正値と、に基づいて、前記移動体の姿勢情報の誤差分散を推定する姿勢誤差分散推定部、をさらに備え、
   前記姿勢予測部は、前記移動体の姿勢情報の誤差分散が大きい又は小さいほど、前記姿勢予測部のプロセス雑音をそれぞれ小さく又は大きく推定する
   ことを特徴とする、請求項１又は２に記載の移動体姿勢計測装置。
4. 前記姿勢予測部は、前記移動体の姿勢情報の誤差分散の逆数と、前記ジャイロセンサの角速度情報の誤差分散と、の和を、前記姿勢予測部のプロセス雑音として推定する
   ことを特徴とする、請求項３に記載の移動体姿勢計測装置。
5. 前記衛星航法システムの観測情報が、取得不能になったときに、前記姿勢補正部が、前記移動体の姿勢情報の補正ゲインを０に維持したうえで、前記姿勢予測部は、前記移動体の姿勢情報の予測値の誤差分散に、前記姿勢予測部のプロセス雑音を加算し続け、
   前記衛星航法システムの観測情報が、取得可能になったときに、前記姿勢補正部は、前記移動体の姿勢情報の予測値の誤差分散に基づいて、前記移動体の姿勢情報の補正ゲインを計算し始めたうえで、前記移動体の姿勢情報の補正値を計算し始める
   ことを特徴とする、請求項１から４のいずれかに記載の移動体姿勢計測装置。
6. 前記ジャイロセンサの角速度情報は、より高いレートで更新されており、前記衛星航法システムの観測情報は、より低いレートで更新されており、前記姿勢予測部は、前記ジャイロセンサの角速度情報のより短い更新間隔で処理を実行し、前記姿勢補正部は、前記衛星航法システムの観測情報のより長い更新間隔でのみ処理を実行する
   ことを特徴とする、請求項１から５のいずれかに記載の移動体姿勢計測装置。
7. 請求項１から６のいずれかに記載の移動体姿勢計測装置が備える、各処理部が行なう各処理ステップを、コンピュータに実行させるための移動体姿勢計測プログラム。

Images