JPWO2020245902A1 - 方位角推定装置、ジャイロシステム、方位角推定方法及びプログラム - Google Patents

Abstract

方位角推定装置（２０）のセンサ出力取得部（２１１）は、アクチュエータの交流駆動信号により共振モードを励起された円状物体（１０）に配置された複数のセンサ（１１）から、変位を示すセンサ出力値を取得する。振動形状導出部（２１２）は、取得したセンサ出力値に基づいて、円状物体（１０）の振動振幅の観測値と、アクチュエータの交流駆動信号に対する円状物体（１０）の共振位相特性の観測値と、を導出する。状態推定部（２１３）は、振動形状導出部（２１２）が導出した振動振幅の観測値及び共振位相特性の観測値と、センサ出力値の時間変化と、を用いた観測方程式及び状態方程式に基づいて波腹方位角推定値を計算する。

Description

本発明は、円状物体の共振器を用いて方位角を推定する方位角推定装置、ジャイロシステム、方位角推定方法及びプログラムに関する。

宇宙機をはじめとする移動体の姿勢検知又は制御に、円状物体の共振器を用いた方位角推定装置が用いられている。半球形状の共振器を用いる半球共振型ジャイロは、半球共振器とフォーサ及びピックオフを備えた振動回転センサである（例えば、特許文献１）。

半球共振型ジャイロは、フォーサにより半球共振器を１次共振させて楕円変形モードを励起して、ピックオフにより半球共振器の振動形状を計測する。角速度が入力されたとき、半球共振器の共振の波腹となる方位角が変化するため、その変化をピックオフの出力から算出することで、角速度が検出される。

特許文献１の半球共振型ジャイロは、共振器の周りに等間隔に配置された８つの変位センサから半球共振器の振動を検出し、波腹方位角を導出している。具体的には、基準となる０度の位置から９０度おきに配置された４つのセンサ出力から算出される０度成分信号と、４５度の位置から９０度おきに配置された４つのセンサ出力から算出される４５度成分信号の振幅比を求めることで波腹方位角を導出している。

この振幅比を求める際に、０度成分信号及び４５度成分信号を一度パルス列に変換してから積分器に入力する。このとき、振幅が小さいパルス列と振幅が大きいパルス列の数を変えて、積分器をつりあい状態にする調整を行う。任意のサンプリングサイクル終了時における入力パルス数の比から波腹方位角を導出することができると説明されている。

特開昭６０−１６６８１８号公報

半球共振型ジャイロの角速度の検出精度は、波腹方位角の測定精度に依存しているため、その測定精度を向上させることが重要である。波腹方位角の測定において、センサ出力信号には雑音が含まれており、測定精度の低下の原因になっていた。

ノイズ低減を図るためには、例えば、ローパスフィルタ（Low-Pass Filter：ＬＰＦ）を含む高域成分を除去する信号処理を施すことができる。しかし、このような信号処理を実行する場合、信号処理前後で遅延が生じてしまい、ジャイロの応答性能及び移動体の姿勢制御精度が悪化するという問題があった。

本発明は上記事情に鑑みてなされたものであり、方位角の推定精度が高く、方位角出力の応答性能が良好な方位角推定装置、ジャイロシステム及び方位角推定方法及びプログラムを提供することを目的とする。

上記目的を達成するため、本発明の方位角推定装置は、アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから、変位を示すセンサ出力値を取得するセンサ出力取得部と、センサ出力値に基づいて、円状物体の振動振幅の観測値と、アクチュエータの交流駆動信号に対する円状物体の共振位相特性の観測値と、を導出する振動形状導出部と、を備える。また、振動形状導出部が導出した振動振幅の観測値及び共振位相特性の観測値と、センサ出力値の時間変化と、を用いた観測方程式及び状態方程式に基づいて波腹方位角推定値を推定する状態推定部を備えることを特徴とする。

本発明によれば、センサ出力値に基づいて算出した振動振幅及び共振位相特性の観測値を、観測方程式及び状態方程式の計算に用いることにより、モデルの精度を高めることができるため、方位角の推定精度を高め、方位角出力の応答性能を良好にすることが可能になる。

本発明の実施の形態１に係るジャイロシステムの全体構成を示すブロック図 実施の形態１に係る状態推定部の処理アルゴリズムを示した図 実施の形態１に係る波腹方位角推定処理のフローチャート 本発明の実施の形態２に係るジャイロシステムの全体構成を示すブロック図 実施の形態２に係る切替判定部の処理アルゴリズムを示した図 実施の形態２，３に係る共分散行列調整部の処理アルゴリズムを示した図 実施の形態２，３に係る状態推定部の処理アルゴリズムを示した図 実施の形態２，３に係る波腹方位角推定処理のフローチャート 本発明の実施の形態３に係る切替判定部の処理アルゴリズムを示した図 本発明の実施の形態４に係る共分散行列調整部の処理アルゴリズムを示した図 実施の形態４に係る状態推定部の処理アルゴリズムを示した図 本発明の実施の形態５に係る共分散行列調整部の処理アルゴリズムを示した図 実施の形態５に係る状態推定部の処理アルゴリズムを示した図

以下に、本発明を実施するための形態について図面を参照して詳細に説明する。なお、図中同一又は相当する部分には同じ符号を付す。

実施の形態１．
図１は実施の形態１に係るジャイロシステム１の全体構成を示すブロック図である。本実施の形態１に係るジャイロシステム１は、図１に示すように、共振器である円状物体１０と、円状物体１０に付した複数のセンサ１１の出力に基づいて方位角を推定する方位角推定装置２０と、を備える。

円状物体１０は、任意の大きさの円を含む形状を有する共振器であり、例えば半球形状を有する。本実施の形態１では、半球形状を有する円状物体１０を用いる場合について説明する。円状物体１０は半球面の縁に沿って、複数のセンサ１１が等間隔で配置されている。

センサ１１は、任意の変位センサである。円状物体１０に角速度が入力されると、円状物体１０の縁部の振動パターンが変化するため、各センサ１１は、変位量を示すセンサ出力値Ｅ_ｍ（ｍ＝１，２，・・・）を出力する。ここで、ｍは複数のセンサ１１に付与した通し番号である。

方位角推定装置２０は、センサ出力値Ｅ_ｍに基づいて波腹方位角推定処理を含む演算処理を実行する演算部２１及び記憶部２２を備える。演算部２１は、例えば、ＣＰＵ（Central Processing Unit：中央演算装置）、ＲＯＭ（Read Only Memory：読み出し専用メモリ）、ＲＡＭ（Random Access Memory：ランダムアクセスメモリ）から構成される。演算部２１は、記憶部２２に記憶されているプログラムを実行することにより、図１に示すように、センサ出力取得部２１１、振動形状導出部２１２、状態推定部２１３として機能する。

センサ出力取得部２１１は、複数のセンサ１１から入力されるセンサ出力値Ｅ_ｍを取得して、振動形状導出部２１２に出力する。また、センサ出力取得部２１１は、センサ出力値Ｅ_ｍの時間変化を示すＥ_Ｒｅｋ，Ｅ_Ｉｍｋ（ｋ＝１，２，・・・）を計算して、状態推定部２１３に出力する。ここで、Ｅ_Ｒｅｋはセンサ出力値Ｅ_ｍの時間変化量の実数部分であり、Ｅ_Ｉｍｋはセンサ出力値Ｅ_ｍの時間変化量の虚数部分である。ｋは予め定めた時間間隔で配列した時刻に対して付与した通し番号である。

振動形状導出部２１２は、センサ出力値Ｅ_ｍから振動形状を示す１次共振モードの振動振幅Ａの観測値、及び、アクチュエータの交流駆動信号に対する１次共振モードの共振位相特性φ_ｒの観測値を算出する。振動振幅Ａの観測値及び共振位相特性φ_ｒの観測値は、状態推定部２１３に対して出力される。

状態推定部２１３は、センサ出力値Ｅ_ｍの時間変化を示すＥ_Ｒｅｋ，Ｅ_Ｉｍｋと、振動形状導出部２１２から受け取った振動形状の情報をもとに、波腹方位角θ_ｒの推定値を算出する。なお、図面において、観測値には文字の上に”~”の記号を付し、推定値には文字の上に”＾”の記号を付している。

センサ出力取得部２１１、振動形状導出部２１２、状態推定部２１３における具体的な処理について、円状物体１０が半球共振器である半球共振型ジャイロの場合について説明する。

半球共振型ジャイロは、円状物体１０である半球共振器をアクチュエータの交流駆動信号によって１次共振させ、当該共振モードの波腹方位角の変化を複数のセンサ１１で検出することにより入力された角速度を検出する。ここでは、半球共振型ジャイロの最も一般的な構成である、８つの変位センサＳ_ｍ（ｍ＝１，２，・・・，８）を４５度間隔で半球共振器周囲に配置した構成を例として記述する。つまり、変位センサＳ_ｍ（ｍ＝１，２，・・・，８）それぞれがセンサ１１である。

半球共振器に励起された１次共振モードの変位であって、波節直角位相振動による半径方向変位は、変位センサＳ_ｍ（ｍ＝１，２，・・・，８）によって検出され、このときのセンサ出力値Ｅ_ｍ（ｍ＝１，２，・・・，８）は、式（１）で与えられる。

ここでアクチュエータの交流駆動信号は、半球共振器を共振させるアクチュエータの正弦波・余弦波駆動信号を指す。ω_ｒはアクチュエータの駆動角周波数であるが、これが半球共振器の１次共振周波数になり、又は、半球共振器の１次共振周波数に近い値になる。また、波節直角位相振動は、密度および剛性に不均一性を有する現実の半球共振器において、１次共振モードの波節を新たな波腹にしたときの１次共振モードに対して９０度位相差で発生する振動を指す。

次に、センサ出力値Ｅ_ｍを合成する。具体的には、半球共振器の軸の中心を原点とし、ｍ＝１のセンサが配置されている方位を０度と定義する場合、０，９０，１８０，２７０度の方位角に配置された変位センサＳ_ｍ（ｍ＝１，３，５，７）によるセンサ出力値Ｅ_ｍ（ｍ＝１，３，５，７）、及び、４５，１３５，２２５，３１５度の方位角に配置された変位センサＳ_ｍ（ｍ＝２，４，６，８）によるセンサ出力値Ｅ_ｍ（ｍ＝２，４，６，８）を、式（２）のように合成する。

Ｅ_Ｒｅを実数部、Ｅ_Ｉｍを虚数部とした複素表現は、式（３）のように表される。

次に、式（３）で与えられる複素表現Ｅを、式（４）を用いて、アクチュエータ駆動周波数ω_ｒに一致する回転速度で正負方向に回転する。

次に、式（４）で与えられる正方向回転座標系表現Ｅ_＋、及び、負方向回転座標系表現Ｅ₋から、低周波数域透過フィルタ等により低周波成分Ｅ_ＤＣ＋、Ｅ_ＤＣ−を求める。低周波成分Ｅ_ＤＣ＋、Ｅ_ＤＣ−は式（５）のように表される。

式（５）で与えられる低周波成分Ｅ_ＤＣ＋、Ｅ_ＤＣ−から、半球共振器に励起された１次共振モードの、アクチュエータの交流駆動信号に対する共振位相特性φ_ｒが、式（６）より求められる。

なお、ここでは、実際の１次共振モードおよび波節直角位相振動の振動振幅が、式（７）の関係を満たすことを用いている。

一方、センサ１１による実測時には、センサ出力取得部２１１が各センサ出力値Ｅ_ｍ（ｍ＝１，２，・・・，８）を取得し、振動形状導出部２１２が、センサ出力値Ｅ_ｍに基づいて、振動形状を導出する。具体的には、まず、振動形状導出部２１２は、センサ出力値Ｅ_ｍの合成値を、式（８）を用いて求める。

次に、Ｅ_Ｒｅを実数部、Ｅ_Ｉｍを虚数部とした、式（９）で示す複素表現を求める。

次に、式（９）で与えられる複素表現Ｅを、式（１０）を用いてアクチュエータ駆動周波数ω_ｒに一致する回転速度で正負方向に回転する。正方向回転座標系表現Ｅ_＋、及び、負方向回転座標系表現Ｅ₋は式（１０）のように表される。

次に、センサ出力値Ｅ_ｍの実測値から式（１０）を用いて得られた正方向回転座標系表現Ｅ_＋、及び、負方向回転座標系表現Ｅ₋の低周波成分Ｅ_DC+及びＥ_DC−を低周波数域透過フィルタ等により算出する。得られた低周波成分Ｅ_DC+及びＥ_DC−を、理論式である式（６）に代入することにより、共振位相特性φ_ｒが得られる。さらに、式（１１）で示されるＥ_ａ，Ｅ_ｂを算出する。

次に、Ｅａ，Ｅｂを用いて、半球共振器に励起された１次共振モードの波腹方位角θ_ｒを式（１２）より算出し、振動振幅Ａを式（１３）より算出する。

このようにして、振動形状導出部２１２は、１次共振モードの振動形状を示す共振位相特性φ_ｒ、１次共振モードの波腹方位角θ_ｒ、振動振幅Ａを導出する。導出された値は共振位相特性φ_ｒ、１次共振モードの波腹方位角θ_ｒ、振動振幅Ａの観測値として状態推定部２１３に受け渡される。

次に、状態推定部２１３の処理アルゴリズムを説明する。具体的には、半球共振器の１次共振モードの波腹方位角θ_ｒの推定値の導出手法について述べる。ここでは、状態推定部２１３の一例としてカルマンフィルタを適用する場合の処理アルゴリズムを記述する。

カルマンフィルタは、ノイズが重畳する観測値に対して、ノイズがガウス分布に従うという仮定のもと、対象のモデルに基づいて逐次的に真の状態を推定するフィルタである。ある時刻ｋにおける対象の状態量をｘ_ｋ、一定時刻後の対象の状態量をｘ_ｋ＋１としたとき、ｘ_ｋ＋１は式（１４）で表される。

式（１４）は状態方程式と呼ばれる。これに対して、ある時刻ｋにおける対象の観測値をｙ_ｋ、状態空間を観測空間に写像する観測モデルをｈ_ｋ、観測値に重畳する観測雑音をｖ_ｋとすると、ｙ_ｋは式（１５）で表される。式（１５）は観測方程式と呼ばれる。

次に、状態方程式と観測方程式の具体例を示す。半球共振型ジャイロは、観測された半球共振器の振動形状をもとにアクチュエータの駆動を制御して、半球共振器の振動形状を一定に保たせる。ここでは、共振器の振動状態が式（１６）に示す定常状態にある場合を仮定する。

このとき、式（２）に示すＥ_ＲｅとＥ_Ｉｍは式（１７）のように書き直せる。

ここで、振動形状導出部２１２が式（６）及び（１３）を用いて算出した振動振幅Ａ及び共振位相特性φ_ｒの観測値を式（１８）のように表す。式（１８）に示した観測値を式（１７）に代入することで、時刻ｋにおける離散時間非線形確率システムの観測方程式は、式（１９）のように書き表せる。この観測方程式は、共振する円状物体１０の時間的及び空間的な変形を表している。具体的には、式（１９）において、実数部のｃｏｓ（２θ_ｒｋ）と虚数部のｓｉｎ（２θ_ｒｋ）が円状物体１０の空間的変形を表している。また、式（１９）において、実数部及び虚数部のｃｏｓ（ω_ｒｋＴ）が円状物体１０の時間的変形を表している。

ここで、式（１９）においてθ_ｒｋは式（２０）のように書き表せる。

波腹方位角変動量Δθ_ｒｋは、半球共振型ジャイロ計測軸において式（２１）に示す関係を持つことから、時刻ｋにおける離散時間非線形確率システムの状態方程式は、式（２２）のように書き表せる。

よって、式（１４）のパラメータを、式（２３）に示すようにおくことにより、式（２４）に示す状態方程式が得られる。

また、式（１９）及び式（２０）より、式（２５）に示す観測方程式が得られる。

次に、状態推定部２１３の計算手順について図２を用いて記述する。図２は状態推定部２１３の処理アルゴリズムを示した図である。カルマンフィルタを適用する場合は、時刻ｋにおける対象の状態量の推定値、及び、誤差共分散行列の推定値を式（２６）のように表記する。

また、時刻における観測値ｙ_ｋをもとに、次の時刻ｋ＋１の状態量を予測した値、及び、誤差共分散行列を予測した値を式（２７）のように表記する。

まず、状態推定部２１３が初期化を行う。具体的には、状態推定部２１３は時刻ｋ＝０の初期値を式（２８）のように与える。

これらをもとに、状態推定部２１３は時刻ｋにおけるカルマンゲインＫ_ｋを算出する。カルマンゲインＫ_ｋは式（２９）で表される。

ここで、Ｒ_ｋは観測雑音共分散行列であり、Ｈ_ｋは式（３０）で表される。

ここで、ｈ_ｋは式（３１）で表される行列である。振動形状導出部２１２が式（６）及び（１３）を用いて算出したＡ，φ_ｒの観測値を式（３１）に代入してｈ_ｋを計算する（ステップＳ１０１）。

よって、式（３０）は式（３２）のように計算される。すなわち、振動形状導出部２１２が式（６）及び（１３）を用いて算出したＡ，φ_ｒの観測値を代入したＨ_ｋが求められる（ステップＳ１０２）。また、式（２９）を用いて、カルマンゲインＫ_ｋを計算する（ステップＳ１０３）。

次に、状態推定部２１３は、センサ出力取得部２１１からセンサ出力値Ｅ_ｍの時間変化を示すＥ_Ｒｅｋ，Ｅ_Ｉｍｋを受け取って式（１９）に代入することにより、観測値ｙ_ｋを計算する（ステップＳ１０４）。さらに、観測値ｙ_ｋとカルマンゲインＫ_ｋを用いて、式（３３）に示す時刻ｋでの状態量の推定値を算出する（ステップＳ１０５）。

また、次の時刻ｋ＋１での状態量の計算のため、時刻ｋでの誤差共分散行列の推定値を式（３４）で算出し（ステップＳ１０６）、状態量の予測値を式（３５）で算出し（ステップＳ１０７）、誤差共分散行列の予測値を式（３６）で算出する（ステップＳ１０８）。

ここで、Ｑ_ｋはシステム雑音共分散行列であり、カルマンフィルタの耐ノイズ性を高めるため、観測雑音共分散行列Ｒ_ｋに対して相対的に小さい値を設定して計算する。また、Ｆ_ｋは、式（３７）で表される。ここで、システム雑音共分散行列Ｑ_ｋが観測雑音共分散行列Ｒ_ｋに対して相対的に小さい場合とは、システム雑音共分散行列Ｑ_ｋの各要素の値が、対応する観測雑音共分散行列Ｒ_ｋの各要素の値に対して相対的に小さい場合を示す。行列のどの要素の値を相対的に小さくするかは、推定する対象に応じて変更する。

時刻ｋでのすべての計算が終了した後、状態推定部２１３は時刻をｋからｋ＋１に更新して（ステップＳ１０９）、次の計算サイクルに移行する。これらの式をもとに各時刻の状態量の推定値の計算をし（ステップＳ１０５）、式（３８）の波腹方位角の推定値の計算を繰り返して（ステップＳ１１０）、最終的に、式（３９）に示す波腹方位角の推定値を計算することができる。

以上説明した本実施の形態１の方位角推定装置２０の動作を図３のフローチャートに沿って説明する。図３は演算部２１が実行する波腹方位角推定処理を示すフローチャートである。

まず、方位角推定装置２０の演算部２１は初期化を行う（ステップＳ２０１）。具体的には、状態推定部２１３に対し、時刻ｋ＝０における初期値を式（２８）のように設定する。

次に、方位角推定装置２０の演算部２１のセンサ出力取得部２１１が円状物体１０に備えられたセンサ１１が出力するセンサ出力値Ｅ_ｍを取得する（ステップＳ２０２）。センサ出力取得部２１１は、センサ出力値Ｅ_ｍを振動形状導出部２１２に出力し、センサ出力の時間変化を示すＥ_Ｒｅｋ，Ｅ_Ｉｍｋを算出して状態推定部２１３に出力する。

次に、振動形状導出部２１２が、センサ出力値Ｅ_ｍに基づいて、振動形状を導出する。具体的には、式（１）〜（１３）を用いて、振動形状導出部２１２が、振動形状を示す値である１次共振モードの振動振幅Ａの観測値と、アクチュエータの交流駆動信号に対する一次共振モードの共振位相特性φ_ｒの観測値と、を計算する（ステップＳ２０３）。式（１８）に示す観測値は、状態推定部２１３に対して出力される。

次に、状態推定部２１３がセンサ出力取得部２１１から入力されたセンサ出力の時間変化を示すＥ_Ｒｅｋ，Ｅ_Ｉｍｋと、振動形状導出部２１２から受け取った振動形状の情報をもとに、円状物体１０の状態を推定し、波腹方位角θ_ｒの推定値を計算する。具体的には、式（１４）〜（３８）を用いて、センサ出力取得部２１１から入力されたセンサ出力の時間変化を示すＥ_Ｒｅｋ，Ｅ_Ｉｍｋと、振動形状導出部２１２から受け取った振動形状を示す振動振幅Ａと共振位相特性φ_ｒの観測値に基づいて、式（３９）に示す波腹方位角θ_ｒの推定値を計算する（ステップＳ２０４）。

最後に、方位角推定装置２０の演算部２１は動作の終了判定を行い、終了指示がない場合は（ステップＳ２０８：Ｎｏ）、ステップＳ２０１に戻り次の計算サイクルに移り、終了指示があった場合は（ステップＳ２０８：Ｙｅｓ）、処理を終了する。

以上説明したように、本実施の形態１によれば、ジャイロシステム１の方位角推定装置２０は、センサ出力取得部２１１がセンサ１１から取得したセンサ出力値Ｅｍに基づいて、振動形状導出部２１２が１次共振モードの振動振幅Ａの観測値と、アクチュエータの交流駆動信号に対する１次共振モードの共振位相特性φ_ｒの観測値を算出する。そして、状態推定部２１３が、振動振幅Ａの観測値及び共振位相特性φ_ｒの観測値を観測方程式に代入することで、波腹方位角θ_ｒを推定することとした。これにより、従来の観測方程式に振動振幅Ａと共振位相特性φ_ｒの固定値を代入する手法に比べて、モデル精度が高まり、より正確に方位角を推定することが可能となる。

実施の形態２．
図４は実施の形態２に係るジャイロシステム２の全体構成を示すブロック図である。本実施の形態２に係るジャイロシステム２は、図４に示すように、共振器である円状物体１０と、円状物体１０に付した複数のセンサ１１の出力に基づいて方位角を推定する方位角推定装置２０と、を備える。円状物体１０及びセンサ１１の構成は実施の形態１と同様である。

本実施の形態２に係る方位角推定装置２０は、センサ出力値Ｅ_ｍに基づいて方位角推定処理を含む演算処理を実行する演算部２１及び記憶部２２を備える。演算部２１は、記憶部２２に記憶されているプログラムを実行することにより、図４に示すように、センサ出力取得部２１１、振動形状導出部２１２、状態推定部２１３、切替判定部２１４、共分散行列調整部２１５として機能する。センサ出力取得部２１１、振動形状導出部２１２の構成は実施の形態１と同様である。

センサ出力取得部２１１は、センサ１１のセンサ出力値Ｅ_ｍ（ｍ＝１，２，・・・）を振動形状導出部２１２に、検出されたセンサの出力の時間変化Ｅ_Ｒｅｋ、Ｅ_Ｉｍｋを状態推定部２１３に渡す。振動形状導出部２１２は、振動形状を示す観測値であって、式（１８）に示す１次共振モードの振動振幅Ａの観測値及びアクチュエータの交流駆動信号に対する１次共振モードの共振位相特性φ_ｒの観測値を式（６）及び（１３）を用いて算出し、状態推定部２１３に渡す。

また、振動形状導出部２１２は、振動形状を示す観測値であって、式（４０）に示す波腹方位角θ_ｒの観測値を、式（１２）を用いて算出し、切替判定部２１４に渡す。

状態推定部２１３は、センサ出力取得部２１１から受け取ったセンサ出力値Ｅ_ｍの時間変化Ｅ_Ｒｅｋ，Ｅ_Ｉｍｋと振動形状導出部２１２から受け取った振動形状を示す情報に基づいて、式（３９）に示す波腹方位角θ_ｒの推定値を算出し、切替判定部２１４に渡す。

切替判定部２１４は、振動形状導出部２１２が算出した式（４０）に示す波腹方位角θ_ｒの観測値と状態推定部２１３が算出した式（３９）に示す波腹方位角θ_ｒの推定値に基づいて切替判定を行い、切替判定結果を共分散行列調整部２１５と状態推定部２１３に送る。

切替判定の結果、切替が必要と判断された場合は、共分散行列調整部２１５が条件に応じて適切な共分散行列を選択又は算出し、状態推定部２１３が当該共分散行列を用いて波腹方位角θ_ｒの推定値を再計算し、得られた波腹方位角θ_ｒの推定値を最終値として出力する。

一方、切替判定の結果、切替が不要と判断された場合は、共分散行列調整部２１５は動作せず、最初に状態推定部２１３が算出した波腹方位角θ_ｒの推定値を最終値として出力する。

センサ出力取得部２１１、振動形状導出部２１２、状態推定部２１３における処理は実施の形態１で述べた処理を含むため、重複する処理については説明を省略する。状態推定部２１３、切替判定部２１４、共分散行列調整部２１５の具体的な処理について、円状物体１０が半球共振器である半球共振型ジャイロの場合について説明する。

振動形状導出部２１２は、センサ出力値Ｅ_ｍに基づいて、実施の形態１と同様の計算を行い、式（１２）を用いて、式（４０）に示す波腹方位角θ_ｒの観測値を算出する。一方、状態推定部２１３は、センサ出力値Ｅ_ｍの時間変化Ｅ_Ｒｅｋ，Ｅ_Ｉｍｋと振動形状導出部２１２から受け取った振動形状を示す情報に基づいて、実施の形態１と同様の計算を行い、式（３９）に示す波腹方位角θ_ｒの推定値を算出する。

図５は切替判定部２１４の処理アルゴリズムを示した図である。切替判定部２１４は、図５に示すように、振動形状導出部２１２から受け取った時刻ｋでの波腹方位角の観測値と、状態推定部２１３から受け取った時刻ｋでの波腹方位角の推定値を用いて、その差分の絶対値が閾値ｃ以上の場合、切替判定フラグを１に変更する。一方、その差分の絶対値が閾値ｃより小さい場合、切替判定フラグを０にする。すなわち、式（４１）のように切替判定フラグを設定する。

切替判定部２１４は、このように決定された切替判定フラグを共分散行列調整部２１５と状態推定部２１３に送る。

図６は共分散行列調整部２１５の処理アルゴリズムを示した図である。共分散行列調整部２１５は、図６に示すように、切替判定フラグが１の場合（ステップＳ３０１：Ｙｅｓ）、システム雑音共分散行列Ｑ_ｋを調整する（ステップＳ３０２）。式（４１）の条件分岐において切替判定フラグが１である場合、波腹方位角θ_ｒの観測値と推定値との差が大きく、現状のフィルタ特性では推定値が観測値に追従していないと判断できる。このため、カルマンフィルタの追従性を高めるために、観測雑音共分散行列Ｒ_ｋに対してシステム雑音共分散行列Ｑ_ｋを相対的に大きくする。

ここで、システム雑音共分散行列Ｑ_ｋを観測雑音共分散行列Ｒ_ｋに対して相対的に大きくするとき、システム雑音共分散行列Ｑ_ｋの各要素の値を、対応する観測雑音共分散行列Ｒ_ｋの各要素の値に対して相対的に大きくする。行列のどの要素の値を相対的に大きくするかは、推定する対象に応じて変更する。

共分散行列調整部２１５は、ステップＳ３０２で更新したシステム雑音共分散行列Ｑ_ｋ’を状態推定部２１３に渡す。一方、切替判定フラグが０の場合（ステップＳ３０１：Ｎｏ）、共分散行列調整部２１５は処理を行わず計算を終了する（ステップＳ３０３）。

次に、状態推定部２１３の処理アルゴリズムを説明する。図７は状態推定部２１３の処理アルゴリズムを示した図である。図７に示した処理のうち、第１ステップに含まれている処理は、実施の形態１に係る状態推定部２１３の処理と同様であるため、説明は省略する。

図７に示すように、第２ステップにおいて、切替判定フラグが１である場合は（ステップＳ１１１：Ｙｅｓ）、共分散行列調整部２１５から受け取ったシステム雑音共分散行列Ｑ’を用いて時刻ｋ−１における誤差共分散行列の予測値を、式（４２）を用いて計算する（ステップＳ１１２）。

さらに、式（４２）で求めた誤差共分散行列の予測値を用いて式（２９），（３３）〜（３６）を再計算する（ステップＳ１１３〜１１７）。そして、ステップＳ１１４で算出した時刻ｋでの状態量の推定値から波腹方位角θ_ｒの推定値を計算して（ステップＳ１１８）出力する。

一方、状態推定部２１３が受け取った切替判定フラグが０である場合は（ステップＳ１１１：Ｎｏ）、再計算を行わず第１ステップ内のステップＳ１０５で導出した状態量の推定値から波腹方位角θ_ｒの推定値を導出して出力する。時刻ｋでのすべての計算が終了した後、状態推定部２１３は時刻をｋからｋ＋１に更新して次の計算サイクルに移行する。このとき、ステップＳ１１６，１１７で計算する状態量の推定値、及び、誤差共分散行列の推定値の更新に関しては、切替判定フラグが１の場合は再計算した値で更新を行う。

以上説明した本実施の形態２の方位角推定装置２０の動作を図８のフローチャートに沿って説明する。図８は演算部２１が実行する波腹方位角推定処理を示すフローチャートである。

まず、方位角推定装置２０の演算部２１は初期化を行う（ステップＳ２０１）。具体的には、状態推定部２１３に対し、時刻ｋ＝０における初期値を式（２８）のように設定する。

次に、方位角推定装置２０の演算部２１のセンサ出力取得部２１１が、円状物体１０に備えられたセンサ１１が出力するセンサ出力値Ｅ_ｍを取得する（ステップＳ２０２）。センサ出力取得部２１１は、センサ出力値Ｅ_ｍを振動形状導出部２１２に出力し、センサ出力の時間変化を示すＥ_Ｒｅｋ，Ｅ_Ｉｍｋを算出して状態推定部２１３に出力する。

次に、振動形状導出部２１２が、センサ出力値Ｅ_ｍに基づいて、振動形状を導出する（ステップＳ２０３）。具体的には、式（１）〜（１３）を用いて、振動形状導出部２１２が、振動形状を示す値である１次共振モードの振動振幅Ａの観測値と、１次共振モードの波腹方位角θ_ｒの観測値と、アクチュエータの交流駆動信号に対する一次共振モードの共振位相特性φ_ｒの観測値と、を算出する。式（１８）に示す観測値は、状態推定部２１３に対して出力され、式（４０）に示す観測値は、切替判定部２１４に対して出力される。

次に、状態推定部２１３がセンサ出力取得部２１１から入力されたセンサ出力値Ｅ_ｍの時間変化を示すＥ_Ｒｅｋ，Ｅ_Ｉｍｋと、振動形状導出部２１２から受け取った振動形状の情報をもとに、円状物体１０の状態を推定し、波腹方位角θ_ｒの推定値を算出する。具体的には、式（１４）〜（３８）を用いて、センサ出力取得部２１１から入力されたセンサ出力の時間変化を示すＥ_Ｒｅｋ，Ｅ_Ｉｍｋと、振動形状導出部２１２から受け取った振動形状を示す振動振幅Ａと共振位相特性φ_ｒの観測値に基づいて、式（３９）に示す波腹方位角θ_ｒの推定値を算出する（ステップＳ２０４）。

次に、切替判定部２１４が切替の要否の判定を行う。切替判定では、式（４１）をもとに切替判定フラグの値を設定する。切替判定の結果、振動形状導出部２１２が出力する波腹方位角の観測値と状態推定部２１３が出力する波腹方位角の推定値の差の絶対値が閾値より小さくて切替判定フラグが０である場合（ステップＳ２０５：Ｎｏ）、再計算を行わずその時点の状態量の推定値から波腹方位角θ_ｒの推定値を算出して処理を終了する。

一方、切替判定フラグが１であった場合、共分散行列調整部２１５がシステム雑音共分散行列の変更を行い（ステップＳ２０６）、当該共分散行列を状態推定部２１３に渡す。そして、状態推定部２１３は、共分散行列調整部２１５から受け取ったシステム雑音共分散行列を用いて波腹方位角θ_ｒの推定値を再計算する（ステップＳ２０７）。最後に、演算部２１は動作の終了判定を行い、終了指示がない場合は（ステップＳ２０８：Ｎｏ）ステップＳ２０１に戻って次の計算サイクルに移り、終了指示があった場合は（ステップＳ２０８：Ｙｅｓ）処理を終了する。

本実施の形態２のアルゴリズムに従って波腹方位角θ_ｒの推定値を算出することで、波腹方位角θ_ｒの観測値と推定値の差の絶対値が大きい場合には追従性を高めたカルマンフィルタにより波腹方位角θ_ｒの推定値を再計算することで、推定値を観測値に追従させることができ、波腹方位角θ_ｒの観測値と推定値の差分の絶対値が小さい場合には、耐ノイズ性を高めたカルマンフィルタにより波腹方位角θ_ｒの推定値を低ノイズ化できる。

つまり、本実施の形態２の構成は、波腹方位角θ_ｒの推定値を高精度で推定するだけでなく、観測値の変化に応じて、応答性と耐ノイズ性を変化させることができるフィルタを備えていると言える。このようなフィルタは、角速度入力を検出した際の波腹方位角の変化がノイズの大きさに対して十分に大きいデバイスに対して大きな効果を発揮する。例えば、半球共振型ジャイロに対して有効である。角速度が入力されない場合は耐ノイズ性を高めて低ノイズ化し、角速度が入力された場合は入力を正確に検知するために追従性を高めることができる。これにより角速度入力に遅延なく応答しつつ、入力がないときのノイズを極めて小さく抑えることができる。

以上説明したように、本実施の形態２によれば、ジャイロシステム２の方位角推定装置２０は、振動形状導出部２１２が、センサ出力値Ｅ_ｍに基づいて波腹方位角θ_ｒの観測値を算出して切替判定部２１４に渡し、状態推定部２１３が、センサ出力値Ｅ_ｍの時間変化と振動形状の情報に基づいて波腹方位角θ_ｒの推定値を算出して切替判定部２１４に渡す。切替判定部２１４は、波腹方位角θ_ｒの観測値と推定値の差の絶対値が大きい場合には、切替が必要と判断し、共分散行列調整部２１５がシステム雑音共分散行列Ｑ_ｋを観測雑音共分散行列Ｒ_ｋに対して相対的に大きくして、状態推定部２１３が、当該システム雑音共分散行列Ｑ_ｋを用いて波腹方位角θ_ｒの推定値を再計算する。一方、切替が不要と判断された場合は、再計算前の状態量の推定値から波腹方位角θ_ｒの推定値を算出することとした。これにより、波腹方位角θ_ｒの観測値の変化に応じて、応答性と耐ノイズ性のいずれを優先させるかを切り替えることが可能となる。

実施の形態３．
実施の形態３に係るジャイロシステム２について、図６−９を用いて説明する。ジャイロシステム２の全体構成及び方位角推定装置２０の構成は実施の形態２と同様であるが、状態推定部２１３、切替判定部２１４、共分散行列調整部２１５の動作が実施の形態２と異なる。なお、図６，７，８は実施の形態２と共通の図である。

実施の形態２では、図８に示す波腹方位角推定処理のフローチャートのステップＳ２０４の段階において、システム雑音共分散行列Ｑ_ｋを観測雑音共分散行列Ｒ_ｋに対して相対的に小さくすることで耐ノイズ性を高めた。そして、式（４１）に示した切替判定を行い、波腹方位角θ_ｒｋの観測値と推定値の差分の絶対値が閾値ｃ以上の場合に、ステップＳ２０６の段階で、システム雑音共分散行列Ｑ_ｋを観測雑音共分散行列Ｒ_ｋに対して相対的に大きくして波腹方位角θ_ｒｋの推定値の再計算を行った。

これに対し、本実施の形態３では、ステップＳ２０４の段階において、システム雑音共分散行列Ｑ_ｋを観測雑音共分散行列Ｒ_ｋに対して相対的に大きくすることで追従性を高める。そして、波腹方位角θ_ｒｋの観測値と推定値の差分の絶対値が閾値ｃより小さい場合に、ステップＳ２０６の段階で、システム雑音共分散行列Ｑ_ｋを観測雑音共分散行列Ｒ_ｋに対して相対的に小さくして耐ノイズ性を高め、波腹方位角θ_ｒｋの推定値の再計算を行う。

本実施の形態３に係る方位角推定装置２０の構成について、振動形状導出部２１２が、式（４０）に示す波腹方位角θ_ｒの観測値を算出し、状態推定部２１３が式（３９）に示す波腹方位角θ_ｒの推定値を算出する点は実施の形態２と同じである。以後、実施の形態２と異なる点を説明する。

図９は切替判定部２１４の処理アルゴリズムを示した図である。切替判定部２１４は、図９に示すように、振動形状導出部２１２から受け取った時刻ｋでの波腹方位角θ_ｒの観測値と、状態推定部２１３から受け取った時刻ｋでの波腹方位角θ_ｒの推定値を用いて、その差分の絶対値が閾値ｃより小さい場合、切替判定フラグを１に変更する。一方、その差分の絶対値が閾値ｃ以上である場合、切替判定フラグを０にする。すなわち、式（４３）のように切替判定フラグを設定する。

切替判定部２１４は、このように決定された切替判定フラグを共分散行列調整部２１５と状態推定部２１３に送る。

次に、共分散行列調整部２１５の動作を、図６を用いて説明する。図６は共分散行列調整部２１５の処理アルゴリズムを示した図である。共分散行列調整部２１５は、図６に示すように、切替判定フラグが１の場合（ステップＳ３０１：Ｙｅｓ）、システム雑音共分散行列Ｑ_ｋを調整する（ステップＳ３０２）。式（４３）の条件分岐において切替判定フラグが１である場合、波腹方位角θ_ｒの観測値と推定値との差が小さく、現状のフィルタ特性で推定値が観測値に十分に追従していると判断できる。このため、カルマンフィルタの耐ノイズ性を高めるために、観測雑音共分散行列Ｒ_ｋに対してシステム雑音共分散行列Ｑ_ｋを相対的に小さくし、更新したシステム雑音共分散行列Ｑ’を状態推定部２１３に渡す。一方、切替判定フラグが０の場合（ステップＳ３０１：Ｎｏ）、共分散行列調整部２１５は処理を行わず計算を終了する（ステップＳ３０３）。

状態推定部２１３の処理アルゴリズムは、実施の形態２と同じである。

以上説明した本実施の形態３の方位角推定装置２０の動作を図８のフローチャートに沿って説明する。図８は演算部２１が実行する波腹方位角推定処理を示すフローチャートである。

ステップＳ２０１〜ステップ２０４までは、実施の形態２と同様であるので説明を省略する。

ステップＳ２０５において、切替判定部２１４が切替判定を行う。切替判定では、式（４３）をもとに切替判定フラグの値を設定する。切替判定の結果、振動形状導出部２１２が出力する波腹方位角θ_ｒの観測値と状態推定部２１３が出力する波腹方位角θ_ｒの推定値の差の絶対値が閾値以上であり切替判定フラグが０である場合（ステップＳ２０５：Ｎｏ）、再計算を行わずその時点の状態量の推定値から波腹方位角θ_ｒの推定値を算出して処理を終了する。

一方、切替判定の結果、振動形状導出部２１２が出力する波腹方位角θ_ｒの観測値と状態推定部２１３が出力する波腹方位角θ_ｒの推定値の差の絶対値が閾値より小さく切替判定フラグが１であった場合、共分散行列調整部２１５がシステム雑音共分散行列の変更を行い（ステップＳ２０６）、当該共分散行列を状態推定部２１３に渡す。そして、状態推定部２１３は、共分散行列調整部２１５から受け取ったシステム雑音共分散行列を用いて波腹方位角θ_ｒの推定値を再計算する（ステップＳ２０７）。最後に、演算部２１は動作の終了判定を行い、終了指示がない場合は（ステップＳ２０８：Ｎｏ）ステップＳ２０１に戻って次の計算サイクルに移り、終了指示があった場合は（ステップＳ２０８：Ｙｅｓ）動作を停止する。

以上説明したように、本実施の形態３によれば、状態推定部２１３が、波腹方位角θｒの推定値を計算する際に、システム雑音共分散行列Ｑ_ｋを観測雑音共分散行列Ｒ_ｋに対して相対的に大きくしておく。切替判定部２１４は、波腹方位角θ_ｒの観測値と推定値の差の絶対値が小さい場合には、切替が必要と判断し、システム雑音共分散行列Ｑ_ｋを観測雑音共分散行列Ｒ_ｋに対して相対的に小さくして、波腹方位角θ_ｒの推定値を再計算する。一方、切替が不要と判断された場合は、再計算前の状態量の推定値から波腹方位角θ_ｒの推定値を算出することとした。これにより、実施の形態２と同様に、波腹方位角θ_ｒの観測値の変化に応じて、応答性と耐ノイズ性のいずれを優先させるかを切り替えることが可能となる。

実施の形態４．
実施の形態４に係るジャイロシステム２について、図１０，１１を用いて説明する。ジャイロシステム２の全体構成及び方位角推定装置２０の構成は実施の形態２，３と同様であるが、状態推定部２１３と共分散行列調整部２１５の動作が実施の形態２，３と異なる。

実施の形態２，３では、共分散行列調整部２１５において、システム雑音共分散行列Ｑ_ｋを変更することでカルマンフィルタの追従性と耐ノイズ性を調整したが、本実施の形態４では、共分散行列調整部２１５が、図１０に示すように、観測雑音共分散行列Ｒ_ｋを変更することで、カルマンフィルタの追従性と耐ノイズ性を調整する。

共分散行列調整部２１５の動作を、図１０を用いて説明する。図１０は共分散行列調整部２１５の処理アルゴリズムを示した図である。共分散行列調整部２１５は、図１０に示すように、切替判定フラグが１の場合（ステップＳ４０１：Ｙｅｓ）、観測雑音共分散行列Ｒ_ｋを調整する（ステップＳ４０２）。更新した観測雑音共分散行列Ｒ_ｋ’を状態推定部２１３に渡す。一方、切替判定フラグが０の場合（ステップＳ４０１：Ｎｏ）、共分散行列調整部２１５は処理を行わず計算を終了する（ステップＳ４０３）。

観測雑音共分散行列Ｒ_ｋの調整により、観測雑音共分散行列Ｒ_ｋに対してシステム雑音共分散行列Ｑ_ｋが相対的に大きくなればカルマンフィルタの追従性が高まり、観測雑音共分散行列Ｒ_ｋに対してシステム雑音共分散行列Ｑ_ｋが相対的に小さくなればカルマンフィルタの耐ノイズ性が高まる。よって、理想とするフィルタの追従性と耐ノイズ性に応じて、システム雑音共分散行列Ｑ_ｋに対する観測雑音共分散行列Ｒ_ｋの相対的な大きさを変更する。

次に、状態推定部２１３の処理アルゴリズムを、図１１を用いて説明する。図１１は状態推定部２１３の処理アルゴリズムを示した図である。図１１に示した処理のうち、第１ステップに含まれている処理は、実施の形態１に係る状態推定部２１３の処理と同様であるため、説明は省略する。

本実施の形態４においては、共分散行列調整部２１５が観測雑音共分散行列Ｒ_ｋを調整するため、実施の形態２，３における式（４２）を計算する必要がなくなり、代わりに調整した観測雑音共分散行列Ｒ_ｋ’を用いてカルマンゲインＫ_ｋを再計算する。すなわち、切替判定フラグが１である場合において（ステップＳ１１１：Ｙｅｓ）、時刻ｋにおけるカルマンゲインＫ_ｋを、式（４４）を用いて再計算する（ステップＳ１１３）。

式（４４）で求めたカルマンゲインＫ_ｋを用いて式（３３）〜（３６）を再計算する（ステップＳ１１４〜１１７）。そして、ステップＳ１１４で算出した時刻ｋでの状態量の推定値から波腹方位角θ_ｒの推定値を導出して（ステップＳ１１８）出力する。

一方、状態推定部２１３が受け取った切替判定フラグが０である場合は（ステップＳ１１１：Ｎｏ）、再計算を行わず第１ステップ内のステップＳ１０５で導出した状態量の推定値から波腹方位角θ_ｒの推定値を導出して出力する。時刻ｋでのすべての計算が終了した後、状態推定部２１３は時刻をｋからｋ＋１に更新して次の計算サイクルに移行する。このとき、ステップＳ１１６，１１７で計算する状態量の推定値、及び、誤差共分散行列の推定値の更新に関しては、切替判定フラグが１の場合は再計算した値で更新を行う。

以上説明した本実施の形態４に係る方位角推定装置２０の動作は、実施の形態２又は３と同様である。

以上説明したように、本実施の形態４によれば、状態推定部２１３が、波腹方位角θｒの推定値を計算又は再計算する際に、観測雑音共分散行列Ｒ_ｋを変更し、システム雑音共分散行列Ｑ_ｋに対して相対的に大きく又は小さくすることとした。これにより、実施の形態２及び３と同様に、カルマンフィルタの追従性と耐ノイズ性を調整することができるとともに、誤差共分散行列の予測値の計算を省略できるため、計算処理時間を短縮することができる。

実施の形態５．
実施の形態５に係るジャイロシステム２について、図１２，１３を用いて説明する。ジャイロシステム２の全体構成及び方位角推定装置２０の構成は実施の形態２−４と同様であるが、状態推定部２１３と共分散行列調整部２１５の動作が実施の形態２−４と異なる。

実施の形態２−４では、共分散行列調整部２１５において、システム雑音共分散行列Ｑ_ｋ又は観測雑音共分散行列Ｒ_ｋを変更することでカルマンフィルタの追従性と耐ノイズ性を調整したが、本実施の形態４では、共分散行列調整部２１５が、図１２に示すように、システム雑音共分散行列Ｑ_ｋと観測雑音共分散行列Ｒ_ｋの両方を変更することで、カルマンフィルタの追従性と耐ノイズ性を調整する。

共分散行列調整部２１５の動作を、図１２を用いて説明する。図１２は共分散行列調整部２１５の処理アルゴリズムを示した図である。共分散行列調整部２１５は、図１２に示すように、切替判定フラグが１の場合（ステップＳ５０１：Ｙｅｓ）、システム雑音共分散行列Ｑ_ｋ及び観測雑音共分散行列Ｒ_ｋを調整する（ステップＳ５０２）。更新したシステム雑音共分散行列Ｑ_ｋ’及び観測雑音共分散行列Ｒ_ｋ’を状態推定部２１３に渡す。一方、切替判定フラグが０の場合（ステップＳ５０１：Ｎｏ）、共分散行列調整部２１５は処理を行わず計算を終了する（ステップＳ５０３）。

システム雑音共分散行列Ｑ_ｋ及び観測雑音共分散行列Ｒ_ｋの調整により、観測雑音共分散行列Ｒ_ｋに対してシステム雑音共分散行列Ｑ_ｋが相対的に大きくなればカルマンフィルタの追従性が高まり、観測雑音共分散行列Ｒ_ｋに対してシステム雑音共分散行列Ｑ_ｋが相対的に小さくなればカルマンフィルタの耐ノイズ性が高まる。よって、理想とするフィルタの追従性と耐ノイズ性に応じて、システム雑音共分散行列Ｑ_ｋ及び観測雑音共分散行列Ｒ_ｋを変更する。

次に、状態推定部２１３の処理アルゴリズムを、図１３を用いて説明する。図１３は状態推定部２１３の処理アルゴリズムを示した図である。図１３に示した処理のうち、第１ステップに含まれている処理は、実施の形態１に係る状態推定部２１３の処理と同様であるため、説明は省略する。

第２ステップにおいて切替判定フラグが１であるとき、時刻ｋ−１における誤差共分散行列の予測値を、調整したシステム雑音共分散行列Ｑ_ｋ’で再計算し（ステップＳ１１２）、カルマンゲインＫ_ｋを、調整した観測雑音共分散行列Ｒ_ｋ’で再計算する（ステップＳ１１３）。その後、式（３３）〜（３６）を再計算する（ステップＳ１１４〜１１７）。そして、ステップＳ１１４で算出した時刻ｋでの状態量の推定値から波腹方位角θ_ｒの推定値を導出して（ステップＳ１１８）出力する。

一方、状態推定部２１３が受け取った切替判定フラグが０である場合は（ステップＳ１１１：Ｎｏ）、再計算を行わず第１ステップ内のステップＳ１０５で導出した状態量の推定値から波腹方位角θ_ｒの推定値を導出して出力する。時刻ｋでのすべての計算が終了した後、状態推定部２１３は時刻をｋからｋ＋１に更新して次の計算サイクルに移行する。このとき、ステップＳ１１６，１１７で計算する状態量の推定値、及び、誤差共分散行列の推定値の更新に関しては、切替判定フラグが１の場合は再計算した値で更新を行う。

以上説明した本実施の形態５に係る方位角推定装置２０の動作は、実施の形態２−４と同様である。

以上説明したように、本実施の形態５によれば、状態推定部２１３が、波腹方位角θｒの推定値を計算又は再計算する際に、システム雑音共分散行列Ｑ_ｋ及び観測雑音共分散行列Ｒ_ｋを変更し、システム雑音共分散行列Ｑ_ｋと観測雑音共分散行列Ｒ_ｋとの相対的な大きさを変えることとした。これにより、実施の形態２−４と同様に、カルマンフィルタの追従性と耐ノイズ性を調整することができる。

このように本発明は、アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから、変位を示すセンサ出力値を取得し、センサ出力値に基づいて、円状物体の振動振幅の観測値と、アクチュエータの交流駆動信号に対する円状物体の共振位相特性の観測値と、を導出する。そして、導出した振動振幅の観測値及び共振位相特性の観測値と、センサ出力値の時間変化と、を用いた観測方程式及び状態方程式に基づいて波腹方位角推定値を推定することとした。これにより、方位角の推定精度を高め、方位角出力の応答性能を良好にすることが可能になる。

なお、本発明は、本発明の広義の精神と範囲を逸脱することなく、様々な実施の形態及び変形が可能とされるものである。また、上述した実施の形態は、この発明を説明するためのものであり、本発明の範囲を限定するものではない。すなわち、本発明の範囲は、実施の形態ではなく、特許請求の範囲によって示される。そして、特許請求の範囲内及びそれと同等の発明の意義の範囲内で施される様々な変形が、この発明の範囲内とみなされる。

例えば、上記実施の形態２−５においては、切替判定部２１４が切替判定する基準となる閾値ｃを一つのみ設定していたが、閾値ｃを複数設けて条件を分岐し、それぞれの条件に対してシステム雑音共分散行列Ｑ_ｋと観測雑音共分散行列Ｒ_ｋの値を設定してもよい。これによれば、複数存在する閾値ｃに応じてシステム雑音共分散行列Ｑ_ｋと観測雑音共分散行列Ｒ_ｋの大きさの比を設定できるため、波腹方位角θ_ｒの観測値に対する推定値の追従性と耐ノイズ性を複数パターン設定することができる。これにより、閾値が一つの場合に生じうる、フィルタ特性を切り替える際の波腹方位角θ_ｒの推定値の急激な変化を抑制し、波腹方位角θ_ｒの観測値に対して推定値を滑らかに追従させることが可能になる。

また、上記実施の形態１−５においては、円状物体１０の振動を測定するために複数の変位センサを用いるとしたが、速度センサで振動を測定し、得られたデータを積分することで振動の変位を求めてもよい。あるいは、加速度センサで振動を測定し、得られたデータを２階積分することで振動の変位を求めてもよい。これにより、様々な円状物体１０の振動の測定手段を選択できる。

また、実施の形態１−５では、状態推定部２１３としてカルマンフィルタを利用するとしたが、状態推定部２１３として拡張カルマンフィルタ、アンセンテッドカルマンフィルタ又はアンサンブルカルマンフィルタを含むカルマンフィルタを応用したフィルタを用いてもよい。例えば、アンセンテッドカルマンフィルタを利用した場合、状態量が非線形状態空間モデルを持つ場合にも適用できる。また、アンサンブルカルマンフィルタを利用した場合は、状態量が非線形・非ガウス状態空間モデルを持つ場合にも適用できる。

あるいは、状態推定部２１３として粒子フィルタを含む状態推定手段を用いてもよい。粒子フィルタを利用した場合、状態量が非線形・非ガウス状態空間モデルを持つ場合にも適用できる。

また、演算部２１が実行した処理のプログラムを、既存のコンピュータで実行させることにより、当該コンピュータを本発明に係る方位角推定装置として機能させることも可能である。

このようなプログラムの配布方法は任意であり、例えば、ＣＤ−ＲＯＭ（Compact Disc Read-Only Memory）、ＤＶＤ（Digital Versatile Disc）、ＭＯ（Magneto Optical Disc）、メモリカード、又は他のコンピュータ読み取り可能な記録媒体に格納して配布してもよいし、インターネットや他の通信ネットワークを介して配布してもよい。

１，２ ジャイロシステム、１０ 円状物体、１１ センサ、２０ 方位角推定装置、２１ 演算部、２２ 記憶部、２１１ センサ出力取得部、２１２ 振動形状導出部、２１３ 状態推定部、２１４ 切替判定部、２１５ 共分散行列調整部。

Claims (18)

1. アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから、変位を示すセンサ出力値を取得するセンサ出力取得部と、
   前記センサ出力値に基づいて、前記円状物体の振動振幅の観測値と、前記アクチュエータの前記交流駆動信号に対する前記円状物体の共振位相特性の観測値と、を導出する振動形状導出部と、
   前記振動形状導出部が導出した前記振動振幅の観測値及び前記共振位相特性の観測値と、前記センサ出力値の時間変化と、を用いた観測方程式及び状態方程式に基づいて波腹方位角推定値を計算する状態推定部と、
   を備えた方位角推定装置。
2. 前記観測方程式は、共振する前記円状物体の時間的及び空間的な変形を表し、
   前記状態推定部は、観測雑音共分散行列とシステム雑音共分散行列を用いた前記観測方程式及び前記状態方程式の計算により、前記波腹方位角推定値を計算する、
   請求項１に記載の方位角推定装置。
3. 前記状態推定部は、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に小さく設定して前記波腹方位角推定値を計算する、
   請求項２に記載の方位角推定装置。
4. 前記振動形状導出部は、前記振動振幅の観測値と前記共振位相特性の観測値に加えて、前記センサ出力値に基づいて波腹方位角の観測値を導出し、
   前記振動形状導出部が導出した前記波腹方位角の観測値と、前記状態推定部が計算した前記波腹方位角の推定値との差分の絶対値に基づいて、前記観測雑音共分散行列又は前記システム雑音共分散行列の切替を要するか否かを判定する切替判定部と、
   前記切替判定部の判定結果に基づいて、前記観測雑音共分散行列と前記システム雑音共分散行列の相対的な大きさを調整する共分散行列調整部と、を更に備える、
   請求項２に記載の方位角推定装置。
5. 前記状態推定部は、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に小さく設定して前記波腹方位角推定値を計算し、
   前記共分散行列調整部は、前記切替判定部が切替を要すると判定した場合に、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に大きく変更し、
   前記状態推定部が、変更後の前記システム雑音共分散行列を用いて前記波腹方位角の推定値を再計算する、
   請求項４に記載の方位角推定装置。
6. 前記状態推定部は、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に大きく設定して前記波腹方位角推定値を計算し、
   前記共分散行列調整部は、前記切替判定部が切替を要すると判定した場合に、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に小さく変更し、
   前記状態推定部が、変更後の前記システム雑音共分散行列を用いて前記波腹方位角の推定値を再計算する、
   請求項４に記載の方位角推定装置。
7. 前記状態推定部は、前記システム雑音共分散行列に対して前記観測雑音共分散行列を相対的に大きく設定して前記波腹方位角推定値を計算し、
   前記共分散行列調整部は、前記切替判定部が切替を要すると判定した場合に、前記システム雑音共分散行列に対して前記観測雑音共分散行列を相対的に小さく変更し、
   前記状態推定部が、変更後の前記観測雑音共分散行列を用いて前記波腹方位角の推定値を再計算する、
   請求項４に記載の方位角推定装置。
8. 前記状態推定部は、前記システム雑音共分散行列に対して前記観測雑音共分散行列を相対的に小さく設定して前記波腹方位角推定値を計算し、
   前記共分散行列調整部は、前記切替判定部が切替を要すると判定した場合に、前記システム雑音共分散行列に対して前記観測雑音共分散行列を相対的に大きく変更し、
   前記状態推定部が、変更後の前記観測雑音共分散行列を用いて前記波腹方位角の推定値を再計算する、
   請求項４に記載の方位角推定装置。
9. 前記切替判定部は、前記振動形状導出部が導出した前記波腹方位角の観測値と、前記状態推定部が計算した前記波腹方位角の推定値との差分の絶対値が予め定めた閾値以上である場合に、切替を要すると判定する、
   請求項５又は７に記載の方位角推定装置。
10. 前記切替判定部は、前記振動形状導出部が導出した前記波腹方位角の観測値と、前記状態推定部が推定した前記波腹方位角の推定値との差分の絶対値が予め定めた閾値より小さい場合に、切替を要すると判定する、
    請求項６又は８に記載の方位角推定装置。
11. 前記切替判定部は、前記閾値を複数有しており、各閾値に応じて、前記システム雑音共分散行列と前記観測雑音共分散行列の相対的な大きさを変更する、
    請求項９又は１０に記載の方位角推定装置。
12. 前記センサは、変位センサ、速度センサ及び加速度センサの少なくとも１つである、
    請求項１から１１のいずれか１項に記載の方位角推定装置。
13. 前記状態推定部は、カルマンフィルタを含み、
    前記共分散行列調整部は、前記カルマンフィルタの追従性又は耐ノイズ性の要求に応じて、前記システム雑音共分散行列と前記観測雑音共分散行列の相対的な大きさを変更する、
    請求項４から１１のいずれか１項に記載の方位角推定装置。
14. 前記状態推定部は、拡張カルマンフィルタ、アンセンテッドカルマンフィルタ、アンサンブルカルマンフィルタの少なくとも１つを含む、
    請求項１３に記載の方位角推定装置。
15. 前記状態推定部は、粒子フィルタを含む、
    請求項１から１２のいずれか１項に記載の方位角推定装置。
16. 請求項１から１５のいずれか１項に記載の方位角推定装置を備え、
    前記円状物体は半球形状を有し、
    前記センサ出力取得部は、前記アクチュエータの前記交流駆動信号により共振モードを励起された前記半球に配置された複数の前記センサから、変位を示す前記センサ出力値を取得する、
    ジャイロシステム。
17. アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから取得した、変位を示すセンサ出力値に基づいて、前記円状物体の振動振幅の観測値と、前記アクチュエータの前記交流駆動信号に対する前記円状物体の共振位相特性の観測値と、を導出する観測値導出ステップと、
    前記振動振幅の観測値及び前記共振位相特性の観測値と、前記センサ出力値の時間変化と、に基づいて波腹方位角推定値を計算する方位角計算ステップと、
    を有する方位角推定方法。
18. コンピュータを、
    アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから取得した、変位を示すセンサ出力値に基づいて、前記円状物体の振動振幅の観測値と、前記アクチュエータの前記交流駆動信号に対する前記円状物体の共振位相特性の観測値と、を導出する振動形状導出部、
    前記振動振幅の観測値及び前記共振位相特性の観測値と、前記センサ出力値の時間変化と、に基づいて波腹方位角推定値を計算する状態推定部、
    として機能させるプログラム。

Images