"移動体の動的な角度変化を検出する角速度センサと、移動体の傾斜角を検出する傾斜計と、角速度センサの出力と傾斜計の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部とを備える姿勢推定装置において、前記相補フィルタ部が、前記角速度センサの線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、角速度センサの出力について動特性を補償する第一補償部と、当該第一補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第一補償部からの出力に対し、相補性条件に基づく第一の周波数帯域でフィルタリングを施すハイパスフィルタである第一フィルタと、当該第一フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第一姿勢変換部と、前記傾斜計の線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、傾斜計の出力について動特性を補償する第二補償部と、当該第二補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第二補償部からの出力に対し、相補性条件に基づく第二の周波数帯域でフィルタリングを施すローパスフィルタである第二フィルタと、当該第二フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第二姿勢変換部と、前記第一姿勢変換部の出力と第二姿勢変換部の出力を相補的に加算合成し、オイラー角により表される姿勢の推定値を得る加算器とを備えることを特徴とする姿勢推定装置。前記請求項1に記載の姿勢推定装置において、前記角速度センサの伝達関数が、下式、ただし、D1s=1a11sa12s2a1nsnD2s=1a21sa22s2a2nsnD3s=1a31sa32s2a3nsnn:任意の自然数で表され、前記傾斜計が、鉛直軸でない二つの軸を測定軸とするものとされ、当該傾斜計の伝達関数が、下式、ただし、Ds=1a1sa2s2amsmm:任意の自然数で表されることを特徴とする姿勢推定装置。前記請求項2に記載の姿勢推定装置において、前記角速度センサの伝達関数が、下式、で表され、前記傾斜計の伝達関数が、下式、ただし、Ds=1a1sa2s2で表されることを特徴とする姿勢推定装置。従来における移動体の姿勢推定に係る相補フィルタは、前記各特許文献に示されるような構成となっているが、姿勢推定に用いるそれぞれのセンサの有効周波数領域は相補的であるとは限らないため、センサ出力を相補フィルタにそのまま入力して用いる場合、いずれのセンサにおいても情報の信頼性が低い周波数領域が存在すると、その周波数領域において推定精度が低下してしまうという問題があった。例えば、角速度センサと傾斜計を姿勢推定のためのセンサとして用いる場合、傾斜計の有効な周波数領域が低いため、角速度センサとの組み合わせでは推定精度の点で問題になる。こうした点から、相補フィルタにおいて、センサの動特性を補償し、有効周波数領域を拡大することが考えられる。例えば、相補フィルタに各センサの逆モデルを導入して、各センサの動特性を補償するという考え方があり、センサ出力を真のセンサ入力に近付けることができると共に、センサの有効周波数領域の拡大も図れると考えられている。しかしながら、センサ特性は非線形であり、逆モデルの伝達関数は不安定で非プロパーになりやすく、特に角速度センサの場合、その逆モデルは複雑な積分要素を含むものとなるなど、演算処理による姿勢推定を行うことは極めて困難であるという課題を有していた。本発明は前記課題を解消するためになされたもので、異なる複数のセンサの各出力に対し、各センサの線形動特性を同定した伝達関数に対する逆伝達関数を用いて補償すると共に、この逆伝達関数に対応して設計されたフィルタを施した上で、最終的に姿勢変換を行い姿勢を推定するようにして、相補フィルタにおける姿勢推定を適切且つ高精度に行える姿勢推定装置を提供することを目的とする。本発明に係る姿勢推定装置は、移動体の動的な角度変化を検出する角速度センサと、移動体の傾斜角を検出する傾斜計と、角速度センサの出力と傾斜計の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部とを備える姿勢推定装置において、前記相補フィルタ部が、前記角速度センサの線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、角速度センサの出力について動特性を補償する第一補償部と、当該第一補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第一補償部からの出力に対し、相補性条件に基づく第一の周波数帯域でフィルタリングを施すハイパスフィルタである第一フィルタと、当該第一フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第一姿勢変換部と、前記傾斜計の線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、傾斜計の出力について動特性を補償する第二補償部と、当該第二補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第二補償部からの出力に対し、相補性条件に基づく第二の周波数帯域でフィルタリングを施すローパスフィルタである第二フィルタと、当該第二フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第二姿勢変換部と、前記第一姿勢変換部の出力と第二姿勢変換部の出力を相補的に加算合成し、オイラー角により表される姿勢の推定値を得る加算器とを備えるものである。このように本発明によれば、角速度センサの出力と傾斜計の出力から、逆モデルを導入した相補フィルタを用いて姿勢推定を行うにあたり、センサの逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似により線形の特性として伝達関数を同定し、この伝達関数に対する逆伝達関数を用いて特性を補償すると共に、各センサの逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることにより、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積についてはプロパーな伝達関数とすることができ、確実に演算処理が可能となり、最終的に変換、相補的な加算を経て適切に姿勢推定が実行でき、複数のセンサを用いて必要な周波数領域で精度のよい姿勢推定を行える。特に、角速度センサの逆伝達関数に積分演算子が含まれる場合でも、ハイパスフィルタである第一フィルタにおける微分演算子と相殺されることとなり、結果的に積分を伴わずに演算を行うことができ、角速度センサの出力について、積分による定常誤差の拡大が生じず、センサにおけるドリフト等の影響を受けることなく、有効な推定が行え、推定精度を高められる。また、本発明に係る姿勢推定装置は必要に応じて、前記角速度センサの伝達関数が、下式、ただし、D1s=1a11sa12s2a1nsnD2s=1a21sa22s2a2nsnD3s=1a31sa32s2a3nsnn:任意の自然数で表され、前記傾斜計が、鉛直軸でない二つの軸を測定軸とするものとされ、当該傾斜計の伝達関数が、下式、ただし、Ds=1a1sa2s2amsmm:任意の自然数で表されるものである。このように本発明によれば、角速度センサの動特性の伝達関数を、その逆伝達関数と共に安定なものとし、且つ、傾斜計の動特性の伝達関数を、その逆伝達関数と共に安定なものとして、各センサの逆伝達関数とフィルタの伝達関数との積を安定且つプロパーな伝達関数とすることにより、センサ出力の補償とフィルタリングに係る演算処理が無理なく速やかに実行できることとなり、相補フィルタによる姿勢推定を高い精度で適切に進められる。また、本発明に係る姿勢推定装置は必要に応じて、前記角速度センサの伝達関数が、下式、で表され、前記傾斜計の伝達関数が、下式、ただし、Ds=1a1sa2s2で表されるものである。このように本発明によれば、角速度センサの動特性の伝達関数における各要素の分母多項式を一次式とすると共に、傾斜計の動特性の伝達関数における分母多項式を二次式として、姿勢推定に対応した適切な次数とすることにより、各センサの動特性の近似を十分高い精度としつつ、伝達関数及びその逆伝達関数を簡略なものとして、センサ出力の補償とフィルタリングに係る演算処理を高速化でき、相補フィルタによる姿勢推定を、十分な精度を確保しながらより高速に実行でき、推定値を姿勢制御等に適切に活用可能となる。以下、本発明の一実施形態に係る姿勢推定装置を前記図1及び図2に基づいて説明する。前記各図において、本実施形態に係る姿勢推定装置1は、移動体の動的な角度変化を検出する角速度センサ10と、移動体の傾斜角を検出する傾斜計20と、角速度センサ10の出力と傾斜計20の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部30とを備える構成である。前記角速度センサ10は、レートジャイロ等の公知のセンサであり、詳細な説明を省略する。相補フィルタ設計にあたっては、角速度センサ10の伝達関数をあらかじめ同定しておく必要がある。この角速度センサ10の動特性に係る伝達関数の同定は、公知の手法、例えば、座標系の各軸とセンサ出力の組合せごとに周波数応答を測定する、すなわち、角速度センサに対する姿勢変動の正弦波入力に基づいたセンサ出力波形を、センサを適用する周波数領域で複数の代表的な周波数について求め、得られた各出力波形から位相遅れ及びゲインを同定して、各値を周波数領域においてプロットし、さらに、各周波数における位相遅れやゲインの値の分布から、最小二乗法による近似で伝達関数を同定する、といった過程で行われる。前記傾斜計20は、加速度計や液面傾斜計などの公知のセンサであり、詳細な説明を省略する。相補フィルタ設計にあたっては、傾斜計20についても、その伝達関数をあらかじめ同定しておく必要がある。この傾斜計20の動特性に係る伝達関数の同定は、公知の手法、例えば、傾斜計に対する姿勢変動の正弦波入力に基づいた傾斜計出力波形を、傾斜計を適用する周波数領域で複数の代表的な周波数について求め、得られた各出力波形から位相遅れ及びゲインを同定して、各値を周波数領域においてプロットし、さらに、各周波数における位相遅れやゲインの値の分布から、最小二乗法による近似で伝達関数を同定する、といった過程で行われる。前記相補フィルタ部30は、周波数領域で複数のセンサ出力を相補的に組合わせて姿勢推定を実行するものである。この相補フィルタ部30は、詳細には、前記角速度センサ10の動特性の逆伝達関数に基づいて、角速度センサ10の動特性を補償する第一補償部31と、この第一補償部31からの出力を相補性条件に基づく第一の周波数帯域でフィルタリングする第一フィルタ32と、この第一フィルタ32の出力をオイラー角へ変換する第一姿勢変換部33と、前記傾斜計20の動特性の逆伝達関数に基づいて、傾斜計20の動特性を補償する第二補償部34と、この第二補償部34からの出力を相補性条件に基づく第二の周波数帯域でフィルタリングする第二フィルタ35と、この第二フィルタ35の出力をオイラー角へ変換する第二姿勢変換部36と、前記第一姿勢変換部33の出力と第二姿勢変換部36の出力を加算する加算器37とを備える構成である。前記第一補償部31は、あらかじめ角速度センサ10の線形に近似された動特性の伝達関数を同定することで得られる逆伝達関数に基づいて、角速度センサ10の動特性を補償し、角速度センサ10の有効周波数領域を拡大するものである。前記第一フィルタ32は、第一補償部31における逆伝達関数の次数より大きいフィルタ次数とされて、第一補償部31からの出力を相補性条件に基づく第一の周波数帯域でフィルタリングするハイパスフィルタである。この第一フィルタ32のフィルタ特性は、第一補償部31での補償後の角速度センサ出力が信頼できる周波数領域に合わせて設計される。前記第一姿勢変換部33は、第一フィルタ32の出力を入力され、移動体の姿勢を表現するオイラー角の推定値へ変換して、前記加算器37へ出力するものである。前記第二補償部34は、あらかじめ傾斜計20の線形に近似された動特性の伝達関数を同定することで得られる逆伝達関数に基づいて、傾斜計20の動特性を補償し、傾斜計20の有効周波数領域を拡大するものである。前記第二フィルタ35は、第二補償部34における逆伝達関数の次数より大きいフィルタ次数とされて、第二補償部34からの出力を相補性条件に基づく第二の周波数帯域でフィルタリングするローパスフィルタである。この第二フィルタ35のフィルタ特性は、第二補償部34での補償後の傾斜計出力が信頼できる周波数領域に合わせて設計されるが、前記第一フィルタ32の伝達関数をF1s、第二フィルタ35の伝達関数をF2sとすると、相補性条件から、F1sF2s=1となるよう設計される必要がある。前記第二姿勢変換部36は、第二フィルタ35の出力を入力され、移動体の姿勢を表現するオイラー角の推定値へ変換して、前記加算器37へ出力するものである。ここで、前記第一姿勢変換部33と第二姿勢変換部36で利用するオイラー角による姿勢表現について説明する。オイラー角で表される、センサユニットが取付けられた移動体の姿勢ηは、鉛直軸以外の軸まわりの回転角β、γy軸まわりβ回転、x軸まわりγ回転を用いて、η=[β,γ]Tと表され、また、回転行列を用いると、姿勢wRbは、と表される。この場合、センサユニット座標系における角速度センサで出力される角速度bωは、ワールド座標系における角速度wωを用いて、下式1のように表せる。式1より、角速度bωは、オイラー角の微分値を用いて表すと、下式2のようになる。続いて、傾斜計の出力とオイラー角による姿勢wRbの関係を球面三角法より導出する。はじめに、y軸を測定軸として持つ傾斜計の出力θ1を、図2における、x軸とy軸がなす平面とwRbxとwRbzがなす平面の交線AOとwRbxがなす角度∠AOC=θ1と仮定する。このとき、2つの球面三角形△ABCと△ADEを考える。ここで、x、y、zはワールド座標系におけるx軸、y軸、z軸を表す単位ベクトルとする。球面三角形の角度A、B、Cと、それぞれの頂点と中心を結ぶ線分がなす角度a、b、cの間に次の関係が成り立つ。sinA/sina=sinB/sinb=sinC/sinc3式3から球面三角形△ADEにおいて次の関係式4が導かれる。同様に、球面三角形△ABCから、下式5が導かれる。球面三角形においても対頂角は等しいので、式4と式5を整理すると次の関係式6を導くことができる。同様に、x軸を測定軸に持つ傾斜計の出力θ2では、次の関係式7が成り立つ。これらから、入力を、求めたい軸まわりの回転角に変換するための関数Hは、それぞれ以下のように表される。角速度センサ出力側については、入力Y1=[Y11Y12]Tに対して、となる。また、傾斜計出力側については、入力Y2=[Y21Y22]Tに対して、となる。相補フィルタは、それぞれのセンサ出力を相補的に重み付けすることで推定を行うフィルタであり、線形な相補フィルタは、推定値Ys、i番目のセンサの測定値Xis、i番目のセンサ出力にかけるフィルタFisを用いて、下式10で表される。これより、前記相補フィルタ部30の、センサの逆モデルを用いて設計される相補フィルタで、姿勢の推定値ηestを表した場合、センサ出力を逆伝達関数に通して推定した入力を、求めたい軸まわりの回転角に変換するための関数Hを用いることで、となる。ここで、は同定したセンサの逆伝達関数である。また、相補フィルタ部30は、図1に示すようなブロック線図で表すこともできる。なお、第一フィルタ32を、第一姿勢変換部33より前段に配置し、また、第二フィルタ35を、第二姿勢変換部36より前段に配置しているが、その根拠について説明する。姿勢をオイラー角で表す場合、従来手法では、角速度センサの出力をオイラー角の微分値に変換し、それらを積分して、オイラー角の推定値を求めた後に、フィルタをかけて出力する。また、傾斜計の出力もオイラー角に変換した後に、フィルタをかけて出力していた。逆モデルを導入した相補フィルタで姿勢推定を行う場合、姿勢の推定値ηestは、と表される。角速度センサの場合、出力の動特性を補償した角速度センサ入力ωを、式2における行列Pの擬似逆行列Pを用いて、下式13によりオイラー角の微分値へ変換する。また、傾斜計では、式6、7の関係を用いて、動特性を補償したセンサ入力θ=[θ1θ2]Tを、下式14によりオイラー角へ変換する。角速度センサ出力側では、入力を微分値に変換し、これを積分して、オイラー角の推定値を求めた後にフィルタをかけるようになっているが、前記式13より、変換は行列の乗算であることから、フィルタと変換部の順序を入換えることができる。変換前にフィルタを施すようにした場合、角速度センサの逆伝達関数が積分演算子1/sを含んでいるのに対応して、フィルタが微分演算子sを含むように設計されていれば、積分演算子1/sと微分演算子sが相殺されることとなり、ドリフトの影響などを回避することが可能となる。一方、傾斜計出力側では、入力をオイラー角へ変換した後にフィルタをかけるようになっているが、この変換後にフィルタを施す場合の伝達関数の積と、入力にフィルタを施した後にオイラー角へ変換する場合の伝達関数の積とは、そのテイラー展開を求めると、2次のオーダまで一致することとなり、フィルタと変換部の順序を入換えても、精度の点で十分許容できる。これらにより、姿勢変換の前に周波数フィルタを施しても、推定値に大きな違いは生じないといえ、相補フィルタ部30において第一フィルタ32を第一姿勢変換部33より前段に配置し、また、第二フィルタ35を、第二姿勢変換部36より前段に配置した構成を採用している。さらに、角速度センサ10と傾斜計20の、それぞれの動特性の伝達関数同定、並びにフィルタ設計についてより詳細に説明する。各センサの伝達関数Gisは、直交する3軸それぞれの軸まわりの回転に対するセンサの周波数応答を元に、各要素が安定且つプロパーとなる行列として同定する。本発明では、異なる軸間の干渉、すなわち各軸まわりの回転における伝達関数は近似的に線形分離可能であると仮定している。また、角速度センサには微分器があり、傾斜計には遅れ要素があると仮定して、軸とセンサ出力の組合せごとに周波数応答を測定し、線形の近似により伝達関数の同定を行う。角速度センサの動特性の伝達関数は、ただし、D1s=1a11sa12s2a1nsnD2s=1a21sa22s2a2nsnD3s=1a31sa32s2a3nsnn:任意の自然数で表される。伝達関数の各要素が一次式の場合、特に、で表される。一方、傾斜計の動特性の伝達関数は、ただし、Ds=1a1sa2s2で表される。なお、伝達関数の各要素の次数は、取扱う事象の複雑さにより決るものであり、姿勢推定の場合、角速度センサのものは一次、傾斜計のものは二次として問題はない。また、フィルタについては、逆伝達関数の次数よりフィルタ次数が大きくなるように設計する。具体的には、例えば、2軸傾斜計と1軸角速度センサを用いる場合、2軸傾斜計の逆伝達関数行列G12の各要素が2次進み要素となることから、逆伝達関数と第二フィルタの伝達関数の積からなる伝達関数F2sG12sをプロパーとするために、フィルタの伝達関数Fiを2次ローパスフィルタとして設計する。また、角速度センサの逆伝達関数G11sには積分演算子1/sが含まれるものの、ハイパスフィルタF1sに微分演算子sを含むようにすれば、積分演算子1/sと微分演算子sが相殺されるため、結果的に積分演算を伴わない。次に、本実施形態に係る姿勢推定装置による姿勢推定の過程について説明する。角速度センサ10から角速度bωが出力され、傾斜計20から各軸についての傾斜角θ=[θ1θ2]Tが出力されると、角速度センサ出力側については、まず第一補償部31で出力の動特性が補償され、次いで第一フィルタ32でハイパスフィルタが施され、出力Y1=[Y11Y12]Tが得られる。これが第一姿勢変換部33に入力され、この入力Y1=[Y11Y12]Tを前記式8によりオイラー角に変換する。また、一方、傾斜計出力側では、第二補償部34で出力の動特性が補償され、次いで第二フィルタ35でローパスフィルタが施され、出力Y2=[Y21Y22]Tが得られる。これが第二姿勢変換部36に入力され、この入力Y2=[Y21Y22]Tを前記式9によりオイラー角に変換する。第一姿勢変換部33で変換された出力と第二姿勢変換部36で変換された出力が加算器37で相補的に合成されると、姿勢の推定値ηest=[βestγest]Tが求められる。このように、本実施形態に係る姿勢推定装置は、角速度センサ10の出力と傾斜計20の出力から、逆モデルを導入した相補フィルタを用いて姿勢推定を行うにあたり、センサの逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似により線形の特性として伝達関数を同定し、この伝達関数に対する逆伝達関数を用いて特性を補償すると共に、各センサの逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることから、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積について確実に数値演算が可能となり、最終的に変換、相補的な加算を経て適切に姿勢推定が実行でき、複数のセンサを用いて必要な周波数領域で精度のよい姿勢推定を行える。特に、角速度センサの逆伝達関数に含まれる積分演算子が、第一フィルタ32における微分演算子と相殺されて、結果的に積分を伴わずに演算を行うことができ、角速度センサの出力について、積分による定常誤差の拡大が生じず、センサにおけるドリフト等の影響を受けることなく、有効な推定が行え、推定精度を高められる。本発明に係る姿勢推定装置を用いて、角速度センサと傾斜計の出力から姿勢推定を行い、得られた結果について、比較例としての逆伝達関数を用いない相補フィルタを適用した場合等の姿勢推定結果と比較評価した。姿勢推定の対象には2軸実験機を用い、SSSJ製1軸角速度センサCRS07-11S、USDigital製2軸傾斜計X3Qをそれぞれ取付けた。2軸実験機の1番目の関節はy軸周りに角度β、2番目の関節はx軸周りに角度γ、それぞれ回転する関節であるとする。測定に先立ち、角速度センサ、傾斜計についてセンサ動特性伝達関数の同定を行った。このセンサ動特性の同定では、異なる軸間の干渉は近似的に線形分離できると仮定し、軸とセンサ出力の組合せごとに正弦波を入力して周波数応答を測定し、最小二乗法により伝達関数を同定する。実際の周波数応答の測定から、それぞれのセンサの伝達関数を、角速度センサは下式15、傾斜計は下式16、17のように同定した。この角速度センサと傾斜計の各伝達関数を用いて、2軸相補フィルタを設計した。実施例1として、この相補フィルタを用いると、姿勢の推定値ηestは下式18で表される。ここで、X1は角速度センサの出力、X2は2軸傾斜角センサの出力である。また、前記式8、式9で示したH1、H2をオイラー角の推定に用いている。なお、式8、9に用いている推定値は、実際は離散時間で使用するために、1ステップ前の推定値を用いている。この相補フィルタでは、2軸傾斜計の逆伝達関数行列G12の各要素が2次進み要素となることから、逆伝達関数と第二フィルタの伝達関数の積からなる伝達関数F2G12をプロパーとするために、第二フィルタの伝達関数Fisを2次ローパスフィルタとして設計した。また、角速度センサの逆伝達関数G11には積分器が含まれるが、ハイパスフィルタである第一フィルタF1の持つ微分器と相殺されるため、結果的に積分演算を伴わないこととなる。また、比較例1として、逆伝達関数を用いない2軸相補フィルタを設計し、姿勢推定に用いるようにした。さらに、比較例2として、角速度センサのみを用いて姿勢推定を行うようにした。この本発明に係る姿勢推定装置を用いる場合実施例1と各比較例の場合について、実験機の2軸に正弦波入力をβ=57.3sint[deg]、γ=25.7sin3t[deg]で与え、姿勢推定を行った。なお、いずれの場合も、第一フィルタ、第二フィルタの伝達関数Fiは同じものを用いている。実施例1における逆伝達関数を用いて設計した2軸相補フィルタと、逆伝達関数を用いない比較例1の相補フィルタでのβ、γの推定結果を、真値と共に図3に示す。この図3中、実線は実施例1による推定値、濃灰色破線は比較例1による推定値、薄灰色点線は真の入力値をそれぞれ示している。また、角速度センサのみを用いた比較例2の推定結果を、真値と共に図4に示す。この図4中、実線は比較例2による推定値、薄灰色点線は真の入力値をそれぞれ示している。図3に示すように、実施例1の逆伝達関数を用いた相補フィルタは、逆伝達関数を用いない比較例1の場合に比べ、遅れが補償され、比較的よい推定が行えていることがわかる。また、図4に示すように、角速度センサのみを用いた比較例2の推定結果によれば、位相を補償することはできているものの、ドリフトが発生しており、時間経過とともに真値からの誤差が増大している。これに対し、逆伝達関数を用いた相補フィルタは、角速度センサのみの推定で発生していたドリフトの補償も行えている。これらから、本発明の姿勢推定装置における相補フィルタは2軸の推定において有効であるといえる。次に、実験機の複雑な動きに対して有効に推定が行われるかどうかについて評価した。複雑な動きとして、実験機に対し、1軸目の入力q1、2軸目の入力q2をそれぞれ与え、姿勢推定を行った。q1を下式19、q2を下式20に示す。また、サンプリング周期1[kHz]、サンプリング時間20[s]としている。q1、q2において、パラメータa1i、b1i、a2i、b2iは、振幅の合計が40[deg]となるように、ランダムに入力した。また、測定周波数f1i、f2iをばらけさせるためにi番目の整数部分はi-1として決定し、小数部分は0.00から0.99[Hz]までをランダムに入力している表1参照。前記同様、姿勢推定の対象には2軸実験機を用い、SSSJ製1軸角速度センサCRS07-11S、USDigital製2軸傾斜計X3M-2をそれぞれ取付けた。2軸実験機の1番目の関節はy軸周りに角度β、2番目の関節はx軸周りに角度γ、それぞれ回転する関節であるとする。測定に先立ち、角速度センサ、傾斜計についてセンサ動特性伝達関数の同定を行った。正弦波入力により、各軸について、角速度センサは図5ないし図9、傾斜計は図10、図11のような周波数応答が得られた。なお、前記図5ないし図11の各図中で、灰色の十字の点は周波数応答の測定値、連続する線は最小二乗法による近似で求めた伝達関数を、それぞれ示している。これらより、それぞれのセンサの伝達関数を、角速度センサは下式21、傾斜計は下式22のように同定した。この角速度センサと傾斜計の各伝達関数を用いて、2軸相補フィルタを設計した。フィルタの遮断周波数は、傾斜計の周波数の半分になるように設定した。実施例2として、この相補フィルタを用いると、姿勢の推定値ηestは下式23で表される。また、比較例3として、逆伝達関数を用いない2軸相補フィルタを設計し、姿勢推定に用いるようにした。加えて、比較例4として、傾斜計の出力信号をそのまま用いて、また、比較例5として、傾斜計の出力信号をローパスフィルタにかけたものを用いて、また、比較例6として、角速度センサの出力信号を球面積分したものを用いて、また、比較例7として、角速度センサの出力信号を球面積分しさらにハイパスフィルタにかけたものを用いて、それぞれ姿勢を求めるようにした。なお、比較例5のローパスフィルタ、比較例7のハイパスフィルタは下式24、25に示されるものとなっている。比較例5のローパスフィルタの遮断周波数は5[Hz]程度の値とし、また、比較例7のハイパスフィルタの遮断周波数は相補フィルタの場合と同様の値とする。前記q1、q2の各入力を与える複雑な動きに対して、本発明に係る姿勢推定装置を用いる場合実施例2と各比較例の場合で、姿勢推定を行った結果を示す。実施例2における逆伝達関数を用いて設計した2軸相補フィルタと、逆伝達関数を用いない比較例3の相補フィルタの推定結果を、真値と共に図12に示す。この図12中、実線は実施例2による推定値、濃灰色破線は比較例3による推定値、薄灰色点線は真の入力値をそれぞれ示している。また、推定結果における5秒から10秒の範囲の拡大図を図13に、各結果の推定誤差を図14にそれぞれ示す。図13中の各線が示すものは図12の場合と同様である。一方、図14中、実線は実施例2の場合の誤差角度、破線は比較例3の場合の誤差角度をそれぞれ示している。さらに、傾斜計の出力信号をそのまま用いた比較例4と、傾斜計の出力信号をローパスフィルタにかけたものを用いた比較例5の各処理結果を、真値と共に図15に示すと共に、各結果の誤差を図16に示す。図15中、濃灰色破線は比較例4による出力値、実線は比較例5による出力値、薄灰色点線は真の入力値をそれぞれ示している。そして、図16中、破線は比較例4の場合の誤差角度、実線は比較例5の場合の誤差角度をそれぞれ示している。また、角速度センサの出力信号を球面積分したものを用いた比較例6と、角速度センサの出力信号を球面積分しさらにハイパスフィルタにかけたものを用いた比較例7の各処理結果を、真値と共に図17に示すと共に、各結果の誤差を図18に示す。図17中、濃灰色破線は比較例6による出力値、実線は比較例7による出力値、薄灰色点線は真の入力値をそれぞれ示している。そして、図18中、破線は比較例6の場合の誤差角度、実線は比較例7の場合の誤差角度をそれぞれ示している。図12に示すように、逆伝達関数を用いた相補フィルタは、逆伝達関数を用いない比較例3の場合に比べ、真値に近く、より精度の高い推定が行えていることがわかる。また、図15に示すように、比較例4の傾斜計のみを用いた結果や、比較例5の傾斜計出力信号をローパスフィルタにかけた結果によれば、遅れが発生しており、ローパスフィルタによってさらに遅れが生じ、精度も低下していることがわかる。また、図17に示すように、比較例6の角速度センサ出力信号を球面積分した場合の結果によれば、球面積分により早い時間ではよい精度が得られているものの、時間の経過と共に真値からの誤差が大きくなっている。加えて、比較例7の球面積分し更にハイパスフィルタにかけた場合の結果によれば、ドリフトの補償は行えているものの、低周波数成分を除去していることで、精度が大きく悪化している。ここで、前記q1、q2の各入力に対する前記実施例2、及び比較例3ないし7の各結果における最大誤差を比較したものを図19にそれぞれ示す。この図19中では、a比較例6、b比較例7、c比較例4、d比較例5、e比較例3、f実施例2、におけるそれぞれの最大誤差角度を示している。これらによれば、実施例2における誤差は、各比較例のそれに比べて小さくなっている。これらの結果から、本発明の姿勢推定装置における相補フィルタは、複雑な動きに対しても有効に推定が行えることがわかる。"