JP2013088162A - 状態推定装置 - Google Patents

Abstract

【課題】高速且つ処理負荷の小さな非線形カルマンフィルタの演算を実現する。
【解決手段】状態推定装置は、３次元磁気センサ７０を含む複数のセンサと、システムの状態を表す複数の状態変数を要素とする状態ベクトルｘ_ｋ及び複数のセンサからの出力値を要素とする観測値ベクトルｙ_ｋを用いて状態ベクトルｘ_ｋを更新することによりシステムの状態を推定する複数のカルマンフィルタＫＦと、互いに異なる複数の初期ベクトルＩＮＩを生成し、状態ベクトルｘ_ｋの初期値として複数のカルマンフィルタＫＦの各々に供給する初期値生成部２００と、複数のカルマンフィルタＫＦの推定精度を各々評価することで、推定精度が最も高いカルマンフィルタを特定し、特定したカルマンフィルタＫＦを除く他のカルマンフィルタＫＦの動作を停止させるカルマンフィルタ制御部４００とを備える。
【選択図】図３

Description

本発明は、状態推定装置に関する。

地磁気の向きや、物体の姿勢等を算出する場合、地磁気センサからの出力結果のみに基づいて算出するよりも、地磁気センサの他に加速度センサや角速度センサ等の異種の物理量を測定する複数のセンサの出力結果を統合して算出した方が、短時間で正確な値を求めることができる。

異種の物理量を測定する複数のセンサの出力を統合し、動的システムの状態を推定する方法として、非線形カルマンフィルタを用いる方法が知られている。例えば、特許文献１には、３軸の角速度センサ及び３軸の加速度センサと非線形カルマンフィルタとを実装した姿勢角計測装置が開示されている。また、非特許文献１には、３軸角速度センサ、３軸加速度センサ、及び３軸地磁気センサからの出力信号を、拡張カルマンフィルタやアンセンテッド変換を用いたシグマポイントカルマンフィルタを用いて統合し、姿勢を推定する方法が開示されている。

一般的に、非線形カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、推定された動的システムの状態から、動的システムの有する複数のセンサが計測する値（観測値）を推定する観測モデルとを有する。そして、非線形カルマンフィルタは、推定された観測値と、複数のセンサが実際に測定する観測値との差分（観測残差）を用いて、動的システムの状態を表す複数の物理量を推定する値（状態変数）を要素とする状態ベクトルを逐次更新することにより、状態ベクトルの各要素の示す値を、実際の物理量（真値）に近づける。
このように、非線形カルマンフィルタは、状態ベクトルと、状態ベクトルに基づいて算出される観測残差とを用いて、状態ベクトルを逐次更新する演算であるため、状態ベクトルの初期値の要素が真値から乖離した値に設定された場合、状態ベクトルの各要素が真値に近い値に収束するまでに長時間を要することがあり、更には、状態ベクトルの各要素が真値とは異なる不正確な値に収束することも起こり得る。

特許文献２には、複数のカルマンフィルタに異なる初期値を与えて並列に動作させ、観測残差が最も少ないカルマンフィルタを用いて状態の推定を実行することで、真値から乖離した初期値が設定されることによる発散状態（状態ベクトルの要素が真値から大きく乖離した状態）を回避する技術が開示されている。

特開平９−５１０４号公報 特開平４−１６９８１４号公報

Wolfgang Gunthner, "Enhancing Cognitive Assistance Systems with Inertial Measurement Units", Springer, 2008

ところで、観測残差にはノイズが重畳しているため、ある時刻で選択したカルマンフィルタが、次の時刻において最も観測残差が小さくなるとは限らない。従って、従来の技術のように、単に観測残差が最小になるカルマンフィルタを選択すると、不適切なものを選択してしまう可能性があるといった問題が存在する。
さらに、従来の技術のように、複数のカルマンフィルタを並列で動作させる場合、状態ベクトルを収束させる時間を短縮することができるが、処理負荷が増加する。すなわち、状態ベクトルが収束するまでの時間と処理負荷とはトレードオフの関係にあるといった問題があった。

本発明は、上述した点に鑑みてなされたものであり、状態ベクトルを適切な値に高速に収束させるとともに、非線形カルマンフィルタの演算に係る処理負荷を低減させることを解決課題とする。

上述した課題を解決するため、本発明に係る状態推定装置は、システムを観測して観測値を各々出力する複数のセンサと、前記システムの状態を表すための複数の状態変数を要素とする状態ベクトルと、前記複数のセンサから各々出力される観測値を要素とする観測値ベクトルとを用いて、前記状態ベクトルを更新することで、前記システムの状態を推定する複数のカルマンフィルタと、互いに異なる複数の初期ベクトルを生成し、複数の前記初期ベクトルのそれぞれを、前記状態ベクトルの初期値として前記複数のカルマンフィルタの各々に供給する初期値生成部と、前記複数のカルマンフィルタの推定精度を各々評価し、前記複数のカルマンフィルタのうち、推定精度が最も高いカルマンフィルタを特定し、特定したカルマンフィルタを除く他のカルマンフィルタの動作を停止させるカルマンフィルタ制御部と、を備えることを特徴とする。

カルマンフィルタは、状態ベクトルと観測値ベクトルとを用いて、状態ベクトルを更新する演算を繰り返すことにより、状態ベクトルの各要素を真値に近い値に収束させ、システムの状態を推定する。従って、状態ベクトルの初期値（すなわち、初期ベクトル）の要素が真値から大きく乖離する場合には、状態ベクトルの各要素が真値に近い値に収束するまでに長時間を要することがあり、更には、状態ベクトルの各要素が真値とは異なる不正確な値に収束することも起こり得る。このような初期ベクトルが供給されるカルマンフィルタは、推定精度が低く、システムの状態を正確に推定することはできない。

この発明によれば、状態推定装置は、複数のカルマンフィルタに対して、互いに異なる複数の初期ベクトルをそれぞれ供給し、これら複数のカルマンフィルタを並列で動作させる。状態推定装置が互いに異なる複数の初期ベクトルを生成する場合、１つの初期ベクトルを生成する場合に比べて、真値に近い値を要素とする初期ベクトルが含まれる可能性が高い。真値に近い値を要素とする初期ベクトルが供給されるカルマンフィルタは、状態ベクトルの各要素が正確且つ高速に真値に収束させることができるため、システムの状態の推定精度も高い。
そして、状態推定装置は、複数のカルマンフィルタの中から、推定精度が最も高いカルマンフィルタを特定し、特定したカルマンフィルタにより状態推定を行うとともに、特定したカルマンフィルタ以外のカルマンフィルタを停止する。従って、本発明に係る状態推定装置によれば、正確且つ高速な状態推定が可能であるとともに、状態推定に係る処理負荷の低減が可能となる。

また、上述した状態推定装置において、前記カルマンフィルタは、前記状態ベクトルと、前記観測値ベクトルとに基づいて観測残差を算出するとともに、前記状態ベクトルの推定誤差の共分散を算出し、前記カルマンフィルタ制御部は、前記複数のカルマンフィルタの各々が出力する前記観測残差に基づいて、前記状態ベクトルと前記システムの状態との誤差を評価する誤差評価値が最小となるカルマンフィルタを選択する観測残差比較部と、前記観測残差比較部によって選択されたカルマンフィルタが出力する前記状態ベクトルの推定誤差の共分散に基づいて、前記状態ベクトルの収束の程度を表す収束評価値を算出し、前記収束評価値が閾値以下であるか否かを判定する収束度判定部と、前記収束度判定部の判定結果が肯定である場合、前記観測残差比較部によって選択されたカルマンフィルタを、推定精度が最も高いカルマンフィルタであると特定し、特定したカルマンフィルタを除く他のカルマンフィルタの動作を停止させる、停止制御部と、を更に備えることが好ましい。

この発明によれば、状態推定装置は、誤差評価値と収束評価値とを用いて、推定精度が最も高いカルマンフィルタを特定する。
誤差評価値は、観測残差に基づいて定められ、状態ベクトルの示す値と真値との誤差を表す値である。すなわち、誤差評価値が小さい場合、状態ベクトルの各要素は真値に近い値を示す。従って、誤差評価値が最小となるカルマンフィルタは、システムの状態を最も正確に推定するカルマンフィルタであると看做すことができる。
一方、収束評価値は、状態ベクトルの推定誤差の共分散に基づいて定められるものであり、状態ベクトルの収束の程度を表す値である。状態ベクトルが収束している場合、ある時刻において状態ベクトルの各要素が真値に近い値を示していれば、当該ある時刻以降においても状態ベクトルの各要素は真値に近い値を示す可能性が高い。すなわち、収束評価値が小さい場合、カルマンフィルタは安定的にシステムの状態を推定することができる。
このような、誤差評価値と、収束評価値とを用いることにより、ある時刻においてシステムの状態を最も正確に推定し、且つ、当該ある時刻以降も安定的にシステムの状態を推定することができるカルマンフィルタを特定することができる。すなわち、本発明に係る状態推定装置は、推定精度の最も高いカルマンフィルタを正確に特定し、正確且つ安定的な状態推定を行うことが可能になる。

また、上述した状態推定装置において、前記観測残差比較部は、前記観測残差のうち少なくとも一部の要素のノルムの、現在時刻から所定時間だけ過去の時刻までの所定期間における最大値を、前記複数のカルマンフィルタごとに算出し、算出した複数の前記最大値が最小となるカルマンフィルタを、前記誤差評価値が最小となるカルマンフィルタとして選択する、ことを特徴とすることが好ましい。

誤差評価値は、観測残差に基づいて算出される値である。しかし、観測残差は、複数のセンサから各々出力される観測値を要素とする観測値ベクトルに基づいて算出されるベクトルであるため、ノイズが重畳する。
この発明によれば、観測残差のうち少なくとも一部の要素のノルムについて、所定期間における最大値を誤差評価値とする。従って、観測残差が観測ノイズ等により変動する場合であっても、ノイズによる影響を小さく抑えて誤差評価値を算出することができ、状態ベクトルの示す値と真値との誤差の大きさを正確に把握することが可能となる。

また、上述した状態推定装置において、前記収束評価値は、前記状態ベクトルの推定誤差の共分散を表す行列のトレースである、ことを特徴とすることが好ましい。

一般的に共分散行列の固有値は、当該固有値に対応する固有ベクトル方向の分散の大きさを表す値である。従って、状態ベクトルの推定誤差の共分散のトレース（すなわち、共分散行列が有する複数の固有値の和）は、状態ベクトルの推定誤差の大きさを表す。
また、カルマンフィルタは、状態ベクトルを、初期状態から現在時刻に至る全ての状態ベクトルの値を考慮して更新する。従って、状態ベクトルの推定誤差の共分散も、初期状態から現在時刻に至る全ての共分散の値を考慮して更新される。よって、状態ベクトルの推定誤差の共分散のトレースが小さな値となる場合、状態ベクトルの推定誤差が小さな値で安定している状態（すなわち、状態ベクトルが収束している状態）であると看做すことができる。
本発明は、収束評価値として、状態ベクトルの推定誤差の共分散のトレースを採用したため、状態ベクトルが集束している状態であるか否かを正確に判定することが可能となる。

また、上述した状態推定装置において、前記カルマンフィルタ制御部が、前記他のカルマンフィルタを停止させる前は、前記複数のカルマンフィルタは、第１のデータ型を用いて演算を行い、前記カルマンフィルタ制御部が、前記他のカルマンフィルタを停止させた後は、前記カルマンフィルタ制御部によって特定されたカルマンフィルタは、前記第１のデータ型よりもデータサイズの大きな第２のデータ型を用いて演算を行うことを特徴とすることが好ましい。

この発明によれば、複数のカルマンフィルタが並列で動作するときは、これら複数のカルマンフィルタは、データサイズの小さな第１のデータ型を用いた演算を行う。従って、本発明に係る状態推定装置は、複数のカルマンフィルタを並列で動作させる場合の計算負荷を、低減することができる。
一方、この発明によれば、特定されたカルマンフィルタが単独で動作するときは、当該特定されたカルマンフィルタは、データサイズの大きな第２のデータ型を用いた演算を行う。従って、本発明に係る状態推定装置は、特定された１つのカルマンフィルタにより演算を行う場合に、高い精度でシステムの状態を推定することが可能となる。

本発明の実施形態に係る携帯機器の構成を示すブロック図である。 携帯機器の外観を示す斜視図である。 本発明の実施形態に係る状態推定プログラムが実行されることにより実現される機能を表す、機能ブロック図である。 本発明の実施形態に係る状態推定装置の動作を説明するためのフローチャートである。 初期姿勢を説明するための概念図である。 初期姿勢を説明するための概念図である。 初期姿勢を説明するための概念図である。 本発明の実施形態に係るカルマンフィルタの機能を説明するための機能ブロック図である。

＜Ａ．実施形態＞
本発明の実施の形態について図面を参照して説明する。

［１． 機器構成及びソフトウェア構成］
図１は、本発明の実施形態に係る携帯機器１のブロック図であり、図２は携帯機器１の外観を示す斜視図である。携帯機器１は、携帯機器１の姿勢の変化に応じて、携帯機器１が備える画面に表示される地図などの画像を回転させ、画像によって表される方位を現実空間の方位に追従させる機能を備える。この機能は、携帯機器１が備える各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯機器１の姿勢または携帯機器１から見た地磁気の向き等を推定することによって実現される。

携帯機器１は、各種の構成要素とバスを介して接続され装置全体を制御するＣＰＵ１０、ＣＰＵ１０の作業領域として機能するＲＡＭ（蓄積部）２０、状態推定プログラム１００等の各種のプログラムやデータを記憶したＲＯＭ３０、通信を実行する通信部４０、画像を表示する表示部５０、及びＧＰＳ部６０を備える。
また、携帯機器１は、地磁気等の磁気を検出して磁気データｑを出力する３次元磁気センサ７０、加速度を検出して加速度データａを出力する３次元加速度センサ８０、及び角速度を検出して角速度データｇを出力する３次元角速度センサ９０を備える。

表示部５０は、ＣＰＵ１０が状態推定プログラム１００を実行することにより推定したシステムの状態に基づいて算出された地磁気の向きや携帯機器１の姿勢μ等を、矢印等の画像により表示する。
ＧＰＳ部６０は、ＧＰＳ衛星からの信号を受信して携帯機器１の位置情報（緯度、経度）を生成する。

３次元磁気センサ７０は、地磁気を検出するためのセンサであり、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及びＺ軸磁気センサ７３を備える。各センサは、ＭＩ素子（磁気インピーダンス素子）、ＭＲ素子（磁気抵抗効果素子）などを用いて構成することができる。磁気センサＩ／Ｆ７４は、各センサからの出力信号をＡＤ変換して磁気データｑを出力する。この磁気データｑは、ｘ軸、ｙ軸およびｚ軸の３成分を有するベクトルとして表されるデータである。より具体的には、磁気データｑは、３次元磁気センサ７０と一体となって運動する携帯機器１に固定された座標系において、Ｘ軸磁気センサ７１からの出力値がｘ軸成分として表され、Ｙ軸磁気センサ７２からの出力値がｙ軸成分として表され、Ｚ軸磁気センサ７３からの出力値がｚ軸成分として表されるベクトルデータである。
なお、３次元磁気センサ７０は、携帯機器１に設けられるため、地磁気の他に、携帯機器１の部品が発する内部磁界と、携帯機器１の外部にある外部物が発する外部磁界とを検出する。但し、本実施形態では、外部磁界は無視できる程度に小さいものと想定する。
従って、磁気データｑを用いて地磁気の向き及び大きさを算出するためには、磁気データｑから、内部磁界を表す成分を減算する補正処理を行う必要がある。この補正処理において、減算される値（つまり、内部磁界を表す成分）を、３次元磁気センサ７０のオフセットと呼ぶ。

３次元加速度センサ８０は、Ｘ軸加速度センサ８１、Ｙ軸加速度センサ８２、及びＺ軸加速度センサ８３を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などどのような検知方式であってもよい。加速度センサＩ／Ｆ８４は、各センサからの出力信号をＡＤ変換して加速度データａを出力する。この加速度データａは、３次元加速度センサ８０と一体となって運動する携帯機器１に固定された座標系における慣性力と重力との合力を、ｘ軸、ｙ軸及びｚ軸の３成分を有するベクトルとして表すデータである。従って、携帯機器１が静止状態または等速直線運動状態にあれば、加速度データａは、携帯機器１に固定された座標系から見た重力加速度の大きさと方向とを示すベクトルデータとなる。

３次元角速度センサ９０は、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及びＺ軸角速度センサ９３を備える。角速度センサＩ／Ｆ９４は、各センサからの出力信号をＡＤ変換して角速度データｇを出力する。角速度データｇは、各軸の回りの角速度を示すベクトルデータである。

ＣＰＵ１０は、ＲＯＭ３０に格納されている状態推定プログラム１００を実行することによって、携帯機器１の姿勢や磁気センサのオフセット等のシステムの状態を表す複数の物理量を推定する。すなわち、携帯機器１は、ＣＰＵ１０が状態推定プログラム１００を実行することで、状態推定装置として機能する。

図３は、状態推定装置のうち、ＣＰＵ１０が状態推定プログラム１００を実行することにより実現される機能を表す、機能ブロック図である。
図３に示すように、状態推定装置は、互いに異なる複数の初期ベクトルを出力する初期値生成部２００、複数のカルマンフィルタを備えカルマンフィルタの演算を行うカルマンフィルタ部３００、カルマンフィルタ部３００の動作を制御するカルマンフィルタ制御部４００、及び、カルマンフィルタ部３００からの出力に基づいて出力情報を生成する出力情報生成部５００を備える。

カルマンフィルタ部３００は、１０個のカルマンフィルタＫＦを備え、各々のカルマンフィルタＫＦは、非線形カルマンフィルタの演算を行う。
なお、説明の便宜上、１０個のカルマンフィルタＫＦを区別する場合、１０個のカルマンフィルタを、それぞれ、ＫＦ［１］、ＫＦ［２］、…、ＫＦ［１０］と表記する。また、１０個のカルマンフィルタの中で、第ｍ番目（ｍは、１≦ｍ≦１０を満たす自然数）のカルマンフィルタを、ＫＦ［ｍ］と表記する。

１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］のそれぞれは、第1演算モードまたは第２演算モードのいずれかの演算モードで、非線形カルマンフィルタの演算を行う。
ここで、第１演算モードとは、ＣＰＵ１０及び状態推定プログラム１００が対応可能な実数を表すデータ型の中で、データサイズの最も小さな第１のデータ型を用いて、非線形カルマンフィルタの演算を行うモードである。また、第２演算モードとは、第１のデータ型よりもデータサイズの大きな第２のデータ型を用いて、非線形カルマンフィルタの演算を行うモードである。すなわち、第２演算モードは、第１演算モードと比べ、演算の精度が高く、処理負荷が大きい演算モードである。

非線形カルマンフィルタの演算とは、以下で説明する状態ベクトルｘ及び観測値ベクトルｙを用いて算出される観測残差ｚに基づいて、状態ベクトルｘを周期的に更新することにより、システムの状態を推定する演算である。なお、詳細は後述するが、観測残差ｚとは、観測値ベクトルｙと、推定観測値ベクトルｙ⁻との差分である。
また、非線形カルマンフィルタの演算は、状態ベクトルの更新と同じ周期で、状態ベクトルｘの推定誤差の共分散Ｐも更新する。

状態ベクトルｘとは、複数の状態変数を要素とするベクトルである。状態ベクトルｘの要素である複数の状態変数は、それぞれ、非線形カルマンフィルタが推定するシステムの状態を示す複数の物理量を表す。すなわち、状態ベクトルｘは、非線形カルマンフィルタが推定するシステムの状態を表すベクトルである。
本実施形態は、状態ベクトルｘの要素（状態変数）として、携帯機器１の姿勢μ、地磁気の強さｒ、地磁気の伏角φ、携帯機器１の角速度ω、角速度センサ９１〜９３のオフセット推定値ｂ_Ｏ、及び磁気センサ７１〜７３のオフセット推定値ｃ_Ｏを採用し、これらを非線形カルマンフィルタの演算における推定の対象とする。
時刻Ｔ＝ｋにおける状態ベクトルｘ_ｋは、以下の式（１）で表される。なお、各状態変数の右下に付された添え字「ｋ」は、当該状態変数が時刻Ｔ＝ｋにおける値であることを表す。

ここで、地磁気の強さｒはスカラーであり、地磁気の伏角φはスカラーであり、角速度ωは３次元ベクトルであり、角速度センサのオフセット推定値ｂ_Ｏは３次元ベクトルであり、磁気センサのオフセット推定値ｃ_Ｏは３次元ベクトルである。
また、姿勢μは、クォータニオンを用いて表現される。クォータニオンとは、物体の姿勢（回転状態）を表す４次元数である。例えば、携帯機器１に固定された座標系の各軸と、地上に固定された座標系の各軸とが一致する姿勢μを基準姿勢と定義し、基準姿勢をμ＝（０、０、０、１）と表現する。このとき、携帯機器１の任意の姿勢μは、基準姿勢に対して単位ベクトルρの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢μは、クォータニオンを用いて、式（２）で与えられる。

一方、観測値ベクトルｙとは、３次元磁気センサ７０から出力される磁気データｑ、３次元加速度センサ８０から出力される加速度データａ、及び、３次元角速度センサ９０から出力される角速度データｇを要素とするベクトルである。時刻Ｔ＝ｋにおける観測値ベクトルｙ_ｋは式（３）で与えられる。

各々のカルマンフィルタＫＦは、非線形カルマンフィルタの演算の結果、周期的に算出される、状態ベクトルｘ_ｋ、観測残差ｚ_ｋ、及び状態ベクトルの推定誤差の共分散Ｐ_ｋを、出力する。なお、説明の便宜上、第ｍ番目のカルマンフィルタＫＦ［ｍ］からの出力は、状態ベクトルｘ_ｋ［ｍ］、観測残差ｚ_ｋ［ｍ］、状態ベクトルの推定誤差の共分散Ｐ_ｋ［ｍ］等のように、符号［ｍ］を付して表記する。
また、詳細は後述するが、本実施形態では、各々のカルマンフィルタＫＦが出力する状態ベクトルｘ_ｋとは、厳密には、更新後の状態ベクトルｘ^＋ _ｋであり、各々のカルマンフィルタＫＦが出力する状態ベクトルの推定誤差の共分散Ｐ_ｋとは、厳密には、更新後の状態ベクトルの推定誤差の共分散Ｐ^＋ _ｋである。

初期値生成部２００は、ＲＯＭ３０に格納された各種設定情報等に基づいて、互いに異なる１０個の初期ベクトルＩＮＩ［１］〜ＩＮＩ［１０］を生成し、これらを出力する。
１０個の初期ベクトルＩＮＩ［１］〜ＩＮＩ［１０］は、それぞれ、１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］に供給され、状態ベクトルｘの初期値（すなわち、時刻Ｔ＝０における状態ベクトルｘ_０）として適用される。

第ｍ番目の初期ベクトルＩＮＩ［ｍ］は、姿勢μの初期値μ_０［ｍ］、地磁気の強さｒの初期値ｒ_０［ｍ］、地磁気の伏角φの初期値φ_０［ｍ］、角速度ωの初期値ω_０［ｍ］、角速度センサのオフセット推定値ｂ_Ｏの初期値ｂ_Ｏ,０［ｍ］、及び磁気センサのオフセット推定値ｃ_Ｏの初期値ｃ_Ｏ,０［ｍ］を要素として含み、以下の式（４）により示される。
なお、初期ベクトルＩＮＩ［ｍ］の各要素の生成方法については、後述する。

カルマンフィルタ制御部４００は、１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］からの出力値に基づいて、１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］の中で、推定精度が最も高いカルマンフィルタＫＦ［Ｓｅｌ］を選択し、選択したカルマンフィルタＫＦ［Ｓｅｌ］以外の９個のカルマンフィルタＫＦの動作を停止する制御を行う。

なお、カルマンフィルタＫＦの推定精度とは、カルマンフィルタＫＦがシステムの状態をどの程度正確且つ安定的に推定しているかについて表す指標であり、具体的には、状態ベクトルｘ_ｋの示す値と状態ベクトルの真値との誤差の大きさを評価する値（誤差評価値）、及び、状態ベクトルｘ_ｋの収束の程度を表す値（収束評価値）により表現される指標である。そして、カルマンフィルタＫＦの推定精度が高い状態とは、状態ベクトルｘ_ｋの示す値と状態ベクトルの真値との誤差が小さく（つまり、誤差評価値が小さく）、且つ、状態ベクトルｘ_ｋが収束している（つまり、収束評価値が小さい）状態のことを意味する。ここで、「真値」とは、状態ベクトルｘ_ｋの各要素に対応する実際の物理量を表す値である。また、「状態ベクトルの真値」とは、状態ベクトルｘ_ｋの各要素に対応する実際の物理量を表す値（真値）を要素とするベクトルである。
状態ベクトルｘ_ｋの示す値と状態ベクトルの真値との誤差が小さい場合（つまり、状態ベクトルｘ_ｋの各要素が真値に近い正確な値を示す場合）、カルマンフィルタＫＦは、時刻Ｔ＝ｋにおいて、システムを正確に推定していると看做すことができる。
また、状態ベクトルｘ_ｋが収束している場合、時刻Ｔ＝ｋにおいて状態ベクトルｘ_ｋの各要素が真値に近い正確な値を示していれば、時刻Ｔ＝ｋ以降においても状態ベクトルｘの各要素が真値に近い正確な値を示す可能性が高い。つまり、状態ベクトルｘ_ｋが収束している場合、カルマンフィルタＫＦは、システムの状態を安定的に推定していると看做すことができる。

以下、カルマンフィルタ制御部４００の詳細について説明する。
カルマンフィルタ制御部４００は、状態ベクトルｘ_ｋの示す値と状態ベクトルの真値との誤差（誤差評価値）が最小のカルマンフィルタを選択する観測残差比較部４２０、状態ベクトルｘ_ｋが収束していると看做すことができるか否かを判定する収束度判定部４４０、及び、カルマンフィルタＫＦの動作を停止させる制御を行う停止制御部４６０を備える。

観測残差比較部４２０は、１０個のピークモニタＰＭ［１］〜ＰＭ［１０］と、比較部４２２とを備える。
第ｍ番目のピークモニタＰＭ［ｍ］は、以下の式（５）に示すように、第ｍ番目のカルマンフィルタＫＦ［ｍ］が出力する観測残差ｚ_ｋ［ｍ］を、時刻Ｔ＝ｋ−τ_０から時刻Ｔ＝ｋまでの所定期間モニターし、当該期間における観測残差ｚ_ｋ［ｍ］のノルムの最大値を、誤差評価値^ＭＡＸｚ_ｋ［ｍ］として出力する。

比較部４２２は、１０個のピークモニタＰＭ［１］〜ＰＭ［１０］がそれぞれ出力する誤差評価値^ＭＡＸｚ_ｋ［１］〜^ＭＡＸｚ_ｋ［１０］のうち、最小となる誤差評価値^ＭＡＸｚ_ｋ［Ｓｅｌ］に対応するカルマンフィルタＫＦ［Ｓｅｌ］を示す選択値Ｓｅｌを出力する（但し、Ｓｅｌは、１≦Ｓｅｌ≦１０を満たす自然数）。
なお、観測残差比較部４２０は、カルマンフィルタ部３００が観測残差ｚ_ｋ［１］〜ｚ_ｋ［１０］の全てを出力する場合のみ、処理を実行する。例えば、ＣＰＵ１０は、カルマンフィルタ部３００が、観測残差ｚ_ｋ［１］〜ｚ_ｋ［１０］のうち１つの観測残差ｚ_ｋ［ｍ］のみを出力する場合には、観測残差比較部４２０の動作を停止させる。

前述のとおり、観測残差ｚ_ｋは、状態ベクトルｘと観測値ベクトルｙとを用いて算出される値であり、観測残差ｚ_ｋに基づいて、状態ベクトルｘの示す値と状態ベクトルの真値との誤差の大きさを表すことができる。しかし、観測値ベクトルｙには、ノイズが重畳し、時刻Ｔの経過に伴い変動する値であるため、観測残差ｚ_ｋも時刻Ｔの経過に伴い変動する。
そこで、観測残差比較部４２０は、観測残差ｚ_ｋ［ｍ］を所定期間モニターし、所定期間における観測残差ｚ_ｋ［ｍ］のノルムの最大値を誤差評価値^ＭＡＸｚ_ｋ［ｍ］として採用し、誤差評価値^ＭＡＸｚ_ｋ［ｍ］を用いて、状態ベクトルｘ_ｋの示す値と状態ベクトルの真値との誤差の大きさを評価する。
これにより、観測残差比較部４２０は、状態ベクトルｘ_ｋの示す値と真値との誤差の大きさを評価する際の、各種センサの測定誤差やノイズ等の影響を低減することができ、状態ベクトルｘ_ｋの示す値と状態ベクトルの真値との誤差が最小となるカルマンフィルタＫＦを、正しく選択することが可能となる。

なお、本実施形態では、一定期間における観測残差ｚ_ｋ［ｍ］のノルムの最大値を誤差評価値^ＭＡＸｚ_ｋ［ｍ］として採用するが、本発明はこのような形態に限定されるものではなく、観測残差ｚ_ｋ［ｍ］の一部の要素のノルムの所定期間における最大値を誤差評価値として採用してもよい。例えば、観測残差比較部４２０は、観測残差ｚ_ｋ［ｍ］を構成する要素のうち、観測値ベクトルｙ_ｋの磁気データｑ_ｋに対応する要素について一定期間モニターし、当該要素のノルムの最大値を誤差評価値として採用するものであってもよい。

また、本実施形態では、状態ベクトルｘ_ｋの示す値と状態ベクトルの真値との誤差の大きさを評価する値として、観測残差ｚ_ｋ［ｍ］に基づいて定められる誤差評価値^ＭＡＸｚ_ｋ［ｍ］を用いるが、本発明はこれに限定するものではなく、誤差評価値^ＭＡＸｚ_ｋ［ｍ］以外であっても、状態ベクトルｘ_ｋの示す値と状態ベクトルの真値との誤差の大きさを評価することのできる値であれば、いかなる値を用いてもよい。

収束度判定部４４０は、１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］のそれぞれが出力する状態ベクトルの推定誤差の共分散Ｐ^＋ _ｋ［１］〜Ｐ^＋ _ｋ［１０］のうち、選択値Ｓｅｌが示すカルマンフィルタＫＦ［Ｓｅｌ］が出力する状態ベクトルの推定誤差の共分散Ｐ^＋ _ｋ［Ｓｅｌ］のトレースＴｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）を、収束評価値として採用する。そして、収束度判定部４４０は、収束評価値Ｔｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）が、閾値Ｐ_ＴＨ以下であるか否かを判定し、判定結果を出力する。

一般的に、共分散行列の固有値は、当該固有値に対応する固有ベクトル方向の分散の大きさを表す。また、ある行列のトレース（対角成分）は、当該ある行列の有する複数の固有値の和を表す。つまり、共分散行列のトレースは、当該共分散行列の全ての固有ベクトルの方向の分散の大きさの和を表す値である。従って、収束評価値Ｔｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）によって、状態ベクトルｘ_ｋの推定誤差の大きさ、つまり状態ベクトルｘ_ｋの収束の程度を表すことができる。
一方、状態ベクトルｘ_ｋは、初期状態Ｔ＝０から時刻Ｔ＝ｋ−１に至る全ての状態ベクトルｘ_０〜ｘ_ｋ−１を考慮して、更新される値であるため、状態ベクトルの推定誤差の共分散Ｐ^＋ _ｋも、初期状態Ｔ＝０から時刻Ｔ＝ｋ−１に至る全ての状態ベクトルの推定誤差の共分散Ｐ^＋ _０〜Ｐ^＋ _ｋ−１を考慮した値となる。よって、収束評価値Ｔｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）は、初期状態Ｔ＝０から時刻Ｔ＝ｋ−１に至るまでの期間における、状態ベクトルｘ_０〜ｘ_ｋ−１の推定誤差を考慮した値となる。つまり、収束評価値Ｔｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）が小さな値となる場合、状態ベクトルｘ_ｋの推定誤差が小さな値で安定していると看做すことができる。例えば、時刻Ｔ＝ｋにおいて、状態ベクトルｘ_ｋの各要素が真値に近い値を示していれば、時刻Ｔ＝ｋ以降においても、状態ベクトルｘの各要素が真値に近い値を示す可能性が高い。
このような収束評価値Ｔｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）を用いることにより、カルマンフィルタＫＦ［Ｓｅｌ］の有する状態ベクトルｘ_ｋの収束の程度を評価することができる。そして、収束評価値Ｔｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）が、閾値Ｐ_ＴＨ以下である場合（すなわち、状態ベクトルｘ_ｋが収束していると看做すことができる場合）、カルマンフィルタＫＦ［Ｓｅｌ］は、システムの状態の正確な推定を安定的に行っていると看做すことができる。

なお、本実施形態では、状態ベクトルｘ_ｋの収束の程度を表す収束評価値として、状態ベクトルの推定誤差の共分散Ｐ^＋ _ｋ［Ｓｅｌ］のトレースＴｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）を採用するが、本発明はこれに限定するものではなく、収束評価値Ｔｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）以外であっても、状態ベクトルｘ_ｋの収束の程度を表す値であれば、いかなる値を用いてもよい。
例えば、収束度判定部４４０は、状態ベクトルの推定誤差の共分散Ｐ^＋ _ｋ［Ｓｅｌ］の最大固有値を収束評価値として採用してもよい。すなわち、収束度判定部４４０は、状態ベクトルの推定誤差の共分散Ｐ^＋ _ｋ［Ｓｅｌ］の最大固有値が、予め定めた閾値以下であるか否かを判定するものであってもよい。この場合、状態ベクトルの推定誤差の共分散Ｐ^＋ _ｋ［Ｓｅｌ］の最大固有値が、閾値以下であれば、状態ベクトルｘ_ｋの推定誤差が小さな値で安定している（つまり、状態ベクトルｘ_ｋが収束している）と看做すことができる。

なお、収束度判定部４４０は、カルマンフィルタ部３００が共分散Ｐ^＋ _ｋ［１］〜Ｐ^＋ _ｋ［１０］の全てを出力する場合のみ、処理を実行する。例えば、ＣＰＵ１０は、カルマンフィルタ部３００が共分散Ｐ^＋ _ｋ［１］〜Ｐ^＋ _ｋ［１０］のうち１個の共分散Ｐ^＋ _ｋ［ｍ］のみを出力する場合、収束度判定部４４０の動作を停止させる。

停止制御部４６０は、収束度判定部４４０が出力する判定結果が閾値Ｐ_ＴＨ以下である場合、比較部４２２が出力する選択値Ｓｅｌが示すカルマンフィルタＫＦ［Ｓｅｌ］以外の９個のカルマンフィルタＫＦの動作を停止させる制御信号Ｃｔｒを出力する。

出力情報生成部５００は、カルマンフィルタ制御部４００が選択したカルマンフィルタＫＦ［Ｓｅｌ］から出力される状態ベクトルｘ_ｋ［Ｓｅｌ］に基づいて、携帯機器１の姿勢や、携帯機器１から見た地磁気の向きなどの出力情報を生成する。
なお、出力情報生成部５００は、カルマンフィルタ部３００から、複数の状態ベクトルｘ_ｋ［１］〜ｘ_ｋ［１０］が出力されている場合には、出力情報の生成は行わず、カルマンフィルタ部３００から、単一の状態ベクトルｘ_ｋ［Ｓｅｌ］が出力される場合のみ、出力情報の生成を行う。

前述のとおり、非線形カルマンフィルタの演算は、時刻Ｔ＝ｋにおける状態ベクトルｘｋを、観測残差ｚ_ｋと時刻Ｔ＝ｋ−１における状態ベクトルｘ_ｋ−１とに基づいて算出する。すなわち、状態ベクトルｘ_ｋは、初期状態（時刻Ｔ＝０）から時刻Ｔ＝ｋ−１に至る全ての状態ベクトルｘ_０〜ｘ_ｋ−１を考慮して、更新される。従って、初期ベクトルＩＮＩが、時刻Ｔ＝０における状態ベクトルの真値から大きく乖離した値に設定された場合、カルマンフィルタ演算処理を行っても、状態ベクトルｘ_ｋの示す値が状態ベクトルの真値に近い値に収束するまでに長い時間を要し、更には状態ベクトルｘ_ｋの示す値が状態ベクトルの真値に近づかないこともある。
これに対して、本実施形態に係る状態推定装置は、１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］に、互いに異なる１０個の初期ベクトルＩＮＩ［１］〜ＩＮＩ［１０］を供給し、これら１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］を並列で動作させる。そして、状態推定装置は、推定精度が最も高いカルマンフィルタＫＦ［Ｓｅｌ］を特定し、特定された１個のカルマンフィルタＫＦ［Ｓｅｌ］から出力される状態ベクトルｘ_ｋ［Ｓｅｌ］に基づいて、出力情報を生成する。
このように、本実施形態に係る状態推定装置は、１０個の初期ベクトルＩＮＩ［１］〜ＩＮＩ［１０］のうち、時刻Ｔ＝０における状態ベクトルの真値に近い値に設定された初期ベクトルＩＮＩ［Ｓｅｌ］に基づいて動作するカルマンフィルタＫＦ［Ｓｅｌ］により状態推定を行うため、状態ベクトルｘ_ｋの正確且つ高速な収束が可能となった。すなわち、本実施形態に係る状態推定装置によれば、正確且つ高速な状態推定を行うことが可能となる。

［２． 状態推定装置の処理フロー］
図４は、状態推定装置の動作を説明するためのフローチャートである。
このフローチャートに示される処理は、ＣＰＵ１０が、状態推定プログラム１００を実行することにより実施される。

ステップＳ１０１において、ＣＰＵ１０は、初期ベクトル生成処理を実行する。具体的には、ＣＰＵ１０は、複数の初期ベクトルＩＮＩ［１］〜ＩＮＩ［１０］を生成する。
すなわち、ＣＰＵ１０は、ステップＳ１０１の初期ベクトル生成処理を実行することにより、初期値生成部２００として機能する。

ステップＳ１０２において、ＣＰＵ１０は、カルマンフィルタ並列演算処理を実行する。具体的には、ＣＰＵ１０は、複数のカルマンフィルタＫＦ［１］〜ＫＦ［１０］の全てを用いて非線形カルマンフィルタの演算を実行することで、観測残差ｚ_ｋ［１］〜ｚ_ｋ［１０］と状態ベクトルの推定誤差の共分散Ｐ^＋ _ｋ［１］〜Ｐ^＋ _ｋ［１０］とを生成する。
なお、カルマンフィルタ並列演算処理において、ＣＰＵ１０は、第１のデータ型を用いて（第１演算モードにより）非線形カルマンフィルタの演算を行う。

カルマンフィルタ並列演算処理は、１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］を並列に動作させる処理であるため、処理負荷が大きい。
しかし、本実施形態に係る状態推定装置は、データサイズの最も小さな第１のデータ型を用いた第１演算モードによりカルマンフィルタ並列演算処理を行うため、処理負荷を大きく増加させることなく、並列処理を実行することができる。

ステップＳ１０３において、ＣＰＵ１０は、観測残差比較処理を実行する。具体的には、ＣＰＵ１０は、ステップＳ１０２のカルマンフィルタ並列演算処理において生成された観測残差ｚ_ｋ［１］〜ｚ_ｋ［１０］に基づいて、誤差評価値^ＭＡＸｚ_ｋ［１］〜^ＭＡＸｚ_ｋ［１０］を算出することにより、状態ベクトルｘ_ｋの示す値と状態ベクトルの真値との誤差が最も小さなカルマンフィルタＫＦ［Ｓｅｌ］を選択する。
すなわち、ＣＰＵ１０は、ステップＳ１０３の観測残差比較処理を実行することにより、観測残差比較部４２０として機能する。

ステップＳ１０４において、ＣＰＵ１０は、収束度判定処理を実行する。具体的には、ＣＰＵ１０は、ステップＳ１０２のカルマンフィルタ並列演算処理において生成された状態ベクトルの推定誤差の共分散Ｐ^＋ _ｋ［Ｓｅｌ］に基づいて、収束評価値Ｔｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）を算出し、収束評価値Ｔｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）が閾値Ｐ_ＴＨ以下であるか否かを判定する。
判定結果が肯定である場合（つまり、収束評価値Ｔｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）が閾値Ｐ_ＴＨ以下である場合）、ＣＰＵ１０は処理をステップＳ１０５に進める。一方、判定結果が否定である場合、ＣＰＵ１０は処理をステップＳ１０２に戻す。
すなわち、ＣＰＵ１０は、ステップＳ１０４の収束度判定処理を実行することにより、収束度判定部４４０として機能する。

ステップＳ１０５において、ＣＰＵ１０は、カルマンフィルタ停止処理を実行する。具体的には、ＣＰＵ１０は、カルマンフィルタＫＦ［Ｓｅｌ］以外の９個のカルマンフィルタの動作を停止させる。
すなわち、ＣＰＵ１０は、ステップＳ１０５のカルマンフィルタ停止処理を実行することにより、停止制御部４６０として機能する。

なお、ＣＰＵ１０は、観測残差比較処理（ステップＳ１０３）及び収束度判定処理（ステップＳ１０４）を実行する間も、カルマンフィルタ並列演算処理を実施する。つまり、ステップＳ１０５のカルマンフィルタ停止処理において、ＣＰＵ１０が９個のカルマンフィルタの動作を停止させるまでは、ＣＰＵ１０は、１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］の全てを用いた非線形カルマンフィルタの並列演算を継続する。

ステップＳ１０６において、ＣＰＵ１０は、カルマンフィルタ単独演算処理を実行する。具体的には、ＣＰＵ１０は、１個のカルマンフィルタＫＦ［Ｓｅｌ］のみを用いて非線形カルマンフィルタの演算を行う。カルマンフィルタ単独演算処理において、ＣＰＵ１０は、第２のデータ型を用いる第２演算モードにより、カルマンフィルタＫＦ［Ｓｅｌ］を動作させることで、非線形カルマンフィルタの演算を行う。
また、ＣＰＵ１０は、ステップＳ１０６のカルマンフィルタ単独演算処理において、カルマンフィルタＫＦ［Ｓｅｌ］が周期的に更新する状態ベクトルｘ_ｋを逐次出力する。
すなわち、ＣＰＵ１０は、ステップＳ１０２のカルマンフィルタ並列演算処理と、ステップＳ１０６のカルマンフィルタ単独演算処理とを実行することにより、カルマンフィルタ部３００として機能する。

ステップＳ１０７において、ＣＰＵ１０は、出力情報生成処理を実行する。具体的には、ＣＰＵ１０は、ステップＳ１０６のカルマンフィルタ単独演算処理において出力された状態ベクトルｘ_ｋに基づいて、出力情報を生成する。本実施形態に係る状態推定装置の出力情報は、携帯機器１の姿勢μである。
ＣＰＵ１０は、ステップＳ１０７の出力情報生成処理を実行することにより、出力情報生成部５００として機能する。
なお、本実施形態では、ステップＳ１０７の出力情報生成処理において、ＣＰＵ１０は、姿勢μを出力情報として生成するが、本発明はこれに限定されるものではなく、姿勢μ以外の値（例えば、３次元磁気センサ７０のオフセット推定値ｃ_Ｏ）を出力情報として生成してもよい。また、本実施形態では、ＣＰＵ１０は、状態ベクトルｘ_ｋのみに基づいて出力情報を生成するが、状態ベクトルｘ_ｋ以外の情報（例えば、観測値ベクトルｙ_ｋ）を用いて出力情報を生成しても構わない。例えば、ＣＰＵ１０は、磁気センサのオフセット推定値ｃ_Ｏ及び磁気データｑに基づいて、地上に固定された座標系における地磁気の向きを算出してもよい。

ステップＳ１０６のカルマンフィルタ単独演算処理は、１個のカルマンフィルタＫＦ［Ｓｅｌ］のみを動作させる処理であるため、処理負荷は、ステップＳ１０２のカルマンフィルタ並列演算処理に比べて小さい。一方、ステップＳ１０７の出力情報生成処理において生成される出力情報は、カルマンフィルタ単独演算処理において出力される状態ベクトルｘ_ｋに基づいて生成されるため、カルマンフィルタ単独演算処理の演算の精度は高いことが好ましい。
本実施形態では、ＣＰＵ１０は、第２のデータ型を用いる第２演算モードによりカルマンフィルタ単独演算処理を行うため、精度の高い状態ベクトルｘ_ｋを算出することができる。これにより、状態推定装置は、正確な出力情報を算出することが可能となる。

次に、ＣＰＵ１０は、状態推定プログラム１００を終了する終了条件を充足するか否かを判定する（ステップＳ１０８）。終了条件は、携帯機器１の仕様等に基づいて適宜定めればよい。例えば、携帯機器１の電源がオフ状態になることを終了条件としてもよい。終了条件を充足する場合、ＣＰＵ１０は、当該フローチャートに示す状態推定処理を終了する。一方、終了条件を充足しない場合、ＣＰＵ１０は、処理をステップＳ１０６に進める。

［３． 初期ベクトルの生成］
前述のとおり、初期値生成部２００は、初期ベクトルＩＮＩ［１］〜ＩＮＩ［１０］を生成する。この初期ベクトルＩＮＩ［１］〜ＩＮＩ［１０］は、時刻Ｔ＝０における状態ベクトルの真値に近い値を示すものが少なくとも１つ含まれるように生成すれば、どのような方法で生成しても構わない。
以下において、初期ベクトルＩＮＩ［１］〜ＩＮＩ［１０］の各要素の生成方法の一例を示す。

まず姿勢μの初期値μ_０［ｍ］は、例えば、以下の式（６）のように設定することが可能である。

ここで、式（６）に示した姿勢の初期値μ_０［ｍ］の意味を、図５乃至図７を用いて説明する。なお、図５乃至図７は、理解を容易にするために、初期姿勢を、ティーポットの図形を用いて例示する。
まず、図５に示すように、初期値μ_０［１］を定める。初期値μ_０［１］は、どのような値に設定してもよいが、本実施形態では、式（６）に示すように基本姿勢（つまり、携帯機器１に固定された座標系の各軸と、地上に固定された座標系の各軸とが一致する姿勢）を、初期値μ_０［１］とする。
次に、図５に示すように、地上に固定された座標系におけるｚ軸を回転軸として、初期値μ_０［１］をそれぞれ９０度、１８０度、２７０度回転させて３つの姿勢を生成し、これら３つの姿勢を、それぞれ、初期値μ_０［２］、μ_０［３］、μ_０［４］とする。
また、図６に示すように、地上に固定された座標系におけるｙ軸を回転軸として、初期値μ_０［１］をそれぞれ９０度、１８０度、２７０度回転させて３つの姿勢を生成し、これら３つの姿勢を、それぞれ、初期値μ_０［５］、μ_０［６］、μ_０［７］とする。
同様に、図７に示すように、地上に固定された座標系におけるｘ軸を回転軸として、初期値μ_０［１］をそれぞれ９０度、１８０度、２７０度回転させて３つの姿勢を生成し、これら３つの姿勢を、それぞれ、初期値μ_０［８］、μ_０［９］、μ_０［１０］とする。
初期値生成部２００は、以上に示した互いに異なる１０個の初期値μ_０［１］〜μ_０［１０］を生成し、それぞれ、初期ベクトルＩＮＩ［１］〜ＩＮＩ［１０］の要素とする。

仮に、姿勢μの初期値μ_０を、事前に定められた１つの固定値（例えば、μ_０＝［０，０，０，１］）に定める場合、初期値μ_０は真の姿勢（真値）から最大で１８０度ずれることになる。
これに対して、本実施形態に係る初期値生成部２００は、基本姿勢を９０度ずつ回転させることで、１０個の初期姿勢を定める。この場合、１０個の初期姿勢のうち、真の姿勢に最も近い初期姿勢と、真の姿勢とのなす角度は、９０度以下となる。従って、本実施形態に係る初期値生成部２００は、携帯機器１がどのような姿勢であっても、真値に近い値となる初期値μ_０［ｍ］を要素とする初期ベクトルＩＮＩ［ｍ］を生成することができる。

次に、３次元磁気センサ７０のオフセット推定値ｃ_Ｏの初期値ｃ_Ｏ,０について、生成方法の一例を述べる。
前述のとおり、磁気センサのオフセットは、携帯機器１に搭載された部品が発する内部磁界の方向及び大きさを、携帯機器１に固定された座標系において表現したベクトルである。内部磁界は、３次元磁気センサ７０の検出対象である地磁気に比べて大きな磁界となる場合があり、内部磁界を無視した場合には、正確な地磁気の向きを算出することはできない。従って、地磁気の向きを正確に算出するためには、オフセットを正確に把握し、３次元磁気センサ７０が出力する磁気データｑから、オフセットを減算することが必要となる。
しかし、３次元磁気センサ７０のオフセット推定値ｃ_Ｏの初期値ｃ_Ｏ,０に対して、事前に定められた固定値（例えば、原点［０，０，０］^Ｔ等）を適用する場合、磁気センサのオフセット推定値ｃ_Ｏの初期値ｃ_Ｏ,０が、磁気センサの真のオフセット（真値）から大きく乖離した値となる可能性が高い。

本実施形態に係る初期値生成部２００は、３次元磁気センサ７０のオフセット推定値ｃ_Ｏの初期値ｃ_Ｏ,０［ｍ］を、姿勢μの初期値μ_０［ｍ］に基づいて算出する。
具体的には、本実施形態に係る初期値生成部２００は、３次元磁気センサ７０のオフセット推定値ｃ_Ｏの初期値ｃ_Ｏ,０［ｍ］を、時刻Ｔ＝０に３次元磁気センサ７０が観測する磁気データｑ_０、姿勢の初期値μ_０［ｍ］、式（９）に示すベクトルＢｇ、及び、式（８）に示す行列Ｂ（μ）を用いて、以下に示す式（７）で与えられる値に設定する。ここで、行列Ｂ（μ）は、携帯機器１が姿勢μである場合に、地上に固定された座標系において表現されたベクトルを、携帯機器１に固定された座標系において表現するための座標変換を行う行列である。また、式（９）に示すベクトルＢｇは、地上に固定された座標系において携帯機器１を使用する地域の地磁気の向き及び大きさを表すベクトルである。なお、本実施形態では、地上に固定された座標系は、ｙ軸が磁極北を向き、ｚ軸が鉛直上を向くように定められることとする。
なお、ＲＯＭ３０には、位置情報と地磁気ベクトルＢｇとを対応づけて記憶したルックアップテーブルＬＵＴ２が記憶されている。初期値生成部２００は、ＧＰＳ部６０で生成される位置情報に基づいてルックアップテーブルＬＵＴ２を参照して地磁気ベクトルＢｇを取得し、式（７）を演算することによって、磁気センサのオフセット推定値ｃ_Ｏの初期値ｃ_Ｏ,０［ｍ］を得る。

前述のとおり、１０個の初期値μ_０［１］〜μ_０［１０］の中には、真の姿勢に近い値となるものが含まれるため、１０個の初期値ｃ_Ｏ,０［１］〜ｃ_Ｏ,０［１０］の中には、磁気センサの真のオフセット（真値）に近い値となるものが含まれる。従って、本実施形態に係る初期値生成部２００は、真のオフセットに近い値を有する３次元磁気センサ７０のオフセット推定値ｃ_Ｏの初期値ｃ_Ｏ,０［ｍ］を要素とする初期ベクトルＩＮＩ［ｍ］を生成することができる。

地磁気の強さｒの初期値ｒ_０［１］〜ｒ_０［１０］、及び地磁気の伏角φの初期値φ_０［１］〜φ_０［１０］は、例えば、ＧＰＳ部６０から供給される位置情報に基づいて生成することができる。これは、地球上の位置が特定できれば、その位置における地磁気の強さ及び伏角を知ることができるからである。具体的には、ＲＯＭ３０には、位置情報と地磁気の強さ及び伏角とを対応づけて記憶したルックアップテーブルＬＵＴ１が格納される。初期値生成部２００は、ルックアップテーブルＬＵＴ１を参照して、位置情報に対応する地磁気の強さ及び伏角を、地磁気の強さｒの初期値ｒ_０［１］〜ｒ_０［１０］及び地磁気の伏角φの初期値φ_０［１］〜φ_０［１０］として設定する。
なお、携帯機器１が衛星からの電波の届かない場所（例えば、地下街）にある場合には、ＧＰＳ部６０から位置情報が得られない。そのような場合には、携帯機器１が使われ得る地域の典型的な値を採用すればよい。その値もルックアップテーブルＬＵＴ１に格納されている。初期値生成部２００は、位置情報の取得が不能な場合には、典型的な値をルックアップテーブルＬＵＴ１から読み出す。例えば、日本で販売された携帯機器１については、明石市の地磁気の強さ及び伏角に基づいて、地磁気の強さｒの初期値ｒ_０［１］〜ｒ_０［１０］、及び地磁気の伏角φの初期値φ_０［１］〜φ_０［１０］を算出する。

角速度ωの初期値ω_０［１］〜ω_０［１０］は、例えば、携帯機器１が静止していることを仮定して、「０」に設定する。また、角速度センサのオフセット推定値ｂ_Ｏの初期値ｂ_Ｏ,０［１］〜ｂ_Ｏ,０［１０］は、通常「０」近辺に調整されているため、「０」に設定する。

このように、本実施形態に係る状態推定装置は、真値に近い値を示すものが少なくとも１個は含まれるように、互いに異なる１０個の初期ベクトルＩＮＩ［１］〜ＩＮＩ［１０］を生成し、１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］を並列動作させる。
従って、本実施形態に係る状態推定装置は、時刻Ｔ＝０における状態ベクトルの真値に近い値を示す初期ベクトルＩＮＩ［ｍ］が供給されて動作するカルマンフィルタＫＦ［ｍ］により、状態ベクトルｘ_ｋを正確且つ高速に収束させることが可能となる。

［４． 非線形カルマンフィルタによる演算］
次に、非線形カルマンフィルタの演算について説明する。

一般的に、カルマンフィルタは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻（時刻Ｔ＝ｋ−１）のシステムの状態を示す状態ベクトルｘ_ｋ−１から、単位時間が経過した後（時刻Ｔ＝ｋ）の状態ベクトルｘ_ｋを推定する。この推定値を、推定状態ベクトルｘ⁻ _ｋと称する。そして、カルマンフィルタは、各種センサからの出力を要素とする観測値ベクトルｙ_ｋと状態ベクトルｘ_ｋとの関係を表す観測モデルを用いて、推定状態ベクトルｘ⁻ _ｋから、観測値ベクトルｙ_ｋを推定する。この推定値を、推定観測値ベクトルｙ⁻ _ｋと称する。次に、カルマンフィルタは、推定観測値ベクトルｙ⁻ _ｋと、実際の観測値を要素とする観測値ベクトルｙ_ｋとの差分を観測残差ｚ_ｋとして算出するとともに、観測残差ｚ_ｋに基づいてカルマンゲインＫ_ｋを算出する。その後、カルマンフィルタは、観測残差ｚ_ｋ、カルマンゲインＫ_ｋ、及び、推定状態ベクトルｘ⁻ _ｋを用いて、状態ベクトルｘ_ｋを更新し、更新後の状態ベクトルｘ^＋ _ｋを算出する。
カルマンフィルタは、以上のような状態ベクトルｘ_ｋの更新を繰り返す演算を行うことにより、状態ベクトルｘ_ｋの各要素を、実際の物理量を正確に表した値（真値）に近い正確な値へと近付ける。

なお、詳細は後述するが、本実施形態ではカルマンフィルタとして、非線形カルマンフィルタであるシグマポイントカルマンフィルタを用いる。シグマポイントカルマンフィルタとは、状態ベクトルｘ_ｋ−１の周囲に複数のシグマポイントχ_ｋ−１を生成し、これらの複数のシグマポイントχ_ｋ−１を状態遷移モデルに適用することで、単位時間経過後のシグマポイントχ⁻ _ｋを推定し、推定された複数のシグマポイントχ⁻ _ｋの平均を算出することにより、推定状態ベクトルｘ⁻ _ｋを求める演算である。従って、推定観測値ベクトルｙ⁻ _ｋは、厳密には、推定状態ベクトルｘ⁻ _ｋの周辺に存在する複数のシグマポイントχ⁻ _ｋに基づいて算出される。

本実施形態における、非線形カルマンフィルタの状態遷移モデルは以下に示す式（１０）で与えられ、観測モデルは以下に示す式（１１）で与えられる。なお、本実施形態では、式（１０）に現れる関数ｆ及び式（１１）に現れる関数ｈは、非線形の関数である。

ここで、状態ベクトルｘ_ｋの次元を「ｄｉｍ（ｘ）」と表し、観測値ベクトルｙ_ｋの次元を「ｄｉｍ（ｙ）」と表す。本実施形態では、ｄｉｍ（ｘ）＝１５であり、ｄｉｍ（ｙ）＝９である。また、式（１０）に現れるプロセスノイズｗ_ｋ及び式（１１）に現れる観測ノイズｖ_ｋは、０を中心とするガウスノイズである。
式（１０）は、時刻Ｔ＝ｋにおける状態ベクトルｘ_ｋが、時刻Ｔ＝ｋ−１における状態ベクトルｘ_ｋ−１を関数ｆに代入して得られた値と、時刻Ｔ＝ｋ−１におけるプロセスノイズｗ_ｋ−１とを加算することにより推定されることを示している。
また、式（１１）は、時刻Ｔ＝ｋにおける観測値ベクトルｙ_ｋが、時刻Ｔ＝ｋにおける状態ベクトルｘ_ｋを関数ｈに代入して得られる値と、時刻Ｔ＝ｋにおける観測ノイズｖ_ｋとを加算することにより推定されることを示している。
なお、プロセスノイズｗ_ｋの共分散をＱ_ｋ、観測ノイズｖ_ｋの共分散をＲ_ｋと表す。

時刻Ｔ＝ｋにおける観測残差ｚ_ｋは、観測値ベクトルｙ_ｋと、推定観測値ベクトルｙ⁻ _ｋとに基づいて定められるベクトルであり、以下に示す式（１２）で表される。カルマンフィルタは、以下の式（１４）に示すように、観測残差ｚ_ｋ、推定状態ベクトルｘ⁻ _ｋ、及び、式（１３）に示すカルマンゲインＫ_ｋを用いて、更新後の状態ベクトルｘ^＋ _ｋを算出する。また、カルマンフィルタは、以下の式（１５）に示すように、状態ベクトルｘ_ｋの推定誤差の共分散Ｐ_ｋを更新する。
ここで、Ｐ⁻ _ｋは、推定状態ベクトルｘ⁻ _ｋの推定誤差の共分散であり、Ｐ^＋ _ｋは、更新後の状態ベクトルｘ^＋ _ｋの推定誤差の共分散である。また、Ｐ^ｙｙ _ｋは、観測残差ｚ_ｋの共分散であり、Ｐ^ｘｙ _ｋは、推定状態ベクトルｘ⁻ _ｋと、推定観測値ベクトルｙ⁻ _ｋとの相互共分散行列である。

なお、推定観測値ベクトルｙ⁻ _ｋは、式（１１）に示すように、状態遷移モデルを用いて算出された推定状態ベクトルｘ⁻ _ｋを、観測モデルに適用することで算出される値である。よって、推定観測値ベクトルｙ⁻ _ｋと実際のセンサ出力に基づく観測値ベクトルｙ_ｋとの差分である観測残差ｚ_ｋに基づいて、推定状態ベクトルｘ⁻ _ｋと実際の物理量を正確に表した値（状態ベクトルの真値）との誤差の大きさを評価することが可能である。
式（１４）に示すように、観測残差ｚ_ｋを用いて、推定状態ベクトルｘ⁻ _ｋを、更新後の状態ベクトルｘ^＋ _ｋへと更新することにより、状態ベクトルｘ_ｋの各要素を真値に近い値へと近付けることができる。

本実施形態において、１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］は、アンセンテッド変換を用いたシグマポイントカルマンフィルタの演算を実行する。以下では、シグマポイントカルマンフィルタの演算について、具体的に説明する。

図８は、状態推定装置がカルマンフィルタＫＦ［ｍ］を実行することで実現される機能を表す機能ブロック図である。以下、図８を参照しつつ、カルマンフィルタＫＦ［ｍ］の動作について説明する。
なお、１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］の構成は同一であり、初期値生成部２００からそれぞれ供給される初期ベクトルＩＮＩ［１］〜ＩＮＩ［１０］のみが相違する。

遅延部３０１は、加算器３１１から出力される更新後の状態ベクトルｘ^＋ _ｋを、単位時間（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋに相当する時間）だけ遅延させることで、状態ベクトルｘ^＋ _ｋ−１を生成し、これを、シグマポイント生成部３０２に対して出力する。なお、初回の演算（時刻Ｔ＝０）では、遅延部３０１は、初期値生成部２００が出力する初期値ＩＮＩ［ｍ］を、状態ベクトルｘ^＋ _ｋ−１として採用する。
また、遅延部３０１は、減算器３１２から出力される更新後の状態ベクトルｘ^＋ _ｋの推定誤差の共分散Ｐ^＋ _ｋを単位時間遅延させることで、状態ベクトルｘ^＋ _ｋ−１の推定誤差の共分散Ｐ^＋ _ｋ−１を生成し、これを、シグマポイント生成部３０２に対して出力する。

シグマポイント生成部３０２は、「ｄｉｍ（ｘ）」行「ｄｉｍ（ｘ）」列の共分散行列Ｐ^＋ _ｋ−１及びＱ_ｋ−１を用いて、「２ｄｉｍ（ｘ）＋１」個のシグマポイントを生成する。具体的には、まず、複数のシグマポイントの平均に対して、平均のまわりのシグマポイントの広がりを表すスケーリングパラメータλを用いて、式（１６）及び式（１７）に示すベクトルσ_ｋを定義する。

このとき、シグマポイント生成部３０２は、ベクトルσ_ｋ−１と状態ベクトルｘ^＋ _ｋ−１とに基づいて、式（１８）及び式（１９）で示される「２ｄｉｍ（ｘ）＋１」個のシグマポイントχ_ｋ−１を生成する。

状態遷移モデル部３０３は、式（２０）に示すように、時刻Ｔ＝ｋ−１における「２ｄｉｍ（ｘ）＋１」個のシグマポイントχ_ｋ−１（ｊ）の各々を、状態遷移モデルに適用することで、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個のシグマポイントの推定値χ⁻ _ｋ（ｊ）を算出する。

次に、平均算出部３０４は、式（２１）に示すように、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個のシグマポイントの推定値χ⁻ _ｋの平均値を計算することで、推定状態ベクトルｘ⁻ _ｋを算出する。

また、平均算出部３０４は、式（２２）に示す、推定状態ベクトルｘ⁻ _ｋの推定誤差の共分散Ｐ⁻ _ｋを算出する。

一方、観測モデル部３０５は、式（２３）に示すように、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個のシグマポイントχ_ｋ（ｊ）の各々を、観測モデルに適用することにより、「２ｄｉｍ（ｘ）＋１」個の推定観測値γ_ｋ（ｊ）を算出する。

そして、平均処理部３０６は、式（２４）に示すように、「２ｄｉｍ（ｘ）＋１」個の推定観測値γ_ｋ（ｊ）の平均を演算することにより、推定観測値ベクトルｙ⁻ _ｋを算出する。

次に、平均処理部３０６は、式（２５）に示す観測残差の共分散Ｐ^ｙｙ _ｋを算出する。

減算器３０７は、式（１２）に示したように、観測値ベクトルｙ_ｋと、推定観測値ベクトルｙ⁻ _ｋとの差分として、観測残差ｚ_ｋを算出する。カルマンフィルタＫＦ［ｍ］は、この観測残差ｚ_ｋを出力する。

カルマンゲイン付与部３０８は、式（２６）に示す相互共分散行列Ｐ^ｘｙ _ｋを算出する。そして、カルマンゲイン付与部３０８は、式（１３）に示したように、観測残差ｚ_ｋの共分散Ｐ^ｙｙ _ｋと、相互共分散行列Ｐ^ｘｙ _ｋとに基づいて、カルマンゲインＫ_ｋを算出し、式（１４）の右辺第２項（Ｋ_ｋｚ_ｋ）の演算を実行する。また、カルマンゲイン付与部３０８は、式（１５）の右辺第２項（Ｋ_ｋＰ^ｙｙ _ｋＫ^Ｔ _ｋ）の演算を実行する。

加算器３１１は、式（１４）に示したように、推定状態ベクトルｘ⁻ _ｋと、カルマンゲイン付与部３０８から出力される式（１４）の右辺第２項（Ｋ_ｋｚ_ｋ）とを加算することにより、更新後の状態ベクトルｘ^＋ _ｋを算出する。カルマンフィルタＫＦ［ｍ］は、更新後の状態ベクトルｘ^＋ _ｋを出力する。

減算器３１２は、式（１５）に示したように、推定状態ベクトルｘ⁻ _ｋの推定誤差の共分散Ｐ⁻ _ｋと、カルマンゲイン付与部３０８から出力される式（１５）の右辺第２項（Ｋ_ｋＰ^ｙｙ _ｋＫ^Ｔ _ｋ）との差分として、更新後の状態ベクトルｘ^＋ _ｋの推定誤差の共分散Ｐ^＋ _ｋを算出する（すなわち、状態ベクトルの推定誤差の共分散Ｐ_ｋを、Ｐ⁻ _ｋからＰ^＋ _ｋに更新する）。カルマンフィルタＫＦ［ｍ］は、更新後の状態ベクトルｘ^＋ _ｋの推定誤差の共分散Ｐ^＋ _ｋを出力する。

次に、状態遷移モデル部３０３が行う演算で用いられる状態遷移モデルについて説明する。

本実施形態に係る状態遷移モデルにおいて、状態ベクトルｘ_ｋを構成する状態変数のうち、姿勢μについての状態遷移は、式（２７）により定義される。式（２７）は、時刻Ｔ＝ｋ−１における姿勢μ_ｋ−１から、単位時間経過後の時刻Ｔ＝ｋにおける姿勢μ_ｋを推定する演算を表す。ここで、μ⁻ _ｋは、推定状態ベクトルｘ⁻ _ｋのうち、姿勢μを表す状態変数に対応する要素である。
なお、式（２７）の右辺の演算子Ωは、式（２８）により定義される。ここで、Ｉ_３×３は３行３列の単位行列を表す。３次元ベクトルｌ＝（ｌ_１，ｌ_２，ｌ_３）に対して、演算子［ｌ×］は、式（２９）で定義される。また、測定時間間隔（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋまでの間隔）をΔｔで表し、時刻Ｔ＝ｋにおける更新後の状態ベクトルｘ^＋ _ｋのうち角速度を表す状態変数に対応する要素をω^＋ _ｋで表したとき、演算子Ωを構成する成分Ψ^＋ _ｋは、式（３０）で定義される。

式（２）に示すように、姿勢μは、クォータニオンで表現され、正規化条件||μ||＝１が満たされる必要がある。しかし、シグマポイントカルマンフィルタを用いて状態ベクトルｘ_ｋを更新する場合、更新後の状態ベクトルｘ^＋ _ｋは、式（２１）に示すように、シグマポイントの推定値χ⁻ _ｋ（ｉ）の平均として算出されるため、（更新前の）状態ベクトルｘ^＋ _ｋ−１のノルムと、更新後の状態ベクトルｘ^＋ _ｋのノルムとは、異なる値となることがある。従って、シグマポイントカルマンフィルタの演算を行う場合、（更新前の）状態ベクトルｘ^＋ _ｋ−１の要素である姿勢μ^＋ _ｋ−１のノルムと、更新後の状態ベクトルｘ^＋ _ｋの要素である姿勢μ^＋ _ｋのノルムとは、異なる値となる可能性が存在する。つまり、シグマポイントカルマンフィルタの演算を行う場合、姿勢μについての正規化条件が満たされなくなる可能性が存在する。
そこで、姿勢μに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化する。なお、より厳密に正規化条件を保つためには、状態ベクトルｘ_ｋを構成する要素のうち姿勢μ_ｋについてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。

地磁気の強さｒ及び地磁気の伏角φは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける地磁気の強さｒ_ｋ及び伏角φ_ｋと、時刻Ｔ＝ｋ−１における地磁気の強さｒ_ｋ−１及び伏角φ_ｋ−１とは等しい値であると仮定する。
同様に、角速度センサのオフセット推定値ｂ_Ｏは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度センサのオフセット推定値ｂ_Ｏ,Ｋと、時刻Ｔ＝ｋ−１における角速度センサのオフセット推定値ｂ_{Ｏ,Ｋ−１}とは等しい値であると仮定する。

携帯機器１の角速度ωは、携帯機器１の利用者による携帯機器１の動かし方に依存して変化するため、時刻Ｔ＝ｋ−１の角速度ω_ｋ−１を用いて、時刻Ｔ＝ｋにおける角速度ω_ｋを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度ω_ｋと、時刻Ｔ＝ｋ−１における角速度ω_ｋ−１とは等しいと仮定する。

前述のとおり、磁気センサのオフセットは、携帯機器１の部品が発する内部磁界の方向及び大きさを携帯機器１に固定された座標系において表現したベクトルである。従って、携帯機器１の内部状態が一定である間は、磁気センサのオフセットも変化しない。一方、携帯機器１の内部状態が変化した場合、例えば、携帯機器１に搭載された部品を流れる電流の大きさが変化した場合や、携帯機器１に搭載された部品の着磁状況が変化した場合には、磁気センサのオフセットも変化する。すなわち、携帯機器１の内部状態は、携帯機器１の利用者による携帯機器１の操作や、携帯機器１の外部の環境等に依存して変化する。従って、磁気センサのオフセットが変化するタイミングや磁気センサのオフセットの変化量を予測することは困難であり、時刻Ｔ＝ｋ−１における磁気センサのオフセット推定値ｃ_{Ｏ,Ｋ−１}を用いて、時刻Ｔ＝ｋにおける磁気センサのオフセット推定値ｃ_Ｏ,Ｋを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける磁気センサのオフセット推定値ｃ_Ｏ,Ｋと、時刻Ｔ＝ｋ−１における磁気センサのオフセット推定値ｃ_{Ｏ,Ｋ−１}とは等しいと仮定する。

このように、本実施形態に係る状態遷移モデルでは、以下に示す式（３１）のように、状態ベクトルｘ_ｋを構成する複数の状態変数のうち、姿勢μを表す状態変数以外は、前の時刻から変化しないという前提を置く。

なお、カルマンフィルタの演算においては、状態ベクトルｘ_ｋの各要素は、観測残差ｚ_ｋに基づいて真値に近づくように更新される。従って、カルマンフィルタの演算において、式（３１）に示された姿勢μ以外の値が全く変化しないわけではない。

次に、観測モデル部３０５が行う演算で用いられる観測モデルについて説明する。

３次元角速度センサ９０から出力される角速度データｇの推定値γ_ｇｙｒｏは、角速度ωと、角速度センサのオフセット推定値ｂ_Ｏとを用いて、式（３２）で与えられる。

また、地上に固定された座標系において地磁気を表すベクトルＢｇは式（３３）で与えられる。従って、３次元磁気センサ７０から出力される磁気データｑの推定値γ_ｍａｇは、磁気センサのオフセット推定値ｃ_Ｏと、式（８）で示した行列Ｂ（μ）とを用いて、式（３４）で与えられる。

また、地上に固定された座標系において、重力加速度を表したベクトルを、重力加速度の大きさで正規化した３次元のベクトルＧ_ＲＶは、以下の式（３５）で表される。従って、３次元加速度センサ８０から出力される加速度データａの推定値γ_ａｃｃは、式（８）で示した行列Ｂ（μ）とベクトルＧ_ＲＶとを用いて、式（３６）で与えられる。

式（３）を式（１２）の右辺第１項に代入し、式（３２）、式（３４）、及び式（３６）を用いて式（１２）の右辺第２項を表すことにより、式（１２）を、以下の式（３７）に変形することができる。このとき、観測残差ｚ_ｋは、式（３７）により算出される。

［５． 結論］
以上に示したように、本実施形態に係る状態推定装置は、１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］に、互いに異なる１０個の初期ベクトルＩＮＩ［１］〜ＩＮＩ［１０］を供給し、これら１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］を並列で動作させる。そして、状態推定装置は、推定精度が最も高いカルマンフィルタＫＦ［Ｓｅｌ］を特定し、特定されたカルマンフィルタＫＦ［Ｓｅｌ］により状態推定を行う。従って、本実施形態に係る状態推定装置は、状態ベクトルｘ_ｋを正確且つ高速に収束させることが可能であり、正確且つ高速な状態推定が可能となった。

また、本実施形態に係る状態推定装置は、カルマンフィルタＫＦの推定精度を、誤差評価値^ＭＡＸｚ_ｋ［ｍ］及び収束評価値Ｔｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）を用いて表現した。
誤差評価値^ＭＡＸｚ_ｋ［ｍ］は、状態ベクトルｘ_ｋの示す値と状態ベクトルの真値との誤差の大きさを評価する。従って、本実施形態に係る状態推定装置は、真値に近い正確な値を要素とする状態ベクトルｘ_ｋを有し、真値に短時間で状態ベクトルｘ_ｋを収束させることができるカルマンフィルタＫＦ［Ｓｅｌ］を選択することが可能となった。
また、収束評価値Ｔｒ（Ｐ^＋ _ｋ［Ｓｅｌ］）は、状態ベクトルｘ_ｋの収束の程度を表す。従って、本実施形態に係る状態推定装置は、システムの状態の正確な推定を安定的に行うことのできるカルマンフィルタＫＦ［Ｓｅｌ］を特定することが可能となった。
このように、本実施形態に係る状態推定装置は、カルマンフィルタＫＦの推定精度に基づいて特定された１個のカルマンフィルタＫＦ［Ｓｅｌ］を用いて、システムの状態を推定するため、正確且つ高速な状態推定を行うことが可能となった。

また、本実施形態に係る状態推定装置は、第１演算モードによりカルマンフィルタ並列演算処理を行うとともに、第２演算モードによりカルマンフィルタ単独演算処理を行う。
これにより、本実施形態に係る状態推定装置は、処理負荷を低減しつつ、正確且つ精度の高い状態推定を行うことが可能となった。

＜Ｂ．変形例＞
本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの２以上の変形例を組み合わせることもできる。

（１）変形例１
上述した実施形態では、カルマンフィルタＫＦは、シグマポイントカルマンフィルタの演算を実行したが、本発明はこれに限定するものではなく、カルマンフィルタＫＦは、拡張カルマンフィルタ等、シグマポイントカルマンフィルタ以外の非線形カルマンフィルタの演算を実行するものであってもよい。
本発明に係る状態推定装置は、複数のカルマンフィルタＫＦの中で、推定精度が最も高いカルマンフィルタＫＦを特定し、当該特定されたカルマンフィルタＫＦを用いてシステムの状態を推定する。そして、カルマンフィルタＫＦが、どのような非線形カルマンフィルタの演算を実行する場合であっても、最も推定精度の高いカルマンフィルタＫＦの状態ベクトルは、正確な値に高速に収束する。すなわち、本発明に係る状態推定装置は、カルマンフィルタＫＦが、どのような非線形カルマンフィルタの演算を実行するものであっても、正確且つ高速な状態推定を行うことができる。

（２）変形例２
上述した実施形態及び変形例では、カルマンフィルタ部３００は、１０個のカルマンフィルタＫＦ［１］〜ＫＦ［１０］を備えるが、本発明はこのような形態に限定されるものではなく、カルマンフィルタ部３００は、２個以上のカルマンフィルタＫＦを備えるものであればよい。この場合、初期値生成部２００は、カルマンフィルタＫＦの個数に対応して、互いに異なる複数の初期ベクトルＩＮＩを生成するものであればよい。
なお、カルマンフィルタ部３００が備えるカルマンフィルタＫＦの個数は、携帯機器１の処理性能を考慮して、適宜定めればよい。例えば、携帯機器１の処理性能が高い場合には、カルマンフィルタ部３００は、１０個よりも多くのカルマンフィルタＫＦを備えてもよい。この場合、状態ベクトルｘを高速且つ正確に収束させることが可能となる。また、携帯機器１の処理性能が低い場合には、カルマンフィルタ部３００は、１０個よりも少ないカルマンフィルタを備えてもよい。この場合、カルマンフィルタ並列演算処理に係る処理負荷を軽減することができる。

（３）変形例３
上述した実施形態及び変形例では、状態推定装置は、第１演算モードによりカルマンフィルタ並列演算処理を行うとともに、第２演算モードによりカルマンフィルタ単独演算処理を行うが、本発明は、状態推定装置の動作をこのような態様に限定するものではない。
例えば、状態推定装置は、カルマンフィルタ並列演算処理及びカルマンフィルタ単独演算処理の双方を、第１演算モードにより実行するものであってもよい。この場合、カルマンフィルタ単独演算処理に係る処理負荷を軽減することができる。
また、状態推定装置は、カルマンフィルタ並列演算処理及びカルマンフィルタ単独演算処理の双方を、第２演算モードにより実行するものであってもよい。この場合、状態推定装置は、精度の高い推定を行うことが可能となる。

（４）変形例４
上述した実施形態及び変形例では、初期値生成部２００は、初期ベクトルＩＮＩを構成する複数の状態変数のうち、姿勢μの初期値μ_０及び３次元磁気センサ７０のオフセット推定値ｃ_Ｏの初期値ｃ_Ｏ,０を互いに異なる値に設定することで、互いに異なる複数の初期ベクトルＩＮＩを生成したが、本発明はこのような初期ベクトルＩＮＩの生成方法に限定するものではない。例えば、初期値生成部２００は、初期ベクトルＩＮＩを構成する状態変数の全てについて、互いに異なる値に設定するものであってもよい。

（５）変形例５
上述した実施形態及び変形例では、状態推定装置は出力情報生成部５００を備えるが、本発明はこのような構成に限定されるものではなく、状態推定装置は、出力情報生成部５００を備えずに構成されてもよい。この場合、状態推定装置は、状態ベクトルｘ_ｋを出力するものであってもよい。また、この場合、状態推定プログラム１００と連携する各種アプリケーションが、姿勢や地磁気の向き等の出力情報を生成してもよい。

（６）変形例６
上述した実施形態及び変形例では、携帯機器１は、３次元磁気センサ７０、３次元加速度センサ８０、及び、３次元角速度センサ９０を備えたが、本発明はこのような形態に限定するものではなく、携帯機器１は、どのようなセンサを備えるものであってもよい。なお、観測値ベクトルｙは、携帯機器１が備える複数のセンサからの出力値の全部または一部を要素とするように定めればよい。
本発明は、異種の物理量を測定する複数のセンサの出力を統合し、動的システムの状態を推定する非線形カルマンフィルタの演算において、正確且つ高速な状態推定を行うものである。従って、この発明によれば、状態推定装置がどのようなセンサを備える場合であっても、正確且つ高速な状態推定を行うことができる。

（７）変形例７
上述した実施形態及び変形例では、状態ベクトルｘを構成する状態変数として、式（１）に示した変数、すなわち、携帯機器１の姿勢μ、地磁気の強さｒ、地磁気の伏角φ、携帯機器１の角速度ω、角速度センサ９１〜９３のオフセット推定値ｂ_Ｏ、及び磁気センサ７１〜７３のオフセット推定値ｃ_Ｏを採用し、これらを推定の対象としたが、本発明は、状態ベクトルｘを式（１）に限定するものではない。
例えば、状態変数として、式（１）に示した変数のうちの一部を採用するものであってもよい。また、状態変数として、式（１）に示した変数以外の変数を採用するものであってもよい。

１…携帯機器、１０…ＣＰＵ、７０…３次元磁気センサ、８０…３次元加速度センサ、９０…３次元角速度センサ、１００…状態推定プログラム、２００…初期値生成部、３００…カルマンフィルタ部、ＫＦ…カルマンフィルタ、４００…カルマンフィルタ制御部、４２０…観測残差比較部、４４０…収束度判定部、４６０…停止制御部、５００…出力情報生成部。

Claims (5)

1. システムを観測して観測値を各々出力する複数のセンサと、
   前記システムの状態を表すための複数の状態変数を要素とする状態ベクトルと、前記複数のセンサから各々出力される観測値を要素とする観測値ベクトルとを用いて、前記状態ベクトルを更新することで、前記システムの状態を推定する複数のカルマンフィルタと、
   互いに異なる複数の初期ベクトルを生成し、複数の前記初期ベクトルのそれぞれを、前記状態ベクトルの初期値として前記複数のカルマンフィルタの各々に供給する初期値生成部と、
   前記複数のカルマンフィルタの推定精度を各々評価し、前記複数のカルマンフィルタのうち、推定精度が最も高いカルマンフィルタを特定し、特定したカルマンフィルタを除く他のカルマンフィルタの動作を停止させるカルマンフィルタ制御部と、
   を備えることを特徴とする状態推定装置。
2. 前記カルマンフィルタは、
   前記状態ベクトルと、前記観測値ベクトルとに基づいて観測残差を算出するとともに、前記状態ベクトルの推定誤差の共分散を算出し、
   前記カルマンフィルタ制御部は、
   前記複数のカルマンフィルタの各々が出力する前記観測残差に基づいて、前記状態ベクトルと前記システムの状態との誤差を評価する誤差評価値が最小となるカルマンフィルタを選択する観測残差比較部と、
   前記観測残差比較部によって選択されたカルマンフィルタが出力する前記状態ベクトルの推定誤差の共分散に基づいて、前記状態ベクトルの収束の程度を表す収束評価値を算出し、前記収束評価値が閾値以下であるか否かを判定する収束度判定部と、
   前記収束度判定部の判定結果が肯定である場合、前記観測残差比較部によって選択されたカルマンフィルタを、推定精度が最も高いカルマンフィルタであると特定し、特定したカルマンフィルタを除く他のカルマンフィルタの動作を停止させる、停止制御部と、
   を備えることを特徴とする、請求項１に記載の状態推定装置。
3. 前記観測残差比較部は、
   前記観測残差のうち少なくとも一部の要素のノルムの、現在時刻から所定時間だけ過去の時刻までの所定期間における最大値を、前記複数のカルマンフィルタごとに算出し、
   算出した複数の前記最大値が最小となるカルマンフィルタを、前記誤差評価値が最小となるカルマンフィルタとして選択する、
   ことを特徴とする、請求項２に記載の状態推定装置。
4. 前記収束評価値は、前記状態ベクトルの推定誤差の共分散を表す行列のトレースである、
   ことを特徴とする、請求項２または３に記載の状態推定装置。
5. 前記カルマンフィルタ制御部が、前記他のカルマンフィルタを停止させる前は、
   前記複数のカルマンフィルタは、第１のデータ型を用いて演算を行い、
   前記カルマンフィルタ制御部が、前記他のカルマンフィルタを停止させた後は、
   前記カルマンフィルタ制御部によって特定されたカルマンフィルタは、前記第１のデータ型よりもデータサイズの大きな第２のデータ型を用いて演算を行う
   ことを特徴とする、請求項１乃至４のうちいずれか１項に記載の状態推定装置。
   

Images