(57)【要約】  方位角推定装置（２０）のセンサ出力取得部（２１１）は、アクチュエータの交流駆動信号により共振モードを励起された円状物体（１０）に配置された複数のセンサ（１１）から、変位を示すセンサ出力値を取得する。振動形状導出部（２１２）は、取得したセンサ出力値に基づいて、円状物体（１０）の振動振幅の観測値と、アクチュエータの交流駆動信号に対する円状物体（１０）の共振位相特性の観測値と、を導出する。状態推定部（２１３）は、振動形状導出部（２１２）が導出した振動振幅の観測値及び共振位相特性の観測値と、センサ出力値の時間変化と、を用いた観測方程式及び状態方程式に基づいて波腹方位角推定値を計算する。
2020245902.tif 000002
【特許請求の範囲】
【請求項１】
アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから、変位を示すセンサ出力値を取得するセンサ出力取得部と、
前記センサ出力値に基づいて、前記円状物体の振動振幅の観測値と、前記アクチュエータの前記交流駆動信号に対する前記円状物体の共振位相特性の観測値と、を導出する振動形状導出部と、
前記振動形状導出部が導出した前記振動振幅の観測値及び前記共振位相特性の観測値と、前記センサ出力値の時間変化と、を用いた観測方程式及び状態方程式に基づいて波腹方位角推定値を計算する状態推定部と、
を備えた方位角推定装置。
【請求項２】
前記観測方程式は、共振する前記円状物体の時間的及び空間的な変形を表し、
前記状態推定部は、観測雑音共分散行列とシステム雑音共分散行列を用いた前記観測方程式及び前記状態方程式の計算により、前記波腹方位角推定値を計算する、
請求項１に記載の方位角推定装置。
【請求項３】
前記状態推定部は、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に小さく設定して前記波腹方位角推定値を計算する、
請求項２に記載の方位角推定装置。
【請求項４】
前記振動形状導出部は、前記振動振幅の観測値と前記共振位相特性の観測値に加えて、前記センサ出力値に基づいて波腹方位角の観測値を導出し、
前記振動形状導出部が導出した前記波腹方位角の観測値と、前記状態推定部が計算した前記波腹方位角の推定値との差分の絶対値に基づいて、前記観測雑音共分散行列又は前記システム雑音共分散行列の切替を要するか否かを判定する切替判定部と、
前記切替判定部の判定結果に基づいて、前記観測雑音共分散行列と前記システム雑音共分散行列の相対的な大きさを調整する共分散行列調整部と、を更に備える、
請求項２に記載の方位角推定装置。
【請求項５】
前記状態推定部は、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に小さく設定して前記波腹方位角推定値を計算し、
前記共分散行列調整部は、前記切替判定部が切替を要すると判定した場合に、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に大きく変更し、
前記状態推定部が、変更後の前記システム雑音共分散行列を用いて前記波腹方位角の推定値を再計算する、
請求項４に記載の方位角推定装置。
【請求項６】
前記状態推定部は、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に大きく設定して前記波腹方位角推定値を計算し、
前記共分散行列調整部は、前記切替判定部が切替を要すると判定した場合に、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に小さく変更し、
前記状態推定部が、変更後の前記システム雑音共分散行列を用いて前記波腹方位角の推定値を再計算する、
請求項４に記載の方位角推定装置。
【請求項７】
前記状態推定部は、前記システム雑音共分散行列に対して前記観測雑音共分散行列を相対的に大きく設定して前記波腹方位角推定値を計算し、
前記共分散行列調整部は、前記切替判定部が切替を要すると判定した場合に、前記システム雑音共分散行列に対して前記観測雑音共分散行列を相対的に小さく変更し、
前記状態推定部が、変更後の前記観測雑音共分散行列を用いて前記波腹方位角の推定値を再計算する、
請求項４に記載の方位角推定装置。
【請求項８】
前記状態推定部は、前記システム雑音共分散行列に対して前記観測雑音共分散行列を相対的に小さく設定して前記波腹方位角推定値を計算し、
前記共分散行列調整部は、前記切替判定部が切替を要すると判定した場合に、前記システム雑音共分散行列に対して前記観測雑音共分散行列を相対的に大きく変更し、
前記状態推定部が、変更後の前記観測雑音共分散行列を用いて前記波腹方位角の推定値を再計算する、
請求項４に記載の方位角推定装置。
【請求項９】
前記切替判定部は、前記振動形状導出部が導出した前記波腹方位角の観測値と、前記状態推定部が計算した前記波腹方位角の推定値との差分の絶対値が予め定めた閾値以上である場合に、切替を要すると判定する、
請求項５又は７に記載の方位角推定装置。
【請求項１０】
前記切替判定部は、前記振動形状導出部が導出した前記波腹方位角の観測値と、前記状態推定部が推定した前記波腹方位角の推定値との差分の絶対値が予め定めた閾値より小さい場合に、切替を要すると判定する、
請求項６又は８に記載の方位角推定装置。
【請求項１１】
前記切替判定部は、前記閾値を複数有しており、各閾値に応じて、前記システム雑音共分散行列と前記観測雑音共分散行列の相対的な大きさを変更する、
請求項９又は１０に記載の方位角推定装置。
【請求項１２】
前記センサは、変位センサ、速度センサ及び加速度センサの少なくとも１つである、
請求項１から１１のいずれか１項に記載の方位角推定装置。
【請求項１３】
前記状態推定部は、カルマンフィルタを含み、
前記共分散行列調整部は、前記カルマンフィルタの追従性又は耐ノイズ性の要求に応じて、前記システム雑音共分散行列と前記観測雑音共分散行列の相対的な大きさを変更する、
請求項４から１１のいずれか１項に記載の方位角推定装置。
【請求項１４】
前記状態推定部は、拡張カルマンフィルタ、アンセンテッドカルマンフィルタ、アンサンブルカルマンフィルタの少なくとも１つを含む、
請求項１３に記載の方位角推定装置。
【請求項１５】
前記状態推定部は、粒子フィルタを含む、
請求項１から１２のいずれか１項に記載の方位角推定装置。
【請求項１６】
請求項１から１５のいずれか１項に記載の方位角推定装置を備え、
前記円状物体は半球形状を有し、
前記センサ出力取得部は、前記アクチュエータの前記交流駆動信号により共振モードを励起された前記半球に配置された複数の前記センサから、変位を示す前記センサ出力値を取得する、
ジャイロシステム。
【請求項１７】
アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから取得した、変位を示すセンサ出力値に基づいて、前記円状物体の振動振幅の観測値と、前記アクチュエータの前記交流駆動信号に対する前記円状物体の共振位相特性の観測値と、を導出する観測値導出ステップと、
前記振動振幅の観測値及び前記共振位相特性の観測値と、前記センサ出力値の時間変化と、に基づいて波腹方位角推定値を計算する方位角計算ステップと、
を有する方位角推定方法。
【請求項１８】
コンピュータを、
アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから取得した、変位を示すセンサ出力値に基づいて、前記円状物体の振動振幅の観測値と、前記アクチュエータの前記交流駆動信号に対する前記円状物体の共振位相特性の観測値と、を導出する振動形状導出部、
前記振動振幅の観測値及び前記共振位相特性の観測値と、前記センサ出力値の時間変化と、に基づいて波腹方位角推定値を計算する状態推定部、
として機能させるプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、円状物体の共振器を用いて方位角を推定する方位角推定装置、ジャイロシステム、方位角推定方法及びプログラムに関する。
【背景技術】
【０００２】
宇宙機をはじめとする移動体の姿勢検知又は制御に、円状物体の共振器を用いた方位角推定装置が用いられている。半球形状の共振器を用いる半球共振型ジャイロは、半球共振器とフォーサ及びピックオフを備えた振動回転センサである（例えば、特許文献１）。【０００３】
半球共振型ジャイロは、フォーサにより半球共振器を１次共振させて楕円変形モードを励起して、ピックオフにより半球共振器の振動形状を計測する。角速度が入力されたとき、半球共振器の共振の波腹となる方位角が変化するため、その変化をピックオフの出力から算出することで、角速度が検出される。【０００４】
特許文献１の半球共振型ジャイロは、共振器の周りに等間隔に配置された８つの変位センサから半球共振器の振動を検出し、波腹方位角を導出している。具体的には、基準となる０度の位置から９０度おきに配置された４つのセンサ出力から算出される０度成分信号と、４５度の位置から９０度おきに配置された４つのセンサ出力から算出される４５度成分信号の振幅比を求めることで波腹方位角を導出している。【０００５】
この振幅比を求める際に、０度成分信号及び４５度成分信号を一度パルス列に変換してから積分器に入力する。このとき、振幅が小さいパルス列と振幅が大きいパルス列の数を変えて、積分器をつりあい状態にする調整を行う。任意のサンプリングサイクル終了時における入力パルス数の比から波腹方位角を導出することができると説明されている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開昭６０−１６６８１８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
半球共振型ジャイロの角速度の検出精度は、波腹方位角の測定精度に依存しているため、その測定精度を向上させることが重要である。波腹方位角の測定において、センサ出力信号には雑音が含まれており、測定精度の低下の原因になっていた。【０００８】
ノイズ低減を図るためには、例えば、ローパスフィルタ（Low-Pass Filter：ＬＰＦ）を含む高域成分を除去する信号処理を施すことができる。しかし、このような信号処理を実行する場合、信号処理前後で遅延が生じてしまい、ジャイロの応答性能及び移動体の姿勢制御精度が悪化するという問題があった。【０００９】
本発明は上記事情に鑑みてなされたものであり、方位角の推定精度が高く、方位角出力の応答性能が良好な方位角推定装置、ジャイロシステム及び方位角推定方法及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
上記目的を達成するため、本発明の方位角推定装置は、アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから、変位を示すセンサ出力値を取得するセンサ出力取得部と、センサ出力値に基づいて、円状物体の振動振幅の観測値と、アクチュエータの交流駆動信号に対する円状物体の共振位相特性の観測値と、を導出する振動形状導出部と、を備える。また、振動形状導出部が導出した振動振幅の観測値及び共振位相特性の観測値と、センサ出力値の時間変化と、を用いた観測方程式及び状態方程式に基づいて波腹方位角推定値を推定する状態推定部を備えることを特徴とする。
【発明の効果】
【００１１】
本発明によれば、センサ出力値に基づいて算出した振動振幅及び共振位相特性の観測値を、観測方程式及び状態方程式の計算に用いることにより、モデルの精度を高めることができるため、方位角の推定精度を高め、方位角出力の応答性能を良好にすることが可能になる。
【図面の簡単な説明】
【００１２】
【図１】本発明の実施の形態１に係るジャイロシステムの全体構成を示すブロック図
【図２】実施の形態１に係る状態推定部の処理アルゴリズムを示した図
【図３】実施の形態１に係る波腹方位角推定処理のフローチャート
【図４】本発明の実施の形態２に係るジャイロシステムの全体構成を示すブロック図
【図５】実施の形態２に係る切替判定部の処理アルゴリズムを示した図
【図６】実施の形態２，３に係る共分散行列調整部の処理アルゴリズムを示した図
【図７】実施の形態２，３に係る状態推定部の処理アルゴリズムを示した図
【図８】実施の形態２，３に係る波腹方位角推定処理のフローチャート
【図９】本発明の実施の形態３に係る切替判定部の処理アルゴリズムを示した図
【図１０】本発明の実施の形態４に係る共分散行列調整部の処理アルゴリズムを示した図
【図１１】実施の形態４に係る状態推定部の処理アルゴリズムを示した図
【図１２】本発明の実施の形態５に係る共分散行列調整部の処理アルゴリズムを示した図
【図１３】実施の形態５に係る状態推定部の処理アルゴリズムを示した図
【発明を実施するための形態】
【００１３】
以下に、本発明を実施するための形態について図面を参照して詳細に説明する。なお、図中同一又は相当する部分には同じ符号を付す。
【００１４】
実施の形態１．
図１は実施の形態１に係るジャイロシステム１の全体構成を示すブロック図である。本実施の形態１に係るジャイロシステム１は、図１に示すように、共振器である円状物体１０と、円状物体１０に付した複数のセンサ１１の出力に基づいて方位角を推定する方位角推定装置２０と、を備える。
【００１５】
円状物体１０は、任意の大きさの円を含む形状を有する共振器であり、例えば半球形状を有する。本実施の形態１では、半球形状を有する円状物体１０を用いる場合について説明する。円状物体１０は半球面の縁に沿って、複数のセンサ１１が等間隔で配置されている。
【００１６】
センサ１１は、任意の変位センサである。円状物体１０に角速度が入力されると、円状物体１０の縁部の振動パターンが変化するため、各センサ１１は、変位量を示すセンサ出力値Ｅｍ（ｍ＝１，２，・・・）を出力する。ここで、ｍは複数のセンサ１１に付与した通し番号である。
【００１７】
方位角推定装置２０は、センサ出力値Ｅｍに基づいて波腹方位角推定処理を含む演算処理を実行する演算部２１及び記憶部２２を備える。演算部２１は、例えば、ＣＰＵ（Central Processing Unit：中央演算装置）、ＲＯＭ（Read Only Memory：読み出し専用メモリ）、ＲＡＭ（Random Access Memory：ランダムアクセスメモリ）から構成される。演算部２１は、記憶部２２に記憶されているプログラムを実行することにより、図１に示すように、センサ出力取得部２１１、振動形状導出部２１２、状態推定部２１３として機能する。
【００１８】
センサ出力取得部２１１は、複数のセンサ１１から入力されるセンサ出力値Ｅｍを取得して、振動形状導出部２１２に出力する。また、センサ出力取得部２１１は、センサ出力値Ｅｍの時間変化を示すＥＲｅｋ，ＥＩｍｋ（ｋ＝１，２，・・・）を計算して、状態推定部２１３に出力する。ここで、ＥＲｅｋはセンサ出力値Ｅｍの時間変化量の実数部分であり、ＥＩｍｋはセンサ出力値Ｅｍの時間変化量の虚数部分である。ｋは予め定めた時間間隔で配列した時刻に対して付与した通し番号である。
【００１９】
振動形状導出部２１２は、センサ出力値Ｅｍから振動形状を示す１次共振モードの振動振幅Ａの観測値、及び、アクチュエータの交流駆動信号に対する１次共振モードの共振位相特性φｒの観測値を算出する。振動振幅Ａの観測値及び共振位相特性φｒの観測値は、状態推定部２１３に対して出力される。
【００２０】
状態推定部２１３は、センサ出力値Ｅｍの時間変化を示すＥＲｅｋ，ＥＩｍｋと、振動形状導出部２１２から受け取った振動形状の情報をもとに、波腹方位角θｒの推定値を算出する。なお、図面において、観測値には文字の上に”~”の記号を付し、推定値には文字の上に”＾”の記号を付している。
【００２１】
センサ出力取得部２１１、振動形状導出部２１２、状態推定部２１３における具体的な処理について、円状物体１０が半球共振器である半球共振型ジャイロの場合について説明する。
【００２２】
半球共振型ジャイロは、円状物体１０である半球共振器をアクチュエータの交流駆動信号によって１次共振させ、当該共振モードの波腹方位角の変化を複数のセンサ１１で検出することにより入力された角速度を検出する。ここでは、半球共振型ジャイロの最も一般的な構成である、８つの変位センサＳｍ（ｍ＝１，２，・・・，８）を４５度間隔で半球共振器周囲に配置した構成を例として記述する。つまり、変位センサＳｍ（ｍ＝１，２，・・・，８）それぞれがセンサ１１である。
【００２３】
半球共振器に励起された１次共振モードの変位であって、波節直角位相振動による半径方向変位は、変位センサＳｍ（ｍ＝１，２，・・・，８）によって検出され、このときのセンサ出力値Ｅｍ（ｍ＝１，２，・・・，８）は、式（１）で与えられる。
【００２４】
【数１】
2020245902.tif 000003
【００２５】
ここでアクチュエータの交流駆動信号は、半球共振器を共振させるアクチュエータの正弦波・余弦波駆動信号を指す。ωｒはアクチュエータの駆動角周波数であるが、これが半球共振器の１次共振周波数になり、又は、半球共振器の１次共振周波数に近い値になる。また、波節直角位相振動は、密度および剛性に不均一性を有する現実の半球共振器において、１次共振モードの波節を新たな波腹にしたときの１次共振モードに対して９０度位相差で発生する振動を指す。
【００２６】
次に、センサ出力値Ｅｍを合成する。具体的には、半球共振器の軸の中心を原点とし、ｍ＝１のセンサが配置されている方位を０度と定義する場合、０，９０，１８０，２７０度の方位角に配置された変位センサＳｍ（ｍ＝１，３，５，７）によるセンサ出力値Ｅｍ（ｍ＝１，３，５，７）、及び、４５，１３５，２２５，３１５度の方位角に配置された変位センサＳｍ（ｍ＝２，４，６，８）によるセンサ出力値Ｅｍ（ｍ＝２，４，６，８）を、式（２）のように合成する。
【００２７】
【数２】
2020245902.tif 000004
【００２８】
ＥＲｅを実数部、ＥＩｍを虚数部とした複素表現は、式（３）のように表される。
【００２９】
【数３】
2020245902.tif 000005
【００３０】
次に、式（３）で与えられる複素表現Ｅを、式（４）を用いて、アクチュエータ駆動周波数ωｒに一致する回転速度で正負方向に回転する。
【００３１】
【数４】
2020245902.tif 000006
【００３２】
次に、式（４）で与えられる正方向回転座標系表現Ｅ＋、及び、負方向回転座標系表現Ｅ−から、低周波数域透過フィルタ等により低周波成分ＥＤＣ＋、ＥＤＣ−を求める。低周波成分ＥＤＣ＋、ＥＤＣ−は式（５）のように表される。
【００３３】
【数５】
2020245902.tif 000007
【００３４】
式（５）で与えられる低周波成分ＥＤＣ＋、ＥＤＣ−から、半球共振器に励起された１次共振モードの、アクチュエータの交流駆動信号に対する共振位相特性φｒが、式（６）より求められる。
【００３５】
【数６】
2020245902.tif 000008
【００３６】
なお、ここでは、実際の１次共振モードおよび波節直角位相振動の振動振幅が、式（７）の関係を満たすことを用いている。
【００３７】
【数７】
2020245902.tif 000009
【００３８】
一方、センサ１１による実測時には、センサ出力取得部２１１が各センサ出力値Ｅｍ（ｍ＝１，２，・・・，８）を取得し、振動形状導出部２１２が、センサ出力値Ｅｍに基づいて、振動形状を導出する。具体的には、まず、振動形状導出部２１２は、センサ出力値Ｅｍの合成値を、式（８）を用いて求める。
【００３９】
【数８】
2020245902.tif 000010
【００４０】
次に、ＥＲｅを実数部、ＥＩｍを虚数部とした、式（９）で示す複素表現を求める。
【００４１】
【数９】
2020245902.tif 000011
【００４２】
次に、式（９）で与えられる複素表現Ｅを、式（１０）を用いてアクチュエータ駆動周波数ωｒに一致する回転速度で正負方向に回転する。正方向回転座標系表現Ｅ＋、及び、負方向回転座標系表現Ｅ−は式（１０）のように表される。
【００４３】
【数１０】
2020245902.tif 000012
【００４４】
次に、センサ出力値Ｅｍの実測値から式（１０）を用いて得られた正方向回転座標系表現Ｅ＋、及び、負方向回転座標系表現Ｅ−の低周波成分ＥDC+及びＥDC−を低周波数域透過フィルタ等により算出する。得られた低周波成分ＥDC+及びＥDC−を、理論式である式（６）に代入することにより、共振位相特性φｒが得られる。さらに、式（１１）で示されるＥａ，Ｅｂを算出する。
【００４５】
【数１１】
2020245902.tif 000013
【００４６】
次に、Ｅａ，Ｅｂを用いて、半球共振器に励起された１次共振モードの波腹方位角θｒを式（１２）より算出し、振動振幅Ａを式（１３）より算出する。
【００４７】
【数１２】
2020245902.tif 000014
【００４８】
【数１３】
2020245902.tif 000015
【００４９】
このようにして、振動形状導出部２１２は、１次共振モードの振動形状を示す共振位相特性φｒ、１次共振モードの波腹方位角θｒ、振動振幅Ａを導出する。導出された値は共振位相特性φｒ、１次共振モードの波腹方位角θｒ、振動振幅Ａの観測値として状態推定部２１３に受け渡される。
【００５０】
次に、状態推定部２１３の処理アルゴリズムを説明する。具体的には、半球共振器の１次共振モードの波腹方位角θｒの推定値の導出手法について述べる。ここでは、状態推定部２１３の一例としてカルマンフィルタを適用する場合の処理アルゴリズムを記述する。
【００５１】
カルマンフィルタは、ノイズが重畳する観測値に対して、ノイズがガウス分布に従うという仮定のもと、対象のモデルに基づいて逐次的に真の状態を推定するフィルタである。ある時刻ｋにおける対象の状態量をｘｋ、一定時刻後の対象の状態量をｘｋ＋１としたとき、ｘｋ＋１は式（１４）で表される。
【００５２】
【数１４】
2020245902.tif 000016
【００５３】
式（１４）は状態方程式と呼ばれる。これに対して、ある時刻ｋにおける対象の観測値をｙｋ、状態空間を観測空間に写像する観測モデルをｈｋ、観測値に重畳する観測雑音をｖｋとすると、ｙｋは式（１５）で表される。式（１５）は観測方程式と呼ばれる。
【００５４】
【数１５】
2020245902.tif 000017
【００５５】
次に、状態方程式と観測方程式の具体例を示す。半球共振型ジャイロは、観測された半球共振器の振動形状をもとにアクチュエータの駆動を制御して、半球共振器の振動形状を一定に保たせる。ここでは、共振器の振動状態が式（１６）に示す定常状態にある場合を仮定する。
【００５６】
【数１６】
2020245902.tif 000018
【００５７】
このとき、式（２）に示すＥＲｅとＥＩｍは式（１７）のように書き直せる。
【００５８】
【数１７】
2020245902.tif 000019
【００５９】
ここで、振動形状導出部２１２が式（６）及び（１３）を用いて算出した振動振幅Ａ及び共振位相特性φｒの観測値を式（１８）のように表す。式（１８）に示した観測値を式（１７）に代入することで、時刻ｋにおける離散時間非線形確率システムの観測方程式は、式（１９）のように書き表せる。この観測方程式は、共振する円状物体１０の時間的及び空間的な変形を表している。具体的には、式（１９）において、実数部のｃｏｓ（２θｒｋ）と虚数部のｓｉｎ（２θｒｋ）が円状物体１０の空間的変形を表している。また、式（１９）において、実数部及び虚数部のｃｏｓ（ωｒｋＴ）が円状物体１０の時間的変形を表している。
【００６０】
【数１８】
2020245902.tif 000020
【００６１】
【数１９】
2020245902.tif 000021
【００６２】
ここで、式（１９）においてθｒｋは式（２０）のように書き表せる。
【００６３】
【数２０】
2020245902.tif 000022
【００６４】
波腹方位角変動量Δθｒｋは、半球共振型ジャイロ計測軸において式（２１）に示す関係を持つことから、時刻ｋにおける離散時間非線形確率システムの状態方程式は、式（２２）のように書き表せる。
【００６５】
【数２１】
2020245902.tif 000023
【００６６】
【数２２】
2020245902.tif 000024
【００６７】
よって、式（１４）のパラメータを、式（２３）に示すようにおくことにより、式（２４）に示す状態方程式が得られる。
【００６８】
【数２３】
2020245902.tif 000025
【００６９】
【数２４】
2020245902.tif 000026
【００７０】
また、式（１９）及び式（２０）より、式（２５）に示す観測方程式が得られる。
【００７１】
【数２５】
2020245902.tif 000027
【００７２】
次に、状態推定部２１３の計算手順について図２を用いて記述する。図２は状態推定部２１３の処理アルゴリズムを示した図である。カルマンフィルタを適用する場合は、時刻ｋにおける対象の状態量の推定値、及び、誤差共分散行列の推定値を式（２６）のように表記する。
【００７３】
【数２６】
2020245902.tif 000028
【００７４】
また、時刻における観測値ｙｋをもとに、次の時刻ｋ＋１の状態量を予測した値、及び、誤差共分散行列を予測した値を式（２７）のように表記する。
【００７５】
【数２７】
2020245902.tif 000029
【００７６】
まず、状態推定部２１３が初期化を行う。具体的には、状態推定部２１３は時刻ｋ＝０の初期値を式（２８）のように与える。
【００７７】
【数２８】
2020245902.tif 000030
【００７８】
これらをもとに、状態推定部２１３は時刻ｋにおけるカルマンゲインＫｋを算出する。カルマンゲインＫｋは式（２９）で表される。
【００７９】
【数２９】
2020245902.tif 000031
【００８０】
ここで、Ｒｋは観測雑音共分散行列であり、Ｈｋは式（３０）で表される。
【００８１】
【数３０】
2020245902.tif 000032
【００８２】
ここで、ｈｋは式（３１）で表される行列である。振動形状導出部２１２が式（６）及び（１３）を用いて算出したＡ，φｒの観測値を式（３１）に代入してｈｋを計算する（ステップＳ１０１）。
【００８３】
【数３１】
2020245902.tif 000033
【００８４】
よって、式（３０）は式（３２）のように計算される。すなわち、振動形状導出部２１２が式（６）及び（１３）を用いて算出したＡ，φｒの観測値を代入したＨｋが求められる（ステップＳ１０２）。また、式（２９）を用いて、カルマンゲインＫｋを計算する（ステップＳ１０３）。
【００８５】
【数３２】
2020245902.tif 000034
【００８６】
次に、状態推定部２１３は、センサ出力取得部２１１からセンサ出力値Ｅｍの時間変化を示すＥＲｅｋ，ＥＩｍｋを受け取って式（１９）に代入することにより、観測値ｙｋを計算する（ステップＳ１０４）。さらに、観測値ｙｋとカルマンゲインＫｋを用いて、式（３３）に示す時刻ｋでの状態量の推定値を算出する（ステップＳ１０５）。
【００８７】
【数３３】
2020245902.tif 000035
【００８８】
また、次の時刻ｋ＋１での状態量の計算のため、時刻ｋでの誤差共分散行列の推定値を式（３４）で算出し（ステップＳ１０６）、状態量の予測値を式（３５）で算出し（ステップＳ１０７）、誤差共分散行列の予測値を式（３６）で算出する（ステップＳ１０８）。
【００８９】
【数３４】
2020245902.tif 000036
【００９０】
【数３５】
2020245902.tif 000037
【００９１】
【数３６】
2020245902.tif 000038
【００９２】
ここで、Ｑｋはシステム雑音共分散行列であり、カルマンフィルタの耐ノイズ性を高めるため、観測雑音共分散行列Ｒｋに対して相対的に小さい値を設定して計算する。また、Ｆｋは、式（３７）で表される。ここで、システム雑音共分散行列Ｑｋが観測雑音共分散行列Ｒｋに対して相対的に小さい場合とは、システム雑音共分散行列Ｑｋの各要素の値が、対応する観測雑音共分散行列Ｒｋの各要素の値に対して相対的に小さい場合を示す。行列のどの要素の値を相対的に小さくするかは、推定する対象に応じて変更する。
【００９３】
【数３７】
2020245902.tif 000039
【００９４】
時刻ｋでのすべての計算が終了した後、状態推定部２１３は時刻をｋからｋ＋１に更新して（ステップＳ１０９）、次の計算サイクルに移行する。これらの式をもとに各時刻の状態量の推定値の計算をし（ステップＳ１０５）、式（３８）の波腹方位角の推定値の計算を繰り返して（ステップＳ１１０）、最終的に、式（３９）に示す波腹方位角の推定値を計算することができる。
【００９５】
【数３８】
2020245902.tif 000040
【００９６】
【数３９】
2020245902.tif 000041
【００９７】
以上説明した本実施の形態１の方位角推定装置２０の動作を図３のフローチャートに沿って説明する。図３は演算部２１が実行する波腹方位角推定処理を示すフローチャートである。
【００９８】
まず、方位角推定装置２０の演算部２１は初期化を行う（ステップＳ２０１）。具体的には、状態推定部２１３に対し、時刻ｋ＝０における初期値を式（２８）のように設定する。
【００９９】
次に、方位角推定装置２０の演算部２１のセンサ出力取得部２１１が円状物体１０に備えられたセンサ１１が出力するセンサ出力値Ｅｍを取得する（ステップＳ２０２）。センサ出力取得部２１１は、センサ出力値Ｅｍを振動形状導出部２１２に出力し、センサ出力の時間変化を示すＥＲｅｋ，ＥＩｍｋを算出して状態推定部２１３に出力する。
【０１００】
次に、振動形状導出部２１２が、センサ出力値Ｅｍに基づいて、振動形状を導出する。具体的には、式（１）〜（１３）を用いて、振動形状導出部２１２が、振動形状を示す値である１次共振モードの振動振幅Ａの観測値と、アクチュエータの交流駆動信号に対する一次共振モードの共振位相特性φｒの観測値と、を計算する（ステップＳ２０３）。式（１８）に示す観測値は、状態推定部２１３に対して出力される。
【０１０１】
次に、状態推定部２１３がセンサ出力取得部２１１から入力されたセンサ出力の時間変化を示すＥＲｅｋ，ＥＩｍｋと、振動形状導出部２１２から受け取った振動形状の情報をもとに、円状物体１０の状態を推定し、波腹方位角θｒの推定値を計算する。具体的には、式（１４）〜（３８）を用いて、センサ出力取得部２１１から入力されたセンサ出力の時間変化を示すＥＲｅｋ，ＥＩｍｋと、振動形状導出部２１２から受け取った振動形状を示す振動振幅Ａと共振位相特性φｒの観測値に基づいて、式（３９）に示す波腹方位角θｒの推定値を計算する（ステップＳ２０４）。
【０１０２】
最後に、方位角推定装置２０の演算部２１は動作の終了判定を行い、終了指示がない場合は（ステップＳ２０８：Ｎｏ）、ステップＳ２０１に戻り次の計算サイクルに移り、終了指示があった場合は（ステップＳ２０８：Ｙｅｓ）、処理を終了する。
【０１０３】
以上説明したように、本実施の形態１によれば、ジャイロシステム１の方位角推定装置２０は、センサ出力取得部２１１がセンサ１１から取得したセンサ出力値Ｅｍに基づいて、振動形状導出部２１２が１次共振モードの振動振幅Ａの観測値と、アクチュエータの交流駆動信号に対する１次共振モードの共振位相特性φｒの観測値を算出する。そして、状態推定部２１３が、振動振幅Ａの観測値及び共振位相特性φｒの観測値を観測方程式に代入することで、波腹方位角θｒを推定することとした。これにより、従来の観測方程式に振動振幅Ａと共振位相特性φｒの固定値を代入する手法に比べて、モデル精度が高まり、より正確に方位角を推定することが可能となる。
【０１０４】
実施の形態２．
図４は実施の形態２に係るジャイロシステム２の全体構成を示すブロック図である。本実施の形態２に係るジャイロシステム２は、図４に示すように、共振器である円状物体１０と、円状物体１０に付した複数のセンサ１１の出力に基づいて方位角を推定する方位角推定装置２０と、を備える。円状物体１０及びセンサ１１の構成は実施の形態１と同様である。
【０１０５】
本実施の形態２に係る方位角推定装置２０は、センサ出力値Ｅｍに基づいて方位角推定処理を含む演算処理を実行する演算部２１及び記憶部２２を備える。演算部２１は、記憶部２２に記憶されているプログラムを実行することにより、図４に示すように、センサ出力取得部２１１、振動形状導出部２１２、状態推定部２１３、切替判定部２１４、共分散行列調整部２１５として機能する。センサ出力取得部２１１、振動形状導出部２１２の構成は実施の形態１と同様である。
【０１０６】
センサ出力取得部２１１は、センサ１１のセンサ出力値Ｅｍ（ｍ＝１，２，・・・）を振動形状導出部２１２に、検出されたセンサの出力の時間変化ＥＲｅｋ、ＥＩｍｋを状態推定部２１３に渡す。振動形状導出部２１２は、振動形状を示す観測値であって、式（１８）に示す１次共振モードの振動振幅Ａの観測値及びアクチュエータの交流駆動信号に対する１次共振モードの共振位相特性φｒの観測値を式（６）及び（１３）を用いて算出し、状態推定部２１３に渡す。
【０１０７】
また、振動形状導出部２１２は、振動形状を示す観測値であって、式（４０）に示す波腹方位角θｒの観測値を、式（１２）を用いて算出し、切替判定部２１４に渡す。
【０１０８】
【数４０】
2020245902.tif 000042
【０１０９】
状態推定部２１３は、センサ出力取得部２１１から受け取ったセンサ出力値Ｅｍの時間変化ＥＲｅｋ，ＥＩｍｋと振動形状導出部２１２から受け取った振動形状を示す情報に基づいて、式（３９）に示す波腹方位角θｒの推定値を算出し、切替判定部２１４に渡す。
【０１１０】
切替判定部２１４は、振動形状導出部２１２が算出した式（４０）に示す波腹方位角θｒの観測値と状態推定部２１３が算出した式（３９）に示す波腹方位角θｒの推定値に基づいて切替判定を行い、切替判定結果を共分散行列調整部２１５と状態推定部２１３に送る。
【０１１１】
切替判定の結果、切替が必要と判断された場合は、共分散行列調整部２１５が条件に応じて適切な共分散行列を選択又は算出し、状態推定部２１３が当該共分散行列を用いて波腹方位角θｒの推定値を再計算し、得られた波腹方位角θｒの推定値を最終値として出力する。
【０１１２】
一方、切替判定の結果、切替が不要と判断された場合は、共分散行列調整部２１５は動作せず、最初に状態推定部２１３が算出した波腹方位角θｒの推定値を最終値として出力する。
【０１１３】
センサ出力取得部２１１、振動形状導出部２１２、状態推定部２１３における処理は実施の形態１で述べた処理を含むため、重複する処理については説明を省略する。状態推定部２１３、切替判定部２１４、共分散行列調整部２１５の具体的な処理について、円状物体１０が半球共振器である半球共振型ジャイロの場合について説明する。
【０１１４】
振動形状導出部２１２は、センサ出力値Ｅｍに基づいて、実施の形態１と同様の計算を行い、式（１２）を用いて、式（４０）に示す波腹方位角θｒの観測値を算出する。一方、状態推定部２１３は、センサ出力値Ｅｍの時間変化ＥＲｅｋ，ＥＩｍｋと振動形状導出部２１２から受け取った振動形状を示す情報に基づいて、実施の形態１と同様の計算を行い、式（３９）に示す波腹方位角θｒの推定値を算出する。
【０１１５】
図５は切替判定部２１４の処理アルゴリズムを示した図である。切替判定部２１４は、図５に示すように、振動形状導出部２１２から受け取った時刻ｋでの波腹方位角の観測値と、状態推定部２１３から受け取った時刻ｋでの波腹方位角の推定値を用いて、その差分の絶対値が閾値ｃ以上の場合、切替判定フラグを１に変更する。一方、その差分の絶対値が閾値ｃより小さい場合、切替判定フラグを０にする。すなわち、式（４１）のように切替判定フラグを設定する。
【０１１６】
【数４１】
2020245902.tif 000043
【０１１７】
切替判定部２１４は、このように決定された切替判定フラグを共分散行列調整部２１５と状態推定部２１３に送る。
【０１１８】
図６は共分散行列調整部２１５の処理アルゴリズムを示した図である。共分散行列調整部２１５は、図６に示すように、切替判定フラグが１の場合（ステップＳ３０１：Ｙｅｓ）、システム雑音共分散行列Ｑｋを調整する（ステップＳ３０２）。式（４１）の条件分岐において切替判定フラグが１である場合、波腹方位角θｒの観測値と推定値との差が大きく、現状のフィルタ特性では推定値が観測値に追従していないと判断できる。このため、カルマンフィルタの追従性を高めるために、観測雑音共分散行列Ｒｋに対してシステム雑音共分散行列Ｑｋを相対的に大きくする。
【０１１９】
ここで、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に大きくするとき、システム雑音共分散行列Ｑｋの各要素の値を、対応する観測雑音共分散行列Ｒｋの各要素の値に対して相対的に大きくする。行列のどの要素の値を相対的に大きくするかは、推定する対象に応じて変更する。
【０１２０】
共分散行列調整部２１５は、ステップＳ３０２で更新したシステム雑音共分散行列Ｑｋ’を状態推定部２１３に渡す。一方、切替判定フラグが０の場合（ステップＳ３０１：Ｎｏ）、共分散行列調整部２１５は処理を行わず計算を終了する（ステップＳ３０３）。
【０１２１】
次に、状態推定部２１３の処理アルゴリズムを説明する。図７は状態推定部２１３の処理アルゴリズムを示した図である。図７に示した処理のうち、第１ステップに含まれている処理は、実施の形態１に係る状態推定部２１３の処理と同様であるため、説明は省略する。
【０１２２】
図７に示すように、第２ステップにおいて、切替判定フラグが１である場合は（ステップＳ１１１：Ｙｅｓ）、共分散行列調整部２１５から受け取ったシステム雑音共分散行列Ｑ’を用いて時刻ｋ−１における誤差共分散行列の予測値を、式（４２）を用いて計算する（ステップＳ１１２）。
【０１２３】
【数４２】
2020245902.tif 000044
【０１２４】
さらに、式（４２）で求めた誤差共分散行列の予測値を用いて式（２９），（３３）〜（３６）を再計算する（ステップＳ１１３〜１１７）。そして、ステップＳ１１４で算出した時刻ｋでの状態量の推定値から波腹方位角θｒの推定値を計算して（ステップＳ１１８）出力する。
【０１２５】
一方、状態推定部２１３が受け取った切替判定フラグが０である場合は（ステップＳ１１１：Ｎｏ）、再計算を行わず第１ステップ内のステップＳ１０５で導出した状態量の推定値から波腹方位角θｒの推定値を導出して出力する。時刻ｋでのすべての計算が終了した後、状態推定部２１３は時刻をｋからｋ＋１に更新して次の計算サイクルに移行する。このとき、ステップＳ１１６，１１７で計算する状態量の推定値、及び、誤差共分散行列の推定値の更新に関しては、切替判定フラグが１の場合は再計算した値で更新を行う。
【０１２６】
以上説明した本実施の形態２の方位角推定装置２０の動作を図８のフローチャートに沿って説明する。図８は演算部２１が実行する波腹方位角推定処理を示すフローチャートである。
【０１２７】
まず、方位角推定装置２０の演算部２１は初期化を行う（ステップＳ２０１）。具体的には、状態推定部２１３に対し、時刻ｋ＝０における初期値を式（２８）のように設定する。
【０１２８】
次に、方位角推定装置２０の演算部２１のセンサ出力取得部２１１が、円状物体１０に備えられたセンサ１１が出力するセンサ出力値Ｅｍを取得する（ステップＳ２０２）。センサ出力取得部２１１は、センサ出力値Ｅｍを振動形状導出部２１２に出力し、センサ出力の時間変化を示すＥＲｅｋ，ＥＩｍｋを算出して状態推定部２１３に出力する。
【０１２９】
次に、振動形状導出部２１２が、センサ出力値Ｅｍに基づいて、振動形状を導出する（ステップＳ２０３）。具体的には、式（１）〜（１３）を用いて、振動形状導出部２１２が、振動形状を示す値である１次共振モードの振動振幅Ａの観測値と、１次共振モードの波腹方位角θｒの観測値と、アクチュエータの交流駆動信号に対する一次共振モードの共振位相特性φｒの観測値と、を算出する。式（１８）に示す観測値は、状態推定部２１３に対して出力され、式（４０）に示す観測値は、切替判定部２１４に対して出力される。
【０１３０】
次に、状態推定部２１３がセンサ出力取得部２１１から入力されたセンサ出力値Ｅｍの時間変化を示すＥＲｅｋ，ＥＩｍｋと、振動形状導出部２１２から受け取った振動形状の情報をもとに、円状物体１０の状態を推定し、波腹方位角θｒの推定値を算出する。具体的には、式（１４）〜（３８）を用いて、センサ出力取得部２１１から入力されたセンサ出力の時間変化を示すＥＲｅｋ，ＥＩｍｋと、振動形状導出部２１２から受け取った振動形状を示す振動振幅Ａと共振位相特性φｒの観測値に基づいて、式（３９）に示す波腹方位角θｒの推定値を算出する（ステップＳ２０４）。
【０１３１】
次に、切替判定部２１４が切替の要否の判定を行う。切替判定では、式（４１）をもとに切替判定フラグの値を設定する。切替判定の結果、振動形状導出部２１２が出力する波腹方位角の観測値と状態推定部２１３が出力する波腹方位角の推定値の差の絶対値が閾値より小さくて切替判定フラグが０である場合（ステップＳ２０５：Ｎｏ）、再計算を行わずその時点の状態量の推定値から波腹方位角θｒの推定値を算出して処理を終了する。
【０１３２】
一方、切替判定フラグが１であった場合、共分散行列調整部２１５がシステム雑音共分散行列の変更を行い（ステップＳ２０６）、当該共分散行列を状態推定部２１３に渡す。そして、状態推定部２１３は、共分散行列調整部２１５から受け取ったシステム雑音共分散行列を用いて波腹方位角θｒの推定値を再計算する（ステップＳ２０７）。最後に、演算部２１は動作の終了判定を行い、終了指示がない場合は（ステップＳ２０８：Ｎｏ）ステップＳ２０１に戻って次の計算サイクルに移り、終了指示があった場合は（ステップＳ２０８：Ｙｅｓ）処理を終了する。
【０１３３】
本実施の形態２のアルゴリズムに従って波腹方位角θｒの推定値を算出することで、波腹方位角θｒの観測値と推定値の差の絶対値が大きい場合には追従性を高めたカルマンフィルタにより波腹方位角θｒの推定値を再計算することで、推定値を観測値に追従させることができ、波腹方位角θｒの観測値と推定値の差分の絶対値が小さい場合には、耐ノイズ性を高めたカルマンフィルタにより波腹方位角θｒの推定値を低ノイズ化できる。
【０１３４】
つまり、本実施の形態２の構成は、波腹方位角θｒの推定値を高精度で推定するだけでなく、観測値の変化に応じて、応答性と耐ノイズ性を変化させることができるフィルタを備えていると言える。このようなフィルタは、角速度入力を検出した際の波腹方位角の変化がノイズの大きさに対して十分に大きいデバイスに対して大きな効果を発揮する。例えば、半球共振型ジャイロに対して有効である。角速度が入力されない場合は耐ノイズ性を高めて低ノイズ化し、角速度が入力された場合は入力を正確に検知するために追従性を高めることができる。これにより角速度入力に遅延なく応答しつつ、入力がないときのノイズを極めて小さく抑えることができる。
【０１３５】
以上説明したように、本実施の形態２によれば、ジャイロシステム２の方位角推定装置２０は、振動形状導出部２１２が、センサ出力値Ｅｍに基づいて波腹方位角θｒの観測値を算出して切替判定部２１４に渡し、状態推定部２１３が、センサ出力値Ｅｍの時間変化と振動形状の情報に基づいて波腹方位角θｒの推定値を算出して切替判定部２１４に渡す。切替判定部２１４は、波腹方位角θｒの観測値と推定値の差の絶対値が大きい場合には、切替が必要と判断し、共分散行列調整部２１５がシステム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に大きくして、状態推定部２１３が、当該システム雑音共分散行列Ｑｋを用いて波腹方位角θｒの推定値を再計算する。一方、切替が不要と判断された場合は、再計算前の状態量の推定値から波腹方位角θｒの推定値を算出することとした。これにより、波腹方位角θｒの観測値の変化に応じて、応答性と耐ノイズ性のいずれを優先させるかを切り替えることが可能となる。
【０１３６】
実施の形態３．
実施の形態３に係るジャイロシステム２について、図６−９を用いて説明する。ジャイロシステム２の全体構成及び方位角推定装置２０の構成は実施の形態２と同様であるが、状態推定部２１３、切替判定部２１４、共分散行列調整部２１５の動作が実施の形態２と異なる。なお、図６，７，８は実施の形態２と共通の図である。
【０１３７】
実施の形態２では、図８に示す波腹方位角推定処理のフローチャートのステップＳ２０４の段階において、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に小さくすることで耐ノイズ性を高めた。そして、式（４１）に示した切替判定を行い、波腹方位角θｒｋの観測値と推定値の差分の絶対値が閾値ｃ以上の場合に、ステップＳ２０６の段階で、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に大きくして波腹方位角θｒｋの推定値の再計算を行った。
【０１３８】
これに対し、本実施の形態３では、ステップＳ２０４の段階において、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に大きくすることで追従性を高める。そして、波腹方位角θｒｋの観測値と推定値の差分の絶対値が閾値ｃより小さい場合に、ステップＳ２０６の段階で、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に小さくして耐ノイズ性を高め、波腹方位角θｒｋの推定値の再計算を行う。
【０１３９】
本実施の形態３に係る方位角推定装置２０の構成について、振動形状導出部２１２が、式（４０）に示す波腹方位角θｒの観測値を算出し、状態推定部２１３が式（３９）に示す波腹方位角θｒの推定値を算出する点は実施の形態２と同じである。以後、実施の形態２と異なる点を説明する。
【０１４０】
図９は切替判定部２１４の処理アルゴリズムを示した図である。切替判定部２１４は、図９に示すように、振動形状導出部２１２から受け取った時刻ｋでの波腹方位角θｒの観測値と、状態推定部２１３から受け取った時刻ｋでの波腹方位角θｒの推定値を用いて、その差分の絶対値が閾値ｃより小さい場合、切替判定フラグを１に変更する。一方、その差分の絶対値が閾値ｃ以上である場合、切替判定フラグを０にする。すなわち、式（４３）のように切替判定フラグを設定する。
【０１４１】
【数４３】
2020245902.tif 000045
【０１４２】
切替判定部２１４は、このように決定された切替判定フラグを共分散行列調整部２１５と状態推定部２１３に送る。
【０１４３】
次に、共分散行列調整部２１５の動作を、図６を用いて説明する。図６は共分散行列調整部２１５の処理アルゴリズムを示した図である。共分散行列調整部２１５は、図６に示すように、切替判定フラグが１の場合（ステップＳ３０１：Ｙｅｓ）、システム雑音共分散行列Ｑｋを調整する（ステップＳ３０２）。式（４３）の条件分岐において切替判定フラグが１である場合、波腹方位角θｒの観測値と推定値との差が小さく、現状のフィルタ特性で推定値が観測値に十分に追従していると判断できる。このため、カルマンフィルタの耐ノイズ性を高めるために、観測雑音共分散行列Ｒｋに対してシステム雑音共分散行列Ｑｋを相対的に小さくし、更新したシステム雑音共分散行列Ｑ’を状態推定部２１３に渡す。一方、切替判定フラグが０の場合（ステップＳ３０１：Ｎｏ）、共分散行列調整部２１５は処理を行わず計算を終了する（ステップＳ３０３）。
【０１４４】
状態推定部２１３の処理アルゴリズムは、実施の形態２と同じである。
【０１４５】
以上説明した本実施の形態３の方位角推定装置２０の動作を図８のフローチャートに沿って説明する。図８は演算部２１が実行する波腹方位角推定処理を示すフローチャートである。
【０１４６】
ステップＳ２０１〜ステップ２０４までは、実施の形態２と同様であるので説明を省略する。
【０１４７】
ステップＳ２０５において、切替判定部２１４が切替判定を行う。切替判定では、式（４３）をもとに切替判定フラグの値を設定する。切替判定の結果、振動形状導出部２１２が出力する波腹方位角θｒの観測値と状態推定部２１３が出力する波腹方位角θｒの推定値の差の絶対値が閾値以上であり切替判定フラグが０である場合（ステップＳ２０５：Ｎｏ）、再計算を行わずその時点の状態量の推定値から波腹方位角θｒの推定値を算出して処理を終了する。
【０１４８】
一方、切替判定の結果、振動形状導出部２１２が出力する波腹方位角θｒの観測値と状態推定部２１３が出力する波腹方位角θｒの推定値の差の絶対値が閾値より小さく切替判定フラグが１であった場合、共分散行列調整部２１５がシステム雑音共分散行列の変更を行い（ステップＳ２０６）、当該共分散行列を状態推定部２１３に渡す。そして、状態推定部２１３は、共分散行列調整部２１５から受け取ったシステム雑音共分散行列を用いて波腹方位角θｒの推定値を再計算する（ステップＳ２０７）。最後に、演算部２１は動作の終了判定を行い、終了指示がない場合は（ステップＳ２０８：Ｎｏ）ステップＳ２０１に戻って次の計算サイクルに移り、終了指示があった場合は（ステップＳ２０８：Ｙｅｓ）動作を停止する。
【０１４９】
以上説明したように、本実施の形態３によれば、状態推定部２１３が、波腹方位角θｒの推定値を計算する際に、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に大きくしておく。切替判定部２１４は、波腹方位角θｒの観測値と推定値の差の絶対値が小さい場合には、切替が必要と判断し、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に小さくして、波腹方位角θｒの推定値を再計算する。一方、切替が不要と判断された場合は、再計算前の状態量の推定値から波腹方位角θｒの推定値を算出することとした。これにより、実施の形態２と同様に、波腹方位角θｒの観測値の変化に応じて、応答性と耐ノイズ性のいずれを優先させるかを切り替えることが可能となる。
【０１５０】
実施の形態４．
実施の形態４に係るジャイロシステム２について、図１０，１１を用いて説明する。ジャイロシステム２の全体構成及び方位角推定装置２０の構成は実施の形態２，３と同様であるが、状態推定部２１３と共分散行列調整部２１５の動作が実施の形態２，３と異なる。
【０１５１】
実施の形態２，３では、共分散行列調整部２１５において、システム雑音共分散行列Ｑｋを変更することでカルマンフィルタの追従性と耐ノイズ性を調整したが、本実施の形態４では、共分散行列調整部２１５が、図１０に示すように、観測雑音共分散行列Ｒｋを変更することで、カルマンフィルタの追従性と耐ノイズ性を調整する。
【０１５２】
共分散行列調整部２１５の動作を、図１０を用いて説明する。図１０は共分散行列調整部２１５の処理アルゴリズムを示した図である。共分散行列調整部２１５は、図１０に示すように、切替判定フラグが１の場合（ステップＳ４０１：Ｙｅｓ）、観測雑音共分散行列Ｒｋを調整する（ステップＳ４０２）。更新した観測雑音共分散行列Ｒｋ’を状態推定部２１３に渡す。一方、切替判定フラグが０の場合（ステップＳ４０１：Ｎｏ）、共分散行列調整部２１５は処理を行わず計算を終了する（ステップＳ４０３）。
【０１５３】
観測雑音共分散行列Ｒｋの調整により、観測雑音共分散行列Ｒｋに対してシステム雑音共分散行列Ｑｋが相対的に大きくなればカルマンフィルタの追従性が高まり、観測雑音共分散行列Ｒｋに対してシステム雑音共分散行列Ｑｋが相対的に小さくなればカルマンフィルタの耐ノイズ性が高まる。よって、理想とするフィルタの追従性と耐ノイズ性に応じて、システム雑音共分散行列Ｑｋに対する観測雑音共分散行列Ｒｋの相対的な大きさを変更する。
【０１５４】
次に、状態推定部２１３の処理アルゴリズムを、図１１を用いて説明する。図１１は状態推定部２１３の処理アルゴリズムを示した図である。図１１に示した処理のうち、第１ステップに含まれている処理は、実施の形態１に係る状態推定部２１３の処理と同様であるため、説明は省略する。
【０１５５】
本実施の形態４においては、共分散行列調整部２１５が観測雑音共分散行列Ｒｋを調整するため、実施の形態２，３における式（４２）を計算する必要がなくなり、代わりに調整した観測雑音共分散行列Ｒｋ’を用いてカルマンゲインＫｋを再計算する。すなわち、切替判定フラグが１である場合において（ステップＳ１１１：Ｙｅｓ）、時刻ｋにおけるカルマンゲインＫｋを、式（４４）を用いて再計算する（ステップＳ１１３）。
【０１５６】
【数４４】
2020245902.tif 000046
【０１５７】
式（４４）で求めたカルマンゲインＫｋを用いて式（３３）〜（３６）を再計算する（ステップＳ１１４〜１１７）。そして、ステップＳ１１４で算出した時刻ｋでの状態量の推定値から波腹方位角θｒの推定値を導出して（ステップＳ１１８）出力する。
【０１５８】
一方、状態推定部２１３が受け取った切替判定フラグが０である場合は（ステップＳ１１１：Ｎｏ）、再計算を行わず第１ステップ内のステップＳ１０５で導出した状態量の推定値から波腹方位角θｒの推定値を導出して出力する。時刻ｋでのすべての計算が終了した後、状態推定部２１３は時刻をｋからｋ＋１に更新して次の計算サイクルに移行する。このとき、ステップＳ１１６，１１７で計算する状態量の推定値、及び、誤差共分散行列の推定値の更新に関しては、切替判定フラグが１の場合は再計算した値で更新を行う。
【０１５９】
以上説明した本実施の形態４に係る方位角推定装置２０の動作は、実施の形態２又は３と同様である。
【０１６０】
以上説明したように、本実施の形態４によれば、状態推定部２１３が、波腹方位角θｒの推定値を計算又は再計算する際に、観測雑音共分散行列Ｒｋを変更し、システム雑音共分散行列Ｑｋに対して相対的に大きく又は小さくすることとした。これにより、実施の形態２及び３と同様に、カルマンフィルタの追従性と耐ノイズ性を調整することができるとともに、誤差共分散行列の予測値の計算を省略できるため、計算処理時間を短縮することができる。
【０１６１】
実施の形態５．
実施の形態５に係るジャイロシステム２について、図１２，１３を用いて説明する。ジャイロシステム２の全体構成及び方位角推定装置２０の構成は実施の形態２−４と同様であるが、状態推定部２１３と共分散行列調整部２１５の動作が実施の形態２−４と異なる。
【０１６２】
実施の形態２−４では、共分散行列調整部２１５において、システム雑音共分散行列Ｑｋ又は観測雑音共分散行列Ｒｋを変更することでカルマンフィルタの追従性と耐ノイズ性を調整したが、本実施の形態４では、共分散行列調整部２１５が、図１２に示すように、システム雑音共分散行列Ｑｋと観測雑音共分散行列Ｒｋの両方を変更することで、カルマンフィルタの追従性と耐ノイズ性を調整する。
【０１６３】
共分散行列調整部２１５の動作を、図１２を用いて説明する。図１２は共分散行列調整部２１５の処理アルゴリズムを示した図である。共分散行列調整部２１５は、図１２に示すように、切替判定フラグが１の場合（ステップＳ５０１：Ｙｅｓ）、システム雑音共分散行列Ｑｋ及び観測雑音共分散行列Ｒｋを調整する（ステップＳ５０２）。更新したシステム雑音共分散行列Ｑｋ’及び観測雑音共分散行列Ｒｋ’を状態推定部２１３に渡す。一方、切替判定フラグが０の場合（ステップＳ５０１：Ｎｏ）、共分散行列調整部２１５は処理を行わず計算を終了する（ステップＳ５０３）。
【０１６４】
システム雑音共分散行列Ｑｋ及び観測雑音共分散行列Ｒｋの調整により、観測雑音共分散行列Ｒｋに対してシステム雑音共分散行列Ｑｋが相対的に大きくなればカルマンフィルタの追従性が高まり、観測雑音共分散行列Ｒｋに対してシステム雑音共分散行列Ｑｋが相対的に小さくなればカルマンフィルタの耐ノイズ性が高まる。よって、理想とするフィルタの追従性と耐ノイズ性に応じて、システム雑音共分散行列Ｑｋ及び観測雑音共分散行列Ｒｋを変更する。
【０１６５】
次に、状態推定部２１３の処理アルゴリズムを、図１３を用いて説明する。図１３は状態推定部２１３の処理アルゴリズムを示した図である。図１３に示した処理のうち、第１ステップに含まれている処理は、実施の形態１に係る状態推定部２１３の処理と同様であるため、説明は省略する。
【０１６６】
第２ステップにおいて切替判定フラグが１であるとき、時刻ｋ−１における誤差共分散行列の予測値を、調整したシステム雑音共分散行列Ｑｋ’で再計算し（ステップＳ１１２）、カルマンゲインＫｋを、調整した観測雑音共分散行列Ｒｋ’で再計算する（ステップＳ１１３）。その後、式（３３）〜（３６）を再計算する（ステップＳ１１４〜１１７）。そして、ステップＳ１１４で算出した時刻ｋでの状態量の推定値から波腹方位角θｒの推定値を導出して（ステップＳ１１８）出力する。
【０１６７】
一方、状態推定部２１３が受け取った切替判定フラグが０である場合は（ステップＳ１１１：Ｎｏ）、再計算を行わず第１ステップ内のステップＳ１０５で導出した状態量の推定値から波腹方位角θｒの推定値を導出して出力する。時刻ｋでのすべての計算が終了した後、状態推定部２１３は時刻をｋからｋ＋１に更新して次の計算サイクルに移行する。このとき、ステップＳ１１６，１１７で計算する状態量の推定値、及び、誤差共分散行列の推定値の更新に関しては、切替判定フラグが１の場合は再計算した値で更新を行う。
【０１６８】
以上説明した本実施の形態５に係る方位角推定装置２０の動作は、実施の形態２−４と同様である。
【０１６９】
以上説明したように、本実施の形態５によれば、状態推定部２１３が、波腹方位角θｒの推定値を計算又は再計算する際に、システム雑音共分散行列Ｑｋ及び観測雑音共分散行列Ｒｋを変更し、システム雑音共分散行列Ｑｋと観測雑音共分散行列Ｒｋとの相対的な大きさを変えることとした。これにより、実施の形態２−４と同様に、カルマンフィルタの追従性と耐ノイズ性を調整することができる。
【０１７０】
このように本発明は、アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから、変位を示すセンサ出力値を取得し、センサ出力値に基づいて、円状物体の振動振幅の観測値と、アクチュエータの交流駆動信号に対する円状物体の共振位相特性の観測値と、を導出する。そして、導出した振動振幅の観測値及び共振位相特性の観測値と、センサ出力値の時間変化と、を用いた観測方程式及び状態方程式に基づいて波腹方位角推定値を推定することとした。これにより、方位角の推定精度を高め、方位角出力の応答性能を良好にすることが可能になる。
【０１７１】
なお、本発明は、本発明の広義の精神と範囲を逸脱することなく、様々な実施の形態及び変形が可能とされるものである。また、上述した実施の形態は、この発明を説明するためのものであり、本発明の範囲を限定するものではない。すなわち、本発明の範囲は、実施の形態ではなく、特許請求の範囲によって示される。そして、特許請求の範囲内及びそれと同等の発明の意義の範囲内で施される様々な変形が、この発明の範囲内とみなされる。
【０１７２】
例えば、上記実施の形態２−５においては、切替判定部２１４が切替判定する基準となる閾値ｃを一つのみ設定していたが、閾値ｃを複数設けて条件を分岐し、それぞれの条件に対してシステム雑音共分散行列Ｑｋと観測雑音共分散行列Ｒｋの値を設定してもよい。これによれば、複数存在する閾値ｃに応じてシステム雑音共分散行列Ｑｋと観測雑音共分散行列Ｒｋの大きさの比を設定できるため、波腹方位角θｒの観測値に対する推定値の追従性と耐ノイズ性を複数パターン設定することができる。これにより、閾値が一つの場合に生じうる、フィルタ特性を切り替える際の波腹方位角θｒの推定値の急激な変化を抑制し、波腹方位角θｒの観測値に対して推定値を滑らかに追従させることが可能になる。
【０１７３】
また、上記実施の形態１−５においては、円状物体１０の振動を測定するために複数の変位センサを用いるとしたが、速度センサで振動を測定し、得られたデータを積分することで振動の変位を求めてもよい。あるいは、加速度センサで振動を測定し、得られたデータを２階積分することで振動の変位を求めてもよい。これにより、様々な円状物体１０の振動の測定手段を選択できる。
【０１７４】
また、実施の形態１−５では、状態推定部２１３としてカルマンフィルタを利用するとしたが、状態推定部２１３として拡張カルマンフィルタ、アンセンテッドカルマンフィルタ又はアンサンブルカルマンフィルタを含むカルマンフィルタを応用したフィルタを用いてもよい。例えば、アンセンテッドカルマンフィルタを利用した場合、状態量が非線形状態空間モデルを持つ場合にも適用できる。また、アンサンブルカルマンフィルタを利用した場合は、状態量が非線形・非ガウス状態空間モデルを持つ場合にも適用できる。
【０１７５】
あるいは、状態推定部２１３として粒子フィルタを含む状態推定手段を用いてもよい。粒子フィルタを利用した場合、状態量が非線形・非ガウス状態空間モデルを持つ場合にも適用できる。
【０１７６】
また、演算部２１が実行した処理のプログラムを、既存のコンピュータで実行させることにより、当該コンピュータを本発明に係る方位角推定装置として機能させることも可能である。
【０１７７】
このようなプログラムの配布方法は任意であり、例えば、ＣＤ−ＲＯＭ（Compact Disc Read-Only Memory）、ＤＶＤ（Digital Versatile Disc）、ＭＯ（Magneto Optical Disc）、メモリカード、又は他のコンピュータ読み取り可能な記録媒体に格納して配布してもよいし、インターネットや他の通信ネットワークを介して配布してもよい。
【符号の説明】
【０１７８】
１，２  ジャイロシステム、１０  円状物体、１１  センサ、２０  方位角推定装置、２１  演算部、２２  記憶部、２１１  センサ出力取得部、２１２  振動形状導出部、２１３  状態推定部、２１４  切替判定部、２１５  共分散行列調整部。
【図１】
2020245902.tif 000047
【図２】
2020245902.tif 000048
【図３】
2020245902.tif 000049
【図４】
2020245902.tif 000050
【図５】
2020245902.tif 000051
【図６】
2020245902.tif 000052
【図７】
2020245902.tif 000053
【図８】
2020245902.tif 000054
【図９】
2020245902.tif 000055
【図１０】
2020245902.tif 000056
【図１１】
2020245902.tif 000057
【図１２】
2020245902.tif 000058
【図１３】
2020245902.tif 000059
【国際調査報告】
2020245902.tif 000060
2020245902.tif 000061