(57)【要約】【課題】慣性センサーの出力信号を用いて十分な精度で物体の姿勢を推定することが可能な姿勢推定方法、姿勢推定装置及びプログラムを提供すること。【解決手段】姿勢推定方法は、角速度センサー１２の出力を用いて、サンプリング周期Δｔを変数とする多項式で近似して対象物の姿勢変化量を計算する工程（Ｓ８）と、加速度センサー１４の出力と角速度センサー１２の出力とを用いて、対象物の速度変化量を計算する工程（Ｓ９）と、姿勢変化量と速度変化量とを用いて、対象物の姿勢を推定する工程（Ｓ１０）と、を含む。【選択図】図４
2015179002.tif 000002
【特許請求の範囲】
【請求項１】
角速度センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して対象物の姿勢変化量を計算する工程と、
加速度センサーの出力と前記角速度センサーの出力とを用いて、前記対象物の速度変化量を計算する工程と、
前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢を推定する工程と、を含む、姿勢推定方法。
【請求項２】
前記対象物の速度変化量を計算する工程において、
前記角速度センサーの出力と前記加速度センサーの出力とを用いて前記サンプリング周期を変数とする多項式で近似して前記対象物の加速度を計算し、当該加速度を用いて前記速度変化量を計算する、請求項１に記載の姿勢推定方法。
【請求項３】
前記対象物の姿勢を推定する工程は、
前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢と速度とを予測する工程と、
前記予測した前記対象物の速度とゼロとの差を用いて、前記予測した前記対象物の姿勢を補正する工程と、を含む、請求項１又は２に記載の姿勢推定方法。
【請求項４】
前記対象物の姿勢を推定する工程は、
前記角速度センサーの出力を用いて、前記対象物の姿勢誤差を更新する工程と、
前記更新された前記姿勢誤差に含まれる方位角誤差を除去する工程と、を含み、
前記対象物の姿勢を補正する工程において、
前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する、請求項３に記載の姿勢推定方法。
【請求項５】
前記対象物の姿勢を推定する工程は、
前記角速度センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する工程を含み、
前記対象物の姿勢を補正する工程において、
前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する、請求項３に記載の姿勢推定方法。
【請求項６】
慣性センサーの出力を用いて、対象物の姿勢を予測する工程と、
前記慣性センサーの出力を用いて、前記対象物の姿勢誤差を更新する工程と、
前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、
前記更新された前記姿勢誤差に含まれる方位角誤差を除去する工程と、を含む、姿勢推定方法。
【請求項７】
慣性センサーの出力を用いて、対象物の姿勢を予測する工程と、
前記慣性センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する工程と、
前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、を含む、姿勢推定方法。
【請求項８】
前記慣性センサーの出力を用いて、前記対象物の速度変化量を計算する工程を含み、
前記対象物の姿勢を予測する工程において、
前記速度変化量を用いて、前記対象物の速度を予測し、
前記対象物の姿勢を補正する工程において、
前記予測した前記対象物の速度とゼロとの差を用いて、前記予測した前記対象物の姿勢を補正する、請求項６又は７に記載の姿勢推定方法。
【請求項９】
前記慣性センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して前記対象物の姿勢変化量を計算する工程を含み、
前記対象物の姿勢を予測する工程において、
前記姿勢変化量を用いて、前記対象物の姿勢を予測する、請求項６乃至８のいずれか一項に記載の姿勢推定方法。
【請求項１０】
角速度センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して対象物の姿勢変化量を計算する姿勢変化量計算部と、
加速度センサーの出力と前記角速度センサーの出力とを用いて、前記対象物の速度変化量を計算する速度変化量計算部と、
前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢を推定する姿勢推定部と、を含む、姿勢推定装置。
【請求項１１】
前記角速度センサー及び前記加速度センサーを含む、請求項１０に記載の姿勢推定装置。
【請求項１２】
慣性センサーの出力を用いて、対象物の姿勢を予測する姿勢予測部と、
前記慣性センサーの出力を用いて、前記対象物の姿勢誤差を更新する姿勢誤差更新部と、
前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する姿勢補正部と、
前記更新された前記姿勢誤差に含まれる方位角誤差を除去する方位角誤差除去部と、を含む、姿勢推定装置。
【請求項１３】
慣性センサーの出力を用いて、対象物の姿勢を予測する姿勢予測部と、
前記慣性センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する姿勢誤差更新部と、
前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する姿勢補正部と、を含む、姿勢推定装置。
【請求項１４】
前記慣性センサーを含む、請求項１２又は１３に記載の姿勢推定装置。
【請求項１５】
角速度センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して対象物の姿勢変化量を計算する工程と、
加速度センサーの出力と前記角速度センサーの出力とを用いて、前記対象物の速度変化量を計算する工程と、
前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢を推定する工程と、をコンピューターに実行させる、プログラム。
【請求項１６】
慣性センサーの出力を用いて、対象物の姿勢を予測する工程と、
前記慣性センサーの出力を用いて、前記対象物の姿勢誤差を更新する工程と、
前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、
前記更新された前記姿勢誤差に含まれる方位角誤差を除去する工程と、をコンピューターに実行させる、プログラム。
【請求項１７】
前記慣性センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まない
ものとして前記対象物の姿勢誤差を更新する工程と、
前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、をコンピューターに実行させる、プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、対象物の姿勢を推定する、姿勢推定方法、姿勢推定装置及びプログラムに関する。
【背景技術】
【０００２】
物体に慣性計測装置（ＩＭＵ：Inertial Measurement Unit）を装着し、慣性計測装置（ＩＭＵ）の出力信号を用いて物体の位置や姿勢を算出する装置やシステムが知られている。慣性計測装置（ＩＭＵ）の出力信号にはバイアス誤差があり、姿勢計算にも誤差が生じるため、カルマンフィルターを用いてこれらの誤差を補正し、物体の正確な姿勢を推定する手法が提案されている。例えば、特許文献１では、ジャイロ、加速度センサー、磁気センサー、速度センサーの観測値をカルマンフィルターに入力し、移動体の姿勢を推定する姿勢推定装置が提案されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１１−２２７０１７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、特許文献１に記載の姿勢推定装置では、高精度な姿勢推定を実現するために、磁気センサーの観測値を用いることが必須であるため、センサーの部品点数が多くなり、低コスト化や小型化が難しいという問題がある。
【０００５】
本発明は、以上のような問題点に鑑みてなされたものであり、本発明のいくつかの態様によれば、慣性センサーの出力信号を用いて十分な精度で物体の姿勢を推定することが可能な姿勢推定方法、姿勢推定装置及びプログラムを提供することができる。
【課題を解決するための手段】
【０００６】
本発明は前述の課題の少なくとも一部を解決するためになされたものであり、以下の態様または適用例として実現することが可能である。
【０００７】
［適用例１］
本適用例に係る姿勢推定方法は、角速度センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して対象物の姿勢変化量を計算する工程と、加速度センサーの出力と前記角速度センサーの出力とを用いて、前記対象物の速度変化量を計算する工程と、前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢を推定する工程と、を含む。
【０００８】
対象物は、例えば、人、動物、移動体、運動器具であってもよい。
【０００９】
慣性センサーは、加速度や角速度等の慣性量を計測可能なセンサーであればよく、例えば、加速度や角速度を計測可能な慣性計測ユニット（ＩＭＵ：Inertial Measurement Unit）でもよい。また、慣性センサーは、対象物に対して脱着可能であってもよいし、例えば、対象物に内蔵されるなど、対象物に固定されていて取り外すことができないものでもよい。
【００１０】
本適用例に係る姿勢推定方法によれば、サンプリング周期の多項式で近似して姿勢変化
量を計算することで、従来よりも姿勢変化量の計算誤差を小さくすることができる。また、本適用例に係る姿勢推定方法によれば、加速度センサーの出力だけでなく角速度センサーの出力も用いて速度変化量を計算することで、対象物の回転がすぐに速度変化量に反映され、対象物が急激に回転した場合でも、速度変化量の計算精度の劣化を低減させることができる。従って、本適用例に係る姿勢推定方法によれば、角速度センサーの出力と加速度センサーの出力とを用いて、十分な精度で物体の姿勢を推定することができる。
【００１１】
［適用例２］
上記適用例に係る姿勢推定方法は、前記対象物の速度変化量を計算する工程において、前記角速度センサーの出力と前記加速度センサーの出力とを用いて前記サンプリング周期を変数とする多項式で近似して前記対象物の加速度を計算し、当該加速度を用いて前記速度変化量を計算してもよい。
【００１２】
本適用例に係る姿勢推定方法によれば、サンプリング周期の多項式で近似して速度変化量を計算することで、従来よりも速度変化量の計算誤差を小さくすることができる。従って、本適用例に係る姿勢推定方法によれば、角速度センサーの出力と加速度センサーの出力とを用いて、十分な精度で物体の姿勢を推定することができる。
【００１３】
［適用例３］
上記適用例に係る姿勢推定方法において、前記対象物の姿勢を推定する工程は、前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢と速度とを予測する工程と、前記予測した前記対象物の速度とゼロとの差を用いて、前記予測した前記対象物の姿勢を補正する工程と、を含んでもよい。
【００１４】
本適用例に係る姿勢推定方法によれば、対象物の運動速度は長期的にはゼロであるとして対象物の速度の観測残差を用いて、対象物の姿勢を補正するので、方位角の観測情報が無くても、対象物の姿勢を精度よく推定することができる。
【００１５】
［適用例４］
上記適用例に係る姿勢推定方法において、前記対象物の姿勢を推定する工程は、前記角速度センサーの出力を用いて、前記対象物の姿勢誤差を更新する工程と、前記更新された前記姿勢誤差に含まれる方位角誤差を除去する工程と、を含み、前記対象物の姿勢を補正する工程において、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正してもよい。
【００１６】
角速度センサーの出力と加速度センサーの出力とを用いて対象物の姿勢を補正する場合、これらの出力には対象物の方位角についての情報が含まれないので、対象物の方位角誤差については補正されない。従って、更新された姿勢誤差に含まれる方位角誤差を残しておくと、方位角誤差の確からしさが単調に減少し、姿勢誤差が単調に増加するため、姿勢の推定精度が劣化するおそれがある。本適用例に係る姿勢推定方法によれば、角速度センサーの出力を用いて更新した対象物の姿勢誤差に含まれる方位角誤差を除去するので、姿勢誤差が単調に増加することを防止し、姿勢の推定精度を劣化させないようにすることができる。
【００１７】
［適用例５］
上記適用例に係る姿勢推定方法において、前記対象物の姿勢を推定する工程は、前記角速度センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する工程を含み、前記対象物の姿勢を補正する工程において、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正してもよい。
【００１８】
角速度センサーの出力と加速度センサーの出力とを用いて対象物の姿勢を補正する場合、これらの出力には対象物の水平方向の回転誤差についての情報が含まれないので、対象物の水平方向の回転誤差については補正されない。従って、対象物の回転誤差が方位角誤差成分を含むものとして対象物の姿勢誤差を更新すると、回転誤差の方位角誤差成分の確からしさが単調に減少し、姿勢誤差が単調に増加するため、姿勢の推定精度が劣化するおそれがある。本適用例に係る姿勢推定方法によれば、角速度センサーの出力を用いて、対象物の回転誤差が方位角誤差成分を含まないものとして対象物の姿勢誤差を更新するので、姿勢誤差が単調に増加することを防止し、姿勢の推定精度を劣化させないようにすることができる。
【００１９】
［適用例６］
本適用例に係る姿勢推定方法は、慣性センサーの出力を用いて、対象物の姿勢を予測する工程と、前記慣性センサーの出力を用いて、前記対象物の姿勢誤差を更新する工程と、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、前記更新された前記姿勢誤差に含まれる方位角誤差を除去する工程と、を含む。
【００２０】
慣性センサーの出力を用いて対象物の姿勢を補正する場合、慣性センサーの出力には対象物の方位角についての情報が含まれないので、対象物の方位角誤差については補正されない。従って、更新された姿勢誤差に含まれる方位角誤差を残しておくと、方位角誤差の確からしさが単調に減少し、姿勢誤差が単調に増加するため、姿勢の推定精度が劣化するおそれがある。本適用例に係る姿勢推定方法によれば、慣性センサーの出力を用いて更新した対象物の姿勢誤差に含まれる方位角誤差を除去するので、姿勢誤差が単調に増加することを防止し、姿勢の推定精度を劣化させないようにすることができる。
【００２１】
［適用例７］
本適用例に係る姿勢推定方法は、慣性センサーの出力を用いて、対象物の姿勢を予測する工程と、前記慣性センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する工程と、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、を含む。
【００２２】
慣性センサーの出力を用いて対象物の姿勢を補正する場合、慣性センサーの出力には対象物の水平方向の回転誤差についての情報が含まれないので、対象物の水平方向の回転誤差については補正されない。従って、対象物の回転誤差が方位角誤差成分を含むものとして対象物の姿勢誤差を更新すると、回転誤差の方位角誤差成分の確からしさが単調に減少し、姿勢誤差が単調に増加するため、姿勢の推定精度が劣化するおそれがある。本適用例に係る姿勢推定方法によれば、慣性センサーの出力を用いて、対象物の回転誤差が方位角誤差成分を含まないものとして対象物の姿勢誤差を更新するので、姿勢誤差が単調に増加することを防止し、姿勢の推定精度を劣化させないようにすることができる。
【００２３】
［適用例８］
上記適用例に係る姿勢推定方法は、前記慣性センサーの出力を用いて、前記対象物の速度変化量を計算する工程を含み、前記対象物の姿勢を予測する工程において、前記速度変化量を用いて、前記対象物の速度を予測し、前記対象物の姿勢を補正する工程において、前記予測した前記対象物の速度とゼロとの差を用いて、前記予測した前記対象物の姿勢を補正してもよい。
【００２４】
本適用例に係る姿勢推定方法によれば、対象物の運動速度は長期的にはゼロであるとして対象物の速度の観測残差を用いて、対象物の姿勢を補正するので、方位角の観測情報が無くても、対象物の姿勢を精度よく推定することができる。
【００２５】
［適用例９］
上記適用例に係る姿勢推定方法は、前記慣性センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して前記対象物の姿勢変化量を計算する工程を含み、前記対象物の姿勢を予測する工程において、前記姿勢変化量を用いて、前記対象物の姿勢を予測してもよい。
【００２６】
本適用例に係る姿勢推定方法によれば、サンプリング周期の多項式で近似して姿勢変化量を計算することで、従来よりも姿勢変化量の計算誤差を小さくすることができる。従って、本適用例に係る姿勢推定方法によれば、慣性センサーの出力を用いて、十分な精度で物体の姿勢を推定することができる。
【００２７】
［適用例１０］
本適用例に係る姿勢推定装置は、角速度センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して対象物の姿勢変化量を計算する姿勢変化量計算部と、加速度センサーの出力と前記角速度センサーの出力とを用いて、前記対象物の速度変化量を計算する速度変化量計算部と、前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢を推定する姿勢推定部と、を含む。
【００２８】
［適用例１１］
上記適用例に係る姿勢推定装置は、前記角速度センサー及び前記加速度センサーを含んでもよい。
【００２９】
［適用例１２］
本適用例に係る姿勢推定装置は、慣性センサーの出力を用いて、対象物の姿勢を予測する姿勢予測部と、前記慣性センサーの出力を用いて、前記対象物の姿勢誤差を更新する姿勢誤差更新部と、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する姿勢補正部と、前記更新された前記姿勢誤差に含まれる方位角誤差を除去する方位角誤差除去部と、を含む。
【００３０】
［適用例１３］
本適用例に係る姿勢推定装置は、慣性センサーの出力を用いて、対象物の姿勢を予測する姿勢予測部と、前記慣性センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する姿勢誤差更新部と、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する姿勢補正部と、を含む。
【００３１】
［適用例１４］
上記適用例に係る姿勢推定装置は、前記慣性センサーを含んでもよい。
【００３２】
［適用例１５］
本適用例に係るプログラムは、角速度センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して対象物の姿勢変化量を計算する工程と、加速度センサーの出力と前記角速度センサーの出力とを用いて、前記対象物の速度変化量を計算する工程と、前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢を推定する工程と、をコンピューターに実行させる。
【００３３】
［適用例１６］
本適用例に係るプログラムは、慣性センサーの出力を用いて、対象物の姿勢を予測する工程と、前記慣性センサーの出力を用いて、前記対象物の姿勢誤差を更新する工程と、前
記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、前記更新された前記姿勢誤差に含まれる方位角誤差を除去する工程と、をコンピューターに実行させる。
【００３４】
［適用例１７］
本適用例に係るプログラムは、前記慣性センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する工程と、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、をコンピューターに実行させる。
【図面の簡単な説明】
【００３５】
【図１】本実施形態の姿勢推定装置の構成例を示す図。
【図２】センサー座標系とローカル座標系について説明するための図。
【図３】本実施形態における処理部の構成例を示す図。
【図４】姿勢推定処理の手順の一例を示すフローチャート図。
【図５】状態ベクトルｘｋ及び誤差共分散行列Σｘ，ｋ２を推定する処理の手順の一例を示すフローチャート図。
【図６】本実施形態の姿勢推定システムの構成例を示す図。
【発明を実施するための形態】
【００３６】
以下、本発明の好適な実施形態について図面を用いて詳細に説明する。なお、以下に説明する実施の形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また以下で説明される構成の全てが本発明の必須構成要件であるとは限らない。
【００３７】
１．姿勢推定理論
１−１．ＩＭＵ出力モデル
慣性計測装置（ＩＭＵ）の（補正済み）出力は、各サンプリング時刻（ｔｋ）における３軸ジャイロセンサー出力ｄω，ｋと３軸加速度センサー出力ｄａ，ｋで構成される。ここで、３軸ジャイロセンサー出力ｄω，ｋは、式（１）のように、サンプリング期間（Δｔ＝ｔｋ−ｔｋ−１）中の角速度ωの平均値と残存バイアス誤差ｂωとの和で表される。
【００３８】
【数１】
2015179002.tif 000003
【００３９】
３軸加速度センサー出力ｄａ，ｋも同様に、式（２）のように、加速度αの平均値と残存バイアス誤差ｂαとの和で表される。
【００４０】
【数２】
2015179002.tif 000004
【００４１】
１−２．角速度積分による３次元姿勢算出
３次元姿勢をクォータニオン（四元数）で表す場合、その姿勢ｑと角速度ω［ｒａｄ／ｓ］との関係は式（３）の微分方程式で表される。
【００４２】
【数３】
2015179002.tif 000005
【００４３】
ここで、○と■を重ねた記号はクォータニオン乗算を表し、例えばｑとｐのクォータニオン乗算の各要素は、式（４）のように計算される。
【００４４】
【数４】
2015179002.tif 000006
【００４５】
また、３次元ベクトルωは、式（５）のように、実（スカラー）成分がゼロで純虚（ベクトル）成分がωの各成分と一致するクォータニオンと等価とみなす。
【００４６】
【数５】
2015179002.tif 000007
【００４７】
微分方程式（３）を解くことで３次元姿勢を算出することができるが、残念ながらその一般解は見つかっていない。さらに角速度ωの値も離散的な平均値としてしか得られないため、式（６）を使って微小（サンプリング）時間ごとに逐一近似計算を行わなければならない。
【００４８】
【数６】
2015179002.tif 000008
【００４９】
式（６）は、姿勢クォータニオンｑと、角速度ωの軸ごとの積分関数を考え、それぞれのｔ＝ｔｋ−１におけるテイラー展開からΔｔの３次項までに基づいて算出した式であり、式中のωｋ−１を含む項がコーニング補正項に相当する。記号■は３次元ベクトルのクロス積（ベクトル積）を表し、例えばｖ■ｗの各要素は式（７）のように計算される。
【００５０】
【数７】
2015179002.tif 000009
【００５１】
１−３．重力加速度による傾き誤差観測
加速度センサーはその運動によって生じる加速度を検出するが、地球上では常に約１［Ｇ］（＝９．８０６６５［ｍ／ｓ２］）の重力加速度が加算されて検出される。この重力加速度は必ず鉛直（上）方向のベクトルであるため、３軸加速度センサー出力との比較によって姿勢の傾き（ロールとピッチ）成分の誤差を知ることができる。そのために、まず３軸加速度センサーによって観測されたセンサー座標系（ｘｙｚ座標教）の加速度ベクトルαを水平な直交軸と鉛直下向き軸によるローカル空間の座標系（ＸＹＺ座標系）でのベクトルα'に変換しなければならない。この座標（回転）変換は姿勢クォータニオンｑとその共役クォータニオンｑ＊を使って式（８）のように計算することができる。
【００５２】
【数８】
2015179002.tif 000010
【００５３】
この式（８）は３次元座標変換行列Ｃを使って式（９）のように表すこともできる。
【００５４】
【数９】
2015179002.tif 000011
【００５５】
この加速度α’を同じローカル空間座標系（ＸＹＺ座標系）における重力加速度ベクトルｇと比較することで傾き誤差を得られる。重力加速度ベクトルｇは、式（１０）で表される。式（１０）において、Δｇは重力加速度ｇの標準値との差［Ｇ］を表す。
【００５６】
【数１０】
2015179002.tif 000012
【００５７】
１−４．ゼロ運動速度観測
特に、ユーザインターフェース用途においてＩＭＵの運動速度は長期的にほぼゼロに等しいと考えられる。ローカル空間座標系における運動速度ｖとセンサー座標系における加速度αおよび角速度ωとの関係は座標変換行列Ｃを使って式（１１）の微分方程式で表される。
【００５８】
【数１１】
2015179002.tif 000013
【００５９】
ここでも加速度αおよび角速度ωの値は離散的な平均値としてしか得られないため、式（１２）を使って微小（サンプリング）時間ごとに逐一近似計算することで運動速度ベクトルを算出する。
【００６０】
【数１２】
2015179002.tif 000014
【００６１】
式（１２）は、速度ベクトルｖと、加速度αおよび角速度ωの軸ごとの積分関数を考え、それぞれのｔ＝ｔｋ−１におけるテイラー展開からΔＴの３次項までに基づいて算出した式である。なお、３次項には残存誤差ελが存在するが十分小さいため式（１２）では無視する。記号・は３次元ベクトルのドット積（スカラー積）を表し、例えばｖ・ｗは式（１３）のように計算される。
【００６２】
【数１３】
2015179002.tif 000015
【００６３】
１−５．姿勢クォータニオンとその誤差
算出される姿勢クォータニオンｑは、式（１４）のようにその真値（＾）に対して誤差εｑを持つと考えられる。
【００６４】
【数１４】
2015179002.tif 000016
【００６５】
ここでΣｑ２は誤差εｑの大きさを表す誤差共分散行列である。またＥ［・］は期待値を、右肩のＴはベクトル・行列の転置をそれぞれ表す。
【００６６】
クォータニオンおよびその誤差はその要素として四つの値を持つが、三次元姿勢（回転変換）の自由度は３つしかない。姿勢クォータニオンが持つ第４の自由度は拡大／縮小変換に相当するが、姿勢検出処理においてはこの拡大／縮小率は常に１に固定されなければならない。実際には各種の演算誤差によってこの拡大／縮小率成分が変化してしまうので、これを抑制するための処理が必要になる。
【００６７】
姿勢クォータニオンｑの場合はその絶対値の二乗が拡大／縮小率に相当するので、式（１５）のように絶対値を１にする正規化処理によってその変化を抑制する。
【００６８】
【数１５】
2015179002.tif 000017
【００６９】
姿勢誤差εｑの場合は、その分散共分散行列Σｑ２のランク（階数）を３に保つ必要がある。そこで（姿勢誤差角度は十分小さいとして）ローカル空間座標での（３次元）誤差回転ベクトルεθを考えて、式（１６）のようにランクを制限する。
【００７０】
【数１６】
2015179002.tif 000018
【００７１】
１−６．方位角誤差の除去（無視）
姿勢検出において磁気センサーなどの方位角観測手段を持たない場合、姿勢誤差の方位角成分は単調に増加するだけで何の役にも立たない。更には、その増大した誤差見積もりがフィードバックゲインを不必要に上げる事となり予期しない方位角の変化や振動の原因となる。そこで、式（１６）から方位角誤差成分εθｚを除去する。
【００７２】
【数１７】
2015179002.tif 000019
【００７３】
１−７．拡張カルマンフィルター
上記の各モデル式に基づいて３次元姿勢を算出する拡張カルマンフィルターを設計することができる。
【００７４】
［状態ベクトルと誤差共分散行列］
式（１８）のように、求めるべき未知の状態値として、姿勢クォータニオンｑ、運動速度ベクトルｖ、ジャイロセンサーバイアスｂω（角速度ωに対するオフセット）、加速度センサーバイアスｂα（加速度αに対するオフセット）、および重力加速度補正値Δｇで拡張カルマンフィルターの状態ベクトルｘ（１４次元のベクトル）を構成し、その誤差の共分散行列Σｘ２を定義する。
【００７５】
【数１８】
2015179002.tif 000020
【００７６】
［プロセスモデル］
プロセスモデルでは、サンプリング間隔Δｔ、ジャイロセンサー出力ｄωおよび加速度センサー出力ｄαの値に基づいて最新の状態ベクトルの値を式（１９）のように予測する。
【００７７】
【数１９】
2015179002.tif 000021
【００７８】
状態誤差の共分散行列は、ジャイロセンサー出力ｄωと加速度センサー出力ｄαのノイズ成分ηω、ηαと、ジャイロセンサーバイアスｂω、加速度センサーバイアスｂα、および重力加速度値ｇの不安定性を表すプロセスノイズρω、ρα、およびρｇの影響を受けて式（２０）のように更新される。
【００７９】
【数２０】
2015179002.tif 000022
【００８０】
ここで０ｎ×ｍはｎ行ｍ列の零行列を、Ｉｎ×ｍはｎ行ｍ列の恒等行列を表し、Ｊ…は、このプロセスモデルの偏微分で求められる式（２１）のような各誤差の伝播係数の行列である。また、Σ…は式（２２）のような各ノイズの共分散行列である。
【００８１】
【数２１】
2015179002.tif 000023
【００８２】
【数２２】
2015179002.tif 000024
【００８３】
［観測モデル］
観測モデルでは、加速度センサー出力ｄαに基づく重力加速度の観測残差と、ゼロ運動速度の観測残差を式（２３）のように算出する。
【００８４】
【数２３】
2015179002.tif 000025
【００８５】
ここに加速度センサー出力ｄαのノイズ成分ηαおよび観測誤差となる運動加速度成分ζａと運動速度成分ζｖを加味して式（２４）のようにカルマン係数Ｋを算出する。
【００８６】
【数２４】
2015179002.tif 000026
【００８７】
ここでＪ…は、この観測モデルの偏微分で求められる式（２５）のような各誤差の伝播係数の行列である。また、Σ…は式（２６）のような各ノイズの共分散行列である。
【００８８】
【数２５】
2015179002.tif 000027
【００８９】
【数２６】
2015179002.tif 000028
【００９０】
ここで、μは予測運動加速度から各軸の誤差ＲＭＳを算出する係数である。このカルマン係数Ｋを使って式（２７）のように状態ベクトルｘを補正するとともに、その誤差共分散行列Σｘ２を更新する。
【００９１】
【数２７】
2015179002.tif 000029
【００９２】
［姿勢正規化モデル］
姿勢正規化モデルは、姿勢クォータニオンとその誤差共分散を適切な値に維持するために式（２８）の更新を行う。
【００９３】
【数２８】
2015179002.tif 000030
【００９４】
ここで、Ｄ’ＴＤ’は姿勢誤差のランク制限と方位角成分除去を行う式（２９）の行列である。
【００９５】
【数２９】
2015179002.tif 000031
【００９６】
１−８．初期値
［状態ベクトルと誤差共分散行列］
状態ベクトルｘと誤差共分散行列Σｘ２の初期値は式（３０）のように与える。
【００９７】
【数３０】
2015179002.tif 000032
【００９８】
［姿勢クォータニオン］
ＩＭＵの姿勢をクォータニオン表現で与える必要がある。航空機の姿勢などで用いられるロール（バンク）角φ［ｒａｄ］、ピッチ（仰）角θ［ｒａｄ］、およびヨー（方位）角ψ［ｒａｄ］からは式（３１）のように姿勢クォータニオンｑを算出できる。
【００９９】
【数３１】
2015179002.tif 000033
【０１００】
また、その誤差共分散行列Σｑ２はロール角誤差のＲＭＳσφ［ｒａｄ ＲＭＳ］とピッチ角誤差のＲＭＳσθ［ｒａｄ ＲＭＳ］から式（３２）のように算出される（ヨー角誤差は無視される）。
【０１０１】
【数３２】
2015179002.tif 000034
【０１０２】
［速度ベクトル］
初期状態が静止ならば、速度ベクトルｖをゼロにするべきである。また、静止かどうかにかかわらず速度ベクトルｖの各軸の誤差ＲＭＳσｖｘ、σｖｙ、およびσｖｚ［Ｇｓ ＲＭＳ］に基づいて式（３３）のように誤差共分散行列Σｖ２を与える。
【０１０３】
【数３３】
2015179002.tif 000035
【０１０４】
［ジャイロ／加速度センサー残存バイアス］
ジャイロセンサーの残存バイアスｂωおよび加速度センサーの残存バイアスｂαが分かっているなら適切に設定されるべきである。不明な場合は期待値としてゼロを与える。また、ジャイロセンサーの各軸の残存バイアスの誤差ＲＭＳσｂωｘ、σｂωｙ、およびσｂωｚ［ｒａｄ／ｓ ＲＭＳ］と、加速度センサーの各軸の残存バイアスの誤差ＲＭＳσｂαｘ、σｂαｙ、およびσｂαｚ［Ｇ ＲＭＳ］に基づいて式（３４）のように誤差共分散行列Σｂω２及びΣｂα２を与える。
【０１０５】
【数３４】
2015179002.tif 000036
【０１０６】
［重力加速度補正値］
重力加速度値が分かっているなら標準値１［Ｇ］（＝９．８０６６５［ｍ／ｓ２］）との差が適切に設定されるべきである。不明な場合は期待値としてゼロを与える。また、その値の誤差ＲＭＳσΔｇ［Ｇ ＲＭＳ］を誤差共分散行列に反映する。
【０１０７】
１−９．設定値
［サンプリング間隔］
ＩＭＳ出力からの姿勢検出処理は原理的に時間積分演算のため、サンプリング間隔Δｔ［ｓ］は重要な値であり、適切に設定されなければならない。
【０１０８】
［ジャイロ／加速度センサー出力ノイズ］
ジャイロセンサーおよび加速度センサーの出力に含まれるノイズ成分ηを、軸ごとに独立した平均ゼロ、分散ση２のホワイトガウスノイズとみなし、その大きさを式（３５）のようにそれぞれのＲＭＳ値（σηωｘ，σηωｙ，σηωｚ）［ｒａｄ／ｓ ＲＭＳ］および（σηαｘ，σηαｙ，σηαｚ）［Ｇ ＲＭＳ］で指定する。
【０１０９】
【数３５】
2015179002.tif 000037
【０１１０】
［ジャイロ／加速度センサーバイアスおよび重力加速度値の不安定性］
ジャイロセンサーおよび加速度センサーのバイアスは一定ではなく時間とともに変動すると考えられる。また、重力加速度値も周囲の環境によってわずかながら変動すると考えられる。これらの変動を個別のランダムウォークとみなして不安定性を式（３６）のように（σρωｘ，σρωｙ，σρωｚ）［ｒａｄ／ｓ／√ｓ］、（σραｘ，σραｙ，σραｚ）［Ｇ／√ｓ］およびσρｇ［Ｇ／√ｓ］で指定する。
【０１１１】
【数３６】
2015179002.tif 000038
【０１１２】
［運動加速度］
姿勢補正のための重力加速度観測において、運動加速度成分は観測誤差ζａとなる。これを単純なホワイトガウスノイズとみなした場合、急激な運動による大きな運動加速度に対して姿勢が過敏に反応する結果となる。そこで、推定運動加速度（観測加速度と推定重力加速度の差）の大きさに応じてレベルが変化する式（３７）のようなノイズモデルを使い、その線形係数μ［Ｎ／Ａ］と定数項（σζαｘ，σζαｙ，σζαｚ）［Ｇ ＲＭＳ］を設定項目とする。
【０１１３】
【数３７】
2015179002.tif 000039
【０１１４】
［運動速度］
ゼロ運動速度観測では、ＩＭＵの運動速度が長期的にほぼゼロであることを観測するため、短期的に現れる運動速度は観測誤差ζｖとなる。これを軸ごとに独立したホワイトガウスノイズとみなし、その大きさを式（３８）のようにＲＭＳ値（σζｖｘ，σζｖｙ，σζｖｚ）［Ｇｓ ＲＭＳ］で指定する。
【０１１５】
【数３８】
2015179002.tif 000040
【０１１６】
２．姿勢推定装置
［姿勢推定装置の構成］
図１は、本実施形態の姿勢推定装置の構成例を示す図である。図１に示すように、本実施形態の姿勢推定装置１は、慣性計測ユニット（ＩＭＵ）１０、処理部２０、ＲＯＭ３０、ＲＡＭ４０、記録媒体５０及び通信部６０を含んで構成されている。ただし、本実施形態の姿勢推定装置１は、これらの一部の要素を変更又は削除し、あるいは、他の要素を追加した構成であってもよい。
【０１１７】
姿勢推定装置１は、ＩＭＵ１０と処理部２０が１つの筐体に収容されて構成されていてもよいし、ＩＭＵ１０が処理部２０を収容する本体から分離されて（あるいは分離可能に）構成されていてもよい。前者の場合は姿勢推定装置１が対象物に装着され、後者の場合はＩＭＵ１０が対象物に装着され、姿勢推定装置１は、当該対象物の姿勢を推定する処理を行う。
【０１１８】
本実施形態では、ＩＭＵ１０は、角速度センサー１２、加速度センサー１４及び信号処理部１６を含んで構成されている。ただし、本実施形態のＩＭＵ１０は、これらの一部の要素を変更又は削除し、あるいは、他の要素を追加した構成であってもよい。
【０１１９】
角速度センサー１２は、互いに交差する（理想的には直交する）３軸方向の各々の角速度を計測し、計測した３軸角速度の大きさ及び向きに応じたアナログ信号を出力する。
【０１２０】
加速度センサー１４は、互いに交差する（理想的には直交する）３軸方向の各々の加速度を計測し、計測した３軸加速度の大きさ及び向きに応じたアナログ信号を出力する。
【０１２１】
信号処理部１６は、角速度センサー１２の出力信号を所定の周期Δｔでサンプリングしてデジタル値の３軸角速度データに変換する処理を行う。また、信号処理部１６は、加速度センサー１４の出力信号を所定の周期Δｔでサンプリングしてデジタル値の３軸加速度データに変換する処理を行う。
【０１２２】
角速度センサー１２及び加速度センサー１４は、それぞれ３軸が、ＩＭＵ１０に対して定義される直交座標系（センサー座標系）の３軸（ｘ軸、ｙ軸、ｚ軸）と一致するようにＩＭＵ１０に取り付けられるのが理想的だが、実際には取り付け角の誤差が生じる。そこで、信号処理部１６は、取り付け角誤差に応じてあらかじめ算出された補正パラメーターを用いて、３軸角速度データ及び３軸加速度データをｘｙｚ座標系のデータに変換する処理も行う。さらに、信号処理部１６は、角速度センサー１２及び加速度センサー１４の温度特性に応じて、３軸角速度データ及び３軸加速度データを温度補正する処理も行う。
【０１２３】
なお、角速度センサー１２及び加速度センサー１４にＡ／Ｄ変換や温度補正の機能が組み込まれていてもよい。
【０１２４】
ＩＭＵ１０は、信号処理部１６による処理後の３軸角速度データｄω，ω及び３軸加速度データｄω，αを処理部２０に出力する。
【０１２５】
ＲＯＭ３０は、処理部２０が各種の処理を行うためのプログラムや、アプリケーション
機能を実現するための各種プログラムやデータ等を記憶している。
【０１２６】
ＲＡＭ４０は、処理部２０の作業領域として用いられ、ＲＯＭ３０から読み出されたプログラムやデータ、処理部２０が各種プログラムに従って実行した演算結果等を一時的に記憶する記憶部である。
【０１２７】
記録媒体５０は、処理部２０の処理により生成されたデータのうち、長期的な保存が必要なデータを記憶する不揮発性の記憶部である。また、記録媒体５０は、処理部２０が各種の処理を行うためのプログラムや、アプリケーション機能を実現するための各種プログラムやデータ等を記憶していてもよい。
【０１２８】
処理部２０は、ＲＯＭ３０あるいは記録媒体５０に記憶されているプログラム、あるいはネットワークを介してサーバーから受信してＲＡＭ４０や記録媒体５０に記憶したプログラムに従って各種の処理を行う。特に、本実施形態では、処理部２０は、当該プログラムを実行することにより、バイアス除去部２２、姿勢変化量計算部２４、速度変化量計算部２６及び姿勢推定部２８として機能し、ＩＭＵ１０がΔｔ間隔で出力する３軸角速度データｄω，ｋ及び３軸加速度データｄω，αに対して所定の演算を行い、対象物の姿勢を推定する処理を行う。
【０１２９】
本実施形態では、図２に示すように、ＩＭＵ１０の座標系であるセンサー座標系（互いに直交するｘ軸、ｙ軸、ｚ軸で構成されるｘｙｚ座標系）と対象物が存在する空間の座標系であるローカル空間座標系（互いに直交するＸ軸、Ｙ軸、Ｚ軸で構成されるＸＹＺ座標系）を考える。処理部２０は、対象物に装着されたＩＭＵ１０から出力されるセンサー座標系の３軸角速度と３軸加速度からローカル空間座標系における対象物の姿勢（ＩＭＵ１０の姿勢ともいえる）を推定する。
【０１３０】
バイアス除去部２２は、角速度センサー１２の出力からバイアス誤差を除去した３軸角速度を計算する処理、及び加速度センサー１４の出力からバイアス誤差を除去した３軸加速度を計算する処理を行う。
【０１３１】
姿勢変化量計算部２４は、角速度センサー１２の出力を用いて（具体的には、バイアス除去部２２によりバイアス誤差が除去された３軸角速度を用いて）、サンプリング周期Δｔを変数とする多項式で近似して対象物の姿勢変化量を計算する処理を行う。
【０１３２】
速度変化量計算部２６は、加速度センサー１４の出力と角速度センサー１２の出力とを用いて（具体的には、バイアス除去部２２によりバイアス誤差が除去された３軸角速度と３軸加速度とを用いて）、対象物の速度変化量を計算する処理を行う。
【０１３３】
姿勢推定部２８は、積分計算部１０１、姿勢予測部１０２、姿勢誤差更新部１０３、補正係数計算部１０４、姿勢補正部１０５、正規化部１０６、姿勢誤差補正部１０７及び方位角誤差除去部１０８として機能し、姿勢変化量計算部２４が計算した姿勢変化量と速度変化量計算部２６が計算した速度変化量とを用いて、対象物の姿勢を推定する処理を行う。実際には、姿勢推定部２８は、拡張カルマンフィルターにより、前出の式（１８）で定義した状態ベクトルｘ及びその誤差共分散行列Σｘ２を推定する処理を行う。
【０１３４】
積分計算部１０１は、対象物の姿勢（具体的には、姿勢補正部１０５により補正され、正規化部１０６により正規化された直前の姿勢の推定値）に姿勢変化量計算部２４が計算した姿勢変化量を積算する積分処理を行う。また、積分計算部１０１は、対象物の速度（具体的には、姿勢補正部１０５により補正され、正規化部１０６により正規化された直前の速度の推定値）に速度変化量計算部２６が計算した速度変化量を積算する積分処理を行
う。
【０１３５】
姿勢予測部１０２は、角速度センサー１２の出力を用いて（具体的には、姿勢変化量計算部２４が計算した姿勢変化量を用いて）、対象物の姿勢を予測する処理を行う。また、姿勢予測部１０２は、速度変化量計算部２６が計算した速度変化量を用いて、対象物の速度を予測する処理も行う。実際には、姿勢予測部１０２は、状態ベクトルｘを予測する処理を行うが、この処理の詳細については後述する。
【０１３６】
姿勢誤差更新部１０３は、角速度センサー１２の出力を用いて（具体的には、バイアス除去部２２によりバイアス誤差が除去された３軸角速度を用いて）、対象物の姿勢誤差を更新する処理を行う。言い換えれば、姿勢誤差更新部１０３は、角速度センサー１２の出力を用いて（具体的には、バイアス除去部２２によりバイアス誤差が除去された３軸角速度を用いて）、対象物の回転誤差が方位角誤差成分を含まないものとして対象物の姿勢誤差を更新する処理を行う。実際には、姿勢誤差更新部１０３は、拡張カルマンフィルターにより、誤差共分散行列Σｘ２を更新する処理を行うが、この処理の詳細については後述する。
【０１３７】
補正係数計算部１０４は、姿勢誤差更新部１０３により更新された姿勢誤差を用いて、姿勢補正部１０５による対象物の姿勢の補正量や姿勢誤差補正部１０７による姿勢誤差の補正量を決める補正係数を計算する処理を行う。実際には、補正係数計算部１０４は、カルマン係数Ｋを計算する処理を行うが、この処理の詳細については後述する。
【０１３８】
姿勢補正部１０５は、姿勢誤差更新部１０３により更新された姿勢誤差を用いて、姿勢予測部１０２が予測した対象物の姿勢を、補正係数計算部１０４が計算した補正係数に応じて補正する処理を行う。特に、姿勢補正部１０５は、姿勢予測部１０２が予測した対象物の速度とゼロとの差を用いて、姿勢予測部１０２が予測した対象物の姿勢を補正する。実際には、姿勢補正部１０５は、拡張カルマンフィルターにより、姿勢予測部１０２が予測した状態ベクトルｘを補正する処理を行うが、この処理の詳細については後述する。
【０１３９】
正規化部１０６は、姿勢補正部１０５が補正した対象物の姿勢を、その大きさが変わらないように正規化する処理を行う。実際には、正規化部１０６は、姿勢補正部１０５が補正した状態ベクトルｘを正規化する処理を行うが、この処理の詳細については後述する。
【０１４０】
姿勢誤差補正部１０７は、姿勢誤差更新部１０３により更新された姿勢誤差を、補正係数計算部１０４が計算した補正係数に応じて補正する処理を行う。実際には、姿勢誤差補正部１０７は、拡張カルマンフィルターにより、姿勢誤差更新部１０３が更新した誤差共分散行列Σｘ２を補正する処理を行うが、この処理の詳細については後述する。
【０１４１】
方位角誤差除去部１０８は、姿勢誤差更新部１０３により更新された姿勢誤差（具体的には、さらに姿勢誤差補正部１０７により補正された姿勢誤差）に含まれる方位角誤差を除去する処理を行う。実際には、方位角誤差除去部１０８は、姿勢誤差補正部１０７が補正した誤差共分散行列Σｘ２に対して、姿勢の誤差共分散行列Σｑ２のランク制限と方位角誤差成分の除去を行った誤差共分散行列Σｘ２を生成する処理を行うが、この処理の詳細については後述する。
【０１４２】
処理部２０が推定した対象物の姿勢の情報は、通信部６０を介して他の装置に送信可能である。
【０１４３】
［処理部の構成］
図３は、処理部２０の具体的な構成例を示す図である。図３において、図１と同じ構成
要素には同じ符号を付している。図３に示すように、ＩＭＵ１０が出力する時刻ｔｋにおける角速度データｄω，ｋ及び加速度データｄα，ｋは、バイアス除去部２２に入力される。角速度データｄω，ｋは、時刻ｔｋ−１から時刻ｔｋまでの期間Δｔの角速度ωの平均値と角速度センサー１２の残存バイアス誤差ｂωの和で表される（前出の式（１）参照）。同様に、加速度データｄα，ｋは、時刻ｔｋ−１から時刻ｔｋまでの期間Δｔの加速度αの平均値と加速度センサー１４の残存バイアス誤差ｂαの和で表される（前出の式（２）参照）。
【０１４４】
バイアス除去部２２は、時刻ｔｋにおける角速度データｄω，ｋから、推定された時刻ｔｋ−１における残存バイアス誤差ｂω，ｋ−１を差し引いて、時刻ｔｋ−１から時刻ｔｋまでの期間Δｔの角速度ωの平均値を計算する（前出の式（１９）参照）。また、バイアス除去部２２は、時刻ｔｋにおける加速度データｄα，ｋから、推定された時刻ｔｋ−１における残存バイアス誤差ｂα，ｋ−１を差し引いて、時刻ｔｋ−１から時刻ｔｋまでの期間Δｔの加速度αの平均値を計算する（前出の式（１９）参照）。
【０１４５】
姿勢変化量計算部２４は、バイアス除去部２２が計算した時刻ｔｋ−１から時刻ｔｋまでの期間Δｔの角速度ωの平均値及び時刻ｔｋ−２から時刻ｔｋ−１までの期間Δｔの角速度ωの平均値を前出の式（６）の多項式に代入し、時刻ｔｋにおける姿勢変化量Δｑｋを近似計算する。
【０１４６】
速度変化量計算部２６は、前出の式（９）により、推定された時刻ｔｋ−１における姿勢クォータニオンｑｋ−１から時刻ｔｋにおける座標変換行列Ｃｋを計算する。また、速度変化量計算部２６は、バイアス除去部２２が計算した時刻ｔｋ−１から時刻ｔｋまでの期間Δｔの加速度αの平均値、角速度ωの平均値及び時刻ｔｋ−２から時刻ｔｋ−１までの期間Δｔの加速度αの平均値、角速度ωの平均値を前出の式（１２）の多項式に代入し、時刻ｔｋにおける加速度λｋを近似計算する。また、速度変化量計算部２６は、推定された時刻ｔｋ−１における重力加速度補正値Δｇｋ−１を前出の式（１０）に代入し、時刻ｔｋ−１における重力加速度ベクトルｇｋ−１を計算する。さらに、速度変化量計算部２６は、計算した時刻ｔｋにおける座標変換行列Ｃｋ，加速度λｋ及び時刻ｔｋ−１における重力加速度ベクトルｇｋ−１から時刻ｔｋにおける速度変化量（Ｃｋλｋ−ｇｋ−１）Δｔを計算する。
【０１４７】
図３に示すように、姿勢推定部２８は、積分計算部１０１、姿勢予測部１０２、姿勢誤差更新部１０３、補正係数計算部１０４、姿勢補正部１０５、正規化部１０６、姿勢誤差補正部１０７及び方位角誤差除去部１０８を含んで構成されており、拡張カルマンフィルターにより、時刻ｔｋにおける状態ベクトルｘ及びその共分散行列Σｘ，ｋ２を推定する。
【０１４８】
積分計算部１０１は、推定された時刻ｔｋ−１における姿勢クォータニオンｑｋ−１と姿勢変化量計算部２４が計算した時刻ｔｋにおける姿勢変化量Δｑｋとのクォータニオン乗算（前出の式（６）の計算）を行う。また、積分計算部１０１は、推定された時刻ｔｋ−１における速度ベクトルｖｋ−１と速度変化量計算部２６が計算した時刻ｔｋにおける速度変化量（Ｃｋλｋ−ｇｋ−１）Δｔとの加算（前出の式（１２）の計算）を行う。
【０１４９】
姿勢予測部１０２は、前出の式（１９）により、時刻ｔｋにおける状態ベクトルｘｋの各要素である、姿勢クォータニオンｑｋ、運動速度ベクトルｖｋ、角速度センサー１２のバイアスｂω，ｋ、加速度センサー１４のバイアスｂα，ｋ、重力加速度補正値Δｇｋを、それぞれ、積分計算部１０１による姿勢クォータニオンｑｋ−１と姿勢変化量Δｑｋとのクォータニオン乗算結果、積分計算部１０１による速度ベクトルｖｋ−１と速度変化量（Ｃｋλｋ−ｇｋ−１）Δｔとの加算結果、推定された時刻ｔｋ−１における角速度セン
サー１２の残存バイアス誤差ｂω，ｋ−１、推定された時刻ｔｋ−１における加速度センサー１４の残存バイアス誤差ｂα，ｋ−１、推定された時刻ｔｋ−１における重力加速度補正値Δｇｋ−１であると予測する。
【０１５０】
姿勢誤差更新部１０３は、姿勢変化量計算部２４が計算した時刻ｔｋにおける姿勢変化量Δｑｋ、速度変化量計算部２６が計算した時刻ｔｋにおける加速度λｋ及び座標変換行列Ｃｋ、推定された時刻ｔｋ−１における姿勢クォータニオンｑｋ−１及び時刻ｔｋ−１における誤差共分散行列Σｘ，ｋ−１２を用いて、前出の式（２０）及び式（２１）より、時刻ｔｋにおける誤差共分散行列Σｘ，ｋ２を更新する。
【０１５１】
補正係数計算部１０４は、姿勢誤差更新部１０３が更新した時刻ｔｋにおける誤差共分散行列Σｘ，ｋ２、速度変化量計算部２６が計算した時刻ｔｋにおける座標変換行列Ｃｋ、バイアス除去部２２が計算した時刻ｔｋ−１から時刻ｔｋまでの期間Δｔの加速度αの平均値、姿勢予測部が予測した時刻ｔｋにおける姿勢クォータニオンｑｋ及び重力加速度補正値Δｇｋを用いて、前出の式（２３）、式（２４）、式（２５）及び式（２６）より、時刻ｔｋにおける変換行列Ｈｋ及びカルマン係数Ｋｋを計算する。
【０１５２】
姿勢補正部１０５は、補正係数計算部１０４が計算した時刻ｔｋにおける変換行列Ｈｋ及びカルマン係数Ｋｋ用いて、前出の式（２７）より、姿勢予測部１０２が予測した時刻ｔｋにおける状態ベクトルｘｋの各要素（姿勢クォータニオンｑｋ、運動速度ベクトルｖｋ、角速度センサー１２のバイアスｂω，ｋ、加速度センサー１４のバイアスｂα，ｋ、重力加速度補正値Δｇｋ）を補正する。
【０１５３】
正規化部１０６は、姿勢補正部１０５が補正した時刻ｔｋにおける状態ベクトルｘｋの各要素（姿勢クォータニオンｑｋ、運動速度ベクトルｖｋ、角速度センサー１２のバイアスｂω，ｋ、加速度センサー１４のバイアスｂα，ｋ、重力加速度補正値Δｇｋ）を、前出の式（２８）により正規化する。
【０１５４】
姿勢誤差補正部１０７は、補正係数計算部１０４が計算した時刻ｔｋにおける変換行列Ｈｋ及びカルマン係数Ｋｋ用いて、前出の式（２７）より、姿勢誤差更新部１０３が更新した時刻ｔｋにおける誤差共分散行列Σｘ，ｋ２を補正する。
【０１５５】
方位角誤差除去部１０８は、正規化部１０６が正規化した時刻ｔｋにおける姿勢クォータニオンｑｋを用いて、前出の式（２９）により行列Ｄ’ＴＤ’を計算し、行列Ｄ’ＴＤ’を用いて、式（２８）により、姿勢誤差補正部１０７が補正した時刻ｔｋにおける誤差共分散行列Σｘ，ｋ２を計算する。これにより、姿勢クォータニオンｑｋの誤差共分散行列Σｑ，ｋ２のランクを３に保つ（制限する）とともに誤差共分散行列Σｑ，ｋ２から方位角誤差成分εθｚが除去された誤差共分散行列Σｘ，ｋ２が生成される。
【０１５６】
姿勢推定部２８は、正規化部１０６が計算した時刻ｔｋにおける状態ベクトルｘｋ及び方位角誤差除去部１０８が計算した時刻ｔｋにおける誤差共分散行列Σｘ，ｋ２を、それぞれ時刻ｔｋにおける状態ベクトル及びその誤差共分散行列であると推定する。そして、このように推定された時刻ｔｋにおける状態ベクトルｘｋ及び誤差共分散行列Σｘ，ｋ２は、次のサンプリング周期Δｔが経過すると、時刻ｔｋ−１における状態ベクトルｘｋ−１及び誤差共分散行列Σｘ，ｋ−１２として、バイアス除去部２２、速度変化量計算部２６、積分計算部１０１、姿勢予測部１０２及び姿勢誤差更新部１０３にフィードバックされる。
【０１５７】
［姿勢推定処理の手順］
図４は、図３に示した処理部２０による姿勢推定処理の手順の一例を示すフローチャー
ト図である。
【０１５８】
図４に示すように、処理部２００は、まず、ｋ＝０、ｔ０＝０に設定し（Ｓ２）、時刻ｔ０における状態ベクトルｘ０及び誤差共分散行列Σｘ，０２を設定（初期化）する（Ｓ２）。具体的には、処理部２００は、前出の式（３０）のように表される、状態ベクトルｘ０の各要素ｑ０，ｖ０，ｂω，０，ｂα，０，Δｇ０及び誤差共分散行列Σｘ，０２に含まれるΣｑ，０２，Σｖ，０２，Σｂω，０２，Σｂα，０２，σΔｇ，０２を設定する。
【０１５９】
処理部２０は、例えば、ＩＭＵ１０の初期姿勢があらかじめ決められたロール角、ピッチ角、ヨー角となるようにしておいて当該ロール角、ピッチ角、ヨー角を前出の式（３１）に代入してｑ０を設定してもよい。あるいは、処理部２０は、ＩＭＵ１０が静止した状態で加速度センサー１４から加速度データを取得し、当該加速度データから重力加速度の方向を特定してロール角、ピッチ角を計算し、ヨー角を所定値（例えば０）として、当該ロール角、ピッチ角、ヨー角を前出の式（３１）に代入してｑ０を設定してもよい。また、処理部２０は、ロール角誤差のＲＭＳσφとピッチ角誤差のＲＭＳσθを式（３２）に代入してΣｑ，０２を設定する。
【０１６０】
また、処理部２０は、例えば、ＩＭＵ１０が静止した状態を初期状態としてｖ０を０に設定し、速度ベクトルｖの各軸の誤差ＲＭＳσｖｘ，σｖｙ、σｖｚを式（３３）に代入してΣｖ，０２を設定する。
【０１６１】
また、処理部２０は、角速度センサー１２の残存バイアスｂω及び加速度センサー１４の残存バイアスｂαがわかっていればこれらの値をｂω，０，ｂα，０に設定し、ｂω，ｂαが不明であれば、ｂω，０，ｂα，０にゼロを設定する。また、処理部２０は、角速度センサー１２の各軸の残存バイアス誤差ＲＭＳσｂωｘ，σｂωｙ，σｂωｚ及び加速度ンサー１１０の各軸の残存バイアス誤差ＲＭＳσｂαｘ，σｂαｙ，σｂαｚを式（３４）に代入してΣｂω，０２，Σｂα，０２を設定する。
【０１６２】
また、処理部２０は、重力加速度値がわかっていれば１Ｇとの差をΔｇ０に設定し、重力加速度値が不明であれば、Δｇ０にゼロを設定する。また、処理部２０は、σΔｇ，０２に重力加速度値の誤差ＲＭＳσΔｇを設定する。
【０１６３】
次に、処理部２０は、サンプリング周期Δｔが経過するまで待機し（Ｓ３のＮ）、サンプリング周期Δｔが経過すると（Ｓ３のＹ）、ｋ＝ｋ＋１に設定した後（Ｓ４）、ｔｋ＝ｔｋ−１＋Δｔに設定する（Ｓ５）。
【０１６４】
次に、処理部２０は、ＩＭＵ１０から角速度データｄω，ｋ及び加速度データｄα，ｋを取得する（Ｓ６）。
【０１６５】
次に、処理部２０（バイアス除去部２２）は、時刻ｔｋにおける角速度データｄω，ｋ及び加速度データｄα，ｋから、それぞれ、時刻ｔｋ−１で推定されたバイアスbω，ｋ−１及びbα，ｋ−１を除去する（Ｓ７）。
【０１６６】
次に、処理部２０（姿勢変化量計算部２４）は、時刻ｔｋにおける姿勢変化量Δｑｋを計算する（Ｓ８）。
【０１６７】
次に、処理部２０（速度変化量計算部２６）は、時刻ｔｋにおける速度変化量（Ｃｋλｋ−ｇｋ−１）を計算する（Ｓ９）。
【０１６８】
次に、処理部２０（姿勢推定部２８）は、時刻ｔｋにおける状態ベクトルｘｋ及び誤差共分散行列Σｘ，ｋ２を推定する（Ｓ１０）。
【０１６９】
そして、処理部２０は、姿勢推定処理の終了イベントが発生するまで（Ｓ１１のＮ）、ステップＳ３〜Ｓ１０の処理を繰り返し、終了イベントが発生すれば（Ｓ１１のＹ）、姿勢推定処理を終了する。
【０１７０】
図５は、図３に示した姿勢推定部２８による時刻ｔｋにおける状態ベクトルｘｋ及び誤差共分散行列Σｘ，ｋ２を推定する処理（図４のステップＳ１０の処理）の手順の一例を示すフローチャート図である。
【０１７１】
図５に示すように、まず、姿勢推定部２８（積分計算部１０１）は、時刻ｔｋ−１における姿勢クォータニオンｑｋ−１と時刻ｔｋにおける姿勢変化量Δｑｋとをクォータニオン乗算する（Ｓ１００）。
【０１７２】
次に、姿勢推定部２８（積分計算部１０１）は、時刻ｔｋ−１における速度ベクトルｖｋ−１と時刻ｔｋにおける速度変化量（Ｃｋλｋ−ｇｋ−１）とを加算する（Ｓ１００）。
【０１７３】
次に、姿勢推定部２８（姿勢予測部１０２）は、時刻ｔｋにおける状態ベクトルｘｋを予測する（Ｓ１０２）。
【０１７４】
次に、姿勢推定部２８（姿勢誤差更新部１０３）は、時刻ｔｋにおける誤差共分散行列Σｘ，ｋ２を更新する（Ｓ１０３）。
【０１７５】
次に、姿勢推定部２８（姿勢誤差更新部１０３）は、時刻ｔｋにおける誤差共分散行列Σｘ，ｋ２を更新する（Ｓ１０３）。
【０１７６】
次に、姿勢推定部２８（補正係数計算部１０４）は、時刻ｔｋにおけるカルマン係数Ｋｋを計算する（Ｓ１０４）。
【０１７７】
次に、姿勢推定部２８（姿勢補正部１０５）は、時刻ｔｋにおける状態ベクトルｘｋを補正する（Ｓ１０５）。
【０１７８】
次に、姿勢推定部２８（正規化部１０６）は、時刻ｔｋにおける状態ベクトルｘｋを正規化する（Ｓ１０６）。
【０１７９】
次に、姿勢推定部２８（姿勢誤差補正部１０７）は、時刻ｔｋにおける誤差共分散行列Σｘ，ｋ２を補正する（Ｓ１０７）。
【０１８０】
最後に、姿勢推定部２８（方位角誤差除去部１０８）は、正規化された姿勢クォータニオンｑｋの誤差共分散行列Σｑ，ｋ２のランク制限と方位角誤差成分εθｚが除去された、時刻ｔｋにおける誤差共分散行列Σｘ，ｋ２を生成する（Ｓ１０８）。
【０１８１】
以上に説明したように、本実施形態の姿勢推定装置１によれば、ＩＭＵの出力モデルの式（１）及び式（２）から導かれる式（６）及び式（１２）を用いて対象物の姿勢変化量及び速度変化量を計算し、この姿勢変化量と速度変化量とを用いて対象物の姿勢を推定する。式（６）及び式（１２）では、Δｔの１次項のみならず２次項及び３次項も用いて姿勢変化量及び速度変化量を計算することで、従来よりも姿勢変化量や速度変化量の計算誤差が小さくなる。
【０１８２】
また、対象物が回転すると座標変換行列Ｃｋが変化するが、座標変換行列Ｃｋはカルマンフィルターによって推定された姿勢クォータニオンｑｋの要素から計算されるため、対象物が急激に回転した場合、座標変換行列Ｃｋにすぐに追従しない場合がある。本実施形態の姿勢推定装置１によれば、式（１２）において、加速度だけでなく角速度も用いて加速度λｋを計算することで、対象物の回転がすぐに加速度λｋに反映され、対象物が急激に回転した場合でも、速度変化量の計算精度の劣化を低減させることができる。
【０１８３】
さらに、本実施形態の姿勢推定装置１によれば、式（２３）より、加速度センサーの出力による重力加速度の観測残差と対象物の運動速度は長期的にはゼロであるとしてゼロ運動速度の観測残差を用いて、式（２４）のカルマン係数Ｋｋを計算するので、方位角の観測情報が無くても、対象物の姿勢を精度よく推定することができる。
【０１８４】
また、角速度センサーの出力と加速度センサーの出力には象物の方位角についての情報が含まれないため対象物の方位角誤差については補正されないが、更新された姿勢誤差に含まれる方位角誤差を残しておくと、方位角誤差の確からしさが単調に減少し、姿勢誤差が単調に増加するため、姿勢の推定精度が劣化するおそれがある。本実施形態の姿勢推定装置１によれば、式（２９）により、角速度センサー１２の出力と加速度センサー１４の出力とを用いて更新した対象物の姿勢誤差に含まれる方位角誤差を除去するので、姿勢誤差が単調に増加することを防止し、姿勢の推定精度を劣化させないようにすることができる。
【０１８５】
以上より、本実施形態の姿勢推定装置１によれば、角速度センサー１２の出力と加速度センサー１４の出力のみを用いて、対象物の姿勢を十分な精度で推定することができる。
【０１８６】
３．姿勢推定システム
図６は、本実施形態の姿勢推定システムの構成例を示す図である。図６に示すように、本実施形態の姿勢推定システム２は、慣性計測装置（ＩＭＵ）３と姿勢推定装置４を含んで構成されている。
【０１８７】
本実施形態では、ＩＭＵ３は、角速度センサー２１２、加速度センサー２１４、信号処理部２１６及び通信部２１８を含んで構成されており、対象物に装着される。ただし、本実施形態のＩＭＵ３は、これらの一部の要素を変更又は削除し、あるいは、他の要素を追加した構成であってもよい。
【０１８８】
角速度センサー２１２は、互いに交差する（理想的には直交する）３軸方向の各々の角速度を計測し、計測した３軸角速度の大きさ及び向きに応じたアナログ信号を出力する。
【０１８９】
加速度センサー２１４は、互いに交差する（理想的には直交する）３軸方向の各々の加速度を計測し、計測した３軸加速度の大きさ及び向きに応じたアナログ信号を出力する。
【０１９０】
信号処理部２１６は、角速度センサー１２と加速度センサー１４から、それぞれ３角速度データと３加速度データを受け取ってＡ／Ｄ変換する処理を行う。
【０１９１】
角速度センサー２１２及び加速度センサー２１４は、それぞれ３軸が、ＩＭＵ３に対して定義される直交座標系（センサー座標系）の３軸（ｘ軸、ｙ軸、ｚ軸）と一致するようにＩＭＵ３に取り付けられるのが理想的だが、実際には取り付け角の誤差が生じる。そこで、信号処理部２１６は、取り付け角誤差に応じてあらかじめ算出された補正パラメーターを用いて、３軸角速度データ及び３軸加速度データをｘｙｚ座標系のデータに変換する処理も行う。また、信号処理部２１６は、角速度センサー２１２及び加速度センサー２１
４の温度特性に応じて、３軸角速度データ及び３軸加速度データを温度補正する処理も行う。
【０１９２】
なお、角速度センサー２１２及び加速度センサー２１４にＡ／Ｄ変換や温度補正の機能が組み込まれていてもよい。
【０１９３】
さらに、信号処理部２１６は、これらの処理後の３軸角速度データｄω，ω及び３軸加速度データｄω，αに時刻情報を付して通信用のフォーマットに合わせたパケットデータを生成し、通信部２１８に出力する。
【０１９４】
通信部２１８は、信号処理部２１６から受け取ったパケットデータを姿勢推定装置４に送信する処理や、姿勢推定装置４から制御コマンドを受信して信号処理部２１６に送る処理等を行う。信号処理部２１６は、制御コマンドに応じた各種処理を行う。
【０１９５】
姿勢推定装置４は、通信部３１０、処理部３２０、ＲＯＭ３３０、ＲＡＭ３４０、記録媒体３５０、操作部３６０、表示部３７０及び音出力部３８０を含んで構成されている。ただし、本実施形態の姿勢推定装置４は、これらの一部の要素を変更又は削除し、あるいは、他の要素を追加した構成であってもよい。姿勢推定装置４は、例えば、パーソナルコンピューター（ＰＣ）やスマートフォンなどの携帯機器であってもよい。
【０１９６】
通信部３１０は、ＩＭＵ３から送信されたパケットデータを受信し、処理部３２０に送る処理や、処理部３２０からの制御コマンドをＩＭＵ３に送信する処理等を行う。
【０１９７】
操作部３６０は、ユーザーからの操作データを取得し、処理部３２０に送る処理を行う。操作部３６０は、例えば、タッチパネル型ディスプレイ、ボタン、キー、マイクなどであってもよい。
【０１９８】
表示部３７０は、処理部３２０の処理結果を文字、グラフ、表、アニメーション、その他の画像として表示するものである。表示部３７０は、例えば、ＣＲＴ、ＬＣＤ、タッチパネル型ディスプレイ、ＨＭＤ（ヘッドマウントディスプレイ）などであってもよい。なお、１つのタッチパネル型ディスプレイで操作部３６０と表示部３７０の機能を実現するようにしてもよい。
【０１９９】
音出力部３８０は、処理部３２０の処理結果を音声やブザー音等の音として表示するものである。音出力部３８０は、例えば、スピーカーやブザーなどであってもよい。
【０２００】
ＲＯＭ３３０は、処理部３２０が各種の処理を行うためのプログラムや、アプリケーション機能を実現するための各種プログラムやデータ等を記憶している。
【０２０１】
ＲＡＭ３４０は、処理部３２０の作業領域として用いられ、ＲＯＭ３３０から読み出されたプログラムやデータ、処理部３２０が各種プログラムに従って実行した演算結果等を一時的に記憶する記憶部である。
【０２０２】
記録媒体３５０は、処理部３２０の処理により生成されたデータのうち、長期的な保存が必要なデータを記憶する不揮発性の記憶部である。また、記録媒体３５０は、処理部３２０が各種の処理を行うためのプログラムや、アプリケーション機能を実現するための各種プログラムやデータ等を記憶していてもよい。
【０２０３】
処理部３２０は、ＲＯＭ３３０あるいは記録媒体３５０に記憶されているプログラム、あるいはネットワークを介してサーバーから受信してＲＡＭ３４０や記録媒体３５０に記
憶したプログラムに従って各種の処理を行う。特に、本実施形態では、処理部３２０は、当該プログラムを実行することにより、データ取得部３２１、バイアス除去部３２２、姿勢変化量計算部３２４、速度変化量計算部３２６及び姿勢推定部３２８として機能する。そして、処理部３２０は、ＩＭＵ３がΔｔ間隔で計測したセンサー座標系（ｘｙｚ座標系）の３軸角速度データｄω，ｋ及び３軸加速度データｄω，αに対して所定の演算を行い、ＩＭＵ１０が装着された対象物のローカル空間座標系（ＸＹＺ座標系）における姿勢（ＩＭＵ３の姿勢ともいえる）を推定する。
【０２０４】
データ取得部３２１は、通信部３１０がＩＭＵ３から受信したパケットデータを受け取り、受け取ったパケットデータから時刻情報及び計測データ（３軸角速度データｄω，ｋ及び３軸加速度データｄω，α）を取得し、バイアス除去部３２２に送る処理を行う。
【０２０５】
バイアス除去部３２２、姿勢変化量計算部３２４及び速度変化量計算部３２６の各処理は、それぞれ、図１のバイアス除去部２２、姿勢変化量計算部２４及び速度変化量計算部２６の各処理と同じであるため、その説明を省略する。
【０２０６】
姿勢推定部３２８は、積分計算部４０１、姿勢予測部４０２、姿勢誤差更新部４０３、補正係数計算部４０４、姿勢補正部４０５、正規化部４０６、姿勢誤差補正部４０７及び方位角誤差除去部４０８として機能し、姿勢変化量計算部３２４が計算した姿勢変化量と速度変化量計算部３２６が計算した速度変化量とを用いて、対象物の姿勢を推定する処理を行う。積分計算部４０１、姿勢予測部４０２、姿勢誤差更新部４０３、補正係数計算部４０４、姿勢補正部４０５、正規化部４０６、姿勢誤差補正部４０７及び方位角誤差除去部４０８の各処理は、それぞれ、図１の積分計算部１０１、姿勢予測部１０２、姿勢誤差更新部１０３、補正係数計算部１０４、姿勢補正部１０５、正規化部１０６、姿勢誤差補正部１０７及び方位角誤差除去部１０８の各処理と同じであるため、その説明を省略する。
【０２０７】
処理部２０は、推定した対象物の姿勢に応じた画像データや音データを生成し、表示部３７０や音出力部３８０に出力する。
【０２０８】
姿勢推定装置４の処理部３２０の具体的な構成は図３と同様であってもよいため、その図示及び説明を省略する。また、処理部３２０による姿勢推定処理の手順は図４のフローチャート図と同様であってもよいため、その図示及び説明を省略する。また、姿勢推定部３２８による時刻ｔｋにおける状態ベクトルｘｋ及び誤差共分散行列Σｘ，ｋ２を推定する処理（図４のステップＳ１０の処理）の手順も図５のフローチャート図と同様であってもよいため、その図示及び説明を省略する。
【０２０９】
本実施形態の姿勢推定システム２によれば、姿勢推定装置４により、角速度センサー２１２の出力と加速度センサー２１４の出力のみを用いて、対象物の姿勢を十分な精度で推定することができる。
【０２１０】
４．変形例
本発明は本実施形態に限定されず、本発明の要旨の範囲内で種々の変形実施が可能である。
【０２１１】
例えば、上述した実施形態では、角速度センサー１２と加速度センサー１４は１つのＩＭＵ１０に収容され、一体化されているが、角速度センサー１２と加速度センサー１４は別体であってもよい。同様に、角速度センサー２１２と加速度センサー２１４は１つのＩＭＵ３に収容され、一体化されているが、角速度センサー２１２と加速度センサー２１４は別体であってもよい。
【０２１２】
また、例えば、上述した実施形態では、姿勢推定装置１あるいは姿勢推定装置４は、対象物の姿勢情報のみを出力しているが、さらに他の情報を出力してもよい。例えば、姿勢推定装置１あるいは姿勢推定装置４は、時刻ｔｋにおける速度ベクトルｖｋに基づく対象物の速度情報や速度ベクトルｖｋを積分して得られる対象物の位置情報を出力してもよい。
【０２１３】
上述した各実施形態および各変形例は一例であって、これらに限定されるわけではない。例えば、各実施形態および各変形例を適宜組み合わせることも可能である。
【０２１４】
本発明は、実施の形態で説明した構成と実質的に同一の構成（例えば、機能、方法及び結果が同一の構成、あるいは目的及び効果が同一の構成）を含む。また、本発明は、実施の形態で説明した構成の本質的でない部分を置き換えた構成を含む。また、本発明は、実施の形態で説明した構成と同一の作用効果を奏する構成又は同一の目的を達成することができる構成を含む。また、本発明は、実施の形態で説明した構成に公知技術を付加した構成を含む。
【符号の説明】
【０２１５】
１  姿勢推定装置、２  姿勢推定システム、３  慣性計測装置（ＩＭＵ）、４  姿勢推定装置、１０  慣性計測ユニット（ＩＭＵ）、１２  角速度センサー、１４  加速度センサー、１６  信号処理部、２０  処理部、２２  バイアス除去部、２４  姿勢変化量計算部、２６  速度変化量計算部、２８  姿勢推定部、３０  ＲＯＭ、４０  ＲＡＭ、５０  記録媒体、６０  通信部、１０１  積分計算部、１０２  姿勢予測部、１０３  姿勢誤差更新部、１０４  補正係数計算部、１０５  姿勢補正部、１０６  正規化部、１０７  姿勢誤差補正部、１０８  方位角誤差除去部、２１２  角速度センサー、２１４  加速度センサー、２１６  信号処理部、２１８  通信部、３１０  通信部、３２０  処理部、３２１
データ取得部、３２２  バイアス除去部、３２４  姿勢変化量計算部、３２６  速度変化量計算部、３２８  姿勢推定部、３３０  ＲＯＭ、３４０  ＲＡＭ、３５０  記録媒体、３６０  操作部、３７０  表示部、３８０  音出力部、４０１  積分計算部、４０２  姿勢予測部、４０３  姿勢誤差更新部、４０４  補正係数計算部、４０５  姿勢補正部、４０６  正規化部、４０７  姿勢誤差補正部、４０８  方位角誤差除去部
【図１】
2015179002.tif 000041
【図２】
2015179002.tif 000042
【図３】
2015179002.tif 000043
【図４】
2015179002.tif 000044
【図５】
2015179002.tif 000045
【図６】
2015179002.tif 000046