JP5861235B2 - 物体の向きをより正確に推定する方法および前記方法を実装した姿勢制御システム - Google Patents

Description

本発明は、固有加速度および磁気的外乱の有無によらず、空間における物体の向きを推定する方法、および前記方法を実装することにより向きの推定を可能にする適当な装置に関する。

一般に、向きを把握するには、モーションキャプチャ装置と呼ばれる機構の一部をなし、姿勢制御装置とも呼ばれる多くのセンサを用いる。

ＭＥＭＳ（微小電子機械システム）センサを用いて上記制御装置を構築することができ、前記ＭＥＭＳセンサは小型且つ安価である利点を有する。そのようなＭＥＭＳセンサを用いることにより、特に在宅高齢者の監視や機能リハビリを行う生医学的分野、スポーツ運動の分析を行うスポーツ分野、自動車、ロボット工学、仮想現実および３次元アニメーション分野、更により一般的に運動を判定または観測する必要がある様々な応用分野で姿勢制御装置の利用を検討することが可能になる。

しかしこれらのＭＥＭＳセンサには、（例えばナビゲーション分野で利用される）非ＭＥＭＳセンサに比べて相対的にノイズやバイアスの影響を受けやすいという短所がある。

更に、加速度計と磁力計の両方を用いる姿勢制御装置の存在が知られており、これらにより３自由度を有する運動、すなわち地球重力場および地球磁場に比べて固有加速度および磁気的外乱が各々無視できる運動を再構成できる。しかし、この仮定が成り立たない、すなわち固有加速度または磁気的外乱を無視できない場合、運動は６〜９自由度を示す。従って、加速度計と磁力計だけを使用する姿勢制御装置を用いて、動いている物体の向きを推定することは不可能である。このため、多岐にわたるモーションキャプチャの用途に対応すべくこれらの制約を克服することが必須である。

従って、追加的なセンサ、特にレートジャイロ、加速度計、および磁力計を組み合わせて用いることが検討されてきた。これらのセンサから得られる測定値は、二つの部分からなる。すなわち動いている物体の向きに直接関係する情報部分、および使用するセンサに依存する性質を有する外乱部分である。第１に、これらは加速度計から供給された測定値の場合は固有加速度、磁力計から得られた測定値の場合は磁気的外乱、およびレートジャイロの場合はバイアスである。これらの外乱により向きの推定が不正確になる恐れがある。

現在、加速度計、磁力計、およびレートジャイロから供給された測定値から、物体の向きを推定する多くの方法がある。

１個以上の最適化基準を実装する所謂最適化方法があるが、これらは計算時間に関して比較的コストが高い。更に、問題が複雑になった場合、最適化基準を定義することが困難である。

また、ニューラルネットワークを実装する方法もあるが、後者は正確な推定を得るために、特にデータベースの大きさと計算時間に関して学習フェーズを必要とする。

更に、最適化方法およびニューラルネットワークを実装する方法の場合、時間経過に伴う状態遷移の概念を考慮することが困難であるため、これらの方法は脆弱である。

観測装置を実装する方法もあり、これらは上述の方法とは異なり、２個のソースからの情報すなわち、センサから供給された測定値から得られた情報およびトレンドモデルから得られた情報を、リアルタイム実行に匹敵する計算時間を維持しつつ統合することができる。

観測装置を用いる公知の方法は、主としてカルマンフィルタの利用に依存する。この技術の利点は、センサから供給された測定値から得られた情報の品質およびモデルの品質を考慮しつつ、データの統合を行うことができる点である。

当業者によく知られた多くの種類のカルマンフィルタがある。

・拡張カルマンフィルタ（またはＥＫＦ）。これは高速且つ実装が容易であり、モーションキャプチャへの応用の一つが特に文献「Ｑｕａｔｅｒｎｉｏｎ−ｂａｓｅｄ ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ ｆｏｒ ｄｅｔｅｒｍｉｎａｔｉｏｎ ｉｎ ｔｈｅ ｏｒｉｅｎｔａｔｉｏｎ ｂｙ ｉｎｅｒｔｉａｌ ａｎｄ ｍａｇｎｅｔｉｃ ｓｅｎｓｉｎｇ」，ＳＡＢＡＴＩＮＩ Ａ．Ｍ．，ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｂｉｏｍｅｄｉｃａｌ Ｅｎｇｉｎｅｅｒｉｎｇ，２００６，５３（７）に記述されている。
・強非線形問題専用のＵＫＦカルマンフィルタ（無香料カルマンフィルタ（Ｕｎｓｃｅｎｔｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ））。時折、モーションキャプチャの関連で遭遇する問題は弱非線形であるため、向きの推定にはあまり適していない。更に、ＥＫＦフィルタに比べて計算コストが大幅に増大する。従って、ＥＫＦフィルタほど関心が払われない。例えば、文献"Ｐｏｒｔａｂｌｅ ｏｒｉｅｎｔａｔｉｏｎ ｅｓｔｉｍａｔｉｏｎ ｄｅｖｉｃｅ ｂａｓｅｄ ｏｎ ａｃｃｅｌｅｒｏｍｅｔｅｒｓ，ｍａｇｎｅｔｏｍｅｔｅｒｓ ａｎｄ ｇｙｒｏｓｃｏｐｅ ｓｅｎｓｏｒｓ ｆｏｒ ｓｅｎｓｏｒ ｎｅｔｗｏｒｋ" ｂｙ ＨＡＲＡＤＡ Ｔ．，ＵＣＨＩＮＯ Ｈ．，ＭＯＲＩ Ｔ．，ＳＡＴＯ Ｔ．，ＩＥＥＥ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｍｕｌｔｉｓｅｎｓｏｒ Ｆｕｓｉｏｎ ａｎｄ Ｉｎｔｅｇｒａｔｉｏｎ ｆｏｒ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ，２００３に、加速度計と磁力計により絶対方位が統合され、且つ回転速度がＵＫＦフィルタにより統合される携帯用方位推定装置を記述している。
・補完的カルマンフィルタ。この場合、状態自体ではなく状態における誤差を推定することが目的であって、実装が極めて複雑である。

フィルタの選択に加え、フィルタに渡される測定値の品質、特にそれらの値の信頼性が極めて重要である。

実際には、上述のように測定値は、動いている物体の向きに直接関係する情報部分、用いられるセンサに依存する性質を有する外乱部分を含んでいる。これらは加速度計から供給された測定値の場合は固有加速度、磁力計から得られた測定値の場合は磁気的外乱、およびレートジャイロの場合はバイアスである。また、測定ノイズを考慮する必要があるが、前記ノイズは従来フィルタ内で処理されていた。

現在、外乱を処理する多くの方法がある。その一つは、文献「Ｄｅｓｉｇｎ，ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ａｎｄ ｅｘｐｅｒｉｍｅｎｔａｌ ｒｅｓｕｌｔｓ ｏｆ ａ ｑｕａｔｅｒｎｉｏｎ−ｂａｓｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ ｆｏｒ ｈｕｍａｎ ｂｏｄｙ ｍｏｔｉｏｎ ｔｒａｃｋｉｎｇ」，ＹＵＮ Ｘ．，ＢＡＣＨＭＡＮＮ Ｅ．Ｒ．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ Ｏｎ Ｒｏｂｏｔｉｃｓ，２００６，２２（６）および「Ａｐｐｌｉｃａｔｉｏｎ ｏｆ ＭＩＭＵ／Ｍａｇｎｅｔｏｍｅｔｅｒ ｉｎｔｅｇｒａｔｅｄ ｓｙｓｔｅｍ ｏｎ ｔｈｅ ａｔｔｉｔｕｄｅ ｄｅｔｅｒｍｉｎａｔｉｏｎ ｏｆ ｍｉｃｒｏ ｓａｔｅｌｌｉｔｅ」，ＳＵ Ｋ．，ＲＥＮ Ｄ．Ｈ．，ＹＯＵ Ｚ．，ＺＨＯＵ Ｑ．，Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｍｅｃｈａｔｒｏｎｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ上，Ａｕｇｕｓｔ ２００４年，Ｃｈｅｎｇｄｕ，Ｃｈｉｎａに記述されているように外乱が無視できると考えてセンサから得られた測定値に対してフィルタを提供するものである。従って、実際にセンサの１個に外乱が生じた場合には、フィルタから得られた測定値には誤差があるが、フィルタはそれを正確なものであると見なす。そのため向きの推定は不正確になる。従って、自由度数によらず所望の精度で推定値を得るために外乱を無視することはできない。このように外乱を受けた測定値が観測装置に供給されるため、性能レベルは大幅に低下する。

推定方法は従って、例えば文献「Ｐｏｒｔａｂｌｅ ｏｒｉｅｎｔａｔｉｏｎ ｅｓｔｉｍａｔｉｏｎ ｄｅｖｉｃｅ ｂａｓｅｄ ｏｎ ａｃｃｅｌｅｒｏｍｅｔｅｒｓ，ｍａｇｎｅｔｏｍｅｔｅｒｓ ａｎｄ ｇｙｒｏｓｃｏｐｅ ｓｅｎｓｏｒｓ ｆｏｒ ｓｅｎｓｏｒ ｎｅｔｗｏｒｋ」，ＨＡＲＡＤＡ Ｔ．、ＵＣＨＩＮＯ Ｈ．、ＭＯＲＩ Ｔ．，ＩＥＥＥ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｍｕｌｔｉｓｅｎｓｏｒ Ｆｕｓｉｏｎ ａｎｄ Ｉｎｔｅｇｒａｔｉｏｎ ｆｏｒ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ，２００３に記述されているように外乱の存在を検知し、且つこれらの測定値の信頼性を更新することにより特定の測定値における偶発的な欠陥を考慮できるようにする。この方法は、測定値における外乱を検知する追加的なステップを提供する。外乱が検知された場合、対応する測定値に対する信用度が最小化される。

従って、外乱を含む測定値が提供する情報は、向きを推定する際に考慮されない。このため向きの推定は他のセンサから供給された測定値だけに依存する。ここで、多くのセンサからの測定値が同時に外乱を示す場合、観測装置はもはや正しい向きの推定値を提供するために十分な情報を有していない。

最後に、文献「Ｉｎｅｒｔｉａｌ ａｎｄ ｍａｇｎｅｔｉｃ ｓｅｎｓｉｎｇ ｏｆ ｈｕｍａｎ ｍｏｔｉｏｎ」，ＲＯＥＴＥＮＢＥＲＧ Ｄ．，ｄｏｃｔｏｒａｌ ｔｈｅｓｉｓ，Ｔｗｅｎｔｅ ｕｎｉｖｅｒｓｉｔｙ，Ｎｅｔｈｅｒｌａｎｄｓ，２００６、および「Ｍｅａｓｕｒｉｎｇ ｏｒｉｅｎｔａｔｉｏｎ ｏｆ ｈｕｍａｎ ｂｏｄｙ ｓｅｇｍｅｎｔ ｕｓｉｎｇ ｍｉｎｉａｔｕｒｅ ｇｙｒｏｓｃｏｐｅｓ ａｎｄ ａｃｃｅｌｅｒｏｍｅｔｅｒｓ」，ＰｈＤ Ｔｈｅｓｉｓ，Ｉｎｅｒｔｉａｌ ｓｅｎｓｉｎｇ ｏｆ ｈｕｍａｎ ｍｏｖｅｍｅｎｔ，ＬＵＩＮＧＥ Ｈ．Ｊ．ｎ ２００２ｂに記述さた方法では、観測装置を用いて外乱の存在を検知および推定している。このため、状態ベクトルが拡張され、より現実に近づいた測定モデル内で外乱が生じる。この技術は原理的には、６〜９自由度を有する運動の場合に適しているように見える。しかし、外乱と向きを組み合わせて推定することは、観測可能性が欠如していることに起因して困難である。また、多数のパラメータを設定する必要があるため、実装の複雑さが増大する。

従って本発明の一つの目的は、固有加速度および磁気的外乱の有無によらず、正確に向きを推定し、既存の方法に比べて簡素化された方法でこれを行う向き推定方法を提供することである。

上述の目的は、加速、磁場、および回転速度の３空間軸に沿った測定値に基づいて向きを推定する方法により実現され、本方法は
・外乱の存在を検知して、無外乱測定値を推定すべくこれらの測定値を前処理するステップと、
・前処理ステップから得られた測定値に基づいて向きを推定するステップと
を含んでいる。

本方法は外乱を無視しない、すなわち推定が誤っていないことを意味する。本方法は常に外乱を推定する。外乱が存在する場合、他の推定方法の場合のように付随する測定値または測定値群を拒絶しない。更に、本方法は外乱を状態ベクトルまたは測定モデルに組み込まないため、モデルが簡素化され、推定が不可能になる状況に陥らない。

従って、２段階の連続するステップで、向きを推定し、恐らくは外乱を推定する。観測装置には従って、加速度計、磁力計、およびレートジャイロから、向きの推定に理想的な条件になるべく近い、すなわち固有加速度、磁気的外乱、およびバイアスが各々存在しない状態での測定値が提供される。

このため、測定値の前処理を実行するための追加情報として、先行する時点で推定された向きを用いる。

本発明による推定方法は従って、センサ測定値から、関心対象である運動によらず、物体の向きを最適な仕方で抽出することを可能にする。本方法は更に、実装が簡単で、ごく少数の設定パラメータだけを含んでいる。

観測装置は拡張カルマンフィルタであることに利点がある。

外乱、特に固有加速度を推定する方法を提供し、積分および二重積分により物体の速度および位置の各々に戻ることが可能になる。

本発明の主題は従って、時刻ｋにおける空間内での物体の向きを、３空間軸に沿った前記物体の全加速度、磁場、および回転速度測定値を用いて推定する方法であって、以下のステップを含んでいる。
Ａ：時点ｋにおける前記測定値を前処理して前記測定値における外乱、すなわち物体の固有加速度、地球磁場に加えられた磁場、および回転速度測定値におけるバイアスを含む群からの外乱の存在を検知して時刻ｋにおける無外乱測定値を推定し、
Ｂ：ステップＡから得られた、時刻ｋで推定された無外乱測定値に基づいて、観測装置により時刻ｋにおける向きを推定する。

ステップＡは以下のステップを含んでいることに利点がある。
Ａ１：回転速度測定値の前処理、
Ａ２：全加速度および磁場の前記測定値内での時刻ｋにおける外乱の有無の検知、
Ａ３：時刻ｋにおいて外乱が存在しない場合、時刻ｋで推定された無外乱測定値は時刻ｋにおける測定値に等しく、外乱が存在する場合、時刻ｋで推定された無外乱測定値は、先行する時点ｋ−１で推定された向きに基づいて計算される。

ステップＡ１は、回転速度測定値から、事前の初期化ステップを実行する間に決定された平均バイアスを減算するものである。この平均バイアスは、回転速度測定値を与える手段を所与の期間停止して、各軸における回転速度測定値の平均を計算することにより得られる。人が装着した姿勢制御装置の場合、この停止は、当人の避け難い震えを除くために当人から制御装置を取り外すことを伴う。

加速および磁場測定値を前処理するＡ２は以下のステップを含んでいてよい。
・ステップＡ２．１、すなわち全加速度測定値のノルムを重力場のノルムと比較するテストからなり、時刻ｋにおける加速度測定値のノルムと重力場のノルムとの差の絶対値が所定の閾値を下回る場合は加速度外乱がゼロであると仮定し、さもなければ外乱が存在し、各軸上で当該外乱が、時刻ｋにおける全加速度測定値と時刻ｋで推定された無外乱加速度測定値との差に等しいと仮定する。
・ステップＡ２．２、すなわち磁場測定値のノルムを地球磁場のノルムと比較するテストからなり、磁場測定値のノルムと地球磁場のノルムとの差の絶対値が所定の閾値を下回る場合は磁気的外乱がゼロであると仮定し、さもなければ、各軸上で磁気的外乱が、時刻ｋにおける磁場測定値と時刻ｋで推定された無外乱磁場測定値との差に等しいと仮定する。

ステップＡ２．１において時刻ｋ−１で推定された外乱に対する追加的なテストを実行し、全加速度測定値のノルムと時刻ｋにおける重力場のノルムとの差の絶対値が所定の閾値を下回る場合、時刻ｋ−１において推定された加速度外乱のノルムが所定の閾値を下回るか否かを調べる検査を実行し、このテストの結果が肯定的である場合、時刻ｋにおける加速度外乱が実質的にゼロであると仮定し、および／またはステップＡ２．２において時刻ｋ−１で推定された磁気的外乱に対する追加的なテストを実行し、磁場測定値のノルムと地球磁場のノルムとの差の絶対値が所定の閾値を下回る場合、時刻ｋ−１において推定された磁気的外乱の絶対値が所定の閾値を下回るか否かを調べる検査を実行し、このテストの結果が肯定的である場合、時刻ｋにおける磁気的外乱が実質的にゼロであると仮定することに利点がある。この追加的なステップにより、本推定方法の精度を向上させることができる。

推定された向きがぶれることがわかっている使用時の好ましくないケースでは、ステップＡ２．１およびＡ２．２のこれらの比較判定法の堅牢さは、状況に応じて異なる限られた時間ウインドウでしか保証されない。比較テストは従って、後述するような時間帯にわたり実行することに利点がある。従って、

を用いて、推定された無外乱測定値と呼ぶ前処理ステップ終了後に得られた測定値を表記すれば、
・固有加速度の検知は加速度測定値のノルムだけにより実行される。このノルムが持続期間Ｔ_Ａのスライディングウインドウの測定値の少なくとも１個に対するＧ_０（地球の重力）のノルムと異なる場合、現時点における測定に外乱が存在すると考えられる。
・磁気的外乱の検知も同様に実行される。
○磁気測定値のノルムが、持続期間Ｔ_Ｍのスライディングウインドウの測定値の少なくとも１個に対するＨ_０（地球磁場）のノルムと異なる場合、
○または磁気測定値と無外乱加速度測定値の逆数

の間の角度がベクトルＧ_０とＨ_０の間の角度とは異なる場合、
現時点における測定値に磁気的外乱が存在する。

Ｔ_Ａは定数パラメータであってよく、一方Ｔ_Ｍの値は運動の速度に関係していてよい。

同一装置上に置かれてユーザにより起動可能なこの変型例を用いて、時刻ｋ−１における測定値の値を探す必要がなく、従ってぶれが解消される。

ステップＢで用いる観測装置は好適には高速且つ簡便な拡張カルマンフィルタである。

時刻ｋにおいて推定された測定値から向きを推定するステップＢは以下を含んでいる。
・時刻ｋ−１において帰納的に推定された状態ベクトルから、時刻ｋにおける先験的な状態ベクトルの推定
・時刻ｋにおける先験的な状態ベクトルの推定から、測定値の先験的な推定と呼ばれる時刻ｋにおける先験的な測定値の推定、
・時刻ｋで推定された無外乱測定値と先験的に推定された測定値との間の差を計算することによる、拡張カルマンフィルタのゲインおよびイノベーションの計算、
・ゲインおよびイノベーションにより時刻ｋにおいて先験的に推定された状態ベクトルの修正により、時刻ｋにおいて推定された向きの計算、

拡張カルマンフィルタで用いる状態ベクトルは、角速度および方位四元数の要素を含んでいてよい。

拡張カルマンフィルタで用いる状態ベクトルは、方位四元数の要素だけを含んでいることに利点があり、それにより状態および測定モデルの構造を簡素化できる。

本発明の主題はまた、加速度測定値の供給に適した手段、磁場を測定する手段、３空間軸に沿って回転速度を測定する手段であって、運動において物体と連動することを意図された手段、および前記測定手段から供給された測定値に基づいて時刻ｋにおける向きを推定する手段を含む姿勢制御装置であり、前記推定手段は、
・前記加速、磁場、および回転の測定値を前処理する手段であって、前記測定値における外乱の存在を検知し、推定された無外乱加速度計の測定値、推定された無外乱磁気測定値、および非バイアス回転速度の配信に適している前処理手段と、
・前処理手段から供給された測定値から観測装置により時刻ｋにおける向きを推定する手段と
を含んでいる。当該観測装置は、拡張カルマンフィルタであってよい。

本発明による姿勢制御装置はまた、制御を行う間、回転速度測定手段の平均バイアスを計算する手段を含んでいてよい。

前処理手段は、加速度測定値における固有加速度の存在を検知する手段および磁場測定値における磁気的外乱の存在を検知する手段を含んでいる。

本発明による姿勢制御装置はまた、固有加速度を推定すると共に物体の速度位置を計算する手段を含んでいてよい。

３空間軸に沿って全加速度測定値、磁場測定値、および回転速度測定値を供給することに適した手段はＭＥＭＳセンサであることに利点がある。

本発明は、以下の記述および添付の図面からより理解されよう。

本発明による方法の時刻ｋにおけるフロー図である。 本発明による加速度計、レートジャイロ、および磁力計の各々からの測定値を前処理するステップの詳細なフロー図を示す。

目的は、空間で移動している物体の向き、例えば人の向きを得ることである。このため、３空間軸に沿って全加速度、磁場、および回転速度測定値を供給することに適したセンサを含む姿勢制御装置を用いる。当該センサは、コストを削減し設置面積を制限できるＭＥＭＳセンサであることに利点がある。

加速度測定値の場合、例えば、各軸上で測定値を提供する３軸加速度計または３個の単軸加速度計であってよい。

同様に、磁場測定値の場合、３軸磁力計または３個の単軸の磁力計であってよい。

回転速度測定の場合、例えば、３個の単軸のレートジャイロでまたは２個の２軸レートジャイロであることに利点がある。

３軸は整列配置されていてもされていなくてもよいが、後者の場合、軸間の相対的な向きは既知でなければならない。

以下に記述において、簡便のため、加速度計または加速度計群を加速度計、磁力計または磁力計群を磁力計、レートジャイロまたはレートジャイロ群をレートジャイロと表記する。これらのセンサは、向きを知りたい物体に取り付けられる。

次式によりモデル化する測定値ｙだけが存在する。

但し、ｙ_Ａ：加速度計から供給される全加速度の３軸測定値、
ｙ_Ｍ：磁力計から供給される磁場の３軸測定値、
ｙ_Ｇ：レートジャイロから供給される回転速度の３軸測定値、
Ｒ:回転行列、
Ｇ_０:地球の重力分野（ベクトル３×１）、
Ｈ_０:地球磁場（ベクトル３×１）、
ω:角速度、
ａ:固有加速度、
ｄ:磁気的外乱、
ｂ:レートジャイロバイアス、
ｖ_Ａ：加速度測定値ノイズ、
ｖ_Ｍ：磁気測定ノイズ、
ｖ_Ｇ：レートジャイロ測定ノイズ
である。

向きはベクトルＧ_０およびＨ_０のデータにより完全に定義される基準座標に対して推定される。例えば、地心座標はベクトルＧ_０（０；０；１）および

により定義される。

簡潔を旨として、３空間方向における測定値を区別しない。

測定値の数学的定義（Ｉ）から明らかなように、これらの測定値の各々は、向きの推定値を得る情報を含む第１の部分「−ＲＧ_０、」、「Ｒ．Ｈ_０」、ω、測定値にランダムに現れる場合がありる外乱を表す第２の部分ａ、ｄ、ｂ、および各センサにおける測定ノイズを表す第３の部分ｖ_Ａ、ｖ_Ｍ、Ｖ_Ｇを含んでいる。

本発明の方法において、測定値を収集した後で、向きの推定を行う処理ステップで当該測定値を用いる前に、前処理ステップを実行する。

図１に本発明による方法の一般的なフロー図を示す。

本方法の以下での記述において、時刻ｋにおける向きの推定（ｋは２以上の適当な整数）を一例として取り上げる。

本発明による方法は、姿勢制御装置を初期化するステップ１００、センサから供給された測定値を前処理するステップ２００、および観測装置による第３の処理ステップ３００を含んでいる。各ステップについて以下に詳述する。

本発明による方法の各種ステップについて詳細に述べる前に、観測装置について述べる。測定値処理ステップで用いる観測装置は、簡単、堅牢、且つすばやく実装できる拡張カルマンフィルタであることに利点がある。

当該フィルタは、当業者に広く知られているため詳述しない。状態モデルおよび測定モデルの数式だけを与える。

カルマンフィルタは、状態の時間的および動的傾向を定義する状態モデル、およびセンサ測定値および状態に関連付けるために用いる測定モデルを含んでいる。

第１のモデリングによれば、カルマンフィルタの状態ベクトルは角速度の３個の要素および向きを定義する四元数の４個の要素からなる。

関連付けられた状態および測定モデルは各々次式であってよい。

但し、ｘ：状態ベクトル
ω：角速度
ｑ：四元数
τ：角速度の進化モデルの時定数：
ｙ：測定値
ｗ_ｘ：モデリングノイズ

簡潔を旨として、４×１次元ベクトル四元数[０，Ｇ_０]は３×１次元ベクトルＧ_０により識別され、同じことが４×１次元ベクトル四元数[０，Ｈ_０］に当てはまり、ベクトルＨ_０も３×１次元である。

第２のモデリングによれば、利点として、４次元以下の四元数の要素だけを含む状態ベクトルが用いることができるのに対し、第１のモデリングでは状態ベクトルは７次元である。次いでジャイロ測定値が直接状態モデルに投入され、測定ベクトルは加速度計および磁力計からの測定値だけを含んでいる。

状態モデルおよび測定モデルは次式で表すことができる。

この第２のモデリングを用いて状態および測定モデルの次元を直接下げるため、これらの構造を簡素化することができる。更に、測定ノイズおよび状態ベクトル推定誤差の設定パラメータ、特にモデリングノイズの共分散行列の要素の数もまた制限されるため、本方法の実装がより容易になる。このようにして得られた推定結果は、第１のモデリングを用いて得られたものと同様の精度である。

ここで本発明によるステップ１００、２００および３００について詳述する。

初期化ステップ１００は、推定したいレートジャイロの平均的外乱を与える。この外乱ｂは実際にはレートジャイロのバイアスであって、２個の限界値の間で変化する。

初期化ステップ（ｋ＝１）の場合、
最初の時点において固有加速度ａ_１および磁気的外乱ｄ_１が既知であり、その場合初期化ステップは状態ベクトルｘ_１の決定を含んでいる。角速度は、最初の時点でゼロと仮定し、四元数は外乱ａ_１およびｄ_１の修正された加速および磁場測定値を用いる最適化により決定されるか、あるいは、初期状態における向きが既知であり、その場合にはａ_１およびｄ_１は導くことができる。

ｋ＝１で制御装置を初期化するステップ１００は以下を含んでいる。
・モデリングノイズ、測定ノイズ、および初期状態ベクトル推定誤差に各々関連付けられた共分散行列Ｑ、Ｒ、およびＰ_１の設定、

と表記するバイアスｂの推定値の計算。姿勢制御装置は所定の時間、例えば約１秒間、静止状態に保たれ、各軸でのレートジャイロからの出力値の平均が計算される。その後、前処理ステップ２００において平均バイアス

の当該推定値がレートジャイロの各々測定値から減算されることにより、バイアスの影響を最小限に抑えて、得られた結果の精度を向上させることができる。

平均バイアス

の当該推定値は好適には、各々の取得開始時に生起する。静止期間中に当該推定値をリフレッシュすることも提案できる。

図２Ａ〜２Ｃに本発明による方法のステップの詳細を示す。

ステップ２００において、３個のセンサにより得られた測定値は、３個のステップ２１０、２２０、および２３０において前処理される。本方法を用いる状況および望まれる精度に応じて、多くの可能な変型実施形態があり得る。

第１のステップ２１０は、全ての実施形態を通じて同一である。図２Ｂに示すステップ２１０において、レートジャイロからの測定値ｙ_Ｇ，ｋが前処理される。上述のように、測定値ｙ_Ｇ，ｋの前処理は、真の測定値から平均バイアス

を減算することにより得られ、時刻ｋにおけるレートジャイロからの前処理された測定値が出力として得られ、

と表記される。

記述に際して、簡潔を旨として、加速度はＧ_０（地球磁場）の倍数で与え、磁場をＨ_０の倍数として与える。

第一の実施形態において、加速度外乱（ステップ２２０）および磁気的外乱（ステップ２３０）の存在を検知する２個のテストを並行して実行するのに利点がある。ステップ２２０において、時刻ｋにおいて加速度計ｙ_Ａ，ｋにより得られた測定値が前処理される。ステップ２２０は、外乱すなわち固有加速度ａの有無を検知する第１のサブステップ２２０．１、および先行する時点ｋ−１で推定された向きに基づく加速

の前処理された測定値を生成する第２のサブステップ２２０．２を含んでいる。

ステップ２２０．１において、固有加速度ａの有無を検知すべく、測定値ｙ_Ａ，ｋのノルムが重力場（注：本方法はＧ_０の倍数で機能する）のノルムと比較されるため、比較は１に対して行われる。すなわち、

か否か。

利点として、上記テストの結果が肯定的である場合、次のテストが追加される。

時刻ｋ−１において推定された固有加速度

のノルムとβ_Ａとの比較をうまく用いて、第１のテストが不十分である特定のケースを除外できる。実際には、時刻ｋ−１において固有加速度の値が高い（すなわちβ_Ａより大きい）場合、時刻ｋで固有加速度がβ_Ａを下回ることはなさそうであると仮定する。α_Ａとβ_Ａは各々、例えば０．０４および０．２に等しい。

この第２のテストは従って、無外乱測定値

の推定の精度、従って向きの推定の精度を向上させる。

上記２個のテストの結果が肯定的である場合、時刻ｋにおいて固有加速度

がゼロであるとの決定がなされる。次いでステップ２２０．２において、推定された測定値がｙ_Ａ，ｋに等しく、観測装置により直接使用することができる。

一方、ステップ２２０．２において、先行する時点で推定された向き

により新たな加速度測定値が生成される。

時刻ｋにおいて推定された無外乱加速度測定値は次いで、測定モデルを用いて次式で表される。

時刻ｋにおける固有加速度の値

もそこから導くことができる（ステップ２２０．３）ため、各々積分および二重積分により物体の速度および位置をそこから導くことが可能になる。

固有加速度は、次式に等しい。

ステップ２２０と同様に、ステップ２３０において、時刻ｋで磁力計により得られた測定値ｙ_Ｍ，ｋが前処理される。このステップ２３０は、磁気的外乱ｄの有無を検知する第１のサブステップ２３０．１と、先行する時点ｋ−１において推定された向きに基づく磁場

の前処理された測定値を生成する第２のサブステップ２３０．２とを含んでいる。

ステップ２３０．１において、磁気的外乱ｄの有無を検知するために、測定値ｙ_Ｍ，ｋのノルムを磁場（注：本方法はＨ_０の倍数で機能する）のノルムと比較されるため、比較は１に対して行われる。すなわち、

か否か。

利点として、磁場の外乱が存在しない旨のテストの結果が肯定的である場合、次のテストが追加される。

時刻ｋ−１において推定された磁気的外乱

のノルムとβ_Ｍとの比較を効果的に用いて、第１のテストが不十分である特定のケースを除外することができる。実際には、時刻ｋ−１において磁気的外乱の値が高い（すなわちβ_Ｍより大きい）場合、時刻ｋで磁気的外乱がβ_Ｍを下回ることはなさそうであると仮定する。α_Ｍとβ_Ｍは各々、例えば０．０４および０．２に等しい。

この第２のテストは従って、無外乱測定値

の推定の精度、従って向きの推定の精度を向上させる。

上記２個のテストの結果が肯定的である場合、時刻ｋにおいて磁気的外乱

がゼロであるとの決定がなされる。次いでステップ２３０．２において、推定された測定値がｙ_Ｍ，ｋに等しく、観測装置により直接使用することができる。

一方、ステップ２３０．２において、先行する時点で推定された向き

により新たな磁場測定値が生成される。

時刻ｋにおいて推定された無外乱磁気測定値は次いで、測定モデルを用いて次式で表される。

時刻ｋにおける磁気的外乱の値もそこから導くことができ（ステップ２３０．３）、次式に等しい。

第２の実施形態は特に、向きの推定が大幅にぶれることが知られる場合に適用できる。これらの場合、加速度および磁気的外乱の存在に関するテストが実行される時間ウインドウを定義することに利点がある。

この変型例において、加速度測定値テストのために、時点ｔ_ｋで終了するウインドウに対してステップ２２０における比較が実行される。固有加速度が検知された（ウインドウ[ｔ_ｋ−Ｔ_Ａ；ｔ_ｋ）の測定値の少なくとも１個に対してノルムＧ_０とα_Ａの範囲内で異なる加速度測定値のノルム）場合、先行する時点で推定された向きにより無外乱加速度測定値が生成され、一方、無外乱加速度測定値は、前処理フェーズに関係する測定値（センサ測定値）に等しい。センサ測定値から固有加速度の値が計算される。単なる例として示すα_ＡおよびＴ_Ａの典型的な値は０．２ｇおよび０．４ｓである。

次いで、固有加速度が存在しないことのテストは次式で表される。

利点として、閾値を超えない場合であっても、第１の変型実施形態と同じ式により固有加速度を系統的に計算することができる。

次いで、または並行して（この第２のオプションは時間を節約できることが利点である）、ステップ２３０における磁気信号の外乱を検知するテストが実行される。磁気的外乱が検知された（ウインドウ[ｔ_ｋ−Ｔ_Ｍ；ｔ_ｋ]の測定値の少なくとも１個に対してＨ_０のノルムとα_Ｍの範囲内で異なる磁気測定値のノルム）場合、先行する時点で推定された向きにより無外乱磁気測定値が生成される。一方、無外乱磁気測定値は前処理フェーズに関係する測定値（センサ測定値）に等しい。次いでセンサ測定値から磁気的外乱の値が計算される。単なる例として示すα_ＭおよびＴ_Ｍの典型的な値は各々０．１ｈおよび０．５ｓである。

次いで、外乱が存在しないことのテストは次式で表される。

利点として、閾値を超えない場合であっても、第１の変型実施形態と同じ式により磁気的外乱を系統的に計算することができる。

これらの最初の二つの実施形態において、ステップ２０２、２０３が並列に実行されることに利点がある。

他方、検知の精度を向上させることが必要であって、装置が三角関数を用いるのに充分な計算記憶手段を含んでいる場合、第３の実施形態を用いて検知の精度を向上させることができる。この場合、検知計算を並列に実行することで得られる利点が無くなり、固有加速度の存在に関するテストの後で磁気的外乱の存在に関するテストを実行する方が有利である。ステップ２０２の第１の計算の出力において、

も計算され、ｕ_０を用いてベクトルＧ_０とＨ_０の間で測定された角度を表す。このパラメータは初期化ステップ１００において計算することができる。次いで、磁気的外乱の検知に対するテストが以下のように実行される。磁気的外乱が検知される（ウインドウ[ｔ_ｋ−Ｔ_Ｍ；ｔ_ｋ]の測定値の少なくとも１個に対してＨ_０のノルムとα_Ｍの範囲内で異なる磁気測定値のノルムまたはｕ_０とα_ｕの範囲内で異なる角度ｕ_ｋ）場合、先行する時点で推定された向きにより無外乱磁気測定値が生成される。一方、無外乱磁気測定値は前処理フェーズに関係する測定値（センサ測定値）に等しい。次いでセンサ測定値から磁気的外乱の値が計算される。利点として、固有加速度の有無に応じてＴ_Ｍの２個の異なる値（各々Ｔ_{Ｍ_ｆａｓｔ}およびＴ_{Ｍ_ｓｌｏｗ}）が用いられる。単なる例として示すα_Ｍ、α_ｕ、Ｔ_{Ｍ_ｆａｓｔ}、およびＴ_{Ｍ_ｓｌｏｗ}の典型的な値は各々０．１ｈ、１０°、０．５ｓ、および３ｓである。

次いで、外乱が存在することのテストは次式で表される。

ステップ３００において、前処理された測定値

が観測装置により使用される。例えば拡張カルマンフィルタが、その因数分解された形式で用いられる。

ここで、フィルタにより実行される計算段階について説明する。

第２のモデリングを用いていると仮定する。しかし、明らかに第１のモデリングを同様に用いることができる。

ステップ３００は、以下のステップを含んでいる。
ａ）状態ベクトルの先験的な推定、
ｂ）測定値の先験的な推定、
ｃ）カルマンフィルタのゲインＫ_ｋ、およびイノベーションＩ_ｋの計算、
ｄ）先験的に推定された状態の修正。

ステップａ）〜ｄ）について以下に詳述する。

ステップａ）において、時刻ｋ−１における状態ベクトルの帰納的な推定値から、時刻ｋにおける状態ベクトルを先験的に推定する。

先験的な状態ベクトルの推定は次式で与えられる。

但し、

：時刻ｋ−１における状態ベクトルの先験的な推定値、

：時刻ｋにおける状態ベクトルの先験的な推定値、
Ｔ_ｅ：サンプリング間隔。

ステップｂ）において、測定モデル（ＩＩＩ）を用いて、ステップａ）で推定された状態ベクトルを用いた測定値の推定値を実行する。

ステップｃ）において、ゲインＫ_ｋおよびイノベーションＩ_ｋが計算される。イノベーションは、前処理された測定値から、先験的な推定された測定値を減算することにより得られる。

次式が得られる。

ステップｄ）において、先験的な推定された状態がゲインおよびイノベーションにより修正される。

この修正は、時刻ｋにおける向き

の先験的な推定値を与える。

利点として、後続するステップｅ）において、推定された四元数が正規化される。

これにより、計算の各ステップにおけるぶれを回避することが可能になる。

本発明による方法は、測定モデルと整合した、無外乱測定値に近い測定値を観測装置に提供する利点をもたらす。従って、向きの推定に対する外乱の影響が大幅に減少する。また、各測定値の信頼性を一定に維持することにより、外乱がある場合であっても各センサから得られる情報を観測装置に永続的に提供することが可能になる。実際、加速度計、磁力計、およびレートジャイロからの測定値を組み合わせて用いることで、推定された向きに対する測定誤差（測定ノイズ、残留外乱、残留レートジャイロバイアス）の影響を減らすことが可能になる。

本発明による方法は、最大９個の自由度で実行された運動によらず、向きだけでなく、各サンプリング間隔における固有加速度および磁気的外乱の推定を可能にする。本方法は基本的な構成ブロックすなわち数値テスト、解析的計算、拡張カルマンフィルタの使用に依存するため、極めて簡単に実装できる。

Claims (19)

1. 人間により運ばれる物体の時刻ｋにおける空間内での向きを、３空間軸に沿った前記物体の加速度計による全加速度（ｙ_Ａ）、磁力計による磁場（ｙ_Ｍ）、およびジャイロメータによる回転速度（ｙ_Ｇ）の測定値を用いて推定する方法であって、
   Ａ：第１のサブユニットが、操作中に測定時刻ｋにおける測定値（ｙ_Ａ，ｙ_Ｍ）を前処理手段により前処理して前記測定値における外乱、すなわち前記物体の固有加速度または地球磁場に加えられた磁場からの外乱の存在を検知して、外乱が検知された時刻ｋにおける推定された無外乱測定値である推定測定値
   

   

   を、ステップｋ−１で取得された向きまたは外乱が検知されていない測定値に基づいて算出するステップと、
   Ｂ：第２のサブユニットが、ステップＡから得られた、時刻ｋで取得された推定測定値
   

   

   および時刻ｋにおける回転速度（ｙ_Ｇ）に基づいて、観測装置により時刻ｋにおける向きを推定するステップと
   を含む方法。
2. 時刻ｋにおける空間内での物体の向きの前記推定が、空間内での３軸に沿った前記物体の全加速度（ｙ_Ａ）、磁場（ｙ_Ｍ）、および回転速度（ｙ_Ｇ）の測定値だけを用いることを特徴とする、請求項１に記載の方法。
3. ステップＡが、
   Ａ２：前記全加速度および磁場（ｙ_Ａ，ｙ_Ｍ）の測定値内での時刻ｋにおける外乱の有無の検知、
   Ａ３：時刻ｋにおいて外乱が存在しない場合、時刻ｋで推定された推定測定値
   

   

   は時刻ｋにおける測定値に等しく、外乱が存在する場合、時刻ｋで推定された推定測定値
   

   

   はステップｋ−１で取得された向きに基づいて計算されること
   を含むことを特徴とする、請求項１に記載の方法。
4. 前記ステップＡ２が、
   Ａ２．１：前記全加速度測定値のノルムを重力場（Ｇ_０）のノルムと比較するテスト、すなわち時刻ｋにおける前記加速度測定値のノルムと前記重力場（Ｇ_０）のノルムとの差の絶対値が所定の閾値（α_Ａ）を下回る場合は前記加速度外乱がゼロであると仮定し、さもなければ外乱が存在し、各軸上で前記外乱が、時刻ｋにおける前記全加速度測定値と時刻ｋで推定された前記無外乱加速度測定値との差に等しいと仮定することを特徴とするテストと、
   Ａ２．２：前記磁場測定値のノルムを地球磁場（Ｈ_０）のノルムと比較するテスト、すなわち前記磁場測定値のノルムと前記地球磁場のノルムとの差の絶対値が所定の閾値（α_Ｍ）を下回る場合は前記磁気的外乱がゼロであると仮定し、さもなければ、各軸上で前記磁気的外乱が、時刻ｋにおける前記磁場測定値と、時刻ｋで推定された前記無外乱磁場測定値との差に等しいと仮定することを特徴とするテストと
   を含んでいることを特徴とする、請求項３に記載の方法。
5. 前記ステップＡ２．１において、時刻ｋ−１で推定された外乱
   

   

   に対する追加的なテストを実行し、前記全加速度測定値のノルムと時刻ｋにおける前記重力場（Ｇ_０）のノルムとの差の絶対値が前記所定の閾値（α_Ａ）を下回る場合、時刻ｋ−１において推定された前記加速度外乱のノルムが所定の閾値（β_Ａ）を下回るか否かを調べる検査を実行し、このテストの結果が肯定的である場合、時刻ｋにおける前記加速度外乱が実質的にゼロであると仮定し、および／またはステップＡ２．２において時刻ｋ−１で推定された磁気的外乱
   

   

   に対する追加的なテストを実行し、前記磁場測定値のノルムと前記地球磁場（Ｈ_０）のノルムとの差の絶対値が前記所定の閾値（α_Ｍ）を下回る場合、時刻ｋ−１において推定された前記磁気的外乱の絶対値が所定の閾値（β_Ｍ）を下回るか否かを調べる検査を実行し、このテストの結果が肯定的である場合、時刻ｋにおける前記磁気的外乱が実質的にゼロであると仮定することを特徴とする、請求項４に記載の方法。
6. ステップＡ２の検知の少なくとも１個がユーザにより設定された時間ウインドウ（Ｔ_Ａ，Ｔ_Ｍ）に対して実行されることを特徴とする、請求項３に記載の方法。
7. 前記固有加速度の検知は、
   時刻ｔ_ｋにおいて測定された加速度（ｙ_Ａ，Ｋ）のノルムと地球重力場を表わすベクトル（Ｇ_０）のノルムとの差の絶対値が所定の閾値（α_Ａ）を超える場合に時間ウィンドウ（Ｔ_Ａ）内に時刻ｔ_ｋが存在するかのテストを実行し、
   このテストの結果が肯定的である場合、時刻ｔ_ｋにおける加速度
   

   

   の無外乱測定値をスカラ（−１）、時刻ｔ_ｋ−１における物体の推定された向き
   

   

   、ベクトル（Ｇ_０）と推定された向き
   

   

   の共役
   

   

   の積と推定し、
   このテスト結果が否定的である場合、時刻ｔ_ｋにおける加速度
   

   

   の無外乱測定値を時刻ｔ_ｋにおいて測定された加速度（ｙ_Ａ，Ｋ）と推定し、
   時刻ｔ_ｋにおける前記固有加速度と推定された
   

   

   がスカラ（−１）、時刻ｔ_ｋ−１における物体の推定された向き
   

   

   、ベクトル（Ｇ_０）と推定された向き
   

   

   の共役
   

   

   の積と時刻ｔ_ｋにおいて測定された加速度（ｙ_Ａ，Ｋ）との差に等しい場合に終了する
   処理により生成されることを特徴とする、請求項６に記載の方法。
8. 前記磁気的外乱の検知は、
   時刻ｔ_ｋにおいて測定された磁場（ｙ_Ｍ，Ｋ）のノルムと地球磁場を表わすベクトル（Ｈ_０）のノルムとの差の絶対値が所定の閾値（α_Ｍ）を超える場合に時間ウィンドウ（Ｔ_Ｍ）内に時刻ｔ_ｋが存在するかのテストを実行し、
   このテストの結果が肯定的である場合、時刻ｔ_ｋにおける磁場
   

   

   の無外乱測定値を時刻ｔ_ｋ−１における物体の推定された向き
   

   

   、ベクトル（Ｈ_０）と推定された向き
   

   

   の共役
   

   

   の積と推定し、
   このテスト結果が否定的である場合、時刻ｔ_ｋにおける磁場
   

   

   の無外乱測定値を時刻ｔ_ｋにおいて測定された磁場（ｙ_Ｍ，Ｋ）と推定し、
   時刻ｔ_ｋにおける地球磁場に加えられた磁場と推定された
   

   

   が時刻ｔ_ｋ−１における物体の推定された向き
   

   

   、ベクトル（Ｈ_０）と推定された向き
   

   

   の共役
   

   

   との積と時刻ｔ_ｋにおいて測定された磁場との差に等しい場合に終了する
   処理により実行されることを特徴とする、請求項６に記載の方法。
9. 前記固有加速度の検知の出力において時刻ｔ_ｋにおける角度
   

   

   がもまた計算され、次いで前記磁気的外乱の検知が、
   時間ウィンドウＴ_Ｍとして、固有加速度が検知された場合は第１の閾値Ｔ_{Ｍｆａｓｔ}を設定し、他の場合は第２の閾値Ｔ_{Ｍｓｌｏｗ}を設定し、
   時刻ｔ_ｋにおいて測定された磁場（ｙ_Ｍ，Ｋ）のノルムと地球磁場を表わすベクトル（Ｈ_０）のノルムとの差の絶対値が所定の閾値（α_Ｍ）を超える場合、または、角度（ｕ_ｋ）と角度変数（ｕ_０）との差の絶対値が所定の閾値（α_ｕ）を超える場合に、時間ウィンドウ（Ｔ_Ｍ）内に時刻ｔ_ｋが存在するかのテストを実行し、
   このテストの結果が肯定的である場合、時刻ｔ_ｋにおける磁場
   

   

   の無外乱測定値を時刻ｔ_ｋ−１における物体の推定された向き
   

   

   、ベクトル（Ｈ_０）と推定された向き
   

   

   の共役
   

   

   の積と推定し、
   このテスト結果が否定的である場合、時刻ｔ_ｋにおける磁場
   

   

   の無外乱測定値を時刻ｔ_ｋにおいて測定された磁場（ｙ_Ｍ，Ｋ）と推定し、
   時刻ｔ_ｋにおける地球磁場に加えられた磁場と推定された
   

   

   が時刻ｔ_ｋ−１における物体の推定された向き
   

   

   、ベクトル（Ｈ_０）と推定された向き
   

   

   の共役
   

   

   との積と時刻ｔ_ｋにおいて測定された磁場との差に等しい場合に終了する
   処理により実行されることを特徴とする、請求項７に記載の方法。
10. 前記ステップＢで用いる前記観測装置が拡張カルマンフィルタであることを特徴とする、請求項１〜９のいずれか１項に記載の方法。
11. 時刻ｋにおいて推定された無外乱測定値から向きを推定するステップＢが、
    時刻ｋ−１において帰納的に推定された状態ベクトル
    

    

    から時刻ｋにおける先験的な状態ベクトル
    

    

    を推定するステップ、
    時刻ｋにおける先験的な状態ベクトル
    

    

    の推定から時刻ｋにおける先験的な測定値
    

    

    を推定するステップ、
    時刻ｋで推定された前記無外乱測定値と前記先験的に推定された測定値
    

    

    との差を計算することにより、前記拡張カルマンフィルタ（Ｋ_ｋ）のゲインおよびイノベーション（Ｉ_ｋ）を計算するステップ、
    前記ゲインおよび前記イノベーションにより時刻ｋにおいて先験的に推定された状態ベクトルを修正することにより、時刻ｋにおける推定された向き
    

    

    を計算するステップ
    を含んでいることを特徴とする、請求項１０に記載の方法。
12. 前記拡張カルマンフィルタで用いる前記状態ベクトルが角速度および方位四元数の要素を含んでいることを特徴とする、請求項１１に記載の方法。
13. 前記拡張カルマンフィルタで用いる前記状態ベクトルが方位四元数の要素だけを含んでいることを特徴とする、請求項１２に記載の方法。
14. 加速度測定値（ｙ_Ａ）を与える検知ユニット、磁場（ｙ_Ｍ）を測定する検知ユニット、３空間軸に沿って回転速度（ｙ_Ｇ）を測定する検知ユニット、および前記検知ユニットから供給された測定値に基づいて時刻ｋにおける向きを推定する処理ユニットを少なくとも含む人間により運ばれる物体の姿勢制御システムであって、
    前記加速度（ｙ_Ａ）および磁場（ｙ_Ｍ）の測定値を前処理する第１のサブユニット、すなわち前記測定値内における、前記物体の固有加速度または地球磁場に加えられた磁場からの外乱の存在を検知すること、並びに外乱が検知された時刻ｋにおける推定された無外乱測定値または外乱が検知されていない測定値である推定測定値
    

    

    を算出する前処理サブユニットと、
    時点ｋにおいて前記第１のサブユニットから取得した推定測定値
    

    

    と前記検知ユニットから取得した時点ｋにおける前記回転速度（ｙ_Ｇ）から時点ｋにおける向きを観測装置により推定する第２のサブユニットと
    を含む姿勢制御システム。
15. 前記第１のサブユニットが、前記加速度測定値内における、固有加速度の存在を検知するモジュール、および前記磁場測定値内における磁気的外乱の存在を検知するモジュールを含んでいることを特徴とする、請求項１４に記載の姿勢制御システム。
16. 前記加速度測定値内における固有加速度の存在を検知する前記モジュールおよび前記磁場測定値内における磁気的外乱の存在を検知する前記モジュールが１個以上の時間ウインドウにおいてこれら検知を実行すべく動作可能であることを特徴とする、請求項１５に記載の姿勢制御システム。
17. 前記固有加速度および前記磁気的外乱を推定すると共に、前記物体の速度および位置を計算するモジュールを更に含んでいる、請求項１４に記載の姿勢制御システム。
18. 前記観測装置が拡張カルマンフィルタであることを特徴とする、請求項１４に記載の姿勢制御システム。
19. 空間内での３軸に沿った前記全加速度（ｙ_Ａ）の測定値、前記磁場（ｙ_Ｍ）の測定値、および前記回転速度（ｙ_Ｇ）の測定値を供給する検知ユニットがＭＥＭＳセンサであることを特徴とする、請求項１４に記載の姿勢制御システム。

Images