JPWO2015162873A1 - 位置姿勢推定装置、画像処理装置及び位置姿勢推定方法 - Google Patents

Abstract

位置姿勢の推定精度を向上することを可能とする。位置姿勢推定装置１０は、移動体の位置姿勢に関する複数の第１の位置姿勢パラメータを検出する第１の検出部１と、検出した複数の第１の位置姿勢パラメータに基づいて、第１のタイミングにおける移動体の第１の位置姿勢を推定する第１の位置姿勢推定部２と、移動体の位置姿勢に関する複数の第２の位置姿勢パラメータを検出する第２の検出部３と、検出した複数の第２の位置姿勢パラメータに基づいて、第１のタイミングと異なる第２のタイミングにおける移動体の第２の位置姿勢を推定する第２の位置姿勢推定部４と、推定した第１の位置姿勢と第２の位置姿勢に基づき第３の位置姿勢を出力する位置姿勢出力部５と、を備える。

Description

本発明は、位置姿勢推定装置、画像処理装置及び位置姿勢推定方法に関し、特に、センサを用いて位置姿勢を推定する位置姿勢推定装置、画像処理装置及び位置姿勢推定方法に関する。

宇宙機や飛翔体、無人航空機、無人自動車両などの移動体の位置姿勢（位置及び姿勢）を推定する位置姿勢推定装置が知られている。例えば、特許文献１〜４には、関連する技術が記載されている。

図７は、特許文献１に記載された関連する移動体位置姿勢推定装置９００の一例を示すブロック図である。関連する移動体位置姿勢推定装置９００は、移動体に搭載されており、各センサの検出に基づいて移動体の位置姿勢を推定する。図７に示すように、関連する移動体位置姿勢推定装置９００は、ジャイロセンサ検出信号９０５を出力するジャイロセンサ９０１、ＧＰＳ（Global Positioning System）受信機出力信号９０６を出力するＧＰＳ受信機９０２を備えている。また、移動体位置姿勢推定装置９００は、加速度センサ検出信号９０７を出力する加速度センサ９０３、移動体位置姿勢推定装置出力信号９０８を出力する演算制御部９０４を備えている。

関連する移動体位置姿勢推定装置９００では、ＧＰＳ受信機９０２が出力するＧＰＳ受信機出力信号９０６と加速度センサ９０３が出力する加速度センサ検出信号９０７とから演算制御部９０４が移動体位置を推定する。移動体位置姿勢推定装置９００では、これと共に、ジャイロセンサ９０１が出力するジャイロセンサ検出信号９０５とＧＰＳ受信機出力信号９０６の変動とから演算制御部９０４が移動体姿勢を推定し、それらをまとめて移動体位置姿勢推定装置出力信号９０８を出力する。

特許文献１では、複合航法システムは、図７の演算制御部９０４が、非線形な運動方程式で表現される移動体運動に対しカルマンフィルタを拡張した誤差モデルを適用する。この構成によって、複合航法システムは、ＭＥＭＳ（Micro Electro Mechanical System）センサなど軽量、小型で、かつ安価なセンサ等を利用しても、検出値を効果的に真値に近い値へ補正可能となっている。

一方、関連する姿勢計測装置の一例として、例えば特許文献２などに示された関連する姿勢計測方式では、カルマンフィルタをベースに伝播処理における積分処理や時間更新における状態推定ならびにその状態量に対する誤差共分散を更新する必要がない。したがってこの姿勢計測装置の処理内容がより簡単となり短時間で物体の姿勢を計測することができ、さらに姿勢計測装置のコストも低減化できる。

また、例えば特許文献３などに記載された関連する技術では、カルマンフィルタでの航法計算で必須となる基準座標決定処理に関し、カルマンフィルタの収束状況が監視され適切な補償処理が施される。このことで、ある程度の動揺環境においても基準座標決定処理の高精度・高速化が実現され、運用上の制約が小さくできる。

さらに、例えば特許文献４などに記載された関連する技術では、カルマンフィルタ処理における座標変換マトリクスの姿勢角誤差修正と方位角誤差修正が分離され、更に方位角誤差修正には方位角の設定値が使用される。このことで、３軸のアライメント演算が短時間で実行され、運用制約の低減化が可能となる。

特開２００７−２７６５０７号公報（段落０００７、図１） 特開２００６−３８６５０号公報（段落００７０、図１） 特開２００１−２６４１０６号公報（段落００２１、図１） 特許第３９１９９１１号公報（段落００６５、図１）

図７に示すような関連する移動体位置姿勢推定装置９００では、ＧＰＳ受信機９０２と加速度センサ９０３とジャイロセンサ９０１の検出信号に基づいて、演算制御部９０４が一定の周期（タイミング）で位置姿勢を推定する。移動体位置姿勢推定装置９００では、この位置姿勢推定処理の周期により所望の推定精度を得ている。しかしながら、この関連する技術では、演算制御部９０４のみの位置姿勢推定処理の周期（タイミング）では、位置姿勢の推定精度を向上させることが困難であるという問題がある。

本発明は、このような課題に鑑み、位置姿勢の推定精度を向上することが可能な位置姿勢推定装置、画像処理装置及び位置姿勢推定方法を提供することを目的とする。

本発明に係る位置姿勢推定装置は、移動体の位置姿勢に関する複数の第１の位置姿勢パラメータを検出する第１の検出部と、検出した前記複数の第１の位置姿勢パラメータに基づいて、第１のタイミングにおける前記移動体の第１の位置姿勢を推定する第１の位置姿勢推定部と、前記移動体の位置姿勢に関する複数の第２の位置姿勢パラメータを検出する第２の検出部と、検出した前記複数の第２の位置姿勢パラメータに基づいて、前記第１のタイミングと異なる第２のタイミングにおける前記移動体の第２の位置姿勢を推定する第２の位置姿勢推定部と、推定した前記第１の位置姿勢と前記第２の位置姿勢に基づき第３の位置姿勢を出力する位置姿勢出力部と、を備えるものである。

本発明に係る画像処理装置は、移動体の位置姿勢を推定する移動体位置姿勢推定部と、前記移動体に搭載されたレーダが観測した信号に対し、前記推定した位置姿勢に基づき画像処理を行う画像処理部と、を備えた画像処理装置であって、前記移動体位置姿勢推定部は、前記移動体の位置姿勢に関する複数の第１の位置姿勢パラメータを検出する第１の検出部と、検出した前記複数の第１の位置姿勢パラメータに基づいて、第１のタイミングにおける前記移動体の第１の位置姿勢を推定する第１の位置姿勢推定部と、前記移動体の位置姿勢に関する複数の第２の位置姿勢パラメータを検出する第２の検出部と、検出した前記複数の第２の位置姿勢パラメータに基づいて、前記第１のタイミングと異なる第２のタイミングにおける前記移動体の第２の位置姿勢を推定する第２の位置姿勢推定部と、推定した前記第１の位置姿勢と前記第２の位置姿勢に基づき第３の位置姿勢を、前記画像処理部へ出力する位置姿勢出力部と、を備えるものである。

本発明に係る位置姿勢推定方法は、移動体の位置姿勢に関する複数の第１の位置姿勢パラメータを検出し、検出した前記複数の第１の位置姿勢パラメータに基づいて、第１のタイミングにおける前記移動体の第１の位置姿勢を推定し、前記移動体の位置姿勢に関する複数の第２の位置姿勢パラメータを検出し、検出した前記複数の第２の位置姿勢パラメータに基づいて、前記第１のタイミングと異なる第２のタイミングにおける前記移動体の第２の位置姿勢を推定し、推定した前記第１の位置姿勢と前記第２の位置姿勢に基づき第３の位置姿勢を出力するものである。

本発明によれば、位置姿勢の推定精度を向上することが可能な位置姿勢推定装置、画像処理装置及び位置姿勢推定方法を提供することができる。

実施の形態に係る位置姿勢推定装置の概要を示す構成図である。 実施の形態１に係る移動体を模式的に示す模式図である。 実施の形態１に係るレーダ装置の構成を示すブロック図である。 実施の形態１に係る位置姿勢推定装置の構成を示すブロック図である。 実施の形態１に係る位置姿勢推定装置の動作を示すフローチャートである。 実施の形態２に係る位置姿勢推定装置の構成を示すブロック図である。 関連する位置姿勢推定装置の構成を示すブロック図である。

（実施の形態の概要）
図１は、実施の形態に係る位置姿勢推定装置１０の概要を示す構成図である。図１に示すように、実施の形態に係る位置姿勢推定装置１０は、第１の検出部１、第１の位置姿勢推定部２、第２の検出部３、第２の位置姿勢推定部４、位置姿勢出力部５を備えている。

第１の検出部１は、移動体の位置姿勢に関する複数の第１の位置姿勢パラメータを検出する。第１の位置姿勢推定部２は、第１の検出部１が検出した複数の第１の位置姿勢パラメータに基づいて、第１のタイミングにおける移動体の第１の位置姿勢を推定する。第２の検出部３は、移動体の位置姿勢に関する複数の第２の位置姿勢パラメータを検出する。第２の位置姿勢推定部４は、第２の検出部３が検出した複数の第２の位置姿勢パラメータに基づいて、第１のタイミングと異なる第２のタイミングにおける移動体の第２の位置姿勢を推定する。位置姿勢出力部５は、推定した第１の位置姿勢と第２の位置姿勢に基づき第３の位置姿勢を出力する。

このように、第１のタイミングと第２のタイミングのそれぞれで移動体の位置姿勢を推定し、推定したこれらの位置姿勢に基づいて第３の位置姿勢を出力する。このことにより、第１または第２の位置姿勢推定部のみで位置姿勢を推定する場合に比べて、位置姿勢の推定精度をより向上することができる。

ここで、上記関連する技術の具体的な問題について説明する。図７に示すような移動体位置姿勢推定装置（特許文献１）では、搭載センサがＧＰＳ受信機９０２と加速度センサ９０３とジャイロセンサ９０１の３種のみで構成される。このため、特に姿勢推定精度は、適用するジャイロセンサ９０１の単体検出性能に依存してしまい、カルマンフィルタにおける誤差モデル精度を向上させても達成可能な推定精度には限界があるという問題点がある。

また、特許文献２に記載された技術では、センサとして３軸加速度センサと３軸ジャイロのみが適用され、それらを用いたカルマンフィルタ処理により姿勢推定が実現されている。このため、ここから導出される姿勢推定精度は、３軸ジャイロの検出性能とカルマンフィルタを構成する誤差モデルの精度との間で生じる精度バランスに依存してしまい、絶対的な姿勢精度を確保することが困難であるという欠点がある。

一方、特許文献３に記載された技術では、構成自体はジャイロセンサと加速度センサからなるカルマンフィルタとなっているものの、推定誤差を除去する構成ではなくあくまでカルマンフィルタの収束状態を監視するものである。したがって、カルマンフィルタの適用目的は本発明と異なっている。

さらに、特許文献４に記載された技術では、姿勢角誤差修正において精度確保が難しい方位角誤差修正部分のみはあえて分離され、他の姿勢角誤差修正値が得られてから逐次的に方位角誤差修正が実施される。このため、得られる方位角誤差修正値は、ベースとする姿勢角誤差修正値の精度に依存する結果となり、３軸姿勢情報の直交性（独立性）が確保できないという問題がある。

上記関連する技術の問題点に鑑み、以下の実施の形態では、宇宙機や飛翔体や無人航空機や無人自動車などの移動体における位置姿勢を高精度に推定することが可能となる。特に、移動体に搭載する駆動機器の現在の姿勢を、移動体本体に搭載した位置姿勢推定装置と、搭載駆動機器に装着した姿勢推定装置とを陽に組み合わせることで、搭載駆動機器の姿勢を高精度に逐次推定可能とする移動体搭載機器の姿勢推定装置を提供することが可能となる。
（実施の形態１）
以下、図面を参照して実施の形態１について説明する。本実施の形態では、レーダ装置を搭載する移動体において、移動体の位置姿勢、特に搭載するレーダアンテナの位置姿勢を推定する例について説明する。

図２は、本実施の形態に係る移動体３０を模式的に示している。本実施の形態に係る移動体３０は、宇宙機、飛翔体、無人航空機、無人自動車などであり、例えば小型航空機やヘリコプターである。移動体３０は、レーダ装置１００を搭載しており、移動しながら地表の状態等を観測する。レーダ装置１００は、一例として、合成開口レーダ（ＳＡＲ：Synthetic Aperture Radar）である。合成開口レーダは、アンテナによって得られた受信波の振幅と位相のデータ（複素データ）から、アンテナ（移動体）の位置姿勢に基づいて観測画像を再生し、地表の状態を判読する。ここでは、合成開口レーダを用いるため、特にアンテナの位置姿勢を精度よく推定する必要がある。なお、レーダ装置１００は、合成開口レーダに限らず、捜索レーダなどその他のレーダであってもよい。

レーダ装置１００は、主に、アンテナを含むアンテナ部１０１、アンテナが送受信する電波を信号処理する信号処理部１０２を備える。例えば、アンテナ部１０１は、移動体３０の機体３１の前方下部に設置され、アンテナから機体３１下方の地表へ向けて電波を照射する。信号処理部１０２は、移動体３０の機体３１内部に設置され、アンテナを介して観測した観測画像をリアルタイムで表示する。さらに、移動体３０の機体３１には、機体３１の位置、速度、角速度を検出する機体センサ１０４が設けられ、アンテナ部１０１には、アンテナの位置、速度、角速度を検出するアンテナセンサ１０３が設けられている。

図３は、本実施の形態に係るレーダ装置１００の機能ブロックを示している。図３に示すように、本実施の形態に係るレーダ装置１００は、送信部１１０、受信部１１１、画像処理部１１２、サーキュレータ１１３、制御部１１４、位置姿勢推定部１１５、アンテナ２０１、アンテナ駆動機構２０２、アンテナセンサ１０３、機体センサ１０４を備えている。

例えば、図２のアンテナ部１０１には、アンテナ２０１、アンテナ駆動機構２０２、アンテナセンサ１０３が含まれ、図２の信号処理部１０２には、送信部１１０、受信部１１１、画像処理部１１２、サーキュレータ１１３、制御部１１４、位置姿勢推定部１１５、機体センサ１０４が含まれる。また、アンテナセンサ１０３、機体センサ１０４、位置姿勢推定部１１５が、移動体３０（アンテナ）の位置姿勢を推定する位置姿勢推定装置１２０を構成する。さらに、位置姿勢推定装置１２０と画像処理部１１２とは、レーダの観測画像（ＳＡＲ画像）を生成する画像処理装置を構成しているとも言える。

送信部１１０は、合成開口レーダで観測を行うための送信信号を生成する。サーキュレータ１１３は、送信部１１０が生成した送信信号をアンテナ２０１から送信し、アンテナ２０１が受信した受信信号を受信部１１１へ出力する。アンテナ駆動機構２０２は、制御部１１４の制御に応じて最適な方向や位置となるようにアンテナ２０１を駆動する。アンテナ２０１は、送信波（送信信号）を観測対象へ送信し、観測対象から反射した受信波（受信信号）を受信する。

機体センサ１０４は、移動体３０の機体３１の移動、姿勢（ロール、ピッチ、ヨーの変位）や振動などを検出する。機体センサ１０４は、移動体３０の位置姿勢に関する位置姿勢パラメータとして、例えば、位置、加速度、角速度を検出する。アンテナセンサ１０３は、移動体３０の移動や振動、アンテナ駆動機構２０２の駆動に応じて変位したアンテナ２０１の変位を検出する。アンテナセンサ１０３は、アンテナ（移動体）の位置姿勢に関する位置姿勢パラメータとして、例えば、位置、加速度、角速度を検出する。すなわち、アンテナセンサ１０３及び機体センサ１０４は、後述のように、ＧＰＳ受信機、加速度センサ、ジャイロセンサなどのセンサを含む。

受信部１１１は、アンテナ２０１が受信した受信信号に対して信号処理を行い、画像処理部１１２が処理可能な信号を生成する。制御部１１４は、レーダ装置の各部を制御する制御部であり、アンテナセンサ１０３及び機体センサ１０４の検出結果や、画像処理部１１２の観測結果に基づいてアンテナ駆動機構２０２などを制御する。

位置姿勢推定部１１５（位置姿勢推定装置１２０）は、アンテナセンサ１０３及び機体センサ１０４の検出に基づいて、移動体３０（アンテナ）の位置姿勢を推定する。画像処理部１１２は、受信部１１１が処理した受信信号に対して、位置姿勢推定部１１５が推定した位置姿勢（姿勢補間推定値）を用いて画像処理を行い、観測対象の検出、観測画像の生成及び表示を行う。例えば、画像処理部１１２が画像処理を行うタイミング（周期）を位置姿勢推定部１１５へ出力し、位置姿勢推定部１１５が、この画像処理のタイミングに合わせて、位置姿勢推定結果を出力してもよい。

図４は、本実施の形態に係る位置姿勢推定装置１２０の構成を示すブロック図である。図４に示すように、本実施の形態に係る位置姿勢推定装置１２０は、高精度位置姿勢推定部１３０、小型軽量位置姿勢推定部１４０、位置姿勢出力部１５０を備えている。ここで図３の位置姿勢推定部１１５は、例えば、高精度位置姿勢推定部１３０の高精度位置姿勢推定演算器１３５、小型軽量位置姿勢推定部１４０の小型軽量位置姿勢推定演算器１４５、位置姿勢出力部１５０を備えている。

高精度位置姿勢推定部１３０は、小型軽量位置姿勢推定部１４０よりも高精度に位置姿勢を推定する推定装置である。高精度位置姿勢推定部１３０は、例えば小型軽量位置姿勢推定部１４０よりも大型の装置である。高精度位置姿勢推定部１３０は、機体センサ１０４の検出に基づき機体の位置姿勢を推定する。高精度位置姿勢推定部１３０は、ＧＰＳ受信機１３１、高精度加速度センサ１３２、高精度ジャイロセンサ１３３、高精度位置姿勢推定演算器１３５を備えている。

機体センサ（第１の検出部）１０４は、ＧＰＳ受信機１３１、高精度加速度センサ１３２、高精度ジャイロセンサ１３３を備えている。ＧＰＳ受信機１３１は、機体の絶対位置を検出し、高精度位置姿勢推定演算器（第１の位置姿勢推定部）１３５に検出した絶対位置を含む移動体絶対位置検出信号１１を出力する。高精度加速度センサ１３２は、機体の加速度を検出し、高精度位置姿勢推定演算器（第１の位置姿勢推定部）１３５に検出した加速度を含む移動体加速度検出信号１２を出力する。

高精度ジャイロセンサ１３３は、機体の角速度を検出し、高精度位置姿勢推定演算器（第１の位置姿勢推定部）１３５に検出した角速度を含む移動体姿勢角速度検出信号１３を出力する。高精度加速度センサ１３２、高精度ジャイロセンサ１３３は、例えば、小型軽量位置姿勢推定部１４０のセンサ（小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ）よりも高精度かつ大型のセンサである。

高精度位置姿勢推定演算器（第１の位置姿勢推定部）１３５は、ＧＰＳ受信機１３１、高精度加速度センサ１３２及び高精度ジャイロセンサ１３３が検出した絶対位置、加速度及び角速度に基づいて、移動体（機体）の位置姿勢を推定する。高精度位置姿勢推定演算器（第１の位置姿勢推定部）１３５は、推定した位置姿勢（位置姿勢伝播値）を含む移動体高精度位置姿勢推定信号１４を出力する。高精度位置姿勢推定演算器１３５は、例えば、パーソナルコンピュータなどの演算装置である。高精度位置姿勢推定演算器１３５は、画像処理部と同じ装置であってもよい。高精度位置姿勢推定演算器１３５は、ＧＰＳ受信機１３１が検出した絶対位置と高精度加速度センサ１３２が検出した加速度と高精度ジャイロセンサ１３３が検出した角速度とから機体の位置姿勢を推定し、推定した位置姿勢（位置姿勢伝播値）を含む移動体高精度位置姿勢推定信号１４を出力する。

高精度位置姿勢推定演算器１３５は、例えば、ＧＰＳ受信機１３１が検出した位置を基準とし、高精度加速度センサ１３２が検出した加速度と高精度ジャイロセンサ１３３が検出した角速度とを用いて、カルマンフィルタ処理を行う。これによって高精度位置姿勢推定演算器１３５は、第１のタイミング（周期）の位置姿勢を推定する。高精度位置姿勢推定演算器１３５は、例えば５０〜２００Ｈｚ周期で、位置姿勢を推定し移動体高精度位置姿勢推定信号１４を出力する。高精度位置姿勢推定演算器１３５が位置姿勢を推定する周期は、小型軽量位置姿勢推定部１４０よりも長い。高精度位置姿勢推定演算器１３５は、検出した加速度及び角速度の値に基づいてカルマンフィルタ処理により演算した位置姿勢（位置姿勢伝播値）を出力する伝播器１３５ａを含む。また高精度位置姿勢推定演算器１３５は、出力した位置姿勢（位置姿勢伝播値）及びＧＰＳ受信機が検出した位置に基づいて次回の加速度及び角速度の検出時における位置姿勢を推定する推定器１３５ｂを含む。

小型軽量位置姿勢推定部１４０は、推定する位置姿勢の精度が高精度位置姿勢推定部１３０よりも低い。また小型軽量位置姿勢推定部１４０は、例えば高精度位置姿勢推定部１３０よりも小型かつ軽量の位置姿勢装置である。小型軽量位置姿勢推定部１４０は、例えばアンテナに設置されるアンテナセンサ１０３の検出する絶対位置、加速度及び角速度に基づきアンテナの位置姿勢を推定する。小型軽量位置姿勢推定部１４０は、ＧＰＳ受信機１４１、小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４、小型軽量位置姿勢推定演算器１４５を備えている。なお、この例では、合成開口レーダの観測を行うため、アンテナに設置されたセンサに基づき、小型軽量位置姿勢推定部１４０がアンテナの位置姿勢を推定することが好ましいが、これに限らず、機体（移動体）に設置されたセンサに基づき、小型軽量位置姿勢推定部１４０が機体（移動体）の位置姿勢を推定してもよい。

アンテナに設置されたアンテナセンサ（第２の検出部）１０３は、ＧＰＳ受信機１４１、小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４を備えている。ＧＰＳ受信機１４１は、アンテナの絶対位置を検出し、小型軽量位置姿勢推定演算器（第２の位置姿勢推定部）１４５に検出した絶対位置を含む絶対位置検出信号１５を出力する。小型軽量加速度センサ１４２は、アンテナの加速度を検出し、小型軽量位置姿勢推定演算器（第２の位置姿勢推定部）１４５に検出した加速度を含む小型軽量加速度センサ検出信号１６を出力する。

小型軽量ジャイロセンサ１４３は、アンテナの角速度を検出し、小型軽量位置姿勢推定演算器（第２の位置姿勢推定部）１４５に検出した角速度を含む小型軽量ジャイロセンサ検出信号１７を出力する。ジャイロセンサ１４４は、アンテナの角速度を検出し、小型軽量位置姿勢推定演算器（第２の位置姿勢推定部）１４５に検出した角速度を含むジャイロセンサ検出信号１８を出力する。

ＧＰＳ受信機１４１は、高精度位置姿勢推定部１３０のＧＰＳ受信機１３１と同様のセンサである。小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３は、高精度位置姿勢推定部１３０の高精度加速度センサ１３２、高精度ジャイロセンサ１３３よりも小型軽量かつ低精度のセンサである。ジャイロセンサ１４４は、中型のセンサであり、小型軽量ジャイロセンサ１４３よりも高精度のセンサである。なお、アンテナセンサ（第２の検出部）１０３がジャイロセンサ１４４を備え、ジャイロセンサ１４４が小型軽量ジャイロセンサ１４３の検出精度を補完することが好ましいが、アンテナセンサ（第２の検出部）１０３はジャイロセンサ１４４を備えることなく、小型軽量ジャイロセンサ１４３のみを備えていてもよい。

小型軽量位置姿勢推定演算器（第２の位置姿勢推定部）１４５は、ＧＰＳ受信機１４１の検出した絶対位置、小型軽量加速度センサ１４２の検出した加速度、小型軽量ジャイロセンサ１４３の検出した角速度、ジャイロセンサ１４４の検出した角速度に基づいて、アンテナの位置姿勢を推定し、推定した位置姿勢（位置姿勢伝播値）を含む小型軽量位置姿勢推定演算器生成信号１９を出力する。なお、小型軽量位置姿勢推定演算器（第２の位置姿勢推定部）１４５は、ＧＰＳ受信機１４１を備えず、高精度位置姿勢推定部１３０のＧＰＳ受信機１３１の検出した絶対位置に基づいて、位置姿勢を推定してもよい。小型軽量位置姿勢推定演算器１４５は、ＣＰＵを含む半導体装置など小型の演算装置である。

小型軽量位置姿勢推定演算器１４５は、ＧＰＳ受信機１４１が検出した位置を基準とし、小型軽量加速度センサ１４２が検出した加速度と小型軽量ジャイロセンサ１４３が検出した角速度とジャイロセンサ１４４が検出した角速度とを用いてカルマンフィルタ処理を行う。これによって小型軽量位置姿勢推定演算器１４５は、第２のタイミング（周期）の位置姿勢を推定する。小型軽量位置姿勢推定演算器１４５は、例えば４００Ｈｚ周期で位置姿勢を推定する。小型軽量位置姿勢推定演算器１４５が位置姿勢を推定する周期は、高精度位置姿勢推定部１３０よりも長い。小型軽量位置姿勢推定演算器１４５は、検出された加速度及び角速度の値に基づいて得られた位置姿勢（位置姿勢伝播値）を出力する伝播器１４５ａと、出力した位置姿勢（位置姿勢伝播値）及びＧＰＳ受信機が検出した位置に基づいて次回の加速度及び角速度の検出時における位置姿勢を推定する推定器１４５ｂを含む。

位置姿勢出力部１５０は、高精度位置姿勢推定部１３０から出力された位置姿勢と小型軽量位置姿勢推定部１４０から出力された位置姿勢に基づいて、移動体（アンテナ）の位置姿勢を推定し、推定した位置姿勢を含む位置姿勢補間推定値２０を出力する。位置姿勢出力部１５０は、高精度位置姿勢推定部１３０と小型軽量位置姿勢推定部１４０が推定した位置姿勢を含めて出力してもよい。位置姿勢出力部１５０は、例えば、パーソナルコンピュータなどの演算装置である。位置姿勢出力部１５０は、高精度位置姿勢推定演算器１３５や画像処理部と同じ装置であってもよい。このとき位置姿勢出力部１５０は、高精度位置姿勢推定演算器１３５が出力した位置姿勢（位置姿勢伝播値）と、小型軽量位置姿勢推定演算器１４５が出力した位置姿勢（位置姿勢伝播値）とに対する補間値となる位置姿勢補間推定値２０を生成する。

位置姿勢出力部１５０は、高精度位置姿勢推定演算器１３５が出力した位置姿勢、または、小型軽量位置姿勢推定演算器１４５が出力した位置姿勢の一方の位置姿勢を基準とし、他方の位置姿勢を用いて、（仮想的な）カルマンフィルタ処理を行う。これによって位置姿勢出力部１５０は、第３のタイミング（周期）での位置姿勢を推定する。すなわち、位置姿勢出力部１５０は、第１の位置姿勢（例えば高精度位置姿勢推定演算器１３５が出力した位置姿勢）と第２の位置姿勢（例えば小型軽量位置姿勢推定演算器１４５が出力した位置姿勢）に基づき第３の位置姿勢を出力する。位置姿勢出力部１５０は、第１のタイミングにおいては、高精度位置姿勢推定演算器１３５が出力した第１の位置姿勢に基づき第３の位置姿勢を出力する。第２のタイミングにおいては、位置姿勢出力部１５０は、小型軽量位置姿勢推定演算器１４５が出力した第２の位置姿勢に基づき第３の位置姿勢を出力する。第１のタイミングと第２のタイミングのいずれとも異なる第３のタイミングにおいては、位置姿勢出力部１５０は、高精度位置姿勢推定演算器１３５が出力した第１の位置姿勢又は小型軽量位置姿勢推定演算器１４５が出力した第２の位置姿勢に基づいて、移動体の第３の位置姿勢を出力する。例えば、位置姿勢出力部１５０が位置姿勢を推定し姿勢補間推定値２０を出力する周期は、３ＫＨｚである。位置姿勢出力部１５０が位置姿勢を出力する周期は、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０が位置姿勢を推定する周期よりも短い。位置姿勢出力部１５０は、カルマンフィルタ処理により演算した位置姿勢（位置姿勢伝播値）を出力する伝播器１５０ａと、出力した位置姿勢（位置姿勢伝播値）及び、高精度位置姿勢推定部１３０または小型軽量位置姿勢推定部１４０が推定した位置に基づいて次回の推定時における位置姿勢を推定する推定器１５０ｂを含む。

次に、図５を用いて、本実施の形態に係る位置姿勢推定装置１２０の動作について説明する。

ここでは、具体例として、上述したように、移動体３０は、機体本体にＧＰＳ受信機１３１と高精度加速度センサ１３２と高精度ジャイロセンサ１３３と高精度位置姿勢推定演算器１３５とを搭載する。また移動体３０は、アンテナ先端（もしくは背面側）にＧＰＳ受信機１４１、小型軽量加速度センサ１４２と小型軽量ジャイロセンサ１４３とジャイロセンサ１４４と小型軽量位置姿勢推定演算器１４５とを搭載する。なおアンテナの指向は、空間内で自由に駆動可能である。位置姿勢推定装置１２０は、アンテナの姿勢角情報を数ｋＨｚオーダーの高サンプリング周波数で推定し生成する。ただし、当該移動体３０はＧＰＳ受信機１３１と高精度加速度センサ１３２と高精度ジャイロセンサ１３３とを、例えば、移動体３０の重心付近に搭載する。また、ＧＰＳ受信機１４１、小型軽量加速度センサ１４２と小型軽量ジャイロセンサ１４３とジャイロセンサ１４４と小型軽量位置姿勢推定演算器１４５とをアンテナ背面部分へ搭載した指向アンテナは、例えば、当該移動体３０の機体の先端に配置されている。

図５に示すように、まず、高精度位置姿勢推定部１３０において、ＧＰＳ受信機１３１、高精度加速度センサ１３２、高精度ジャイロセンサ１３３が検出を行い（Ｓ１０１）、高精度位置姿勢推定演算器１３５（伝播器１３５ａ）が、位置姿勢伝播処理を実行し（Ｓ１０２）、高精度位置姿勢推定演算器１３５（推定器１３５ｂ）が、位置姿勢推定処理（カルマンフィルタ処理）を実行する（Ｓ１０３）。

高精度位置姿勢推定演算器１３５が使用するカルマンフィルタ処理では、例えば、次の式（１）（２）で表されるベクトル^０ｚ_ｎを観測量、ベクトル^０ｘ_ｎを状態量とする。
^０ｚ_ｎ＝（Δ^０ｐ Δ^０ｐ’ Δ^０θ）・・・（１）
^０ｘ_ｎ＝（Δ^０ｐ Δ^０ｐ’ Δ^０θ Δ^０ａ_ｂ Δ^０ω_ｂ）・・・（２）
ここで、Δ^０ｐは、図２の慣性座標系で記述した移動体３０の現在位置ベクトルｐ＝（ｘ ｙ ｚ）の誤差ベクトルである。また、Δ^０ｐ’は、図２の慣性座標系で記述した移動体３０の速度ベクトルｐ’＝（ｘ’ ｙ’ ｚ’）の誤差ベクトルである。また、Δ^０θは、図２の慣性座標系で記述した移動体３０の姿勢ベクトルθ＝（φ ρ Ψ）の誤差ベクトルである。また、Δ^０ａ_ｂは、図２の慣性座標系で記述した移動体３０の高精度加速度センサ１３２のバイアスノイズを示すノイズベクトルａ_ｂ＝（ａ_ｂｘ ａ_ｂｙ ａ_ｂｚ）に対する誤差ベクトルである。また、Δ^０ω_ｂは、図２の慣性座標系で記述した移動体３０の高精度ジャイロセンサ１３３のバイアスノイズを示すノイズベクトルω_ｂ＝（ω_ｂｘ ω_ｂｙ ω_ｂｚ）に対する誤差ベクトルである。
なお、誤差ベクトルとは、加速度センサ、ジャイロセンサで検出された値から算出された値の、基準となる値（それぞれ状態量の絶対的な真値）に対する誤差を表したものである。現在位置ベクトルｐは、ＧＰＳ受信機１３１で直接検出する値を検出値とし、前回の検出時において予測されているカルマンフィルタ処理後の位置を基準値として、検出値と基準値の差を誤差ベクトルとすることができる。速度ベクトルｐ’は高精度加速度センサ１３２の検出値を積分した値を検出値とする。前回の検出時において予測されているカルマンフィルタ処理後の速度及びＧＰＳ受信機１３１の検出値から計算した速度から基準値を算出する。検出値と基準値の差を誤差ベクトルとすることができる。姿勢ベクトルθは、高精度ジャイロセンサ１３３の検出値を積分した値を検出値とする。前回の検出時において予測されているカルマンフィルタ処理後の姿勢及びＧＰＳ受信機１３１の検出値をもとに移動体（航空機）の位置ダイナミクスから導出した姿勢から基準値を算出する。検出値と基準値の差を誤差ベクトルとすることができる。なお、移動体（航空機）にコンパスを備え、コンパスにより検出される方位角を基準とすることもできる。また重力方向に対する機体の傾きを測定する測定器を備え、その傾きの測定値を基準とすることもできる。

また、高精度位置姿勢推定演算器１３５が使用するカルマンフィルタ処理で使用される時刻ｔ_ｎ＋１（次回の検出時）における状態量^０ｘ_ｎ＋１及び時刻ｔ_ｎ（今回の検出時）における観測量^０ｚ_ｎの線形確率システム（状態空間モデル）は、例えば、次の式（３）（４）のように表される。
^０ｘ_ｎ＋１＝^０Φ_ｎ ^０ｘ_ｎ+^０Ｇ_ｎ ^０ｗ_ｎ+^０Ｄ_ｎ ^０ｕ_ｎ・・・（３）
^０ｚ_ｎ＝^０Ｈ_ｎ ^０ｘ_ｎ+^０ｖ_ｎ・・・（４）
ここで、^０ｘ_ｎは時刻ｔ_ｎ（今回の検出時）における状態量、^０Φ_ｎは時刻ｔ_ｎにおける状態遷移行列、^０Ｇ_ｎは時刻ｔ_ｎにおける駆動ノイズ行列、^０ｗ_ｎは時刻ｔ_ｎにおけるシステムノイズ、^０Ｄ_ｎは時刻ｔ_ｎにおける駆動入力行列、^０ｕ_ｎは時刻ｔ_ｎにおけるシステム入力ベクトル、^０Ｈ_ｎは時刻ｔ_ｎにおける観測行列、^０ｖ_ｎは時刻ｔ_ｎにおける観測ノイズである。

システム入力ベクトル^０ｕ_ｎは、次回の検出時（時刻ｔ_ｎ＋１）までに機体（移動体）の状態量に加えられる状態量の変化分であり、例えば、高精度加速度センサ１３２が検出した加速度と、高精度ジャイロセンサ１３３が検出した角速度とにより構成される。

駆動入力行列^０Ｄ_ｎは、システム入力ベクトル^０ｕ_ｎによって次回の検出時（時刻ｔ_ｎ＋１）の状態量^０ｘ_ｎ＋１に与えられる変化を予測するための行列である。

観測行列^０Ｈ_ｎは、観測ノイズ^０ｖ_ｎが０ベクトルの場合の、時刻ｔ_ｎにおける状態量^０ｘ_ｎと観測量^０ｚ_ｎとの関係を示す行列である。

状態遷移行列^０Φ_ｎは、時刻ｔ_ｎにおける状態量^０ｘ_ｎからシステムノイズ^０ｗ_ｎ及びシステム入力ベクトル^０ｕ_ｎが０ベクトルの場合の、時刻ｔ_ｎ＋１における状態量^０ｘ_ｎ＋１への遷移関係を示す行列である。

システムノイズ^０ｗ_ｎと観測ノイズ^０ｖ_ｎは、互いに独立な白色ノイズであると仮定してよい。

カルマンフィルタ処理には、カルマンフィルタの数式で用いられる行列を今回の観測に基づいて更新し、更新された行列により今回の状態量を推定するカルマンフィルタ更新処理と、今回のカルマンフィルタ処理後の推定値から次回の状態量を予測するカルマンフィルタ予測処理が含まれる。
まずカルマンフィルタ更新処理で用いられる式について説明する。

推定器１３５ｂにおけるカルマンフィルタ更新処理は次の式（５）、式（６）、式（７）を用いて実施できる。
^０Ｋ_ｎ＝^０Ｐ_ｎ ^−０Ｈ_ｎ（^０Ｈ_ｎ ^Ｔ０Ｐ_ｎ ^−０Ｈ_ｎ+^０Ｒ_ｎ）^−１・・・（５）
^０θ_ｎ ^＋＝^０θ_ｎ ⁻＋^０Ｋ_ｎ（^０ｚ_ｎ−^０Ｈ_ｎ ^Ｔ０θ_ｎ ⁻）・・・（６）
^０Ｐ_ｎ ^＋＝^０Ｐ_ｎ ⁻−^０Ｋ_ｎ ^０Ｈ_ｎ ^Ｔ０Ｐ_ｎ ⁻・・・（７）
ここで、^０Ｐ_ｎは、時刻ｔ_ｎにおける誤差共分散行列、^０Ｒ_ｎは、観測ノイズ^０ｖ_ｎの共分散行列である。^０Ｒ_ｎはカルマンフィルタ設計者がＧＰＳのノイズ特性に応じ設定する設計パラメータである。
また上付きの「Ｔ」は転置行列、上付きの「−１」は逆行列を示す。

次にカルマンフィルタ予測処理で用いられる式について説明する。
時刻ｔ_ｎ+1におけるシステム状態量の予測値^０ｘ_ｎ＋１ ⁻と誤差共分散行列の予測値^０Ｐ_ｎ＋１ ⁻は、次の式（８）及び式（９）のように表される。
^０ｘ_ｎ+1 ⁻＝^０Φ_ｎ ^０ｘ_ｎ ^＋＋^０Ｄ_ｎ ^０ｕ_ｎ・・・（８）
^０Ｐ_ｎ+1 ⁻＝^０Φ_ｎ ^０Ｐ_ｎ ^＋０Φ_ｎ ^Ｔ＋^０Ｇ_ｎ ^０Ｑ_ｎ ^＋０Ｇ_ｎ ^Ｔ・・・（９）
ただし、下付きの「ｎ」は、時刻ｔ_ｎにおけるベクトル及び行列、下付きの「ｎ＋１」は、時刻ｔ_ｎ＋１におけるベクトル及び行列、上付きの「＋」はカルマンフィルタ処理後の推定値、上付きの「−」はカルマンフィルタ処理前の予測値、上付きの「Ｔ」は転置行列を示す。また^０Ｑ_ｎは、式（１）のシステムノイズ^０ｗ_ｎの共分散行列を示す。ただし^０Ｑ_ｎはカルマンフィルタ設計者が飛翔体ダイナミクス特性に応じ設定する設計パラメータとしてもよい。

上記の式を用いる高精度位置姿勢推定演算器１３５の動作を説明する。

ステップＳ１０１において、ＧＰＳ受信機１３１は、時刻ｔ_ｎにおいて、位置を検出する。高精度加速度センサ１３２は、加速度を検出する。高精度ジャイロセンサ１３３は、角速度を検出する。

次にＳ１０２において、位置姿勢伝播処理を行う。まず現在位置ベクトルｐについて、伝播器１３５ａは、前回の検出時において予測されているカルマンフィルタ処理後の位置を基準値とする。伝播器１３５ａは、ＧＰＳ受信機１３１が検出した位置の検出値と基準値との差をΔ^０ｐとする。速度ベクトルｐ’については、伝播器１３５ａは、高精度加速度センサ１３２が検出した加速度を積分して速度の検出値を求める。伝播器１３５ａは、時刻ｔ_ｎにおいてＧＰＳ受信機１３１が検出した位置から算出した速度及び前回の検出時において予測されているカルマンフィルタ処理後の速度から速度の基準値を計算する。伝播器１３５ａは、速度の検出値と基準値との差をΔ^０ｐとする。姿勢ベクトルθについては、伝播器１３５ａは、高精度ジャイロセンサ１３３が検出した角速度を積分して姿勢の検出値を求める。伝播器１３５ａは、時刻ｔ_ｎにおいてＧＰＳ受信機１３１が測定した位置から移動体（航空機）の位置ダイナミクスを用いて姿勢及び前回の検出時において予測されているカルマンフィルタ処理後の姿勢から姿勢の基準値を計算する。伝播器１３５ａは、姿勢の検出値と基準値との差をΔ^０θとする。このようにして、ＧＰＳ受信機１３１、高精度加速度センサ１３２、高精度ジャイロセンサ１３３が検出した位置、加速度、角速度から、時刻ｔ_ｎにおける観測量^０ｚ_ｎが得られる。

前回の検出時の動作において式（９）からカルマンフィルタ処理前の共分散行列の予測行列^０Ｐ_ｎ ⁻が得られている。^０Ｒ_ｎはカルマンフィルタ設計者がＧＰＳのノイズ特性に応じ設定する設計パラメータである。式（５）により、カルマンゲイン^０Ｋ_ｎが得られる。

また、伝播器１３５ａは、前回の検出時の動作において得られているシステム状態量の予測量^０ｘ_ｎ ⁻と、Ｓ１０１において観測された観測量^０ｚ_ｎに基づいて、式（６）により時刻ｔ_ｎにおけるカルマンフィルタ処理後のシステム状態量^０ｘ_ｎ ^＋を算出する。

また伝播器１３５ａは、前回の検出時の動作において得られている共分散行列^０Ｐ_ｎの予測行列^０Ｐ_ｎ ⁻と、カルマンゲイン^０Ｋ_ｎと、観測行列^０Ｈ_ｎに基づいて、式（７）により時刻ｔ_ｎにおけるカルマンフィルタ処理後の共分散行列^０Ｐ_ｎ ^＋も算出する。

このようにしてＳ１０２において、高精度位置姿勢推定演算器１３５の伝播器１３５ａは、システム状態量と誤差共分散行列を算出する。高精度位置姿勢推定演算器１３５は、算出した位置姿勢を出力する。

次に、Ｓ１０３において高精度位置姿勢推定演算器１３５の推定器１３５ｂは、例えば、高精度加速度センサ１３２が検出した加速度と、高精度ジャイロセンサ１３３が検出した角速度とからシステム入力ベクトル^０ｕ_ｎを算出し、Ｓ１０２において算出されたシステム状態量と誤差共分散行列に基づいて、式（８）、式（９）によって時刻ｔ_ｎ＋１のシステム状態量の予測量^０ｘ_ｎ＋１ ⁻と共分散行列の予測値^０Ｐ_ｎ＋１ ⁻を算出する。
また推定器１３５ｂは、時刻ｔ_ｎ＋１で検出又は算出されるΔ^０ｐ、Δ^０ｐ’、Δ^０θとの差を算出するための位置、速度、姿勢の予測量を算出する。

一方、小型軽量位置姿勢推定部１４０において、図５に示すようにＧＰＳ受信機１４１、小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４が検出を行い（Ｓ１１１）、小型軽量位置姿勢推定演算器１４５（伝播器１４５ａ）が、位置姿勢伝播処理を実行し（Ｓ１１２）、小型軽量位置姿勢推定演算器１４５（推定器１４５ｂ）が、位置姿勢推定処理（カルマンフィルタ処理）を実行する（Ｓ１１３）。

小型軽量位置姿勢推定演算器１４５が実行するカルマンフィルタ処理で用いられる数式について説明する。小型軽量位置姿勢推定演算器１４５が実行するカルマンフィルタ処理で用いられる数式は、基本的に高精度位置姿勢推定演算器１３５が実行するカルマンフィルタ処理で用いられる数式と同様になる。

小型軽量位置姿勢推定演算器１４５が使用するカルマンフィルタ処理では、例えば、次の式（１０）（１１）で表されるベクトル^１ｚ_ｋを観測量、ベクトル^１ｘ_ｋを状態量とする。
^１ｚ_ｋ＝（Δ^１ｐ Δ^１ｐ’ Δ^１θ）・・・（１０）
^１ｘ_ｋ＝（Δ^１ｐ Δ^１ｐ’ Δ^１θ Δ^１ａ_ｂ Δ^１ω_ｂ）・・・（１１）
ここで、Δ^１ｐは、小型軽量位置姿勢推定部１４０における図２の慣性座標系で記述した移動体３０（アンテナ）の現在位置ベクトルｐ＝（ｘ ｙ ｚ）の誤差ベクトルである。また、Δ^１ｐ’は、小型軽量位置姿勢推定部１４０における図２の慣性座標系で記述した移動体３０（アンテナ）の速度ベクトルｐ’＝（ｘ’ ｙ’ ｚ’）の誤差ベクトルである。また、Δ^１θは、小型軽量位置姿勢推定部１４０における図２の慣性座標系で記述した移動体３０（アンテナ）の姿勢ベクトルθ＝（φ ρ Ψ）の誤差ベクトルである。また、Δ^１ａ_ｂは、小型軽量位置姿勢推定部１４０における図２の慣性座標系で記述した移動体３０（アンテナ）の小型軽量加速度センサ１４２のバイアスノイズを示すノイズベクトルａ_ｂ＝（ａ_ｂｘ ａ_ｂｙ ａ_ｂｚ）に対する誤差ベクトル、Δ^１ω_ｂは、小型軽量位置姿勢推定部１４０における図２の慣性座標系で記述した移動体３０（アンテナ）の小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４の検出信号を補間により組み合わせた検出信号のバイアスノイズを示すノイズベクトルω_ｂ＝（ω_ｂｘ ω_ｂｙ ω_ｂｚ）に対する誤差ベクトルである。
なお、現在位置ベクトルｐは、ＧＰＳ受信機１３１で直接検出する値を検出値とし、前回の検出時において予測されているカルマンフィルタ処理後の位置を基準として、検出値と基準値の差を誤差ベクトルとすることができる。速度ベクトルｐ’は高精度加速度センサ１３２の検出値を積分した値を検出値とし、前回の検出時において予測されているカルマンフィルタ処理後の速度及びＧＰＳ受信機１３１の検出値から計算した速度から基準値を算出して、検出値と基準値の差を誤差ベクトルとすることができる。姿勢ベクトルθは、高精度ジャイロセンサ１３３の検出値を積分した値を検出値とし、前回の検出時において予測されているカルマンフィルタ処理後の姿勢及びＧＰＳ受信機１３１の検出値をもとに移動体（航空機）の位置ダイナミクスから導出した姿勢から基準値を算出して、検出値と基準値の差を誤差ベクトルとすることができる。

ＭＥＭＳなどを利用した小型軽量ジャイロセンサ１４３では周波数帯域が比較的広い反面ＤＣ成分に対するドリフトノイズは大きいため、検出性能を確保するにはこのドリフトノイズを何らかの方法で補償する必要がある。ここでは小型軽量ジャイロセンサ１４３に対し、周波数帯域は狭いもののドリフトノイズが比較的小さい機械式などのジャイロセンサ１４４を組み合わせそれぞれの検出信号を補間により組み合わせることで、周波数帯域と検出精度の両立を図る。具体的には、小型軽量ジャイロセンサ検出信号１７にハイパスフィルタを通過させ、ジャイロセンサ検出信号１８にローパスフィルタを通過させた後、これらの信号が組み合わされる。

また、小型軽量位置姿勢推定演算器１４５が使用するカルマンフィルタ処理で使用される時刻ｔ_ｋ＋１（次回の検出時）における状態量^１ｘ_ｋ＋１及び時刻ｔ_ｋ（今回の検出時）における観測量^１ｚ_ｋの線形確率システム（状態空間モデル）は、例えば、次の式（１２）（１３）のように表される。
^１ｘ_ｋ+1＝^１Φ_ｋ ^１ｘ_ｋ+^１Ｇ_ｋ ^１ｗ_ｋ+^１Ｄ_ｋ ^１ｕ_ｋ・・・（１２）
^１ｚ_ｋ＝^１Ｈ_ｋ ^１ｘ_ｋ+^１ｖ_ｋ・・・（１３）
ここで、^１ｘ_ｋは時刻ｔ_ｋ（今回の検出時）における状態量、^１Φ_ｋは時刻ｔ_ｋにおける状態遷移行列、^１Ｇ_ｋは時刻ｔ_ｋにおける駆動ノイズ行列、^１ｗ_ｋは時刻ｔ_ｋにおけるシステムノイズ、^１Ｄ_ｋは時刻ｔ_ｋにおける駆動入力行列、^１ｕ_ｋは時刻ｔ_ｋにおけるシステム入力ベクトル、^１Ｈ_ｋは時刻ｔ_ｋにおける観測行列、^１ｖ_ｋは時刻ｔ_ｋにおける観測ノイズである。
また小型軽量位置姿勢推定演算器１４５の推定器１４５ｂにおけるカルマンフィルタ更新処理は次の式（１４）、式（１５）、式（１６）を用いて実施できる。
^１Ｋ_ｋ＝^１Ｐ_ｋ ^−１Ｈ_ｋ（^１Ｈ_ｋ ^Ｔ１Ｐ_ｋ ^−１Ｈ_ｋ+^１Ｒ_ｋ）^−１・・・（１４）
^１θ_ｋ ^＋＝^１θ_ｋ ⁻＋^１Ｋ_ｋ（^１ｚ_ｋ−^１Ｈ_ｋ ^Ｔ１θ_ｋ ⁻）・・・（１５）
^１Ｐ_ｋ ^＋＝^１Ｐ_ｋ ⁻−^１Ｋ_ｋ ^１Ｈ_ｋ ^Ｔ１Ｐ_ｋ ⁻・・・（１６）
ここで、^１Ｐ_ｋは、時刻ｔ_ｋにおける誤差共分散行列、^１Ｒ_ｋは、観測ノイズ^１ｖ_ｋの共分散行列である。^１Ｒ_ｋはカルマンフィルタ設計者がＧＰＳのノイズ特性に応じ設定する設計パラメータである。
また上付きの「Ｔ」は転置行列、上付きの「−１」は逆行列を示す。
また小型軽量位置姿勢推定演算器１４５の伝播器１４５ａにおけるカルマンフィルタ予測処理において、時刻ｔ_ｋ＋１のシステム状態量の予測値^１ｘ_ｋ＋１ ⁻と誤差共分散行列の予測値^１Ｐ_ｋ＋１ ⁻は、次の式（１７）及び式（１８）のように表される。
^１ｘ_ｋ+1 ⁻＝^１Φ_ｋ ^１ｘ_ｋ ^＋＋^１Ｄ_ｋ ^１ｕ_ｋ・・・（１７）
^１Ｐ_ｋ+1 ⁻＝^１Φ_ｋ ^１Ｐ_ｋ ^＋１Φ_ｋ ^Ｔ＋^１Ｇ_ｋ ^１Ｑ_ｋ ^＋１Ｇ_ｋ ^Ｔ・・・（１８）
ただし、下付きの「ｋ」は、時刻ｔ_ｋにおけるベクトル及び行列、下付きの「ｋ＋１」は、時刻ｔ_ｋ＋１におけるベクトル及び行列、上付きの「＋」はカルマンフィルタ処理後の推定値、上付きの「−」はカルマンフィルタ処理前の予測値、上付きの「Ｔ」は転置行列を示す。また^１Ｑ_ｋは、式（１）のシステムノイズ^１ｗ_ｋの共分散行列を示す。ただし^１Ｑ_ｋはカルマンフィルタ設計者が飛翔体ダイナミクス特性に応じ設定する設計パラメータとしてもよい。
上記の式を用いる小型軽量位置姿勢推定演算器１４５の動作を説明する。小型軽量位置姿勢推定部１４０は、観測及びカルマンフィルタ処理の周期が高精度位置姿勢推定部１３０における、測定及びカルマンフィルタ処理より短く、例えば４００Ｈｚのサンプリング周波数で測定及びカルマンフィルタ処理を実行する。

ステップＳ１１１において、ＧＰＳ受信機１４１は、時刻ｔ_ｋにおいて、位置を検出する。小型軽量加速度センサ１４２は、加速度を検出する。また小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４は、それぞれ角速度を検出する。

次にＳ１１２において、位置姿勢伝播処理を行う。まず現在位置ベクトルｐについて、伝播器１４５ａは、前回の検出時において予測されているカルマンフィルタ処理後の位置を基準値とする。伝播器１４５ａは、位置の検出値と基準値との差をΔ^０ｐとする。速度ベクトルｐ’については、伝播器１４５ａは、小型軽量加速度センサ１４２が検出した加速度を積分して速度の検出値を求める。伝播器１４５ａは、時刻ｔ_ｋにおいてＧＰＳ受信機１４１が検出した位置から算出した速度及び前回の検出時において予測されているカルマンフィルタ処理後の速度から速度の基準値を計算する。伝播器１４５ａは、速度の検出値と基準値との差をΔ^０ｐとする。姿勢ベクトルθについては、伝播器１４５ａは、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４の検出した角速度を補間により組み合わせた角速度を積分して姿勢の検出値を求める。伝播器１３５ａは、時刻ｔ_ｋにおいてＧＰＳ受信機１３１が測定した位置から移動体（航空機）の位置ダイナミクスを用いて姿勢及び前回の検出時において予測されているカルマンフィルタ処理後の姿勢から姿勢の基準値を計算する。伝播器１４５ａは、姿勢の検出値と基準値との差をΔ^０θとする。このようにして、ＧＰＳ受信機１４１、小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４が検出した位置、加速度、角速度から時刻ｔ_ｋにおける観測量^０ｚ_ｋが得られる。前回の検出時の動作において式（１８）から時刻ｔ_ｋのカルマンフィルタ処理前の共分散行列の予測行列^１Ｐ_ｋ ⁻が得られている。^１Ｒ_ｋはカルマンフィルタ設計者がＧＰＳのノイズ特性に応じ設定する設計パラメータである。式（１４）により、時刻ｔ_ｋにおけるカルマンゲイン^１Ｋ_ｋが得られる。

また、伝播器１４５ａは、前回の検出時の動作において得られているシステム状態量の予測値^１ｘ_ｋ ⁻と、Ｓ１１１において観測された観測量^０ｚ_ｋに基づいて、式（１５）により時刻ｔ_ｋにおけるカルマンフィルタ処理後のシステム状態量^１ｘ_ｋ ^＋を算出する。

また伝播器１４５ａは、前回の検出時の動作において得られている共分散行列の予測行列^１Ｐ_ｋ ⁻と、カルマンゲイン^１Ｋ_ｋと、観測行列^１Ｈ_ｋに基づいて、式（１６）により時刻ｔ_ｋにおけるカルマンフィルタ処理後の共分散行列^１Ｐ_ｋ ^＋を算出する。

このようにしてＳ１１２において、小型軽量位置姿勢推定演算器１４５の伝播器１３５ａは、システム状態量と誤差共分散行列を算出する。小型軽量位置姿勢推定演算器１４５は、算出した位置姿勢を出力する。

次に、Ｓ１１３において小型軽量位置姿勢推定演算器１４５の推定器１４５ｂは、Ｓ１１２において算出されたシステム状態量と誤差共分散行列に基づいて、式（１７）、式（１８）によって時刻ｔ_ｋ＋１のシステム状態量の予測値^１ｘ_ｋ＋１ ⁻と共分散行列の予測値^１Ｐ_ｋ＋１ ⁻を算出する。また推定器１３５ｂは、時刻ｔ_ｋ＋１において検出又は算出されるΔ^１ｐ、Δ^１ｐ’、Δ^１θとの差を算出するための位置、速度、姿勢の予測値を算出する。

このようにして小型軽量位置姿勢推定部１４０は、アンテナに搭載するＧＰＳ受信機１４１が検出した絶対位置と、小型軽量加速度センサ１４２が検出した加速度と、ＭＥＭＳなどにより構成した小型軽量ジャイロセンサ１４３が検出した角速度と、この小型軽量ジャイロセンサ１４３の検出性能制約を補間する機械式などのジャイロセンサ１４４が検出した角速度とを用いて、カルマンフィルタ処理を実施する。小型軽量位置姿勢推定部１４０は、カルマンフィルタ処理によって、例えば４００Ｈｚのサンプリング周波数で、算出された位置姿勢を含む小型軽量位置姿勢推定演算器生成信号１９を出力する。

次に、位置姿勢出力部１５０（伝播器１５０ａ）が位置姿勢伝播処理を実行し（Ｓ１２１）、位置姿勢出力部１５０（推定器１５０ｂ）が位置姿勢推定処理（仮想的なカルマンフィルタ処理）を実行する（Ｓ１２２）。

位置姿勢出力部１５０は、例えば５０〜２００Ｈｚの比較的低サンプリング周波数１／Δｔ_ｎＨｚで発生する高精度位置姿勢伝播値を含む移動体高精度位置姿勢推定信号１４と、例えば４００Ｈｚのサンプリング周波数１／Δｔ_ｋＨｚで発生する位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号１９を用いて、サンプリング周波数１／Δｔ_ｍＨｚ（ただしΔｔ_ｍ≦Δｔ_ｋ）で発生する位置姿勢補間推定値２０を生成する。

位置姿勢出力部１５０の伝播器１５０ａは、位置姿勢出力部１５０におけるシステム状態量と誤差共分散行列をそれぞれ、^ｆθ_ｍ、^ｆＰ_ｍと設定する。

位置姿勢出力部１５０が使用するカルマンフィルタ処理では、例えば、次の式（１９）（２０）で表されるベクトル^ｆｚ_ｍを観測量、ベクトル^ｆθ_ｍを状態量とする。
^ｆｚ_ｍ＝（Δ^ｆｐ Δ^ｆθ）・・・（１９）
^ｆθ_ｍ＝（Δ^ｆｐ Δ^ｆθ）・・・（２０）
ここで、Δ^ｆｐは、現在位置ベクトルｐ＝（ｘ ｙ ｚ）の誤差ベクトル、Δ^ｆθは、姿勢ベクトルθ＝（φ ρ Ψ）の誤差ベクトルである。
なお、現在位置ベクトルｐ及び姿勢ベクトルθは、前回の検出時（時刻ｔ_ｍ-1）において予測されているカルマンフィルタ処理後の位置を基準とする。高精度位置姿勢推定部１３０又は小型軽量位置姿勢推定部１４０のうち最後に出力された値を検出値とする。検出値と基準値の差を誤差ベクトルとすることができる。また高精度位置姿勢推定部１３０が出力したタイミングでは高精度位置姿勢推定部１３０の出力した値を検出値とする。小型軽量位置姿勢推定部１４０が出力したタイミングでは小型軽量位置姿勢推定部１４０の出力した値を検出値とする。高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０のいずれの出力と異なるタイミングでは、例えば、高精度位置姿勢推定部１３０又は小型軽量位置姿勢推定部１４０のうち最後に出力された値を検出値とする。前回の検出時（時刻ｔ_ｍ-1）において予測されているカルマンフィルタ処理後の位置を基準とする。検出値と基準値の差を誤差ベクトルとすることができる。

位置姿勢出力部１５０が使用するカルマンフィルタ処理で使用される線形確率システム（状態空間モデル）は、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０と同様に、例えば、次の式（２１）（２２）のように表される。
なお、状態量^ｆθ_ｍ＋１は次回の検出時である時刻ｔ_ｍ＋１における状態量、観測量^ｆｚ_ｍ今回の検出時である時刻ｔ_ｍにおける観測量である。
^ｆθ_ｍ＋１＝^ｆΦ_ｍ ^ｆθ_ｍ＋^ｆＧ_ｍ ^ｆｗ_ｍ＋^ｆＤ_ｍ ^ｆｕ_ｍ・・・（２１）
^ｆｚ_ｍ＝^ｆＨ_ｍ ^ｆθ_ｍ+^ｆｖ_ｍ・・・（２２）
ここで、^ｆθ_ｍは時刻ｔ_ｍにおける状態量、^ｆΦ_ｍは時刻ｔ_ｍにおける状態遷移行列、^ｆＧ_ｍは時刻ｔ_ｍにおける駆動ノイズ行列、^ｆｗ_ｍは時刻ｔ_ｍにおけるシステムノイズ、^ｆＤ_ｍは時刻ｔ_ｍにおける駆動入力行列、^ｆｕ_ｍは時刻ｔ_ｍにおけるシステム入力ベクトル、^ｆＨ_ｍは時刻ｔ_ｍにおける観測行列、^ｆｖ_ｍは時刻ｔ_ｍにおける観測ノイズである。

位置姿勢出力部１５０は、システム入力ベクトル^ｆｕ_ｍを生成してカルマンフィルタ処理に用いる。例えば、位置姿勢出力部１５０は、サンプリング周波数１／Δｔ_ｍＨｚで、姿勢の補間推定値を生成してカルマンフィルタ処理に用いる。

例えば、位置姿勢出力部１５０は、サンプリング時間間隔Δｔ_ｍの間、^ｆｕ_ｍを、一定値の角速度ベクトル^ｆω_ｋ＝［ω_ｘｋ ω_ｙｋ ω_ｚｋ］^Ｔとして生成してもよい。例えば、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０は、位置姿勢とともに角速度ベクトル^０ω_ｎ及び¹ω_ｋを出力するものとしてもよい。また一定値の角速度ベクトル^ｆω_ｋは、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０が、時刻ｔ_ｍの前、最後に出力した角速度ベクトルを^ｆω_ｋとしてもよい。

推定器１５０ｂにおけるカルマンフィルタ更新処理は高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０と同様に、次の式（２３）、式（２４）、式（２５）を用いて実施できる。
^ｆＫ_ｍ＝^ｆＰ_ｍ ^−ｆＨ_ｍ（^ｆＨ_ｍ ^ＴｆＰ_ｍ ^−ｆＨ_ｍ+^ｆＲ_ｍ）^−１・・・（２３）
^ｆθ_ｍ ^＋＝^ｆθ_ｍ ⁻＋^ｆＫ_ｍ（^ｆｚ_ｍ−^ｆＨ_ｍ ^Ｔｆθ_ｍ ⁻）・・・（２４）
^ｆＰ_ｍ ^＋＝^ｆＰ_ｍ ⁻−^ｆＫ_ｍ ^ｆＨ_ｍ ^ＴｆＰ_ｍ ⁻・・・（２５）
ここで、^ｆＰ_ｍは、今回の観測時刻ｔ_ｍにおける誤差共分散行列、^ｆＲ_ｍは、観測ノイズ^ｆｖ_ｍの共分散行列である。^ｆＲ_ｍはカルマンフィルタ設計者がＧＰＳのノイズ特性に応じ設定する設計パラメータである。

また次回の観測時刻ｔ_ｍ＋１におけるシステム状態量の予測値^ｆｘ_ｍ＋１ ⁻と誤差共分散行列の予測値^ｆＰ_ｍ＋１ ⁻は、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０と同様に、次の式（２６）及び式（２７）のように表される。
^ｆθ_ｍ+1 ⁻＝^ｆΦ_ｍ ^ｆθ_ｍ ^＋＋^ｆＤ_ｍ ^ｆｕ_ｍ・・・（２６）
^ｆＰ_ｍ+1 ⁻＝^ｆΦ_ｍ ^ｆＰ_ｍ ^＋ｆΦ_ｍ ^Ｔ＋^ｆＧ_ｍ ^ｆＱ_ｍ ^＋ｆＧ_ｍ ^Ｔ・・・（２７）
ここで、^ｆＱ_ｍは、式（１９）のシステムノイズ^ｆｗ_ｍの共分散行列を示す。

上記の式を用いる位置姿勢出力部１５０の動作を説明する。まずＳ１２１の処理を説明する。前回の観測時刻ｔ_ｍ−１における動作において、式（２７）から今回の観測時系列「ｍ」のカルマンフィルタ処理前の共分散行列の予測行列^ｆＰ_ｍ ⁻が得られている。また式（２３）により、時刻ｔ_ｍにおける観測時系列「ｍ」のカルマンゲイン^ｆＫ_ｍが得られている。

伝播器１５０ａは、システム状態量の予測量^ｆθ_ｍ ⁻と、時刻ｔ_ｍにおける観測量^ｆｚ_ｍに基づいて、式（２４）により時刻ｔ_ｍにおけるカルマンフィルタ処理後のシステム状態量^ｆθ_ｍ ^＋を算出する。ここで時刻ｔ_ｍにおける観測量^ｆｚ_ｍ＝（Δ^ｆｐ Δ^ｆθ）は、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０から最後に取得した位置姿勢を用いて算出したものとしてもよい。すなわち、高精度位置姿勢推定部１３０が出力したタイミングでは高精度位置姿勢推定部１３０の出力した現在位置ベクトルｐ及び姿勢ベクトルθの値を検出値として用い、小型軽量位置姿勢推定部１４０が出力したタイミングでは小型軽量位置姿勢推定部１４０の出力した現在位置ベクトルｐ及び姿勢ベクトルθの値を検出値として用い、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０のいずれの出力と異なるタイミングでは、例えば、高精度位置姿勢推定部１３０又は小型軽量位置姿勢推定部１４０のうち最後に出力された現在位置ベクトルｐ及び姿勢ベクトルθの値を検出値として用いて観測量^ｆｚ_ｍを算出してもよい。

また伝播器１５０ａは、共分散行列の予測行列^ｆＰ_ｍ ⁻と、カルマンゲイン^ｆＫ_ｍと、観測行列^ｆＨ_ｍに基づいて、式（２５）により時刻ｔ_ｍにおけるカルマンフィルタ処理後の共分散行列^ｆＰ_ｍ ^＋も算出する。

このようにしてＳ１２１において、伝播器１５０ａは、システム状態量と誤差共分散行列を算出する。位置姿勢出力部１５０は、算出した状態量を出力する。

次に、まずＳ１２２の処理を説明する。位置姿勢出力部１５０の推定器１５０ｂは、システム入力ベクトル^ｆｕ_ｍを生成する。推定器１５０ｂは、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０にて最後に検出した角速度ベクトル^０ω_ｎ又は^１ω_ｋをシステム入力ベクトル^ｆｕ_ｍとしてもよい。推定器１５０ｂは、生成されたシステム入力ベクトル^ｆｕ_ｍと、Ｓ１２１において算出されたシステム状態量^ｆθ_ｍ ^＋と、誤差共分散行列^ｆＰ_ｍ ^＋に基づいて、式（２６）、式（２７）によって次回の観測時系列「ｍ＋１」のシステム状態量の予測値^ｆθ_ｍ＋１ ⁻（位置姿勢補間推定値２０）と共分散行列の予測値^ｆＰ_ｍ＋１ ⁻を算出する。
また推定器１５０ｂは、次回の観測時系列「ｍ＋１」で算出されるΔ^ｆｐ及びΔ^０θの検出値との差を求めるためのカルマンフィルタ処理後の位置及び姿勢の予測値を算出する。

なお、推定器１５０ｂは、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０にて最後に検出した角速度ベクトル^０ω_ｎ又は^１ω_ｋをシステム入力ベクトル^ｆｕ_ｍとしてもよいと説明したが、これに限られない。推定器１５０ｂは、システムへの入力ベクトル^ｆｕ_ｍを、次の式（２８）で表す値としてもよい。

ここで、時刻ｔ_ｋは、小型軽量位置姿勢推定部１４０にて最後に角速度ベクトルを検出した時刻、時刻ｔ_ｋ−１は、時刻ｔ_ｋより前に小型軽量位置姿勢推定部１４０から角速度ベクトルを取得した時刻、^ｆω_ｋ、^ｆω_ｋ-1は、時刻ｔ_ｋ、ｔ_ｋ-1（＜ｔ_ｍ＋１）において小型軽量位置姿勢推定部１４０から取得した角速度ベクトルである。

このように、本実施の形態によれば、位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号１９を、高精度位置姿勢伝播値を含む移動体高精度位置姿勢推定信号１４に基づき補間処理を実施することとなる。このため、位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号１９を単独利用する補間処理と比較して更なる精度向上が可能となる。

なお、ここで示した位置姿勢出力部１５０での補間処理は、高精度位置姿勢伝播値と位置姿勢伝播値とを、小型軽量位置姿勢推定部１４０で適用するジャイロセンサのノイズモデルに基づき適切に組み合わせることで実現した技術であり、関連する技術を単純に組み合わせたものではない。

本実施の形態では、移動体に搭載する駆動機器の姿勢情報を生成する位置姿勢推定装置において、移動体に搭載した複数のセンサの検出信号を組み合わせることで高精度位置姿勢推定部が位置姿勢を推定する。また、駆動機器に搭載した複数のセンサの検出信号を組み合わせることで小型軽量位置姿勢推定部が位置姿勢を推定する。さらに、高精度位置姿勢推定部と小型軽量位置姿勢推定部が推定した信号を組み合わせることで位置姿勢補間推定値を生成することとした。
特に、本実施の形態では、移動体本体に搭載した高精度位置姿勢推定部に対し、小型軽量加速度センサと小型軽量ジャイロセンサとから構成し駆動機器に装着した小型軽量位置姿勢推定部を組み合わせる。また、高精度位置姿勢推定部が生成する位置姿勢伝播値と小型軽量位置姿勢推定部が生成する位置姿勢伝播値とを利用することで、小型軽量位置姿勢推定部が出力した位置姿勢伝播値を高サンプリングで補間可能な位置姿勢推定補間値を生成する。

その結果、本実施の形態では、小型軽量位置姿勢推定部が生成する位置姿勢伝播値の検出間隔が十分短く無く精度的にも十分高く無い場合でも、小型軽量位置姿勢推定部が位置姿勢伝搬値と同時に生成する姿勢角速度情報を組み合わせ用いる。これによって、離散的に生成する位置姿勢伝播値の変動を補間可能となり、移動体に搭載する搭載駆動機器のリアルタイム高精度位置姿勢推定が達成できる。またさらに、小型軽量位置姿勢推定部が生成する位置姿勢伝播値を時系列データとして保存しそれらを複数組み合わせることで、リアルタイム性は犠牲にしながらも位置姿勢推定精度の向上が達成できる。
（実施の形態２）
以下、図面を参照して実施の形態２について説明する。本実施の形態では、実施の形態１に対し、小型軽量位置姿勢推定部が、高精度位置姿勢推定部の出力信号を用いて位置姿勢を推定する。ここでは、主に実施の形態１と相違する点について説明する。

図６は、本実施の形態に係る位置姿勢推定装置１２０の機能ブロック図を示している。図６に示すように、本実施の形態では、小型軽量位置姿勢推定部１４０は、ＧＰＳ受信機１４１を備えていない。すなわち、小型軽量位置姿勢推定部１４０は、小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４、小型軽量位置姿勢推定演算器１４５を備えている。

小型軽量位置姿勢推定演算器１４５は、小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４の検出、及び高精度位置姿勢推定部１３０の推定に基づいて、アンテナの位置姿勢を推定する。すなわち、小型軽量位置姿勢推定演算器１４５は、小型軽量加速度センサ検出信号１６と小型軽量ジャイロセンサ検出信号１７とジャイロセンサ検出信号１８と移動体高精度位置姿勢推定信号１４とに基づき小型軽量位置姿勢推定演算器生成信号１９を生成する。

小型軽量位置姿勢推定演算器１４５は、高精度位置姿勢推定部１３０が推定した位置姿勢を基準とする。小型軽量位置姿勢推定演算器１４５は、小型軽量加速度センサ１４２が検出した加速度と小型軽量ジャイロセンサ１４３が検出した角速度とジャイロセンサ１４４が検出した角速度とを用いて、（仮想的な）カルマンフィルタ処理を行う。これによって小型軽量位置姿勢推定演算器１４５は、第２のタイミング（周期）の位置姿勢を推定する。

本実施形態の小型軽量位置姿勢推定演算器１４５が実行するカルマンフィルタ処理で用いられる数式は、基本的に第１の実施形態の小型軽量位置姿勢推定演算器１４５が実行するカルマンフィルタ処理で用いられる数式と同様になる。

また、小型軽量位置姿勢推定演算器１４５が使用するカルマンフィルタ処理で使用される状態量^１ｘ_ｋ＋１及び観測量^１ｚ_ｋの線形確率システム（状態空間モデル）の式は、基本的に第１の実施形態の小型軽量位置姿勢推定演算器１４５で用いられる数式と同様になる。また、推定器１４５ｂにおけるカルマンフィルタ更新処理の式も、基本的に第１の実施形態の小型軽量位置姿勢推定演算器１４５で用いられる数式と同様になる。また、伝播器１４５ａにおけるカルマンフィルタ予測処理におけるシステム状態量の予測値^１ｘ_ｋ＋１ ⁻と誤差共分散行列の予測値^１Ｐ_ｋ＋１ ⁻の式も、基本的に第１の実施形態の小型軽量位置姿勢推定演算器１４５で用いられる数式と同様になる。

次に、本実施の形態に係る位置姿勢推定装置１２０の動作について説明する。本実施の形態の動作は、基本的に実施の形態１の図５と同様であり、主に、小型軽量位置姿勢推定部１４０（小型軽量位置姿勢推定演算器１４５）における、Ｓ１１２、Ｓ１１３の処理のみが異なる。

小型軽量位置姿勢推定部１４０は、小型軽量加速度センサ検出信号１６と小型軽量ジャイロセンサ検出信号１７とジャイロセンサ検出信号１８に加え、高精度位置姿勢推定部１３０が生成する移動体高精度位置姿勢推定信号１４とを用いて、カルマンフィルタの枠組みを流用した仮想的な推定処理を実施する。これによって、小型軽量位置姿勢推定部１４０は、高々４００Ｈｚのサンプリング周波数で位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号１９を生成する。

ステップＳ１１１においては、第１の実施形態と同様に、小型軽量加速度センサ１４２が加速度を検出し、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４が角速度を検出する。

Ｓ１１２における位置姿勢伝播処理は、実施の形態１と同様である。上記の式（１１）及び式（１２）において、上付きの「＋」は仮想的なカルマンフィルタ処理後の推定値である。また、上付きの「−」は仮想的なカルマンフィルタ処理前の伝播予測値である。また、^１Ｑ_ｋはシステムノイズ^１ｗ_ｋの共分散行列（仮想的なカルマンフィルタの設計者が飛翔体ダイナミクス特性などに応じ設定する設計パラメータ）となる。

ステップＳ１１２において、本実施形態では、現在位置ベクトルｐについて、伝播器１４５ａは、小型軽量加速度センサ１４２が検出した加速度を２回積分して位置の検出値を求める。伝播器１４５ａは、高精度位置姿勢推定部１３０が推定した位置及び前回の検出時において予測されているカルマンフィルタ処理後の位置から基準値を算出する。伝播器１４５ａは、位置の検出値と基準値との差をΔ^０ｐとする。

速度ベクトルｐ’については、伝播器１４５ａは、小型軽量加速度センサ１４２が検出した加速度を積分して速度の検出値を求める。伝播器１４５ａは、高精度位置姿勢推定部１３０が推定した位置及び前回の検出時において予測されているカルマンフィルタ処理後の速度から速度の基準値を計算する。伝播器１４５ａは、速度の検出値と基準値との差をΔ^０ｐとする。

姿勢ベクトルθについては、伝播器１４５ａは、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４の検出した角速度を補間により組み合わせた角速度を積分して姿勢の検出値を求める。伝播器１４５ａは、高精度位置姿勢推定部１３０が推定した姿勢及び前回の検出時において予測されているカルマンフィルタ処理後の姿勢から基準値を計算する。伝播器１４５ａは、姿勢の検出値と基準値との差をΔ^０θとする。

このようにして、ステップＳ１１１において、機体センサが位置、加速度、角速度を検出し、時刻ｔ_ｋにおける観測量^０ｚ_ｋが得られる。

そして、第１の実施形態と同様に、小型軽量位置姿勢推定演算器１４５の伝播器１４５ａは、システム状態量と誤差共分散行列を算出する。小型軽量位置姿勢推定演算器１４５は、算出した位置姿勢を出力する。

また、Ｓ１１３において、小型軽量位置姿勢推定演算器１４５の推定器１４５ｂは、Ｓ１１２において算出されたシステム状態量と誤差共分散行列に基づいて、式（１７）、式（１８）によって時刻ｔ_ｍ＋１のシステム状態量の予測値^１θ_ｍ＋１ ⁻と共分散行列の予測値^１Ｐ_ｍ＋１ ⁻を算出する。また推定器１４５ｂは、位置、速度、姿勢の検出値との差を算出するための位置、速度、姿勢のカルマンフィルタ処理による予測値を算出する。

この結果、実施の形態１と同様に、小型軽量位置姿勢推定部１４０は、状態量の更新後に、システム状態量予測値^１ｘ_ｍ＋１ ⁻と誤差共分散行列予測値^１Ｐ_ｍ＋１ ⁻とを、位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号１９として出力する。

このように、本実施の形態では、小型軽量位置姿勢推定部１４０は高精度位置姿勢推定部１３０が推定した位置姿勢を基準として推定処理を行う。

さらに、位置姿勢出力部１５０は、小型軽量位置姿勢推定部１４０が推定した位置姿勢を基準として、第１の実施形態と同様に、仮想的なカルマンフィルタ処理を用いた推定処理を行うことにより補間処理を実施する。したがって、位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号１９を単独利用する補間処理と比較して更なる精度向上が可能となる。

以上、実施形態を参照して本願発明を説明したが、本願発明は、上記実施形態に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解しうる様々な変更をすることができる。

例えば、上述の位置姿勢出力部１５０が使用するカルマンフィルタ処理で用いられる観測量及び状態量は、現在位置ベクトルｐ＝（ｘ ｙ ｚ）の誤差ベクトル、姿勢ベクトルθ＝（φ ρ Ψ）の誤差ベクトルから構成されるものとして説明しているが、これに限られない。例えば位置姿勢出力部１５０は、小型軽量位置姿勢推定演算器１４５を経由してカルマンフィルタ処理に用いる小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４の検出信号の値を取得することができるよう構成されてもよい。このとき位置姿勢出力部１５０がカルマンフィルタ処理で用いる観測量及び状態量は、速度ベクトルｐ’＝（ｘ’ ｙ’ ｚ’）の誤差ベクトルを含むとしてもよい。さらに、小型軽量加速度センサ１４２のバイアスノイズを示すノイズベクトルａ_ｂ＝（ａ_ｂｘ ａ_ｂｙ ａ_ｂｚ）に対する誤差ベクトルを含むとしてもよい。さらに、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４の検出信号を補間により組み合わせた検出信号のバイアスノイズを示すベクトルω_ｂ＝（ω_ｂｘ ω_ｂｙ ω_ｂｚ）に対する誤差ベクトルを含むとしてもよい。

上述の実施形態における各構成は、ハードウェア又はソフトウェア、もしくはその両方によって構成され、１つのハードウェア又はソフトウェアから構成されてもよいし、複数のハードウェア又はソフトウェアから構成されてもよい。レーダ装置や位置姿勢推定装置の各機能（各処理）は、ＣＰＵやメモリ等を有するコンピュータにより実現されてもよい。例えば、記憶装置（記憶媒体）に実施形態における制御方法を行うための制御プログラムを格納し、各機能を、記憶装置に格納された制御プログラムをＣＰＵが実行することにより実現されてもよい。

上記の実施形態の一部または全部は、以下の付記のようにも記載され得るが、以下には限られない。
（付記１）
飛翔体や宇宙機や車両など移動体に搭載する駆動機器の姿勢情報を生成する移動体搭載機器の姿勢推定装置において、前記移動体に搭載し前記移動体の位置情報を移動体絶対位置検出信号として検出するためのＧＰＳ受信機と、前記移動体に搭載し前記移動体の加速度情報を移動体加速度検出信号として検出するための高精度加速度センサと、前記移動体に搭載し前記移動体の角速度情報を移動体姿勢角速度検出信号として検出するための高精度ジャイロセンサと、前記駆動機器に搭載し前記駆動機器の加速度情報を小型軽量加速度センサ検出信号として検出するための小型軽量加速度センサと、前記移動体絶対位置検出信号と前記移動体加速度検出信号と前記移動体姿勢角速度検出信号とを組み合わせることで高精度位置姿勢伝播値を含む移動体高精度位置姿勢推定信号を生成する高精度位置姿勢推定装置と、前記駆動機器に搭載し前記駆動機器の角速度情報を小型軽量ジャイロセンサ検出信号として検出するための小型軽量ジャイロセンサと、前記駆動機器に搭載し前記駆動機器の角速度情報をジャイロセンサ検出信号として検出するためのジャイロセンサと、前記駆動機器に搭載し前記駆動機器の位置姿勢角情報を前記小型軽量加速度センサ検出信号と前記小型軽量ジャイロセンサ検出信号と前記ジャイロセンサ検出信号とを組み合わせることで位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号として生成する小型軽量位置姿勢推定演算器と、前記移動体に搭載し前記駆動機器の姿勢角情報を前記高精度位置姿勢伝播値を含む移動体高精度位置姿勢推定信号と前記位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号とを組み合わせることで姿勢補間推定値を生成する姿勢推定装置を有することを特徴とする移動体搭載機器の姿勢推定装置。
（付記２）
前記姿勢推定装置が、前記高精度位置姿勢伝播値を含む移動体高精度位置姿勢推定信号と前記位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号とを組み合わせることで構成する仮想的なカルマンフィルタ演算処理により前記姿勢補間推定値を生成することを特徴とする付記１記載の移動体搭載機器の姿勢推定装置。
（付記３）
前記姿勢推定装置が、前記位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号に含まれる角速度信号に対し、前回生成した１サンプリング前の角速度信号と今回生成した角速度信号との間でサンプリング間隔における角速度信号の変化率（角加速度信号）を用いて角速度の変化を推定し、その推定に基づいた角速度信号を入力とする仮想的なカルマンフィルタ演算処理により前記姿勢補間推定値を生成することを特徴とする付記１記載の移動体搭載機器の姿勢推定装置。

この出願は、２０１４年４月２５日に出願された日本出願特願２０１４−０９１２７９を基礎とする優先権を主張し、その開示の全てをここに取り込む。

１ 第１の検出部
２ 第１の位置姿勢推定部
３ 第２の検出部
４ 第２の位置姿勢推定部
５ 位置姿勢出力部
１０ 位置姿勢推定装置
１１ 移動体絶対位置検出信号
１２ 移動体加速度検出信号
１３ 移動体姿勢角速度検出信号
１４ 移動体高精度位置姿勢推定信号
１５ 絶対位置検出信号
１６ 小型軽量加速度センサ検出信号
１７ 小型軽量ジャイロセンサ検出信号
１８ ジャイロセンサ検出信号
１９ 小型軽量位置姿勢推定演算器生成信号
２０ 位置姿勢補間推定値
３０ 移動体
３１ 機体
１００ レーダ装置
１０１ アンテナ部
１０２ 信号処理部
１０３ アンテナセンサ
１０４ 機体センサ
１１０ 送信部
１１１ 受信部
１１２ 画像処理部
１１３ サーキュレータ
１１４ 制御部
１１５ 位置姿勢推定部
１２０ 位置姿勢推定装置
１３０ 高精度位置姿勢推定部
１３１ ＧＰＳ受信機
１３２ 高精度加速度センサ
１３３ 高精度ジャイロセンサ
１３５ 高精度位置姿勢推定演算器
１３５ａ 伝播器
１３５ｂ 推定器
１４０ 小型軽量位置姿勢推定部
１４１ ＧＰＳ受信機
１４２ 小型軽量加速度センサ
１４３ 小型軽量ジャイロセンサ
１４４ ジャイロセンサ
１４５ 小型軽量位置姿勢推定演算器
１４５ａ 伝播器
１４５ｂ 推定器
１５０ 位置姿勢出力部
１５０ａ 伝播器
１５０ｂ 推定器
２０１ アンテナ
２０２ アンテナ駆動機構

Claims (17)

1. 移動体の位置姿勢に関する複数の第１の位置姿勢パラメータを検出する第１の検出手段と、
   検出した前記複数の第１の位置姿勢パラメータに基づいて、第１のタイミングにおける前記移動体の第１の位置姿勢を推定する第１の位置姿勢推定手段と、
   前記移動体の位置姿勢に関する複数の第２の位置姿勢パラメータを検出する第２の検出手段と、
   検出した前記複数の第２の位置姿勢パラメータに基づいて、前記第１のタイミングと異なる第２のタイミングにおける前記移動体の第２の位置姿勢を推定する第２の位置姿勢推定手段と、
   推定した前記第１の位置姿勢と前記第２の位置姿勢に基づき第３の位置姿勢を出力する位置姿勢出力手段と、
   を備える位置姿勢推定装置。
2. 前記第１の検出手段は、前記移動体の第１の位置を検出する第１の位置検出手段、前記移動体の第１の加速度を検出する第１の加速度検出手段及び前記移動体の第１の角速度を検出する第１の角速度検出手段を備え、
   前記第１の位置姿勢推定手段は、前記検出した第１の位置、第１の加速度及び第１の角速度に基づいて、前記第１の位置姿勢を推定する、
   請求項１に記載の位置姿勢推定装置。
3. 前記第１の位置姿勢推定手段は、前記検出した第１の位置を基準とし、前記検出した第１の加速度及び第１の角速度を用いたカルマンフィルタ処理を行うことで前記第１の位置姿勢を推定する、
   請求項２に記載の位置姿勢推定装置。
4. 前記第２の検出手段は、前記移動体の第２の位置を検出する第２の位置検出手段、前記移動体の第２の加速度を検出する第２の加速度検出手段及び前記移動体の第２の角速度を検出する第２の角速度検出手段を備え、
   前記第２の位置姿勢推定手段は、前記検出した第２の位置、第２の加速度及び第２の角速度に基づいて、前記第２の位置姿勢を推定する、
   請求項２または３に記載の位置姿勢推定装置。
5. 前記第２の位置姿勢推定手段は、前記検出した第２の位置を基準とし、前記検出した第２の加速度及び第２の角速度を用いたカルマンフィルタ処理を行うことで前記第２の位置姿勢を推定する、
   請求項４に記載の位置姿勢推定装置。
6. 前記第２の検出手段は、前記移動体の第２の加速度を検出する第２の加速度検出手段及び前記移動体の第２の角速度を検出する第２の角速度検出手段を備え、
   前記第２の位置姿勢推定手段は、前記検出した第１の位置、前記検出した第２の加速度及び第２の角速度に基づいて、前記第２の位置姿勢を推定する、
   請求項２または３に記載の位置姿勢推定装置。
7. 前記第２の位置姿勢推定手段は、前記検出した第１の位置を基準とし、前記検出した第２の加速度及び第２の角速度を用いたカルマンフィルタ処理を行うことで前記第２の位置姿勢を推定する、
   請求項６に記載の位置姿勢推定装置。
8. 前記第２の検出手段は、前記移動体の第２の加速度を検出する第２の加速度検出手段及び前記移動体の第２の角速度を検出する第２の角速度検出手段を備え、
   前記第２の位置姿勢推定手段は、前記第１の位置姿勢推定手段が推定した位置、前記検出した第２の加速度及び第２の角速度に基づいて、前記第２の位置姿勢を推定する、
   請求項２または３に記載の位置姿勢推定装置。
9. 前記第２の位置姿勢推定手段は、前記第１の位置姿勢推定手段が推定した位置を基準とし、前記検出した第２の加速度及び第２の角速度を用いたカルマンフィルタ処理を行うことで前記第２の位置姿勢を推定する、
   請求項８に記載の位置姿勢推定装置。
10. 前記第１の加速度検出手段は、前記第２の加速度検出手段よりも高精度に加速度を検出し、
    前記第１の角速度検出手段は、前記第２の角速度検出手段よりも高精度に角速度を検出する、
    請求項４乃至９のいずれか一項に記載の位置姿勢推定装置。
11. 前記第２の位置姿勢を推定する周期は、前記第１の位置姿勢を推定する周期よりも短い、
    請求項１乃至１０のいずれか一項に記載の位置姿勢推定装置。
12. 前記第２の検出手段は、前記移動体に搭載されて駆動される駆動手段に関する前記第２の位置姿勢パラメータを検出する、
    請求項１乃至１１のいずれか一項に記載の位置姿勢推定装置。
13. 前記移動体は、前記第３の位置姿勢に基づいて観測を行うレーダアンテナを備え、
    前記第１の検出手段は前記移動体に設けられ、前記第２の検出手段は前記レーダアンテナに設けられる、
    請求項１乃至１２のいずれか一項に記載の位置姿勢推定装置。
14. 前記位置姿勢出力手段は、
    第１のタイミングにおいては、前記推定した第１の位置姿勢に基づき前記第３の位置姿勢を出力し、
    第２のタイミングにおいては、前記推定した第２の位置姿勢に基づき前記第３の位置姿勢を出力し、
    第１のタイミングと第２のタイミングのいずれとも異なる第３のタイミングにおいては、前記推定した第１の位置姿勢又は第２の位置姿勢に基づいて、前記移動体の第３の位置姿勢を出力する、
    請求項１乃至１３のいずれか一項に記載の位置姿勢推定装置。
15. 前記位置姿勢出力手段は、
    第１のタイミングにおいては前記推定した第１の位置姿勢を検出値としてカルマンフィルタ処理後のシステム状態量を算出し、
    第２のタイミングにおいては前記推定した第２の位置姿勢を検出値としてカルマンフィルタ処理後のシステム状態量を算出し、
    第１のタイミングと第２のタイミングのいずれとも異なる第３のタイミングにおいては、前記推定した第１の位置姿勢又は第２の位置姿勢のうち最後に出力された位置姿勢を検出値としてカルマンフィルタ処理後のシステム状態量を算出する
    請求項１４に記載の位置姿勢推定装置。
16. 移動体の位置姿勢を推定する移動体位置姿勢推定手段と、前記移動体に搭載されたレーダが観測した信号に対し、前記推定した位置姿勢に基づき画像処理を行う画像処理手段と、を備えた画像処理装置であって、
    前記移動体位置姿勢推定手段は、
    前記移動体の位置姿勢に関する複数の第１の位置姿勢パラメータを検出する第１の検出手段と、
    検出した前記複数の第１の位置姿勢パラメータに基づいて、第１のタイミングにおける前記移動体の第１の位置姿勢を推定する第１の位置姿勢推定手段と、
    前記移動体の位置姿勢に関する複数の第２の位置姿勢パラメータを検出する第２の検出手段と、
    検出した前記複数の第２の位置姿勢パラメータに基づいて、前記第１のタイミングと異なる第２のタイミングにおける前記移動体の第２の位置姿勢を推定する第２の位置姿勢推定手段と、
    推定した前記第１の位置姿勢と前記第２の位置姿勢に基づき第３の位置姿勢を、前記画像処理手段へ出力する位置姿勢出力手段と、
    を備える画像処理装置。
17. 移動体の位置姿勢に関する複数の第１の位置姿勢パラメータを検出し、
    検出した前記複数の第１の位置姿勢パラメータに基づいて、第１のタイミングにおける前記移動体の第１の位置姿勢を推定し、
    前記移動体の位置姿勢に関する複数の第２の位置姿勢パラメータを検出し、
    検出した前記複数の第２の位置姿勢パラメータに基づいて、前記第１のタイミングと異なる第２のタイミングにおける前記移動体の第２の位置姿勢を推定し、
    推定した前記第１の位置姿勢と前記第２の位置姿勢に基づき第３の位置姿勢を出力する、
    位置姿勢推定方法。

Images