(57)【特許請求の範囲】
【請求項１】
移動体の機体に取り付けた角速度センサが出力する角速度信号及び加速度センサが出力する加速度信号に基づいて、前記移動体の姿勢角及び位置移動量を導出する慣性計測方法であって、
前記角速度信号に重畳しているノイズ成分を低減するフィルタリング処理と、
前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整するアライメント処理と、
前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する角速度・加速度バイアス算出処理と、
前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する姿勢角算出処理と、
前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する位置移動量算出処理とで構成され、
前記移動体が静止している時に、前記フィルタリング処理、前記アライメント処理及び前記角速度・加速度バイアス算出処理を順番に実行し、その後、前記フィルタリング処理、前記姿勢角算出処理及び前記位置移動量算出処理を順番に繰り返し実行し、
前記移動体が静止している時の前記フィルタリング処理、前記移動体が静止している時の前記アライメント処理、前記移動体が静止している時の前記姿勢角算出処理、及び前記移動体が静止している時の前記位置移動量算出処理は、信号に重畳しているノイズを低減するとともに、前記各バイアスに起因する積分値の誤差の増大を抑制する処理により行い、
前記移動体が運動している時の前記フィルタリング処理、前記移動体が運動している時の前記姿勢角算出処理、及び前記移動体が運動している時の前記位置移動量算出処理は、信号に重畳しているノイズを低減する処理により行うことを特徴とする慣性計測方法。
【請求項２】
前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記アライメント処理では、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出し、前記角速度・加速度バイアス算出処理では、地球角速度をゼロと仮定して前記角速度バイアスを算出し、前記姿勢角算出処理では、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出する請求項１記載の慣性計測方法。
【請求項３】
移動体の機体に取り付けた角速度センサが出力する角速度信号及び加速度センサが出力する加速度信号に基づいて、前記移動体の姿勢角及び位置移動量を導出する慣性計測装置であって、
前記角速度信号に重畳しているノイズ成分を低減する処理を行うフィルタリング部と、
前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整する処理を行うアライメント部と、
前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する処理を行う角速度・加速度バイアス算出部と、
前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する処理を行う姿勢角算出部と、
前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する処理を行う位置移動量算出部とで構成され、
前記移動体が静止している時に前記フィルタリング部、前記アライメント部及び前記角速度・加速度バイアス算出部が前記の各処理を順番に実行し、その後、前記フィルタリング部、前記姿勢角算出部及び前記位置移動量算出部が前記の各処理を順番に繰り返し実行し、
前記移動体が静止している時に前記フィルタリング部が行う処理、前記移動体が静止している時に前記アライメント部が行う処理、前記移動体が静止している時に前記姿勢角算出部が行う処理、及び前記移動体が静止している時に前記位置移動量算出部が行う処理は、信号に重畳しているノイズを低減するとともに、前記各バイアスに起因する積分値の誤差の増大を抑制する処理により行われ、
前記移動体が運動している時に前記フィルタリング部が行う処理、前記移動体が運動している時に前記姿勢角算出部が行う処理、及び前記移動体が運動している時に前記位置移動量算出部が行う処理は、信号に重畳しているノイズを低減する処理により行われることを特徴とする慣性計測装置。
【請求項４】
前記アライメント部は、前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出する処理を行い、前記角速度・加速度バイアス算出部は、地球角速度をゼロと仮定して前記角速度バイアスを算出する処理を行い、前記姿勢角算出部は、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出する処理を行う請求項３記載の慣性計測装置。
【請求項５】
請求項１又は２記載の慣性計測方法を実行させるための前記各処理の実行用プログラムにより構成された慣性計測プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、角速度センサや加速度センサ等を使用して移動体の姿勢・位置を計測する慣性計測方法と慣性計測装置及び慣性計測プログラムに関する。
【背景技術】
【０００２】
従来、マルチコプタ、ロボット、自動車等の移動体に角速度センサや加速度センサを取り付け、これらの出力信号を積分して移動体の姿勢や位置等を検出する慣性計測方法が種々の用途に用いられている。この慣性計測方法において、各センサの出力信号を単純積分すると、出力信号に含まれる白色雑音が累積されて、出力値に重畳する誤差の値がランダムウォークし（ゼロ点がランダムにドリフトし）、高精度に検出することができないため、従来から、ランダムウォークを抑制するための様々な技術が提案されている。
【０００３】
従来、例えば特許文献１に開示されているように、移動体の角速度を検出する精角速度センサと、移動体の姿勢角を検出する粗姿勢角検出手段と、粗姿勢角検出手段及び精角速度センサの出力信号に基づいて移動体の姿勢角を推定する姿勢角推定器とを備えた移動体姿勢角検出装置があった。姿勢角推定器は、粗姿勢角信号と精加速度信号に動的カルマンフィルタを適用することにより、移動体の姿勢角及び精角速度センサのドリフト値を推定し、ドリフトをキャンセルした姿勢角推定値を出力する。粗姿勢角検出手段は、例えばＧＰＳ単体又は複数のＧＰＳからなるＤＧＰＳ（ディファレンシャル測位方式）を用いて構成される。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０００−２９２１７０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１の移動体姿勢角検出装置は、メインの検出手段である精角速度センサに加え、第二の検出手段である粗姿勢角検出手段を別個に設ける必要があるので、装置の構成が複雑になる。また、ＤＧＰＳを用いて粗姿勢角検出手段を構成した場合、測位衛星からの電波が届かない非ＧＰＳ環境下では高精度な検出ができなくなるので、工場や倉庫の中、橋梁の下、トンネルの中等で使用される移動体には適用することができない。さらに、ＤＧＰＳを用いたこの移動体姿勢角検出装置でも、白色雑音による影響を十分に除去することはできず、姿勢角の検出精度には限界があった。
【０００６】
また、慣性計測における誤差は、上述したランダムウォーク以外に、信号に重畳しているノイズによる誤差、積分を行う際に、初期のバイアスに起因する積分値の誤差等があり、その他、加速度センサと角速度センサによって検出される重力加速度と地球角速度（地球の自転や公転によって発生する角速度）も誤差となる。例えば、マルチコプタ等の慣性計測を行う場合、図２３、図２４に示すように、空中で移動体１２が傾いたり揺動したりすると加速度センサで検出される重力加速度の成分が変化するので、出力信号から重力加速度の成分をキャンセルする（補正する）のは容易ではない。地球角速度の成分についても同様である。
本発明は、上記背景技術に鑑みて成されたものであり、様々な誤差要因を効果的にキャンセルすることができ、非ＧＰＳ環境下でも高精度な検出結果が得られる移動体の慣性計測方法と慣性計測装置及び慣性計測プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明は、移動体の機体に取り付けた角速度センサが出力する角速度信号及び加速度センサが出力する加速度信号に基づいて、前記移動体の姿勢角及び位置移動量を導出する慣性計測方法であって、
前記角速度信号に重畳しているノイズ成分を低減するフィルタリング処理と、前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整するアライメント処理と、前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する角速度・加速度バイアス算出処理と、前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する姿勢角算出処理と、前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する位置移動量算出処理とで構成され、
前記移動体が静止している時に、前記フィルタリング処理、前記アライメント処理及び前記角速度・加速度バイアス算出処理を順番に実行し、その後、前記フィルタリング処理、前記姿勢角算出処理及び前記位置移動量算出処理を順番に繰り返し実行し、
前記移動体が静止している時の前記フィルタリング処理、前記移動体が静止している時の前記アライメント処理、前記移動体が静止している時の前記姿勢角算出処理、及び前記移動体が静止している時の前記位置移動量算出処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値のランダムウォークを抑制するとともに、バイアスに起因する積分値の誤差の増大を抑制する静止状態推定フィルタを使用した推定により行い、
前記移動体が運動している時の前記フィルタリング処理、前記移動体が運動している時の前記姿勢角算出処理、及び前記移動体が運動している時の前記位置移動量算出処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値の誤差を抑制する運動状態推定フィルタを使用した推定により行う慣性計測方法である。
【０００８】
この慣性計測方法は、前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記アライメント処理では、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出し、前記角速度・加速度バイアス算出処理では、地球角速度をゼロと仮定して前記角速度バイアスを算出し、前記姿勢角算出処理では、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出するようにしてもよい。
【０００９】
本発明は、移動体の機体に取り付けた角速度センサが出力する角速度信号及び加速度センサが出力する加速度信号に基づいて、前記移動体の姿勢角及び位置移動量を導出する慣性計測装置であって、
前記角速度信号に重畳しているノイズ成分を低減する処理を行うフィルタリング部と、前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整する処理を行うアライメント部と、前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する処理を行う角速度・加速度バイアス算出部と、前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する処理を行う姿勢角算出部と、前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する処理を行う位置移動量算出部とで構成され、
前記移動体が静止している時に前記フィルタリング部、前記アライメント部及び前記角速度・加速度バイアス算出部が前記の各処理を順番に実行し、その後、前記フィルタリング部、前記姿勢角算出部及び前記位置移動量算出部が前記の各処理を順番に繰り返し実行し、
前記移動体が静止している時に前記フィルタリング部が行う処理、前記移動体が静止している時に前記アライメント部が行う処理、前記移動体が静止している時に前記姿勢角算出部が行う処理、及び前記移動体が静止している時に前記位置移動量算出部が行う処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値のランダムウォークを抑制するとともに、バイアスに起因する積分値の誤差の増大を抑制する静止状態推定フィルタを使用した推定により行われ、
前記移動体が運動している時に前記フィルタリング部が行う処理、前記移動体が運動している時に前記姿勢角算出部が行う処理、及び前記移動体が運動している時に前記位置移動量算出部が行う処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値の誤差を抑制する運動状態推定フィルタを使用した推定により行われる慣性計測装置である。
【００１０】
この慣性計測装置は、前記アライメント部は、前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出する処理を行い、前記角速度・加速度バイアス算出部は、地球角速度をゼロと仮定して前記角速度バイアスを算出する処理を行い、前記姿勢角算出部は、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出するようにしてもよい。
【００１１】
本発明は、前記慣性計測方法を実行させるための各処理の実行用プログラムにより構成された慣性計測プログラムである。
【発明の効果】
【００１２】
本発明の慣性計測方法及び慣性計測装置は、静止している時に独特なアライメント処理と角速度・加速度バイアス算出処理とを行い、さらに姿勢角及び位置移動量を算出する時に、重力加速度及び地球角速度の成分をキャンセルする処理を行うので、重力加速度及び地球角速度に起因する誤差を小さくすることができる。
【００１３】
さらに、本発明の移動体の慣性計測方法と慣性計測装置及び慣性計測プログラムによれば、静止時と運動時を分けて所定の処理を行うことにより、慣性計測の誤差を効果的に抑制し、姿勢角及び位置移動量を高精度に計測することができる。
【００１４】
本発明により、初期の方位角の検出用に電子コンパスの方位信号を利用し、地球角速度を無視することによって、比較的安価な角速度センサを選択することが可能になり、装置全体のコストダウンを図ることができる。
【００１５】
さらに、本発明の慣性計測装置は、汎用のコンピュータに本発明の慣性計測プログラムを用いて容易に構成することができ、しかも、特許文献１の移動体姿勢角検出装置のように、ランダムウォークを抑制するために第二の検出手段を付設する必要もない。従って、装置を非常にシンプルに構成することができる。
【図面の簡単な説明】
【００１６】
【図１】本発明の慣性計測装置の第一の実施形態のシステム構成図である。
【図２】第一の実施形態の慣性計測装置が行う慣性計測方法（本発明の慣性計測方法の第一の実施形態）を示すフローチャートである。
【図３】図２の中のアライメント処理の内容を示すフローチャートである。
【図４】アライメント処理で行う初期姿勢角算出ステップの内容を示すフローチャートである。
【図５】アライメント処理で行う第１の初期姿勢角調整ステップの内容を示すフローチャートである。
【図６】アライメント処理で行う第２の初期姿勢角調整ステップの内容を示すフローチャートである。
【図７】アライメント処理で行う第３の初期姿勢角調整ステップの内容を示すフローチャートである。
【図８】図２の中の角速度・加速度バイアス算出処理の内容を示すフローチャートである。
【図９】図２の中の姿勢角算出処理の内容を示すフローチャートであって、静止時に行う処理（ａ）、運動時に行う処理（ｂ）である。
【図１０】図２の中の位置移動量算出処理の内容を示すフローチャートであって、静止時に行う処理（ａ）、運動時に行う処理（ｂ）である。
【図１１】静止状態推定フィルタと運動状態推定フィルタの設定方法の違いを説明するタイミングチャート（ａ）、（ｂ）である。
【図１２】静止状態推定フィルタの効果を確認するために行ったシミュレーションの条件を示す図（ａ）、比較例のシミュレーションの条件を示す図（ｂ）である。
【図１３】図１２（ａ）、（ｂ）の条件で行ったシミュレーション結果を示す波形（ａ）、（ｂ）である。
【図１４】静止状態推定フィルタの効果を確認するために行ったシミュレーションの条件を示す図（ａ）、比較例のシミュレーションの条件を示す図（ｂ）である。
【図１５】図１４（ａ）、（ｂ）の条件で行ったシミュレーション結果を示す波形（ａ）、（ｂ）である。
【図１６】運動状態推定フィルタの効果を確認するために行ったシミュレーションの条件を示す図（ａ）、比較例のシミュレーションの条件を示す図（ｂ）である。
【図１７】図１６（ａ）、（ｂ）の条件で行ったシミュレーション結果を示す波形（ａ）、（ｂ）である。
【図１８】本発明の慣性計測装置の第二の実施形態のシステム構成図である。
【図１９】第二の実施形態の慣性計測装置が行うアライメント処理の内容を示すフローチャートである。
【図２０】図１９の中の初期姿勢角算出ステップの内容を示すフローチャートである。
【図２１】第二の実施形態の慣性計測装置が行う角速度・加速度バイアス算出処理の内容を示すフローチャートである。
【図２２】第二の実施形態の慣性計測装置が行う姿勢角算出処理の内容を示すフローチャートであって、静止時に行う処理（ａ）、運動時に行う処理（ｂ）である。
【図２３】地理座標系と機体座標系を示す図である。
【図２４】移動体が傾いた時の重力加速度の成分の変化を示す図（ａ）、（ｂ）である。
【発明を実施するための形態】
【００１７】
以下、本発明の慣性計測方法と慣性計測装置及び慣性計測プログラムの第一の実施形態について、図１〜図１７に基づいて説明する。この実施形態の慣性計測装置１０は、図１に示すように、移動体１２の機体１２ａに取り付けた角速度センサ１４が出力する角速度信号ωB及び加速度センサ１６が出力する加速度信号ａBに基づいて、移動体１２の姿勢角φ（ロール），θ（ピッチ），ψ（ヨー）及び位置移動量ｒGを導出する装置であり、この実施形態の慣性計測方法は、この慣性計測方法を実行させるための各処理の実行用プログラムにより構成された慣性計測プログラムを、慣性計測装置１０にインストールして実行される。なお、□B，□Gという表記の記号はベクトルであり、「B」は各軸成分を機体座標系で表したものであることを示し、「G」は各軸成分を地理座標系で表したものであることを示す。
【００１８】
慣性計測装置１０は、フィルタリング部１８、アライメント部２０、角速度・加速度バイアス算出部２２、姿勢角算出部２４、位置移動量算出部２６とで構成されている。まず、各ブロックの機能を簡単に説明する。
【００１９】
フィルタリング部１８は、角速度信号ωBに重畳しているノイズ成分を低減する処理（フィルタリング処理S1s，S1u）を行う。
【００２０】
アライメント部２０は、加速度信号ａB及び角速度信号ωBから概略の初期姿勢角φ0，θ0，ψ0を算出した後、加速度信号ａBから重力加速度ｇpBを差し引いた加速度ahBを積分して求まる速度VhBの絶対値が一定以下になり、且つ角速度信号ωBから地球角速度ωpBを差し引いた角速度αhBを積分して求まる角度の絶対値が一定以下の値になるように、初期姿勢角φ0，θ0，ψ0を微調整する処理（アライメント処理S2）を行う。
【００２１】
角速度・加速度バイアス算出部２２は、角速度信号ωBから地球角速度ωpBを差し引いた角速度バイアスωbBを算出するとともに、加速度信号ａBから重力加速度ｇpBを差し引いた加速度バイアスａbBを算出する処理（角速度・加速度バイアス算出処理S3）を行う。
【００２２】
姿勢角算出部２４は、角速度信号ωBから地球角速度ωpB及び角速度バイアスωbBを差し引いた補正角速度ωhBを算出し、これを積分して姿勢角φ，θ，ψを算出する処理（姿勢角算出処理S4s，S4u）を行う。
【００２３】
位置移動量算出部２６は、加速度信号ａBから重力加速度ｇpB及び加速度バイアスａbBを差し引いた補正加速度ａhGを算出し、これを２階積分して位置移動量ｒGを算出する処理（位置移動量算出処理S5s，S5u）を行う。
【００２４】
慣性計測装置１０は、図２に示すように、移動体１２が静止している時に、フィルタリング処理S1s、アライメント処理S2、角速度・加速度バイアス算出処理S3を順番に実行し、その後、フィルタリング処理S1s又はS1u、姿勢角算出処理S4s又はS4u、位置移動量算出処理S5s又はS5uを順番に繰り返し実行する。
【００２５】
移動体１２がマルチコプタの場合、「静止」とは、例えば、地面に着地してスタンバイしている状態等のことであり、飛行を開始すると「運動」の状態になる。静止状態か運動状態かの判断は、慣性計測装置１０が角速度信号ωBと加速度信号ａBを適宜の方法で分析することによって行う。
【００２６】
以下、各処理の詳細な内容について順番に説明する。図２に示すように、まず静止状態か運動状態かの判断を行い、静止状態と判断した場合、フィルタリング処理S1sを行う。フィルタリングは、角速度信号ωBに重畳しているノイズ成分を低減する処理を、静止状態推定フィルタＡs1を使用した推定によって行う。静止状態推定フィルタＡs1は、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。
【００２７】
フィルタリング処理S1sが終了した後、静止状態でアライメントが未終了の場合は、アライメント処理S2を行う。アライメント処理S2は、初期姿勢角算出ステップS21と３つの初期姿勢角調整ステップS22．S23，S24を行う。図３に示すように、アライメント処理S2を開始し、初期姿勢角φ0，θ0，ψ0が未算出の場合、初期姿勢角算出ステップS21を行う。
【００２８】
初期姿勢角算出ステップS21では、図４に示すように、まず、ステップS211で、加速度信号ａBとフィルタリング処理後の角速度信号ωBを取得し、規定時間における各信号の平均値と分散を計算する。そして、規定時間の経過後、ステップS212に進み、ステップS211で算出した加速度信号ａBの平均値が真の重力加速度のｇpBと等しいと仮定し、さらに角速度信号ωBの平均値が真の地球角速度ωpBと等しいと仮定して、初期姿勢角φ0，θ0，ψ0を算出する（ステップS212）。この仮定は、アライメント処理S2は静止時の加速度信号ａB及び角速度信号ωBに基づいて行うことが前提であり、上記の仮定により、概略の初期姿勢角φ0，θ0，ψ0を、図４の中に記載したシンプルな式で簡単に求めることができる。
【００２９】
初期姿勢角φ0，θ0，ψ0を算出した後、初期姿勢角θ0が未調整の場合は、第１の初期姿勢角調整ステップS22に進む。第１の初期姿勢角調整ステップS22は、初期姿勢角θ0を微調整するステップである。図５に示すように、まずステップS221に進み、地理座標系の重力加速度ｇpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の重力加速度ｇpBに変換する。座標変換行列Ｃ(G-B)は公知の行列であり、右辺のCφ，Cθ，Cψは各々cosφ，cosθ，cosψであり、Sφ，Sθ，Sψは各々sinφ，sinθ，sinψである。
【数１】
0006712037.tif 000002
【００３０】
ステップS221を行った後はステップS222に進み、加速度信号ａBのＸ軸成分a(x)から重力加速度ｇpBのＸ軸成分gp(x)を差し引いて、重力加速度の影響をキャンセルした補正加速度ah(x)を算出する。そして、補正加速度ah(x)に静止状態推定フィルタＡs1を適用し、積分値であるＸ軸方向の速度vh(x)を推定する（ステップS223）。この静止状態推定フィルタＡs1は、フィルタリング処理S1sで使用したのと同じものである。
【００３１】
速度vh(x)を推定すると、規定時間における速度vh(x)の平均値を計算する（ステップS224）。そして、速度vh(x)の絶対値が一定以上に大きい場合はステップS225に進み、初期姿勢角θ0を直前の値から変更（増減）させる。初期姿勢角θ0を変更してステップS221〜S224を繰り返し実行すると、やがて速度vh(x)の絶対値が一定以下（≒0）になるので、S226に進み、その時のθ0を調整後の初期姿勢角θ0とする。これでθ0の調整が終了する。
【００３２】
初期姿勢角θ0の調整が終了した後、初期姿勢角φ0が未調整の場合は、図３に示すように、第２の初期姿勢角調整ステップS23に進む。第２の初期姿勢角調整ステップS23は、初期姿勢角φ0を微調整するステップである。図６に示すように、まずステップS231に進み、地理座標系の重力加速度ｇpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の重力加速度ｇpBに変換する。ステップS231は、先に説明したステップS221と同様の処理である。
【００３３】
ステップS231を行った後はステップS232に進み、加速度信号ａBのＹ軸成分a(y)から重力加速度ｇpBのＹ軸成分gp(y)を差し引いて、重力加速度の影響をキャンセルした補正加速度ah(y)を算出する。そして、補正加速度ah(y)に静止状態推定フィルタＡs1を適用し、積分値であるＹ軸方向の速度vh(y)を推定する（ステップS233）。この静止状態推定フィルタＡs1も、フィルタリング処理S1sで使用したのと同じものである。
【００３４】
速度vh(y)を推定すると、規定時間における速度vh(y)の平均値を計算する（ステップS234）。そして、速度vh(y)の絶対値が一定以上に大きい場合はステップS235に進み、初期姿勢角φ0を直前の値から変更（増減）させる。初期姿勢角φ0を変更してステップS231〜S234を繰り返し実行すると、やがて速度vh(y)の絶対値が一定以下（≒0）になるので、S236に進み、その時のφ0を調整後の初期姿勢角φ0とする。これでφ0の調整が終了する。
【００３５】
初期姿勢角φ0の調整が終了した後、初期姿勢角ψ0が未調整の場合は、図３に示すように、第３の初期姿勢角調整ステップS24に進む。第３の初期姿勢角調整ステップS24は、初期姿勢角ψ0を微調整するステップである。図７に示すように、まずステップS241に進み、地理座標系の地球角速度ωpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の地球角速度ωpBに変換する。
【００３６】
ステップS241を行った後はステップS242に進み、角速度信号ωBのＸ軸成分ω(x)から地球角速度ωpBのＸ軸成分ωp(x)を差し引いて、地球角速度の影響をキャンセルした補正角速度ωh(x)を算出する。そして、補正角速度ωh(x)に静止状態推定フィルタＡs1を適用し、積分値であるＸ軸方向の角度αh(x)を推定する（ステップS243）。この静止状態推定フィルタＡs1も、フィルタリング処理S1sで使用したのと同じものである。
【００３７】
角度αh(x)を推定すると、規定時間における角度αh(x)の平均値を計算する（ステップS244）。そして、角度αh(y)の絶対値が一定以上に大きい場合はステップS245に進み、初期姿勢角ψ0を直前の値から変更（増減）させる。初期姿勢角ψ0を変更してステップS241〜S244を繰り返し実行すると、やがて角度αh(x)の絶対値が一定以下（≒0）になるので、S246に進み、その時のψ0を調整後の初期姿勢角ψ0とする。これでψ0の調整が終了し、アライメント処理S2が終了する。
【００３８】
アライメント処理S2が終了した後、静止状態で角速度・加速度バイアスωbB，ａbB未算出の場合、角速度・加速度バイアス算出処理S3を行う。角速度・加速度バイアス算出処理S3は、図８に示すように、角速度バイアスωbBが未算出の場合、まずステップS31に進み、地理座標系の地球角速度ωpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の地球角速度ωpBに変換する。そして、ステップS32に進み、角速度信号ωBから地球角速度ωpBを差し引いて、角速度誤差ΔωBを算出する。
【００３９】
角速度誤差ΔωBを算出すると、規定時間における角速度誤差ΔωBの平均値を計算し、その結果を角速度バイアスωbBとし（ステップS34）、これで角速度バイアスωbBの算出が終了する。
【００４０】
角速度バイアスωbBの算出が終了した後、加速度バイアスａbBが未算出の場合はステップS35に進み、地理座標系の重力加速度ｇpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の重力加速度ｇpBに変換する。そして、ステップS36に進み、加速度信号ａBから重力加速度ωpBを差し引いて、加速度誤差ΔａBを算出する。
【００４１】
加速度誤差ΔａBを算出すると、規定時間における加速度誤差ΔａBの平均値を計算し、その結果を加速度バイアスａbBとする（ステップS34）。これで加速度バイアスａbBの算出が終了し、角速度・加速度バイアス算出処理S3が終了する。
【００４２】
角速度・加速度バイアス算出処理S3が終了した後、静止状態の時は、図２に示すように、フィルタリング処理S1sを経て、姿勢角算出処理S4sを行う。姿勢角算出処理S4sは、図９（ａ）に示すように、まずステップS41sに進み、地理座標系の地球角速度ωpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の地球角速度ωpBに変換する。そして、ステップS42sに進み、角速度信号ωBから地球角速度ωpBと角速度バイアスωbBを差し引いて、地球角速度等の誤差要因をキャンセルした補正角速度ωhBを算出する。
【００４３】
補正角速度ωhBを算出すると、ステップS43sに進み、機体座標系の補正角速度ωhBを、式（２）に示す座標変換行列Ｒ(B-A)を用いて姿勢角速度φ（ドット），θ（ドット），ψ（ドット）に変換する。座標変換行列Ｒ(B-A)は公知の行列である。
【数２】
0006712037.tif 000003
【００４４】
ステップS43sを行った後はステップS44sに進み、姿勢角速度φ（ドット），θ（ドット），ψ（ドット）にそれぞれ静止状態推定フィルタＡs1を適用し、積分値である姿勢角φ，θ，ψを推定する。この静止状態推定フィルタＡs1は、フィルタリング処理S1sで使用したのと同じものである。そして、座標変換行列Ｃ(G-B)を更新して姿勢角算出処理S4sが終了する（ステップS45s）。
【００４５】
姿勢角算出処理S4sが終了した後、静止状態の時は、図２に示すように、位置移動量算出処理S5sを行う。位置移動量算出処理S5sは、図１０（ａ）に示すように、まずステップS51sに進み、地理座標系の重力加速度ｇpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の重力加速度ｇpBに変換する。そして、ステップS52sに進み、加速度信号ａBから重力加速度ｇpBと加速度バイアスａbBを差し引いて、重力加速度等の誤差要因をキャンセルした補正加速度ａhBを算出する。
【００４６】
補正加速度ａhBを算出すると、ステップS53sに進み、機体座標系の補正加速度ａhBを、式（１）に示す座標変換行列Ｃ(G-B)を用いて地理座標系の加速度ａhGに変換する。
【００４７】
ステップS53sを行った後はステップS54sに進み、加速度ａhGの各軸成分に静止状態推定フィルタＡs2を適用し、２階積分値である位置移動量ｒGを推定する。静止状態推定フィルタＡs2は、先の静止状態推定フィルタＡs1と同様に、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。これで位置移動量算出処理S5sが終了する。
【００４８】
角速度・加速度バイアス算出処理S3が終了した後、運動状態の時は、図２に示すようにフィルタリング処理S1uを経て、姿勢角算出処理S4uを行う。フィルタリング処理S1uは、上記のフィルタリング処理S1sとは異なり、角速度信号ωBに重畳しているノイズ成分を低減する処理を、運動状態推定フィルタＡu1を使用した推定によって行う。運動状態推定フィルタＡu1は、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。
【００４９】
姿勢角算出処理S4uは、図９（ｂ）に示すように、ステップS41u〜S45uを順に行う。この中のステップS41u〜S43u，S45uは静止時のステップS41s〜S43s，S45sと同じ内容であり、異なるのはステップS44u（下線部）である。
【００５０】
ステップS44uでは、ステップS43uの処理結果である姿勢角速度φ（ドット），θ（ドット），ψ（ドット）に対して運動状態推定フィルタＡu1を適用し、積分値である姿勢角φ，θ，ψを推定する。この運動状態推定フィルタＡu1は、フィルタリング処理S1uで使用したのと同じものである。そして、座標変換行列Ｃ(G-B)を更新し、姿勢角算出処理S4uが終了する（ステップS45u）。
【００５１】
姿勢角算出処理S4uが終了した後、運動状態の時は、図２に示すように、位置移動量算出処理S5uを行う。位置移動量算出処理S5uは、図１０（ｂ）に示すように、ステップS51u〜S54uを順に行う。この中のステップS51u〜S53uは静止時のステップS51s〜S53sと同じ内容であり、異なるのはステップS54u（下線部）である。
【００５２】
ステップS54uでは、ステップS53uの処理結果である加速度ａhGの各軸成分に対して運動状態推定フィルタＡu2を適用し、２階積分値である位置移動量ｒGを推定する。この運動状態推定フィルタＡu2は、先の運動状態推定フィルタＡu1と同様に、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。これで位置移動量算出処理S5uが終了する。
【００５３】
このように、慣性計測装置１０は、移動体１２が静止している時に、フィルタリング処理S1s、アライメント処理S2、角速度・加速度バイアス算出処理S3を順番に実行し、その後、フィルタリング処理S1s又はS1u、姿勢角算出処理S4s又はS4u、位置移動量算出処理S5s又はS5uを順番に繰り返し実行することによって、角速度信号ωB及び加速度信号ａBに基づいて、移動体１２の姿勢角φ（ロール），θ（ピッチ），ψ（ヨー）及び位置移動量ｒGを導出する。
【００５４】
次に、４つの状態推定フィルタＡs1，Ａs2，Ａu1，Ａu2の中の、静止状態推定フィルタＡs1と運動状態推定フィルタＡu1について説明する。
【００５５】
静止状態推定フィルタＡs1は、静止時のアライメント処理S2、姿勢角算出処理S4sで１階積分を行う際に使用されるフィルタであり、静止時のフィルタリング処理S1sでも使用されている。静止状態推定フィルタＡs1には、推定を行うための演算式として次の式（３）、（４）が設定されている。
【数３】
0006712037.tif 000004
【数４】
0006712037.tif 000005
【００５６】
式（３）、（４）では、状態変数をｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。ｖはａの積分値であり、例えばａを加速度とすればｖが速度になり、ａを角速度とすればｖが角度になる。従って、ここでは変数は時間であり、一般的にtで表される。そして、式（３）の中の４行４列の行列が、静止時の信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値のランダムウォーク、及び積分を行う際の、バイアス起因の積分値の誤差の増大を抑制するように設定されている。行列式（３）の中のdtは、時間軸での変数tの微小期間である。この実施形態では例えば、時間軸上の上記タイミングｋの微小間隔であり、以下の式においても同様である。
【００５７】
運動状態推定フィルタＡu1は、運動時の姿勢角算出処理S4uで１階積分を行う際に使用されるフィルタであり、運動時のフィルタリング処理S1uでも使用されている。運動状態推定フィルタＡu1には、推定を行うための演算式として、上記の式（４）と次の式（５）とが設定されている。
【数５】
0006712037.tif 000006
【００５８】
式（４）、（５）では、状態変数をｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。そして、式（５）の中の４行４列の行列は、運動時の信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されている。
【００５９】
式（３）と式（５）を比較して分かるように、各行列の内容が異なるのは第２行である。静止状態推定フィルタＡs1の第２行は、図１１（ａ）に示すように、タイミングk-1の後のしばらくの間（例えば、タイミングk-1とタイミングkの中間点までの微小期間）、積分値ｖが一定に保持されるという考え方で設定されている。一方、運動状態推定フィルタＡu1の第２行は、図１１（ｂ）に示すように、タイミングk-1からタイミングkまでの微小期間、積分値ｖが一定の傾きで変化するという考え方で設定されている。この第２行の設定方法の違いにより、静止状態推定フィルタＡs1は静止状態に適した推定を行うことができ、運動状態推定フィルタＡu1は、運動状態に適した推定を行うことができる。
【００６０】
発明者は、静止状態推定フィルタＡs1の動作を確認するため、シミュレーションによる試験を行った。第１の試験は、図１２（ａ）に示すように、信号ａを白色雑音（統計的に制御されたノイズ発生手段が発生させた平均値０、分散１の信号）とし、静止状態推定フィルタＡs1を適用して信号ａと積分値ｖの推定を行った。また、比較例として、図１２（ｂ）に示すように、一般的な積分器２８を用いて信号ａの積分値ｖを計算した。
【００６１】
第１の試験の結果は、図１３（ａ）、（ｂ）に示す通りである。静止状態推定フィルタＡs1の推定ａを見ると、白色雑音（＝信号ａ）が低減していることが分かる。また、積分器２８の計算ｖ（比較例）を見ると、時間とともにゼロ点がランダムにドリフトしており、上述した積分を行う際の、ノイズ起因の積分値のランダムウォークが発生していることが分かる。これに対して、静止状態推定フィルタＡs1の推定ｖを見ると、ゼロ点がほとんどドリフトせず、ランダムウォークが効果的に抑制されている。
【００６２】
また、静止状態推定フィルタＡs1の動作を確認するため、第２の試験を行った。第２の試験は、図１４（ａ）に示すように、信号ａをバイアス（信号レベル１）に上記の白色雑音を重畳させたものとし、静止状態推定フィルタＡs1を適用して信号ａと積分値ｖの推定を行った。また、比較例として、図１４（ｂ）に示すように、一般的な積分器２８を用いて信号ａの積分値ｖを計算した。
【００６３】
第２の試験の結果は、図１５（ａ）、（ｂ）に示す通りである。静止状態推定フィルタＡs1の推定ａを見ると、信号ａに含まれる白色雑音が低減していることが分かる。また、積分器２８の計算ｖ（比較例）を見ると、時間とともにゼロ点が右肩上がりにドリフトしており、上述した積分を行う際の、バイアス起因の積分値の誤差の増大が発生していることが分かる。これに対して、静止状態推定フィルタＡs1の推定ｖを見ると、ゼロ点がほとんどドリフトせず、バイアス起因の誤差の増大が効果的に抑制されている。
【００６４】
さらに、運動状態推定フィルタＡu1の動作を確認するため、第３の試験を行った。第３の試験は、図１６（ａ）に示すように、信号ａを理想正弦波（振幅１、周波数４Hz）に上記の白色雑音を重畳させたものとし、運動状態推定フィルタＡu1を適用して信号ａと積分値ｖの推定を行った。また、比較例として、図１６（ｂ）に示すように、一般的な積分器２８を用いて信号ａの積分値ｖを計算した。
【００６５】
第３の試験の結果は、図１７（ａ）、（ｂ）に示す通りである。運動状態推定フィルタＡu1の推定ａを見ると、信号ａに含まれる白色雑音が低減していることが分かる。また、積分器２８の計算ｖ（比較例）を見ると、ノイズ起因による積分値の誤差が見られるが、運動状態推定フィルタＡu1の推定ｖを見ると、この積分によるノイズ起因の誤差が効果的に抑制されている。
【００６６】
次に、４つの状態推定フィルタＡs1，Ａs2，Ａu1，Ａu2の中の、静止状態推定フィルタＡs2と運動状態推定フィルタＡu2について説明する。
【００６７】
静止状態推定フィルタＡs2は、静止時の位置移動量算出処理S5sで２階積分を行う際に使用されるフィルタである。静止状態推定フィルタＡs2には、推定を行うための演算式として次の式（６）、（７）が設定されている。
【数６】
0006712037.tif 000007
【数７】
0006712037.tif 000008
【００６８】
式（６）、（７）では、状態変数をｒ(k)、ｒ(k-1)、ｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。ｖはaの積分値であり、ｒはｖの積分値である。つまり、ａを加速度とすれば、ｖが速度でｒが位置移動量になる。そして、式（６）の中の６行６列の行列が、静止時の信号に重畳しているノイズを低減し、且つノイズ起因の積分値のランダムウォーク及びバイアス起因の積分値の誤差の増大を抑制するように設定されている。
【００６９】
運動状態推定フィルタＡu2は、運動時の位置移動量算出処理S5uで２階積分を行う際に使用されるフィルタである。運動状態推定フィルタＡu2には、推定を行うための演算式として、上記の式（７）と次の式（８）とが設定されている。
【数８】
0006712037.tif 000009
【００７０】
式（７）、（８）では、状態変数をｒ(k)、ｒ(k-1)、ｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。そして、式（８）の中の６行６列の行列は、運動時の信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されている。
【００７１】
式（６）と式（８）を比較して分かるように、各行列の内容が異なるのは第２行と第４行である。第４行の違いは、図１１（ａ）、（ｂ）を用いて先に説明した通りである。すなわち、静止状態推定フィルタＡs2の第４行は、タイミングk-1の後のしばらくの間、積分値ｖが一定に保持されるという考え方で設定され、運動状態推定フィルタＡu2の第４行は、図１１（ｂ）に示すように、タイミングk-1からタイミングkまでの間、積分値ｖが一定の傾きで変化するという考え方で設定されている。
【００７２】
第２行の考え方も、第４行の考え方と同じである。すなわち、静止状態推定フィルタＡs2の第２行は、タイミングk-1の後のしばらくの間、積分値ｒが一定に保持されるという考え方で設定され、運動状態推定フィルタＡu2の第２行は、タイミングk-1からタイミングkまでの間、積分値ｒが一定の傾きで変化するという考え方で設定されている。
【００７３】
この第２行及び第４行の設定方法の違いにより、静止状態推定フィルタＡs2は、静止状態に適した推定を行うことができ、運動状態推定フィルタＡu2は、運動状態に適した推定を行うことができる。
【００７４】
以上説明したように、この実施形態の慣性計測方法及びその慣性計測プログラムがインストールされた慣性計測装置１０は、静止している時にアライメント処理S2と角速度・加速度バイアス算出処理S3を行い、姿勢角及び位置移動量を算出する時に、重力加速度及び地球角速度の成分をキャンセルする処理を行うので、重力加速度及び地球角速度に起因する誤差を小さくすることができる。
【００７５】
さらに、各処理で推定を行うとき、静止状態推定フィルタＡs1，Ａs2と運動状態推定フィルタＡu1，Ａs2とを使い分けている。従って、慣性計測で問題になる（ａ）信号に重畳しているノイズによる誤差、（ｂ）ノイズ起因の積分値のランダムウォーク、（ｃ）バイアス起因の積分値の誤差の増大、（ｄ）ノイズ起因の積分値の誤差等に対し、静止時は静止状態推定フィルタＡs1，Ａs2を使用した推定を行って（ａ）、（ｂ）、（ｃ）を効果的に抑制し、運動時は運動状態推定フィルタＡu1，Ａu2を使用した推定を行って（ａ）、（ｄ）を効果的に抑制することができ、姿勢角φ，θ，ψ及び位置移動量ｒGを高精度に計測することができる。
【００７６】
慣性計測装置１０は、汎用のコンピュータで容易に構成することができ、しかも、特許文献１の移動体姿勢角検出装置のように、ランダムウォークを抑制するために第二の検出手段を付設する必要もない。従って、装置を非常にシンプルに構成することができる。
【００７７】
次に、以下、本発明の慣性計測装置及び慣性計測方法の第二の実施形態について、図１８〜図２１に基づいて説明する。この実施形態の慣性計測装置３０（及び慣性計測方法）は、上記の慣性計測装置１０（及び慣性計測方法）の構成の一部を変更したものであり、ここでは、慣性計測装置１０と同様の構成は同一の符号を付して説明を省略する。
【００７８】
第一の実施形態の慣性計測装置１０は、必要なセンサ素子の種類が少ない（角速度センサ１４及び加速度センサ１６の２種類だけ）という特徴があるが、上記のアライメントを効果的に行うには、地球角速度を精度よく検出できる高性能な角速度センサ１４を使用する必要があり、装置が高価になる可能性がある。これに対して、第二の実施形態の慣性計測装置３０は、図１８に示すように、初期姿勢角ψ0の算出に電子コンパス３２の方位信号Ｈを利用することにより、安価な角速度センサ１４が使用可能になり、装置全体のコストを抑えることができるという特徴がある。
【００７９】
慣性計測装置３０が行う慣性計測方法の全体の流れは、慣性計測装置１０の流れ（図２）と同様であるが、アライメント処理S2、角速度・加速度バイアス算出処理S3及び姿勢角算出処理S4s，S4uの内容に少し異なる部分があるので、異なる部分について順番に説明する。
【００８０】
慣性計測装置３０が行うアライメント処理S2は、図１９に示すように、３つの初期姿勢角φ0，θ0，ψ0のうち、φ0，θ0についての処理は慣性計測装置１０と同じであるが、初期姿勢角ψ0は、ステップS62に示すように、電子コンパス３２の方位信号Ｈに基づいて算出し、調整は行わない。従って、初期姿勢角算出ステップS61では、図２０に示すように、初期姿勢角φ0，θ0だけを算出し(ステップS611)、これを調整する（ステップS612）という処理になっている。安価な角速度センサは、微小な地球角速度を高精度に検出することが難しいが、電子コンパス３２の方位信号Ｈを利用することによって、初期姿勢角ψ0を算出することができ、アライメント処理を行うことができる。地球角速度よりも高速の角速度の検出は、安価な角速度センサでも可能なので、角速度センサ１４のコストダウンを図ることができる。
【００８１】
その他、角速度・加速度バイアス算出処理S3では、図２１のステップS71，S72に示すように、角速度誤差ΔωBを算出する時、地球角速度ωpBをゼロと仮定して行う。同様に、姿勢角算出処理S4s，S4uでは、図２２のステップS81s，S82sとステップS81u，S82uに示すように、角速度誤差ΔωBを算出する時、地球角速度ωpBをゼロと仮定して行う。
【００８２】
慣性計測装置３０及びその慣性計測方法によれば、慣性計測装置１０の場合と同様の効果を得ることができ、必要なセンサの種類は増えるものの、装置全体のコストダウンを図ることができる。
【００８３】
なお、本発明の慣性計測方法と慣性計測装置及び慣性計測プログラムは、上記実施形態に限定されるものではない。例えば、静止状態推定フィルタの設定は、上記の静止状態推定フィルタＡs1（式（３）と（４））やＡs2（式（６）と（７））の設定に限定されるものではなく、上述した効果が得られる範囲で、式の内容は変更してもよい。また、上記実施形態では、静止状態推定フィルタＡs1を複数の処理で兼用しているが、個々の処理内容に応じて設定を変えたものを使用してもよい。
【００８４】
運動状態推定フィルタの設定は、上記の運動状態推定フィルタＡu1（式（４）と（５））やＡu2（式（７）と（８））の設定に限定されるものではなく、上述した効果が得られる範囲で、式の内容は変更してもよい。また、上記実施形態では、運動状態推定フィルタＡu1を複数の処理で兼用しているが、個々の処理内容に応じて設定を変えたものを使用してもよい。
【００８５】
また、本発明の慣性計測方法及び慣性計測装置は、空中を飛行するマルチコプタ等の慣性計測に好適であるが、マルチコプタ以外の移動体にも適用できることは言うまでもなく、上記の優れた効果を得ることができる。
【符号の説明】
【００８６】
１０，３０  慣性計測装置
１２  移動体
１２ａ  機体
１４  角速度センサ
１６  加速度センサ
１８  フィルタリング部
２０  アライメント部
２２  角速度・加速度バイアス算出部
２４  姿勢角算出部
２６  位置移動量算出部
２８  積分器
Ａs1，Ａs2  静止状態推定フィルタ
Ａu1，Ａu2  運動状態推定フィルタ
Ｓ1s，Ｓ1u  フィルタリング処理
Ｓ2  アライメント処理
Ｓ3  角速度・加速度バイアス算出処理
Ｓ4s，Ｓ4u  姿勢角算出処理
Ｓ5s，Ｓ5u  位置移動量算出処理
【図１】
0006712037.tif 000010
【図２】
0006712037.tif 000011
【図３】
0006712037.tif 000012
【図４】
0006712037.tif 000013
【図５】
0006712037.tif 000014
【図６】
0006712037.tif 000015
【図７】
0006712037.tif 000016
【図８】
0006712037.tif 000017
【図９】
0006712037.tif 000018
【図１０】
0006712037.tif 000019
【図１１】
0006712037.tif 000020
【図１２】
0006712037.tif 000021
【図１３】
0006712037.tif 000022
【図１４】
0006712037.tif 000023
【図１５】
0006712037.tif 000024
【図１６】
0006712037.tif 000025
【図１７】
0006712037.tif 000026
【図１８】
0006712037.tif 000027
【図１９】
0006712037.tif 000028
【図２０】
0006712037.tif 000029
【図２１】
0006712037.tif 000030
【図２２】
0006712037.tif 000031
【図２３】
0006712037.tif 000032
【図２４】
0006712037.tif 000033