(57)【特許請求の範囲】
【請求項１】
移動体の第１の位置を検出する第１の位置検出手段、前記移動体の第１の加速度を検出する第１の加速度検出手段及び前記移動体の第１の角速度を検出する第１の角速度検出手段を備え、前記第１の位置、前記第１の加速度及び前記第１の角速度を検出する第１の検出手段と、
前記検出した第１の位置を基準とし、前記検出した第１の加速度及び第１の角速度を用いたカルマンフィルタ処理を行うことで第１のタイミングにおける前記移動体の第１の位置姿勢を推定する第１の位置姿勢推定手段と、
前記移動体の第２の加速度を検出する第２の加速度検出手段及び前記移動体の第２の角速度を検出する第２の角速度検出手段を備え、前記第２の加速度及び前記第２の角速度を検出する第２の検出手段と、
前記第１の位置姿勢推定手段が推定した前記第１の位置姿勢を基準とし、前記検出した第２の加速度及び第２の角速度を用いたカルマンフィルタ処理を行うことで前記第１のタイミングと異なる第２のタイミングにおける前記移動体の第２の位置姿勢を推定する前記第２の位置姿勢を推定する第２の位置姿勢推定手段と、
推定した前記第１の位置姿勢と前記第２の位置姿勢に基づき第３の位置姿勢を出力する位置姿勢出力手段と、
を備える位置姿勢推定装置。
【請求項２】
前記第１の加速度検出手段は、前記第２の加速度検出手段よりも高精度に加速度を検出し、
前記第１の角速度検出手段は、前記第２の角速度検出手段よりも高精度に角速度を検出する、
請求項１に記載の位置姿勢推定装置。
【請求項３】
前記第２の位置姿勢を推定する周期は、前記第１の位置姿勢を推定する周期よりも短い、
請求項１又は２に記載の位置姿勢推定装置。
【請求項４】
前記第２の検出手段は、前記移動体に搭載されて駆動される駆動手段に関する第２の位置、前記第２の加速度及び前記第２の角速度を検出する、
請求項１乃至３のいずれか一項に記載の位置姿勢推定装置。
【請求項５】
前記移動体は、前記第３の位置姿勢に基づいて観測を行うレーダアンテナを備え、
前記第１の検出手段は前記移動体に設けられ、前記第２の検出手段は前記レーダアンテナに設けられる、
請求項１乃至４のいずれか一項に記載の位置姿勢推定装置。
【請求項６】
前記位置姿勢出力手段は、
第１のタイミングにおいては、前記推定した第１の位置姿勢に基づき前記第３の位置姿勢を出力し、
第２のタイミングにおいては、前記推定した第２の位置姿勢に基づき前記第３の位置姿勢を出力し、
第１のタイミングと第２のタイミングのいずれとも異なる第３のタイミングにおいては、前記推定した第１の位置姿勢と第２の位置姿勢に基づいて、前記移動体の第３の位置姿勢を出力する、
請求項１乃至５のいずれか一項に記載の位置姿勢推定装置。
【請求項７】
前記位置姿勢出力手段は、前記第１の位置姿勢または前記第２の位置姿勢の一方を基準とし、前記第１の位置姿勢または前記第２の位置姿勢の他方を用いたカルマンフィルタ処理を行うことで前記第３の位置姿勢を推定する、
請求項６に記載の位置姿勢推定装置。
【請求項８】
移動体の位置姿勢を推定する移動体位置姿勢推定手段と、前記移動体に搭載されたレーダが観測した信号に対し、前記推定した位置姿勢に基づき画像処理を行う画像処理手段と、を備えた画像処理装置であって、
前記移動体位置姿勢推定手段は、
前記移動体の第１の位置を検出する第１の位置検出手段、前記移動体の第１の加速度を検出する第１の加速度検出手段及び前記移動体の第１の角速度を検出する第１の角速度検出手段を備え、前記第１の位置、前記第１の加速度及び前記第１の角速度を検出する第１の検出手段と、
前記検出した第１の位置を基準とし、前記検出した第１の加速度及び第１の角速度を用いたカルマンフィルタ処理を行うことで第１のタイミングにおける前記移動体の第１の位置姿勢を推定する第１の位置姿勢推定手段と、
前記移動体の第２の加速度を検出する第２の加速度検出手段及び前記移動体の第２の角速度を検出する第２の角速度検出手段を備え、前記第２の加速度及び前記第２の角速度を検出する第２の検出手段と、
前記第１の位置姿勢推定手段が推定した前記第１の位置姿勢を基準とし、前記検出した第２の加速度及び第２の角速度を用いたカルマンフィルタ処理を行うことで前記第１のタイミングと異なる第２のタイミングにおける前記移動体の第２の位置姿勢を推定する第２の位置姿勢推定手段と、
推定した前記第１の位置姿勢と前記第２の位置姿勢に基づき第３の位置姿勢を出力する位置姿勢出力手段と、
を備える画像処理装置。
【請求項９】
移動体の第１の位置を検出し、
前記移動体の第１の加速度を検出し、
前記移動体の第１の角速度を検出し、
前記検出した第１の位置を基準とし、前記検出した第１の加速度及び第１の角速度を用いたカルマンフィルタ処理を行うことで第１のタイミングにおける前記移動体の第１の位置姿勢を推定し、
前記移動体の第２の加速度を検出し、
前記移動体の第２の角速度を検出し、
前記推定した第１の位置姿勢を基準とし、前記検出した第２の加速度及び第２の角速度を用いたカルマンフィルタ処理を行うことで前記第１のタイミングと異なる第２のタイミングにおける前記移動体の第２の位置姿勢を推定し、
推定した前記第１の位置姿勢と前記第２の位置姿勢に基づき第３の位置姿勢を出力する、
位置姿勢推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置姿勢推定装置、画像処理装置及び位置姿勢推定方法に関し、特に、センサを用いて位置姿勢を推定する位置姿勢推定装置、画像処理装置及び位置姿勢推定方法に関する。
【背景技術】
【０００２】
宇宙機や飛翔体、無人航空機、無人自動車両などの移動体の位置姿勢（位置及び姿勢）を推定する位置姿勢推定装置が知られている。例えば、特許文献１〜４には、関連する技術が記載されている。【０００３】
図７は、特許文献１に記載された関連する移動体位置姿勢推定装置９００の一例を示すブロック図である。関連する移動体位置姿勢推定装置９００は、移動体に搭載されており、各センサの検出に基づいて移動体の位置姿勢を推定する。図７に示すように、関連する移動体位置姿勢推定装置９００は、ジャイロセンサ検出信号９０５を出力するジャイロセンサ９０１、ＧＰＳ（Global Positioning System）受信機出力信号９０６を出力するＧＰＳ受信機９０２を備えている。また、移動体位置姿勢推定装置９００は、加速度センサ検出信号９０７を出力する加速度センサ９０３、移動体位置姿勢推定装置出力信号９０８を出力する演算制御部９０４を備えている。【０００４】
関連する移動体位置姿勢推定装置９００では、ＧＰＳ受信機９０２が出力するＧＰＳ受信機出力信号９０６と加速度センサ９０３が出力する加速度センサ検出信号９０７とから演算制御部９０４が移動体位置を推定する。移動体位置姿勢推定装置９００では、これと共に、ジャイロセンサ９０１が出力するジャイロセンサ検出信号９０５とＧＰＳ受信機出力信号９０６の変動とから演算制御部９０４が移動体姿勢を推定し、それらをまとめて移動体位置姿勢推定装置出力信号９０８を出力する。【０００５】
特許文献１では、複合航法システムは、図７の演算制御部９０４が、非線形な運動方程式で表現される移動体運動に対しカルマンフィルタを拡張した誤差モデルを適用する。この構成によって、複合航法システムは、ＭＥＭＳ（Micro Electro Mechanical System）センサなど軽量、小型で、かつ安価なセンサ等を利用しても、検出値を効果的に真値に近い値へ補正可能となっている。【０００６】
一方、関連する姿勢計測装置の一例として、例えば特許文献２などに示された関連する姿勢計測方式では、カルマンフィルタをベースに伝播処理における積分処理や時間更新における状態推定ならびにその状態量に対する誤差共分散を更新する必要がない。したがってこの姿勢計測装置の処理内容がより簡単となり短時間で物体の姿勢を計測することができ、さらに姿勢計測装置のコストも低減化できる。【０００７】
また、例えば特許文献３などに記載された関連する技術では、カルマンフィルタでの航法計算で必須となる基準座標決定処理に関し、カルマンフィルタの収束状況が監視され適切な補償処理が施される。このことで、ある程度の動揺環境においても基準座標決定処理の高精度・高速化が実現され、運用上の制約が小さくできる。【０００８】
さらに、例えば特許文献４などに記載された関連する技術では、カルマンフィルタ処理における座標変換マトリクスの姿勢角誤差修正と方位角誤差修正が分離され、更に方位角誤差修正には方位角の設定値が使用される。このことで、３軸のアライメント演算が短時間で実行され、運用制約の低減化が可能となる。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開２００７−２７６５０７号公報（段落０００７、図１）
【特許文献２】特開２００６−３８６５０号公報（段落００７０、図１）
【特許文献３】特開２００１−２６４１０６号公報（段落００２１、図１）
【特許文献４】特許第３９１９９１１号公報（段落００６５、図１）
【発明の概要】
【発明が解決しようとする課題】
【００１０】
図７に示すような関連する移動体位置姿勢推定装置９００では、ＧＰＳ受信機９０２と加速度センサ９０３とジャイロセンサ９０１の検出信号に基づいて、演算制御部９０４が一定の周期（タイミング）で位置姿勢を推定する。移動体位置姿勢推定装置９００では、この位置姿勢推定処理の周期により所望の推定精度を得ている。しかしながら、この関連する技術では、演算制御部９０４のみの位置姿勢推定処理の周期（タイミング）では、位置姿勢の推定精度を向上させることが困難であるという問題がある。【００１１】
本発明は、このような課題に鑑み、位置姿勢の推定精度を向上することが可能な位置姿勢推定装置、画像処理装置及び位置姿勢推定方法を提供することを目的とする。
【課題を解決するための手段】
【００１２】
本発明に係る位置姿勢推定装置は、移動体の位置姿勢に関する複数の第１の位置姿勢パラメータを検出する第１の検出部と、検出した前記複数の第１の位置姿勢パラメータに基づいて、第１のタイミングにおける前記移動体の第１の位置姿勢を推定する第１の位置姿勢推定部と、前記移動体の位置姿勢に関する複数の第２の位置姿勢パラメータを検出する第２の検出部と、検出した前記複数の第２の位置姿勢パラメータに基づいて、前記第１のタイミングと異なる第２のタイミングにおける前記移動体の第２の位置姿勢を推定する第２の位置姿勢推定部と、推定した前記第１の位置姿勢と前記第２の位置姿勢に基づき第３の位置姿勢を出力する位置姿勢出力部と、を備えるものである。【００１３】
本発明に係る画像処理装置は、移動体の位置姿勢を推定する移動体位置姿勢推定部と、前記移動体に搭載されたレーダが観測した信号に対し、前記推定した位置姿勢に基づき画像処理を行う画像処理部と、を備えた画像処理装置であって、前記移動体位置姿勢推定部は、前記移動体の位置姿勢に関する複数の第１の位置姿勢パラメータを検出する第１の検出部と、検出した前記複数の第１の位置姿勢パラメータに基づいて、第１のタイミングにおける前記移動体の第１の位置姿勢を推定する第１の位置姿勢推定部と、前記移動体の位置姿勢に関する複数の第２の位置姿勢パラメータを検出する第２の検出部と、検出した前記複数の第２の位置姿勢パラメータに基づいて、前記第１のタイミングと異なる第２のタイミングにおける前記移動体の第２の位置姿勢を推定する第２の位置姿勢推定部と、推定した前記第１の位置姿勢と前記第２の位置姿勢に基づき第３の位置姿勢を、前記画像処理部へ出力する位置姿勢出力部と、を備えるものである。【００１４】
本発明に係る位置姿勢推定方法は、移動体の位置姿勢に関する複数の第１の位置姿勢パラメータを検出し、検出した前記複数の第１の位置姿勢パラメータに基づいて、第１のタイミングにおける前記移動体の第１の位置姿勢を推定し、前記移動体の位置姿勢に関する複数の第２の位置姿勢パラメータを検出し、検出した前記複数の第２の位置姿勢パラメータに基づいて、前記第１のタイミングと異なる第２のタイミングにおける前記移動体の第２の位置姿勢を推定し、推定した前記第１の位置姿勢と前記第２の位置姿勢に基づき第３の位置姿勢を出力するものである。
【発明の効果】
【００１５】
本発明によれば、位置姿勢の推定精度を向上することが可能な位置姿勢推定装置、画像処理装置及び位置姿勢推定方法を提供することができる。
【図面の簡単な説明】
【００１６】
【図１】実施の形態に係る位置姿勢推定装置の概要を示す構成図である。
【図２】実施の形態１に係る移動体を模式的に示す模式図である。
【図３】実施の形態１に係るレーダ装置の構成を示すブロック図である。
【図４】実施の形態１に係る位置姿勢推定装置の構成を示すブロック図である。
【図５】実施の形態１に係る位置姿勢推定装置の動作を示すフローチャートである。
【図６】実施の形態２に係る位置姿勢推定装置の構成を示すブロック図である。
【図７】関連する位置姿勢推定装置の構成を示すブロック図である。
【発明を実施するための形態】
【００１７】
（実施の形態の概要）
図１は、実施の形態に係る位置姿勢推定装置１０の概要を示す構成図である。図１に示すように、実施の形態に係る位置姿勢推定装置１０は、第１の検出部１、第１の位置姿勢推定部２、第２の検出部３、第２の位置姿勢推定部４、位置姿勢出力部５を備えている。
【００１８】
第１の検出部１は、移動体の位置姿勢に関する複数の第１の位置姿勢パラメータを検出する。第１の位置姿勢推定部２は、第１の検出部１が検出した複数の第１の位置姿勢パラメータに基づいて、第１のタイミングにおける移動体の第１の位置姿勢を推定する。第２の検出部３は、移動体の位置姿勢に関する複数の第２の位置姿勢パラメータを検出する。第２の位置姿勢推定部４は、第２の検出部３が検出した複数の第２の位置姿勢パラメータに基づいて、第１のタイミングと異なる第２のタイミングにおける移動体の第２の位置姿勢を推定する。位置姿勢出力部５は、推定した第１の位置姿勢と第２の位置姿勢に基づき第３の位置姿勢を出力する。
【００１９】
このように、第１のタイミングと第２のタイミングのそれぞれで移動体の位置姿勢を推定し、推定したこれらの位置姿勢に基づいて第３の位置姿勢を出力する。このことにより、第１または第２の位置姿勢推定部のみで位置姿勢を推定する場合に比べて、位置姿勢の推定精度をより向上することができる。
【００２０】
ここで、上記関連する技術の具体的な問題について説明する。図７に示すような移動体位置姿勢推定装置（特許文献１）では、搭載センサがＧＰＳ受信機９０２と加速度センサ９０３とジャイロセンサ９０１の３種のみで構成される。このため、特に姿勢推定精度は、適用するジャイロセンサ９０１の単体検出性能に依存してしまい、カルマンフィルタにおける誤差モデル精度を向上させても達成可能な推定精度には限界があるという問題点がある。
【００２１】
また、特許文献２に記載された技術では、センサとして３軸加速度センサと３軸ジャイロのみが適用され、それらを用いたカルマンフィルタ処理により姿勢推定が実現されている。このため、ここから導出される姿勢推定精度は、３軸ジャイロの検出性能とカルマンフィルタを構成する誤差モデルの精度との間で生じる精度バランスに依存してしまい、絶対的な姿勢精度を確保することが困難であるという欠点がある。
【００２２】
一方、特許文献３に記載された技術では、構成自体はジャイロセンサと加速度センサからなるカルマンフィルタとなっているものの、推定誤差を除去する構成ではなくあくまでカルマンフィルタの収束状態を監視するものである。したがって、カルマンフィルタの適用目的は本発明と異なっている。
【００２３】
さらに、特許文献４に記載された技術では、姿勢角誤差修正において精度確保が難しい方位角誤差修正部分のみはあえて分離され、他の姿勢角誤差修正値が得られてから逐次的に方位角誤差修正が実施される。このため、得られる方位角誤差修正値は、ベースとする姿勢角誤差修正値の精度に依存する結果となり、３軸姿勢情報の直交性（独立性）が確保できないという問題がある。
【００２４】
上記関連する技術の問題点に鑑み、以下の実施の形態では、宇宙機や飛翔体や無人航空機や無人自動車などの移動体における位置姿勢を高精度に推定することが可能となる。特に、移動体に搭載する駆動機器の現在の姿勢を、移動体本体に搭載した位置姿勢推定装置と、搭載駆動機器に装着した姿勢推定装置とを陽に組み合わせることで、搭載駆動機器の姿勢を高精度に逐次推定可能とする移動体搭載機器の姿勢推定装置を提供することが可能となる。
（実施の形態１）
以下、図面を参照して実施の形態１について説明する。本実施の形態では、レーダ装置を搭載する移動体において、移動体の位置姿勢、特に搭載するレーダアンテナの位置姿勢を推定する例について説明する。
【００２５】
図２は、本実施の形態に係る移動体３０を模式的に示している。本実施の形態に係る移動体３０は、宇宙機、飛翔体、無人航空機、無人自動車などであり、例えば小型航空機やヘリコプターである。移動体３０は、レーダ装置１００を搭載しており、移動しながら地表の状態等を観測する。レーダ装置１００は、一例として、合成開口レーダ（ＳＡＲ：Synthetic Aperture Radar）である。合成開口レーダは、アンテナによって得られた受信波の振幅と位相のデータ（複素データ）から、アンテナ（移動体）の位置姿勢に基づいて観測画像を再生し、地表の状態を判読する。ここでは、合成開口レーダを用いるため、特にアンテナの位置姿勢を精度よく推定する必要がある。なお、レーダ装置１００は、合成開口レーダに限らず、捜索レーダなどその他のレーダであってもよい。
【００２６】
レーダ装置１００は、主に、アンテナを含むアンテナ部１０１、アンテナが送受信する電波を信号処理する信号処理部１０２を備える。例えば、アンテナ部１０１は、移動体３０の機体３１の前方下部に設置され、アンテナから機体３１下方の地表へ向けて電波を照射する。信号処理部１０２は、移動体３０の機体３１内部に設置され、アンテナを介して観測した観測画像をリアルタイムで表示する。さらに、移動体３０の機体３１には、機体３１の位置、速度、角速度を検出する機体センサ１０４が設けられ、アンテナ部１０１には、アンテナの位置、速度、角速度を検出するアンテナセンサ１０３が設けられている。
【００２７】
図３は、本実施の形態に係るレーダ装置１００の機能ブロックを示している。図３に示すように、本実施の形態に係るレーダ装置１００は、送信部１１０、受信部１１１、画像処理部１１２、サーキュレータ１１３、制御部１１４、位置姿勢推定部１１５、アンテナ２０１、アンテナ駆動機構２０２、アンテナセンサ１０３、機体センサ１０４を備えている。
【００２８】
例えば、図２のアンテナ部１０１には、アンテナ２０１、アンテナ駆動機構２０２、アンテナセンサ１０３が含まれ、図２の信号処理部１０２には、送信部１１０、受信部１１１、画像処理部１１２、サーキュレータ１１３、制御部１１４、位置姿勢推定部１１５、機体センサ１０４が含まれる。また、アンテナセンサ１０３、機体センサ１０４、位置姿勢推定部１１５が、移動体３０（アンテナ）の位置姿勢を推定する位置姿勢推定装置１２０を構成する。さらに、位置姿勢推定装置１２０と画像処理部１１２とは、レーダの観測画像（ＳＡＲ画像）を生成する画像処理装置を構成しているとも言える。
【００２９】
送信部１１０は、合成開口レーダで観測を行うための送信信号を生成する。サーキュレータ１１３は、送信部１１０が生成した送信信号をアンテナ２０１から送信し、アンテナ２０１が受信した受信信号を受信部１１１へ出力する。アンテナ駆動機構２０２は、制御部１１４の制御に応じて最適な方向や位置となるようにアンテナ２０１を駆動する。アンテナ２０１は、送信波（送信信号）を観測対象へ送信し、観測対象から反射した受信波（受信信号）を受信する。
【００３０】
機体センサ１０４は、移動体３０の機体３１の移動、姿勢（ロール、ピッチ、ヨーの変位）や振動などを検出する。機体センサ１０４は、移動体３０の位置姿勢に関する位置姿勢パラメータとして、例えば、位置、加速度、角速度を検出する。アンテナセンサ１０３は、移動体３０の移動や振動、アンテナ駆動機構２０２の駆動に応じて変位したアンテナ２０１の変位を検出する。アンテナセンサ１０３は、アンテナ（移動体）の位置姿勢に関する位置姿勢パラメータとして、例えば、位置、加速度、角速度を検出する。すなわち、アンテナセンサ１０３及び機体センサ１０４は、後述のように、ＧＰＳ受信機、加速度センサ、ジャイロセンサなどのセンサを含む。
【００３１】
受信部１１１は、アンテナ２０１が受信した受信信号に対して信号処理を行い、画像処理部１１２が処理可能な信号を生成する。制御部１１４は、レーダ装置の各部を制御する制御部であり、アンテナセンサ１０３及び機体センサ１０４の検出結果や、画像処理部１１２の観測結果に基づいてアンテナ駆動機構２０２などを制御する。
【００３２】
位置姿勢推定部１１５（位置姿勢推定装置１２０）は、アンテナセンサ１０３及び機体センサ１０４の検出に基づいて、移動体３０（アンテナ）の位置姿勢を推定する。画像処理部１１２は、受信部１１１が処理した受信信号に対して、位置姿勢推定部１１５が推定した位置姿勢（姿勢補間推定値）を用いて画像処理を行い、観測対象の検出、観測画像の生成及び表示を行う。例えば、画像処理部１１２が画像処理を行うタイミング（周期）を位置姿勢推定部１１５へ出力し、位置姿勢推定部１１５が、この画像処理のタイミングに合わせて、位置姿勢推定結果を出力してもよい。
【００３３】
図４は、本実施の形態に係る位置姿勢推定装置１２０の構成を示すブロック図である。図４に示すように、本実施の形態に係る位置姿勢推定装置１２０は、高精度位置姿勢推定部１３０、小型軽量位置姿勢推定部１４０、位置姿勢出力部１５０を備えている。ここで図３の位置姿勢推定部１１５は、例えば、高精度位置姿勢推定部１３０の高精度位置姿勢推定演算器１３５、小型軽量位置姿勢推定部１４０の小型軽量位置姿勢推定演算器１４５、位置姿勢出力部１５０を備えている。
【００３４】
高精度位置姿勢推定部１３０は、小型軽量位置姿勢推定部１４０よりも高精度に位置姿勢を推定する推定装置である。高精度位置姿勢推定部１３０は、例えば小型軽量位置姿勢推定部１４０よりも大型の装置である。高精度位置姿勢推定部１３０は、機体センサ１０４の検出に基づき機体の位置姿勢を推定する。高精度位置姿勢推定部１３０は、ＧＰＳ受信機１３１、高精度加速度センサ１３２、高精度ジャイロセンサ１３３、高精度位置姿勢推定演算器１３５を備えている。
【００３５】
機体センサ（第１の検出部）１０４は、ＧＰＳ受信機１３１、高精度加速度センサ１３２、高精度ジャイロセンサ１３３を備えている。ＧＰＳ受信機１３１は、機体の絶対位置を検出し、高精度位置姿勢推定演算器（第１の位置姿勢推定部）１３５に検出した絶対位置を含む移動体絶対位置検出信号１１を出力する。高精度加速度センサ１３２は、機体の加速度を検出し、高精度位置姿勢推定演算器（第１の位置姿勢推定部）１３５に検出した加速度を含む移動体加速度検出信号１２を出力する。
【００３６】
高精度ジャイロセンサ１３３は、機体の角速度を検出し、高精度位置姿勢推定演算器（第１の位置姿勢推定部）１３５に検出した角速度を含む移動体姿勢角速度検出信号１３を出力する。高精度加速度センサ１３２、高精度ジャイロセンサ１３３は、例えば、小型軽量位置姿勢推定部１４０のセンサ（小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ）よりも高精度かつ大型のセンサである。
【００３７】
高精度位置姿勢推定演算器（第１の位置姿勢推定部）１３５は、ＧＰＳ受信機１３１、高精度加速度センサ１３２及び高精度ジャイロセンサ１３３が検出した絶対位置、加速度及び角速度に基づいて、移動体（機体）の位置姿勢を推定する。高精度位置姿勢推定演算器（第１の位置姿勢推定部）１３５は、推定した位置姿勢（位置姿勢伝播値）を含む移動体高精度位置姿勢推定信号１４を出力する。高精度位置姿勢推定演算器１３５は、例えば、パーソナルコンピュータなどの演算装置である。高精度位置姿勢推定演算器１３５は、画像処理部と同じ装置であってもよい。高精度位置姿勢推定演算器１３５は、ＧＰＳ受信機１３１が検出した絶対位置と高精度加速度センサ１３２が検出した加速度と高精度ジャイロセンサ１３３が検出した角速度とから機体の位置姿勢を推定し、推定した位置姿勢（位置姿勢伝播値）を含む移動体高精度位置姿勢推定信号１４を出力する。
【００３８】
高精度位置姿勢推定演算器１３５は、例えば、ＧＰＳ受信機１３１が検出した位置を基準とし、高精度加速度センサ１３２が検出した加速度と高精度ジャイロセンサ１３３が検出した角速度とを用いて、カルマンフィルタ処理を行う。これによって高精度位置姿勢推定演算器１３５は、第１のタイミング（周期）の位置姿勢を推定する。高精度位置姿勢推定演算器１３５は、例えば５０〜２００Ｈｚ周期で、位置姿勢を推定し移動体高精度位置姿勢推定信号１４を出力する。高精度位置姿勢推定演算器１３５が位置姿勢を推定する周期は、小型軽量位置姿勢推定部１４０よりも長い。高精度位置姿勢推定演算器１３５は、検出した加速度及び角速度の値に基づいてカルマンフィルタ処理により演算した位置姿勢（位置姿勢伝播値）を出力する伝播器１３５ａを含む。また高精度位置姿勢推定演算器１３５は、出力した位置姿勢（位置姿勢伝播値）及びＧＰＳ受信機が検出した位置に基づいて次回の加速度及び角速度の検出時における位置姿勢を推定する推定器１３５ｂを含む。
【００３９】
小型軽量位置姿勢推定部１４０は、推定する位置姿勢の精度が高精度位置姿勢推定部１３０よりも低い。また小型軽量位置姿勢推定部１４０は、例えば高精度位置姿勢推定部１３０よりも小型かつ軽量の位置姿勢装置である。小型軽量位置姿勢推定部１４０は、例えばアンテナに設置されるアンテナセンサ１０３の検出する絶対位置、加速度及び角速度に基づきアンテナの位置姿勢を推定する。小型軽量位置姿勢推定部１４０は、ＧＰＳ受信機１４１、小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４、小型軽量位置姿勢推定演算器１４５を備えている。なお、この例では、合成開口レーダの観測を行うため、アンテナに設置されたセンサに基づき、小型軽量位置姿勢推定部１４０がアンテナの位置姿勢を推定することが好ましいが、これに限らず、機体（移動体）に設置されたセンサに基づき、小型軽量位置姿勢推定部１４０が機体（移動体）の位置姿勢を推定してもよい。
【００４０】
アンテナに設置されたアンテナセンサ（第２の検出部）１０３は、ＧＰＳ受信機１４１、小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４を備えている。ＧＰＳ受信機１４１は、アンテナの絶対位置を検出し、小型軽量位置姿勢推定演算器（第２の位置姿勢推定部）１４５に検出した絶対位置を含む絶対位置検出信号１５を出力する。小型軽量加速度センサ１４２は、アンテナの加速度を検出し、小型軽量位置姿勢推定演算器（第２の位置姿勢推定部）１４５に検出した加速度を含む小型軽量加速度センサ検出信号１６を出力する。
【００４１】
小型軽量ジャイロセンサ１４３は、アンテナの角速度を検出し、小型軽量位置姿勢推定演算器（第２の位置姿勢推定部）１４５に検出した角速度を含む小型軽量ジャイロセンサ検出信号１７を出力する。ジャイロセンサ１４４は、アンテナの角速度を検出し、小型軽量位置姿勢推定演算器（第２の位置姿勢推定部）１４５に検出した角速度を含むジャイロセンサ検出信号１８を出力する。
【００４２】
ＧＰＳ受信機１４１は、高精度位置姿勢推定部１３０のＧＰＳ受信機１３１と同様のセンサである。小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３は、高精度位置姿勢推定部１３０の高精度加速度センサ１３２、高精度ジャイロセンサ１３３よりも小型軽量かつ低精度のセンサである。ジャイロセンサ１４４は、中型のセンサであり、小型軽量ジャイロセンサ１４３よりも高精度のセンサである。なお、アンテナセンサ（第２の検出部）１０３がジャイロセンサ１４４を備え、ジャイロセンサ１４４が小型軽量ジャイロセンサ１４３の検出精度を補完することが好ましいが、アンテナセンサ（第２の検出部）１０３はジャイロセンサ１４４を備えることなく、小型軽量ジャイロセンサ１４３のみを備えていてもよい。
【００４３】
小型軽量位置姿勢推定演算器（第２の位置姿勢推定部）１４５は、ＧＰＳ受信機１４１の検出した絶対位置、小型軽量加速度センサ１４２の検出した加速度、小型軽量ジャイロセンサ１４３の検出した角速度、ジャイロセンサ１４４の検出した角速度に基づいて、アンテナの位置姿勢を推定し、推定した位置姿勢（位置姿勢伝播値）を含む小型軽量位置姿勢推定演算器生成信号１９を出力する。なお、小型軽量位置姿勢推定演算器（第２の位置姿勢推定部）１４５は、ＧＰＳ受信機１４１を備えず、高精度位置姿勢推定部１３０のＧＰＳ受信機１３１の検出した絶対位置に基づいて、位置姿勢を推定してもよい。小型軽量位置姿勢推定演算器１４５は、ＣＰＵを含む半導体装置など小型の演算装置である。
【００４４】
小型軽量位置姿勢推定演算器１４５は、ＧＰＳ受信機１４１が検出した位置を基準とし、小型軽量加速度センサ１４２が検出した加速度と小型軽量ジャイロセンサ１４３が検出した角速度とジャイロセンサ１４４が検出した角速度とを用いてカルマンフィルタ処理を行う。これによって小型軽量位置姿勢推定演算器１４５は、第２のタイミング（周期）の位置姿勢を推定する。小型軽量位置姿勢推定演算器１４５は、例えば４００Ｈｚ周期で位置姿勢を推定する。小型軽量位置姿勢推定演算器１４５が位置姿勢を推定する周期は、高精度位置姿勢推定部１３０よりも長い。小型軽量位置姿勢推定演算器１４５は、検出された加速度及び角速度の値に基づいて得られた位置姿勢（位置姿勢伝播値）を出力する伝播器１４５ａと、出力した位置姿勢（位置姿勢伝播値）及びＧＰＳ受信機が検出した位置に基づいて次回の加速度及び角速度の検出時における位置姿勢を推定する推定器１４５ｂを含む。
【００４５】
位置姿勢出力部１５０は、高精度位置姿勢推定部１３０から出力された位置姿勢と小型軽量位置姿勢推定部１４０から出力された位置姿勢に基づいて、移動体（アンテナ）の位置姿勢を推定し、推定した位置姿勢を含む位置姿勢補間推定値２０を出力する。位置姿勢出力部１５０は、高精度位置姿勢推定部１３０と小型軽量位置姿勢推定部１４０が推定した位置姿勢を含めて出力してもよい。位置姿勢出力部１５０は、例えば、パーソナルコンピュータなどの演算装置である。位置姿勢出力部１５０は、高精度位置姿勢推定演算器１３５や画像処理部と同じ装置であってもよい。このとき位置姿勢出力部１５０は、高精度位置姿勢推定演算器１３５が出力した位置姿勢（位置姿勢伝播値）と、小型軽量位置姿勢推定演算器１４５が出力した位置姿勢（位置姿勢伝播値）とに対する補間値となる位置姿勢補間推定値２０を生成する。
【００４６】
位置姿勢出力部１５０は、高精度位置姿勢推定演算器１３５が出力した位置姿勢、または、小型軽量位置姿勢推定演算器１４５が出力した位置姿勢の一方の位置姿勢を基準とし、他方の位置姿勢を用いて、（仮想的な）カルマンフィルタ処理を行う。これによって位置姿勢出力部１５０は、第３のタイミング（周期）での位置姿勢を推定する。すなわち、位置姿勢出力部１５０は、第１の位置姿勢（例えば高精度位置姿勢推定演算器１３５が出力した位置姿勢）と第２の位置姿勢（例えば小型軽量位置姿勢推定演算器１４５が出力した位置姿勢）に基づき第３の位置姿勢を出力する。位置姿勢出力部１５０は、第１のタイミングにおいては、高精度位置姿勢推定演算器１３５が出力した第１の位置姿勢に基づき第３の位置姿勢を出力する。第２のタイミングにおいては、位置姿勢出力部１５０は、小型軽量位置姿勢推定演算器１４５が出力した第２の位置姿勢に基づき第３の位置姿勢を出力する。第１のタイミングと第２のタイミングのいずれとも異なる第３のタイミングにおいては、位置姿勢出力部１５０は、高精度位置姿勢推定演算器１３５が出力した第１の位置姿勢又は小型軽量位置姿勢推定演算器１４５が出力した第２の位置姿勢に基づいて、移動体の第３の位置姿勢を出力する。例えば、位置姿勢出力部１５０が位置姿勢を推定し姿勢補間推定値２０を出力する周期は、３ＫＨｚである。位置姿勢出力部１５０が位置姿勢を出力する周期は、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０が位置姿勢を推定する周期よりも短い。位置姿勢出力部１５０は、カルマンフィルタ処理により演算した位置姿勢（位置姿勢伝播値）を出力する伝播器１５０ａと、出力した位置姿勢（位置姿勢伝播値）及び、高精度位置姿勢推定部１３０または小型軽量位置姿勢推定部１４０が推定した位置に基づいて次回の推定時における位置姿勢を推定する推定器１５０ｂを含む。
【００４７】
次に、図５を用いて、本実施の形態に係る位置姿勢推定装置１２０の動作について説明する。
【００４８】
ここでは、具体例として、上述したように、移動体３０は、機体本体にＧＰＳ受信機１３１と高精度加速度センサ１３２と高精度ジャイロセンサ１３３と高精度位置姿勢推定演算器１３５とを搭載する。また移動体３０は、アンテナ先端（もしくは背面側）にＧＰＳ受信機１４１、小型軽量加速度センサ１４２と小型軽量ジャイロセンサ１４３とジャイロセンサ１４４と小型軽量位置姿勢推定演算器１４５とを搭載する。なおアンテナの指向は、空間内で自由に駆動可能である。位置姿勢推定装置１２０は、アンテナの姿勢角情報を数ｋＨｚオーダーの高サンプリング周波数で推定し生成する。ただし、当該移動体３０はＧＰＳ受信機１３１と高精度加速度センサ１３２と高精度ジャイロセンサ１３３とを、例えば、移動体３０の重心付近に搭載する。また、ＧＰＳ受信機１４１、小型軽量加速度センサ１４２と小型軽量ジャイロセンサ１４３とジャイロセンサ１４４と小型軽量位置姿勢推定演算器１４５とをアンテナ背面部分へ搭載した指向アンテナは、例えば、当該移動体３０の機体の先端に配置されている。
【００４９】
図５に示すように、まず、高精度位置姿勢推定部１３０において、ＧＰＳ受信機１３１、高精度加速度センサ１３２、高精度ジャイロセンサ１３３が検出を行い（Ｓ１０１）、高精度位置姿勢推定演算器１３５（伝播器１３５ａ）が、位置姿勢伝播処理を実行し（Ｓ１０２）、高精度位置姿勢推定演算器１３５（推定器１３５ｂ）が、位置姿勢推定処理（カルマンフィルタ処理）を実行する（Ｓ１０３）。
【００５０】
高精度位置姿勢推定演算器１３５が使用するカルマンフィルタ処理では、例えば、次の式（１）（２）で表されるベクトル０ｚｎを観測量、ベクトル０ｘｎを状態量とする。
０ｚｎ＝（Δ０ｐ  Δ０ｐ’  Δ０θ）・・・（１）
０ｘｎ＝（Δ０ｐ  Δ０ｐ’  Δ０θ  Δ０ａｂ  Δ０ωｂ）・・・（２）
ここで、Δ０ｐは、図２の慣性座標系で記述した移動体３０の現在位置ベクトルｐ＝（ｘ  ｙ  ｚ）の誤差ベクトルである。また、Δ０ｐ’は、図２の慣性座標系で記述した移動体３０の速度ベクトルｐ’＝（ｘ’  ｙ’  ｚ’）の誤差ベクトルである。また、Δ０θは、図２の慣性座標系で記述した移動体３０の姿勢ベクトルθ＝（φ  ρ  Ψ）の誤差ベクトルである。また、Δ０ａｂは、図２の慣性座標系で記述した移動体３０の高精度加速度センサ１３２のバイアスノイズを示すノイズベクトルａｂ＝（ａｂｘ  ａｂｙ  ａｂｚ）に対する誤差ベクトルである。また、Δ０ωｂは、図２の慣性座標系で記述した移動体３０の高精度ジャイロセンサ１３３のバイアスノイズを示すノイズベクトルωｂ＝（ωｂｘ  ωｂｙ  ωｂｚ）に対する誤差ベクトルである。
なお、誤差ベクトルとは、加速度センサ、ジャイロセンサで検出された値から算出された値の、基準となる値（それぞれ状態量の絶対的な真値）に対する誤差を表したものである。現在位置ベクトルｐは、ＧＰＳ受信機１３１で直接検出する値を検出値とし、前回の検出時において予測されているカルマンフィルタ処理後の位置を基準値として、検出値と基準値の差を誤差ベクトルとすることができる。速度ベクトルｐ’は高精度加速度センサ１３２の検出値を積分した値を検出値とする。前回の検出時において予測されているカルマンフィルタ処理後の速度及びＧＰＳ受信機１３１の検出値から計算した速度から基準値を算出する。検出値と基準値の差を誤差ベクトルとすることができる。姿勢ベクトルθは、高精度ジャイロセンサ１３３の検出値を積分した値を検出値とする。前回の検出時において予測されているカルマンフィルタ処理後の姿勢及びＧＰＳ受信機１３１の検出値をもとに移動体（航空機）の位置ダイナミクスから導出した姿勢から基準値を算出する。検出値と基準値の差を誤差ベクトルとすることができる。なお、移動体（航空機）にコンパスを備え、コンパスにより検出される方位角を基準とすることもできる。また重力方向に対する機体の傾きを測定する測定器を備え、その傾きの測定値を基準とすることもできる。
【００５１】
また、高精度位置姿勢推定演算器１３５が使用するカルマンフィルタ処理で使用される時刻ｔｎ＋１（次回の検出時）における状態量０ｘｎ＋１及び時刻ｔｎ（今回の検出時）における観測量０ｚｎの線形確率システム（状態空間モデル）は、例えば、次の式（３）（４）のように表される。
０ｘｎ＋１＝０Φｎ０ｘｎ+０Ｇｎ０ｗｎ+０Ｄｎ０ｕｎ・・・（３）
０ｚｎ＝０Ｈｎ０ｘｎ+０ｖｎ・・・（４）
ここで、０ｘｎは時刻ｔｎ（今回の検出時）における状態量、０Φｎは時刻ｔｎにおける状態遷移行列、０Ｇｎは時刻ｔｎにおける駆動ノイズ行列、０ｗｎは時刻ｔｎにおけるシステムノイズ、０Ｄｎは時刻ｔｎにおける駆動入力行列、０ｕｎは時刻ｔｎにおけるシステム入力ベクトル、０Ｈｎは時刻ｔｎにおける観測行列、０ｖｎは時刻ｔｎにおける観測ノイズである。
【００５２】
システム入力ベクトル０ｕｎは、次回の検出時（時刻ｔｎ＋１）までに機体（移動体）の状態量に加えられる状態量の変化分であり、例えば、高精度加速度センサ１３２が検出した加速度と、高精度ジャイロセンサ１３３が検出した角速度とにより構成される。
【００５３】
駆動入力行列０Ｄｎは、システム入力ベクトル０ｕｎによって次回の検出時（時刻ｔｎ＋１）の状態量０ｘｎ＋１に与えられる変化を予測するための行列である。
【００５４】
観測行列０Ｈｎは、観測ノイズ０ｖｎが０ベクトルの場合の、時刻ｔｎにおける状態量０ｘｎと観測量０ｚｎとの関係を示す行列である。
【００５５】
状態遷移行列０Φｎは、時刻ｔｎにおける状態量０ｘｎからシステムノイズ０ｗｎ及びシステム入力ベクトル０ｕｎが０ベクトルの場合の、時刻ｔｎ＋１における状態量０ｘｎ＋１への遷移関係を示す行列である。
【００５６】
システムノイズ０ｗｎと観測ノイズ０ｖｎは、互いに独立な白色ノイズであると仮定してよい。
【００５７】
カルマンフィルタ処理には、カルマンフィルタの数式で用いられる行列を今回の観測に基づいて更新し、更新された行列により今回の状態量を推定するカルマンフィルタ更新処理と、今回のカルマンフィルタ処理後の推定値から次回の状態量を予測するカルマンフィルタ予測処理が含まれる。
まずカルマンフィルタ更新処理で用いられる式について説明する。
【００５８】
推定器１３５ｂにおけるカルマンフィルタ更新処理は次の式（５）、式（６）、式（７）を用いて実施できる。
０Ｋｎ＝０Ｐｎ−０Ｈｎ（０ＨｎＴ０Ｐｎ−０Ｈｎ+０Ｒｎ）−１・・・（５）
０θｎ＋＝０θｎ−＋０Ｋｎ（０ｚｎ−０ＨｎＴ０θｎ−）・・・（６）
０Ｐｎ＋＝０Ｐｎ−−０Ｋｎ０ＨｎＴ０Ｐｎ−・・・（７）
ここで、０Ｐｎは、時刻ｔｎにおける誤差共分散行列、０Ｒｎは、観測ノイズ０ｖｎの共分散行列である。０Ｒｎはカルマンフィルタ設計者がＧＰＳのノイズ特性に応じ設定する設計パラメータである。
また上付きの「Ｔ」は転置行列、上付きの「−１」は逆行列を示す。
【００５９】
次にカルマンフィルタ予測処理で用いられる式について説明する。
時刻ｔｎ+1におけるシステム状態量の予測値０ｘｎ＋１−と誤差共分散行列の予測値０Ｐｎ＋１−は、次の式（８）及び式（９）のように表される。
０ｘｎ+1−＝０Φｎ０ｘｎ＋＋０Ｄｎ０ｕｎ・・・（８）
０Ｐｎ+1−＝０Φｎ０Ｐｎ＋０ΦｎＴ＋０Ｇｎ０Ｑｎ＋０ＧｎＴ・・・（９）
ただし、下付きの「ｎ」は、時刻ｔｎにおけるベクトル及び行列、下付きの「ｎ＋１」は、時刻ｔｎ＋１におけるベクトル及び行列、上付きの「＋」はカルマンフィルタ処理後の推定値、上付きの「−」はカルマンフィルタ処理前の予測値、上付きの「Ｔ」は転置行列を示す。また０Ｑｎは、式（１）のシステムノイズ０ｗｎの共分散行列を示す。ただし０Ｑｎはカルマンフィルタ設計者が飛翔体ダイナミクス特性に応じ設定する設計パラメータとしてもよい。
【００６０】
上記の式を用いる高精度位置姿勢推定演算器１３５の動作を説明する。
【００６１】
ステップＳ１０１において、ＧＰＳ受信機１３１は、時刻ｔｎにおいて、位置を検出する。高精度加速度センサ１３２は、加速度を検出する。高精度ジャイロセンサ１３３は、角速度を検出する。
【００６２】
次にＳ１０２において、位置姿勢伝播処理を行う。まず現在位置ベクトルｐについて、伝播器１３５ａは、前回の検出時において予測されているカルマンフィルタ処理後の位置を基準値とする。伝播器１３５ａは、ＧＰＳ受信機１３１が検出した位置の検出値と基準値との差をΔ０ｐとする。速度ベクトルｐ’については、伝播器１３５ａは、高精度加速度センサ１３２が検出した加速度を積分して速度の検出値を求める。伝播器１３５ａは、時刻ｔｎにおいてＧＰＳ受信機１３１が検出した位置から算出した速度及び前回の検出時において予測されているカルマンフィルタ処理後の速度から速度の基準値を計算する。伝播器１３５ａは、速度の検出値と基準値との差をΔ０ｐとする。姿勢ベクトルθについては、伝播器１３５ａは、高精度ジャイロセンサ１３３が検出した角速度を積分して姿勢の検出値を求める。伝播器１３５ａは、時刻ｔｎにおいてＧＰＳ受信機１３１が測定した位置から移動体（航空機）の位置ダイナミクスを用いて姿勢及び前回の検出時において予測されているカルマンフィルタ処理後の姿勢から姿勢の基準値を計算する。伝播器１３５ａは、姿勢の検出値と基準値との差をΔ０θとする。このようにして、ＧＰＳ受信機１３１、高精度加速度センサ１３２、高精度ジャイロセンサ１３３が検出した位置、加速度、角速度から、時刻ｔｎにおける観測量０ｚｎが得られる。
【００６３】
前回の検出時の動作において式（９）からカルマンフィルタ処理前の共分散行列の予測行列０Ｐｎ−が得られている。０Ｒｎはカルマンフィルタ設計者がＧＰＳのノイズ特性に応じ設定する設計パラメータである。式（５）により、カルマンゲイン０Ｋｎが得られる。
【００６４】
また、伝播器１３５ａは、前回の検出時の動作において得られているシステム状態量の予測量０ｘｎ−と、Ｓ１０１において観測された観測量０ｚｎに基づいて、式（６）により時刻ｔｎにおけるカルマンフィルタ処理後のシステム状態量０ｘｎ＋を算出する。
【００６５】
また伝播器１３５ａは、前回の検出時の動作において得られている共分散行列０Ｐｎの予測行列０Ｐｎ−と、カルマンゲイン０Ｋｎと、観測行列０Ｈｎに基づいて、式（７）により時刻ｔｎにおけるカルマンフィルタ処理後の共分散行列０Ｐｎ＋も算出する。
【００６６】
このようにしてＳ１０２において、高精度位置姿勢推定演算器１３５の伝播器１３５ａは、システム状態量と誤差共分散行列を算出する。高精度位置姿勢推定演算器１３５は、算出した位置姿勢を出力する。
【００６７】
次に、Ｓ１０３において高精度位置姿勢推定演算器１３５の推定器１３５ｂは、例えば、高精度加速度センサ１３２が検出した加速度と、高精度ジャイロセンサ１３３が検出した角速度とからシステム入力ベクトル０ｕｎを算出し、Ｓ１０２において算出されたシステム状態量と誤差共分散行列に基づいて、式（８）、式（９）によって時刻ｔｎ＋１のシステム状態量の予測量０ｘｎ＋１−と共分散行列の予測値０Ｐｎ＋１−を算出する。
また推定器１３５ｂは、時刻ｔｎ＋１で検出又は算出されるΔ０ｐ、Δ０ｐ’、Δ０θとの差を算出するための位置、速度、姿勢の予測量を算出する。
【００６８】
一方、小型軽量位置姿勢推定部１４０において、図５に示すようにＧＰＳ受信機１４１、小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４が検出を行い（Ｓ１１１）、小型軽量位置姿勢推定演算器１４５（伝播器１４５ａ）が、位置姿勢伝播処理を実行し（Ｓ１１２）、小型軽量位置姿勢推定演算器１４５（推定器１４５ｂ）が、位置姿勢推定処理（カルマンフィルタ処理）を実行する（Ｓ１１３）。
【００６９】
小型軽量位置姿勢推定演算器１４５が実行するカルマンフィルタ処理で用いられる数式について説明する。小型軽量位置姿勢推定演算器１４５が実行するカルマンフィルタ処理で用いられる数式は、基本的に高精度位置姿勢推定演算器１３５が実行するカルマンフィルタ処理で用いられる数式と同様になる。
【００７０】
小型軽量位置姿勢推定演算器１４５が使用するカルマンフィルタ処理では、例えば、次の式（１０）（１１）で表されるベクトル１ｚｋを観測量、ベクトル１ｘｋを状態量とする。
１ｚｋ＝（Δ１ｐ  Δ１ｐ’  Δ１θ）・・・（１０）
１ｘｋ＝（Δ１ｐ  Δ１ｐ’  Δ１θ  Δ１ａｂ  Δ１ωｂ）・・・（１１）
ここで、Δ１ｐは、小型軽量位置姿勢推定部１４０における図２の慣性座標系で記述した移動体３０（アンテナ）の現在位置ベクトルｐ＝（ｘ  ｙ  ｚ）の誤差ベクトルである。また、Δ１ｐ’は、小型軽量位置姿勢推定部１４０における図２の慣性座標系で記述した移動体３０（アンテナ）の速度ベクトルｐ’＝（ｘ’  ｙ’  ｚ’）の誤差ベクトルである。また、Δ１θは、小型軽量位置姿勢推定部１４０における図２の慣性座標系で記述した移動体３０（アンテナ）の姿勢ベクトルθ＝（φ  ρ  Ψ）の誤差ベクトルである。また、Δ１ａｂは、小型軽量位置姿勢推定部１４０における図２の慣性座標系で記述した移動体３０（アンテナ）の小型軽量加速度センサ１４２のバイアスノイズを示すノイズベクトルａｂ＝（ａｂｘ  ａｂｙ  ａｂｚ）に対する誤差ベクトル、Δ１ωｂは、小型軽量位置姿勢推定部１４０における図２の慣性座標系で記述した移動体３０（アンテナ）の小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４の検出信号を補間により組み合わせた検出信号のバイアスノイズを示すノイズベクトルωｂ＝（ωｂｘ  ωｂｙ  ωｂｚ）に対する誤差ベクトルである。
なお、現在位置ベクトルｐは、ＧＰＳ受信機１３１で直接検出する値を検出値とし、前回の検出時において予測されているカルマンフィルタ処理後の位置を基準として、検出値と基準値の差を誤差ベクトルとすることができる。速度ベクトルｐ’は高精度加速度センサ１３２の検出値を積分した値を検出値とし、前回の検出時において予測されているカルマンフィルタ処理後の速度及びＧＰＳ受信機１３１の検出値から計算した速度から基準値を算出して、検出値と基準値の差を誤差ベクトルとすることができる。姿勢ベクトルθは、高精度ジャイロセンサ１３３の検出値を積分した値を検出値とし、前回の検出時において予測されているカルマンフィルタ処理後の姿勢及びＧＰＳ受信機１３１の検出値をもとに移動体（航空機）の位置ダイナミクスから導出した姿勢から基準値を算出して、検出値と基準値の差を誤差ベクトルとすることができる。
【００７１】
ＭＥＭＳなどを利用した小型軽量ジャイロセンサ１４３では周波数帯域が比較的広い反面ＤＣ成分に対するドリフトノイズは大きいため、検出性能を確保するにはこのドリフトノイズを何らかの方法で補償する必要がある。ここでは小型軽量ジャイロセンサ１４３に対し、周波数帯域は狭いもののドリフトノイズが比較的小さい機械式などのジャイロセンサ１４４を組み合わせそれぞれの検出信号を補間により組み合わせることで、周波数帯域と検出精度の両立を図る。具体的には、小型軽量ジャイロセンサ検出信号１７にハイパスフィルタを通過させ、ジャイロセンサ検出信号１８にローパスフィルタを通過させた後、これらの信号が組み合わされる。
【００７２】
また、小型軽量位置姿勢推定演算器１４５が使用するカルマンフィルタ処理で使用される時刻ｔｋ＋１（次回の検出時）における状態量１ｘｋ＋１及び時刻ｔｋ（今回の検出時）における観測量１ｚｋの線形確率システム（状態空間モデル）は、例えば、次の式（１２）（１３）のように表される。
１ｘｋ+1＝１Φｋ１ｘｋ+１Ｇｋ１ｗｋ+１Ｄｋ１ｕｋ・・・（１２）
１ｚｋ＝１Ｈｋ１ｘｋ+１ｖｋ・・・（１３）
ここで、１ｘｋは時刻ｔｋ（今回の検出時）における状態量、１Φｋは時刻ｔｋにおける状態遷移行列、１Ｇｋは時刻ｔｋにおける駆動ノイズ行列、１ｗｋは時刻ｔｋにおけるシステムノイズ、１Ｄｋは時刻ｔｋにおける駆動入力行列、１ｕｋは時刻ｔｋにおけるシステム入力ベクトル、１Ｈｋは時刻ｔｋにおける観測行列、１ｖｋは時刻ｔｋにおける観測ノイズである。
また小型軽量位置姿勢推定演算器１４５の推定器１４５ｂにおけるカルマンフィルタ更新処理は次の式（１４）、式（１５）、式（１６）を用いて実施できる。
１Ｋｋ＝１Ｐｋ−１Ｈｋ（１ＨｋＴ１Ｐｋ−１Ｈｋ+１Ｒｋ）−１・・・（１４）
１θｋ＋＝１θｋ−＋１Ｋｋ（１ｚｋ−１ＨｋＴ１θｋ−）・・・（１５）
１Ｐｋ＋＝１Ｐｋ−−１Ｋｋ１ＨｋＴ１Ｐｋ−・・・（１６）
ここで、１Ｐｋは、時刻ｔｋにおける誤差共分散行列、１Ｒｋは、観測ノイズ１ｖｋの共分散行列である。１Ｒｋはカルマンフィルタ設計者がＧＰＳのノイズ特性に応じ設定する設計パラメータである。
また上付きの「Ｔ」は転置行列、上付きの「−１」は逆行列を示す。
また小型軽量位置姿勢推定演算器１４５の伝播器１４５ａにおけるカルマンフィルタ予測処理において、時刻ｔｋ＋１のシステム状態量の予測値１ｘｋ＋１−と誤差共分散行列の予測値１Ｐｋ＋１−は、次の式（１７）及び式（１８）のように表される。
１ｘｋ+1−＝１Φｋ１ｘｋ＋＋１Ｄｋ１ｕｋ・・・（１７）
１Ｐｋ+1−＝１Φｋ１Ｐｋ＋１ΦｋＴ＋１Ｇｋ１Ｑｋ＋１ＧｋＴ・・・（１８）
ただし、下付きの「ｋ」は、時刻ｔｋにおけるベクトル及び行列、下付きの「ｋ＋１」は、時刻ｔｋ＋１におけるベクトル及び行列、上付きの「＋」はカルマンフィルタ処理後の推定値、上付きの「−」はカルマンフィルタ処理前の予測値、上付きの「Ｔ」は転置行列を示す。また１Ｑｋは、式（１）のシステムノイズ１ｗｋの共分散行列を示す。ただし１Ｑｋはカルマンフィルタ設計者が飛翔体ダイナミクス特性に応じ設定する設計パラメータとしてもよい。
上記の式を用いる小型軽量位置姿勢推定演算器１４５の動作を説明する。小型軽量位置姿勢推定部１４０は、観測及びカルマンフィルタ処理の周期が高精度位置姿勢推定部１３０における、測定及びカルマンフィルタ処理より短く、例えば４００Ｈｚのサンプリング周波数で測定及びカルマンフィルタ処理を実行する。
【００７３】
ステップＳ１１１において、ＧＰＳ受信機１４１は、時刻ｔｋにおいて、位置を検出する。小型軽量加速度センサ１４２は、加速度を検出する。また小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４は、それぞれ角速度を検出する。
【００７４】
次にＳ１１２において、位置姿勢伝播処理を行う。まず現在位置ベクトルｐについて、伝播器１４５ａは、前回の検出時において予測されているカルマンフィルタ処理後の位置を基準値とする。伝播器１４５ａは、位置の検出値と基準値との差をΔ０ｐとする。速度ベクトルｐ’については、伝播器１４５ａは、小型軽量加速度センサ１４２が検出した加速度を積分して速度の検出値を求める。伝播器１４５ａは、時刻ｔｋにおいてＧＰＳ受信機１４１が検出した位置から算出した速度及び前回の検出時において予測されているカルマンフィルタ処理後の速度から速度の基準値を計算する。伝播器１４５ａは、速度の検出値と基準値との差をΔ０ｐとする。姿勢ベクトルθについては、伝播器１４５ａは、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４の検出した角速度を補間により組み合わせた角速度を積分して姿勢の検出値を求める。伝播器１３５ａは、時刻ｔｋにおいてＧＰＳ受信機１３１が測定した位置から移動体（航空機）の位置ダイナミクスを用いて姿勢及び前回の検出時において予測されているカルマンフィルタ処理後の姿勢から姿勢の基準値を計算する。伝播器１４５ａは、姿勢の検出値と基準値との差をΔ０θとする。このようにして、ＧＰＳ受信機１４１、小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４が検出した位置、加速度、角速度から時刻ｔｋにおける観測量０ｚｋが得られる。前回の検出時の動作において式（１８）から時刻ｔｋのカルマンフィルタ処理前の共分散行列の予測行列１Ｐｋ−が得られている。１Ｒｋはカルマンフィルタ設計者がＧＰＳのノイズ特性に応じ設定する設計パラメータである。式（１４）により、時刻ｔｋにおけるカルマンゲイン１Ｋｋが得られる。
【００７５】
また、伝播器１４５ａは、前回の検出時の動作において得られているシステム状態量の予測値１ｘｋ−と、Ｓ１１１において観測された観測量０ｚｋに基づいて、式（１５）により時刻ｔｋにおけるカルマンフィルタ処理後のシステム状態量１ｘｋ＋を算出する。
【００７６】
また伝播器１４５ａは、前回の検出時の動作において得られている共分散行列の予測行列１Ｐｋ−と、カルマンゲイン１Ｋｋと、観測行列１Ｈｋに基づいて、式（１６）により時刻ｔｋにおけるカルマンフィルタ処理後の共分散行列１Ｐｋ＋を算出する。
【００７７】
このようにしてＳ１１２において、小型軽量位置姿勢推定演算器１４５の伝播器１３５ａは、システム状態量と誤差共分散行列を算出する。小型軽量位置姿勢推定演算器１４５は、算出した位置姿勢を出力する。
【００７８】
次に、Ｓ１１３において小型軽量位置姿勢推定演算器１４５の推定器１４５ｂは、Ｓ１１２において算出されたシステム状態量と誤差共分散行列に基づいて、式（１７）、式（１８）によって時刻ｔｋ＋１のシステム状態量の予測値１ｘｋ＋１−と共分散行列の予測値１Ｐｋ＋１−を算出する。また推定器１３５ｂは、時刻ｔｋ＋１において検出又は算出されるΔ１ｐ、Δ１ｐ’、Δ１θとの差を算出するための位置、速度、姿勢の予測値を算出する。
【００７９】
このようにして小型軽量位置姿勢推定部１４０は、アンテナに搭載するＧＰＳ受信機１４１が検出した絶対位置と、小型軽量加速度センサ１４２が検出した加速度と、ＭＥＭＳなどにより構成した小型軽量ジャイロセンサ１４３が検出した角速度と、この小型軽量ジャイロセンサ１４３の検出性能制約を補間する機械式などのジャイロセンサ１４４が検出した角速度とを用いて、カルマンフィルタ処理を実施する。小型軽量位置姿勢推定部１４０は、カルマンフィルタ処理によって、例えば４００Ｈｚのサンプリング周波数で、算出された位置姿勢を含む小型軽量位置姿勢推定演算器生成信号１９を出力する。
【００８０】
次に、位置姿勢出力部１５０（伝播器１５０ａ）が位置姿勢伝播処理を実行し（Ｓ１２１）、位置姿勢出力部１５０（推定器１５０ｂ）が位置姿勢推定処理（仮想的なカルマンフィルタ処理）を実行する（Ｓ１２２）。
【００８１】
位置姿勢出力部１５０は、例えば５０〜２００Ｈｚの比較的低サンプリング周波数１／ΔｔｎＨｚで発生する高精度位置姿勢伝播値を含む移動体高精度位置姿勢推定信号１４と、例えば４００Ｈｚのサンプリング周波数１／ΔｔｋＨｚで発生する位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号１９を用いて、サンプリング周波数１／ΔｔｍＨｚ（ただしΔｔｍ≦Δｔｋ）で発生する位置姿勢補間推定値２０を生成する。
【００８２】
位置姿勢出力部１５０の伝播器１５０ａは、位置姿勢出力部１５０におけるシステム状態量と誤差共分散行列をそれぞれ、ｆθｍ、ｆＰｍと設定する。
【００８３】
位置姿勢出力部１５０が使用するカルマンフィルタ処理では、例えば、次の式（１９）（２０）で表されるベクトルｆｚｍを観測量、ベクトルｆθｍを状態量とする。
ｆｚｍ＝（Δｆｐ  Δｆθ）・・・（１９）
ｆθｍ＝（Δｆｐ  Δｆθ）・・・（２０）
ここで、Δｆｐは、現在位置ベクトルｐ＝（ｘ  ｙ  ｚ）の誤差ベクトル、Δｆθは、姿勢ベクトルθ＝（φ  ρ  Ψ）の誤差ベクトルである。
なお、現在位置ベクトルｐ及び姿勢ベクトルθは、前回の検出時（時刻ｔｍ-1）において予測されているカルマンフィルタ処理後の位置を基準とする。高精度位置姿勢推定部１３０又は小型軽量位置姿勢推定部１４０のうち最後に出力された値を検出値とする。検出値と基準値の差を誤差ベクトルとすることができる。また高精度位置姿勢推定部１３０が出力したタイミングでは高精度位置姿勢推定部１３０の出力した値を検出値とする。小型軽量位置姿勢推定部１４０が出力したタイミングでは小型軽量位置姿勢推定部１４０の出力した値を検出値とする。高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０のいずれの出力と異なるタイミングでは、例えば、高精度位置姿勢推定部１３０又は小型軽量位置姿勢推定部１４０のうち最後に出力された値を検出値とする。前回の検出時（時刻ｔｍ-1）において予測されているカルマンフィルタ処理後の位置を基準とする。検出値と基準値の差を誤差ベクトルとすることができる。
【００８４】
位置姿勢出力部１５０が使用するカルマンフィルタ処理で使用される線形確率システム（状態空間モデル）は、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０と同様に、例えば、次の式（２１）（２２）のように表される。
なお、状態量ｆθｍ＋１は次回の検出時である時刻ｔｍ＋１における状態量、観測量ｆｚｍ今回の検出時である時刻ｔｍにおける観測量である。
ｆθｍ＋１＝ｆΦｍｆθｍ＋ｆＧｍｆｗｍ＋ｆＤｍｆｕｍ・・・（２１）
ｆｚｍ＝ｆＨｍｆθｍ+ｆｖｍ・・・（２２）
ここで、ｆθｍは時刻ｔｍにおける状態量、ｆΦｍは時刻ｔｍにおける状態遷移行列、ｆＧｍは時刻ｔｍにおける駆動ノイズ行列、ｆｗｍは時刻ｔｍにおけるシステムノイズ、ｆＤｍは時刻ｔｍにおける駆動入力行列、ｆｕｍは時刻ｔｍにおけるシステム入力ベクトル、ｆＨｍは時刻ｔｍにおける観測行列、ｆｖｍは時刻ｔｍにおける観測ノイズである。
【００８５】
位置姿勢出力部１５０は、システム入力ベクトルｆｕｍを生成してカルマンフィルタ処理に用いる。例えば、位置姿勢出力部１５０は、サンプリング周波数１／ΔｔｍＨｚで、姿勢の補間推定値を生成してカルマンフィルタ処理に用いる。
【００８６】
例えば、位置姿勢出力部１５０は、サンプリング時間間隔Δｔｍの間、ｆｕｍを、一定値の角速度ベクトルｆωｋ＝［ωｘｋ  ωｙｋ  ωｚｋ］Ｔとして生成してもよい。例えば、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０は、位置姿勢とともに角速度ベクトル０ωｎ及び1ωｋを出力するものとしてもよい。また一定値の角速度ベクトルｆωｋは、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０が、時刻ｔｍの前、最後に出力した角速度ベクトルをｆωｋとしてもよい。
【００８７】
推定器１５０ｂにおけるカルマンフィルタ更新処理は高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０と同様に、次の式（２３）、式（２４）、式（２５）を用いて実施できる。
ｆＫｍ＝ｆＰｍ−ｆＨｍ（ｆＨｍＴｆＰｍ−ｆＨｍ+ｆＲｍ）−１・・・（２３）
ｆθｍ＋＝ｆθｍ−＋ｆＫｍ（ｆｚｍ−ｆＨｍＴｆθｍ−）・・・（２４）
ｆＰｍ＋＝ｆＰｍ−−ｆＫｍｆＨｍＴｆＰｍ−・・・（２５）
ここで、ｆＰｍは、今回の観測時刻ｔｍにおける誤差共分散行列、ｆＲｍは、観測ノイズｆｖｍの共分散行列である。ｆＲｍはカルマンフィルタ設計者がＧＰＳのノイズ特性に応じ設定する設計パラメータである。
【００８８】
また次回の観測時刻ｔｍ＋１におけるシステム状態量の予測値ｆｘｍ＋１−と誤差共分散行列の予測値ｆＰｍ＋１−は、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０と同様に、次の式（２６）及び式（２７）のように表される。
ｆθｍ+1−＝ｆΦｍｆθｍ＋＋ｆＤｍｆｕｍ・・・（２６）
ｆＰｍ+1−＝ｆΦｍｆＰｍ＋ｆΦｍＴ＋ｆＧｍｆＱｍ＋ｆＧｍＴ・・・（２７）
ここで、ｆＱｍは、式（１９）のシステムノイズｆｗｍの共分散行列を示す。
【００８９】
上記の式を用いる位置姿勢出力部１５０の動作を説明する。まずＳ１２１の処理を説明する。前回の観測時刻ｔｍ−１における動作において、式（２７）から今回の観測時系列「ｍ」のカルマンフィルタ処理前の共分散行列の予測行列ｆＰｍ−が得られている。また式（２３）により、時刻ｔｍにおける観測時系列「ｍ」のカルマンゲインｆＫｍが得られている。
【００９０】
伝播器１５０ａは、システム状態量の予測量ｆθｍ−と、時刻ｔｍにおける観測量ｆｚｍに基づいて、式（２４）により時刻ｔｍにおけるカルマンフィルタ処理後のシステム状態量ｆθｍ＋を算出する。ここで時刻ｔｍにおける観測量ｆｚｍ＝（Δｆｐ  Δｆθ）は、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０から最後に取得した位置姿勢を用いて算出したものとしてもよい。すなわち、高精度位置姿勢推定部１３０が出力したタイミングでは高精度位置姿勢推定部１３０の出力した現在位置ベクトルｐ及び姿勢ベクトルθの値を検出値として用い、小型軽量位置姿勢推定部１４０が出力したタイミングでは小型軽量位置姿勢推定部１４０の出力した現在位置ベクトルｐ及び姿勢ベクトルθの値を検出値として用い、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０のいずれの出力と異なるタイミングでは、例えば、高精度位置姿勢推定部１３０又は小型軽量位置姿勢推定部１４０のうち最後に出力された現在位置ベクトルｐ及び姿勢ベクトルθの値を検出値として用いて観測量ｆｚｍを算出してもよい。
【００９１】
また伝播器１５０ａは、共分散行列の予測行列ｆＰｍ−と、カルマンゲインｆＫｍと、観測行列ｆＨｍに基づいて、式（２５）により時刻ｔｍにおけるカルマンフィルタ処理後の共分散行列ｆＰｍ＋も算出する。
【００９２】
このようにしてＳ１２１において、伝播器１５０ａは、システム状態量と誤差共分散行列を算出する。位置姿勢出力部１５０は、算出した状態量を出力する。
【００９３】
次に、まずＳ１２２の処理を説明する。位置姿勢出力部１５０の推定器１５０ｂは、システム入力ベクトルｆｕｍを生成する。推定器１５０ｂは、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０にて最後に検出した角速度ベクトル０ωｎ又は１ωｋをシステム入力ベクトルｆｕｍとしてもよい。推定器１５０ｂは、生成されたシステム入力ベクトルｆｕｍと、Ｓ１２１において算出されたシステム状態量ｆθｍ＋と、誤差共分散行列ｆＰｍ＋に基づいて、式（２６）、式（２７）によって次回の観測時系列「ｍ＋１」のシステム状態量の予測値ｆθｍ＋１−（位置姿勢補間推定値２０）と共分散行列の予測値ｆＰｍ＋１−を算出する。
また推定器１５０ｂは、次回の観測時系列「ｍ＋１」で算出されるΔｆｐ及びΔ０θの検出値との差を求めるためのカルマンフィルタ処理後の位置及び姿勢の予測値を算出する。
【００９４】
なお、推定器１５０ｂは、高精度位置姿勢推定部１３０及び小型軽量位置姿勢推定部１４０にて最後に検出した角速度ベクトル０ωｎ又は１ωｋをシステム入力ベクトルｆｕｍとしてもよいと説明したが、これに限られない。推定器１５０ｂは、システムへの入力ベクトルｆｕｍを、次の式（２８）で表す値としてもよい。
0006471749.tif 000002
【００９５】
ここで、時刻ｔｋは、小型軽量位置姿勢推定部１４０にて最後に角速度ベクトルを検出した時刻、時刻ｔｋ−１は、時刻ｔｋより前に小型軽量位置姿勢推定部１４０から角速度ベクトルを取得した時刻、ｆωｋ、ｆωｋ-1は、時刻ｔｋ、ｔｋ-1（＜ｔｍ＋１）において小型軽量位置姿勢推定部１４０から取得した角速度ベクトルである。
【００９６】
このように、本実施の形態によれば、位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号１９を、高精度位置姿勢伝播値を含む移動体高精度位置姿勢推定信号１４に基づき補間処理を実施することとなる。このため、位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号１９を単独利用する補間処理と比較して更なる精度向上が可能となる。
【００９７】
なお、ここで示した位置姿勢出力部１５０での補間処理は、高精度位置姿勢伝播値と位置姿勢伝播値とを、小型軽量位置姿勢推定部１４０で適用するジャイロセンサのノイズモデルに基づき適切に組み合わせることで実現した技術であり、関連する技術を単純に組み合わせたものではない。
【００９８】
本実施の形態では、移動体に搭載する駆動機器の姿勢情報を生成する位置姿勢推定装置において、移動体に搭載した複数のセンサの検出信号を組み合わせることで高精度位置姿勢推定部が位置姿勢を推定する。また、駆動機器に搭載した複数のセンサの検出信号を組み合わせることで小型軽量位置姿勢推定部が位置姿勢を推定する。さらに、高精度位置姿勢推定部と小型軽量位置姿勢推定部が推定した信号を組み合わせることで位置姿勢補間推定値を生成することとした。
特に、本実施の形態では、移動体本体に搭載した高精度位置姿勢推定部に対し、小型軽量加速度センサと小型軽量ジャイロセンサとから構成し駆動機器に装着した小型軽量位置姿勢推定部を組み合わせる。また、高精度位置姿勢推定部が生成する位置姿勢伝播値と小型軽量位置姿勢推定部が生成する位置姿勢伝播値とを利用することで、小型軽量位置姿勢推定部が出力した位置姿勢伝播値を高サンプリングで補間可能な位置姿勢推定補間値を生成する。
【００９９】
その結果、本実施の形態では、小型軽量位置姿勢推定部が生成する位置姿勢伝播値の検出間隔が十分短く無く精度的にも十分高く無い場合でも、小型軽量位置姿勢推定部が位置姿勢伝搬値と同時に生成する姿勢角速度情報を組み合わせ用いる。これによって、離散的に生成する位置姿勢伝播値の変動を補間可能となり、移動体に搭載する搭載駆動機器のリアルタイム高精度位置姿勢推定が達成できる。またさらに、小型軽量位置姿勢推定部が生成する位置姿勢伝播値を時系列データとして保存しそれらを複数組み合わせることで、リアルタイム性は犠牲にしながらも位置姿勢推定精度の向上が達成できる。
（実施の形態２）
以下、図面を参照して実施の形態２について説明する。本実施の形態では、実施の形態１に対し、小型軽量位置姿勢推定部が、高精度位置姿勢推定部の出力信号を用いて位置姿勢を推定する。ここでは、主に実施の形態１と相違する点について説明する。
【０１００】
図６は、本実施の形態に係る位置姿勢推定装置１２０の機能ブロック図を示している。図６に示すように、本実施の形態では、小型軽量位置姿勢推定部１４０は、ＧＰＳ受信機１４１を備えていない。すなわち、小型軽量位置姿勢推定部１４０は、小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４、小型軽量位置姿勢推定演算器１４５を備えている。
【０１０１】
小型軽量位置姿勢推定演算器１４５は、小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４の検出、及び高精度位置姿勢推定部１３０の推定に基づいて、アンテナの位置姿勢を推定する。すなわち、小型軽量位置姿勢推定演算器１４５は、小型軽量加速度センサ検出信号１６と小型軽量ジャイロセンサ検出信号１７とジャイロセンサ検出信号１８と移動体高精度位置姿勢推定信号１４とに基づき小型軽量位置姿勢推定演算器生成信号１９を生成する。
【０１０２】
小型軽量位置姿勢推定演算器１４５は、高精度位置姿勢推定部１３０が推定した位置姿勢を基準とする。小型軽量位置姿勢推定演算器１４５は、小型軽量加速度センサ１４２が検出した加速度と小型軽量ジャイロセンサ１４３が検出した角速度とジャイロセンサ１４４が検出した角速度とを用いて、（仮想的な）カルマンフィルタ処理を行う。これによって小型軽量位置姿勢推定演算器１４５は、第２のタイミング（周期）の位置姿勢を推定する。
【０１０３】
本実施形態の小型軽量位置姿勢推定演算器１４５が実行するカルマンフィルタ処理で用いられる数式は、基本的に第１の実施形態の小型軽量位置姿勢推定演算器１４５が実行するカルマンフィルタ処理で用いられる数式と同様になる。
【０１０４】
また、小型軽量位置姿勢推定演算器１４５が使用するカルマンフィルタ処理で使用される状態量１ｘｋ＋１及び観測量１ｚｋの線形確率システム（状態空間モデル）の式は、基本的に第１の実施形態の小型軽量位置姿勢推定演算器１４５で用いられる数式と同様になる。また、推定器１４５ｂにおけるカルマンフィルタ更新処理の式も、基本的に第１の実施形態の小型軽量位置姿勢推定演算器１４５で用いられる数式と同様になる。また、伝播器１４５ａにおけるカルマンフィルタ予測処理におけるシステム状態量の予測値１ｘｋ＋１−と誤差共分散行列の予測値１Ｐｋ＋１−の式も、基本的に第１の実施形態の小型軽量位置姿勢推定演算器１４５で用いられる数式と同様になる。
【０１０５】
次に、本実施の形態に係る位置姿勢推定装置１２０の動作について説明する。本実施の形態の動作は、基本的に実施の形態１の図５と同様であり、主に、小型軽量位置姿勢推定部１４０（小型軽量位置姿勢推定演算器１４５）における、Ｓ１１２、Ｓ１１３の処理のみが異なる。
【０１０６】
小型軽量位置姿勢推定部１４０は、小型軽量加速度センサ検出信号１６と小型軽量ジャイロセンサ検出信号１７とジャイロセンサ検出信号１８に加え、高精度位置姿勢推定部１３０が生成する移動体高精度位置姿勢推定信号１４とを用いて、カルマンフィルタの枠組みを流用した仮想的な推定処理を実施する。これによって、小型軽量位置姿勢推定部１４０は、高々４００Ｈｚのサンプリング周波数で位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号１９を生成する。
【０１０７】
ステップＳ１１１においては、第１の実施形態と同様に、小型軽量加速度センサ１４２が加速度を検出し、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４が角速度を検出する。
【０１０８】
Ｓ１１２における位置姿勢伝播処理は、実施の形態１と同様である。上記の式（１１）及び式（１２）において、上付きの「＋」は仮想的なカルマンフィルタ処理後の推定値である。また、上付きの「−」は仮想的なカルマンフィルタ処理前の伝播予測値である。また、１Ｑｋはシステムノイズ１ｗｋの共分散行列（仮想的なカルマンフィルタの設計者が飛翔体ダイナミクス特性などに応じ設定する設計パラメータ）となる。
【０１０９】
ステップＳ１１２において、本実施形態では、現在位置ベクトルｐについて、伝播器１４５ａは、小型軽量加速度センサ１４２が検出した加速度を２回積分して位置の検出値を求める。伝播器１４５ａは、高精度位置姿勢推定部１３０が推定した位置及び前回の検出時において予測されているカルマンフィルタ処理後の位置から基準値を算出する。伝播器１４５ａは、位置の検出値と基準値との差をΔ０ｐとする。
【０１１０】
速度ベクトルｐ’については、伝播器１４５ａは、小型軽量加速度センサ１４２が検出した加速度を積分して速度の検出値を求める。伝播器１４５ａは、高精度位置姿勢推定部１３０が推定した位置及び前回の検出時において予測されているカルマンフィルタ処理後の速度から速度の基準値を計算する。伝播器１４５ａは、速度の検出値と基準値との差をΔ０ｐとする。
【０１１１】
姿勢ベクトルθについては、伝播器１４５ａは、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４の検出した角速度を補間により組み合わせた角速度を積分して姿勢の検出値を求める。伝播器１４５ａは、高精度位置姿勢推定部１３０が推定した姿勢及び前回の検出時において予測されているカルマンフィルタ処理後の姿勢から基準値を計算する。伝播器１４５ａは、姿勢の検出値と基準値との差をΔ０θとする。
【０１１２】
このようにして、ステップＳ１１１において、機体センサが位置、加速度、角速度を検出し、時刻ｔｋにおける観測量０ｚｋが得られる。
【０１１３】
そして、第１の実施形態と同様に、小型軽量位置姿勢推定演算器１４５の伝播器１４５ａは、システム状態量と誤差共分散行列を算出する。小型軽量位置姿勢推定演算器１４５は、算出した位置姿勢を出力する。
【０１１４】
また、Ｓ１１３において、小型軽量位置姿勢推定演算器１４５の推定器１４５ｂは、Ｓ１１２において算出されたシステム状態量と誤差共分散行列に基づいて、式（１７）、式（１８）によって時刻ｔｍ＋１のシステム状態量の予測値１θｍ＋１−と共分散行列の予測値１Ｐｍ＋１−を算出する。また推定器１４５ｂは、位置、速度、姿勢の検出値との差を算出するための位置、速度、姿勢のカルマンフィルタ処理による予測値を算出する。
【０１１５】
この結果、実施の形態１と同様に、小型軽量位置姿勢推定部１４０は、状態量の更新後に、システム状態量予測値１ｘｍ＋１−と誤差共分散行列予測値１Ｐｍ＋１−とを、位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号１９として出力する。
【０１１６】
このように、本実施の形態では、小型軽量位置姿勢推定部１４０は高精度位置姿勢推定部１３０が推定した位置姿勢を基準として推定処理を行う。
【０１１７】
さらに、位置姿勢出力部１５０は、小型軽量位置姿勢推定部１４０が推定した位置姿勢を基準として、第１の実施形態と同様に、仮想的なカルマンフィルタ処理を用いた推定処理を行うことにより補間処理を実施する。したがって、位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号１９を単独利用する補間処理と比較して更なる精度向上が可能となる。
【０１１８】
以上、実施形態を参照して本願発明を説明したが、本願発明は、上記実施形態に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解しうる様々な変更をすることができる。
【０１１９】
例えば、上述の位置姿勢出力部１５０が使用するカルマンフィルタ処理で用いられる観測量及び状態量は、現在位置ベクトルｐ＝（ｘ  ｙ  ｚ）の誤差ベクトル、姿勢ベクトルθ＝（φ  ρ  Ψ）の誤差ベクトルから構成されるものとして説明しているが、これに限られない。例えば位置姿勢出力部１５０は、小型軽量位置姿勢推定演算器１４５を経由してカルマンフィルタ処理に用いる小型軽量加速度センサ１４２、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４の検出信号の値を取得することができるよう構成されてもよい。このとき位置姿勢出力部１５０がカルマンフィルタ処理で用いる観測量及び状態量は、速度ベクトルｐ’＝（ｘ’  ｙ’  ｚ’）の誤差ベクトルを含むとしてもよい。さらに、小型軽量加速度センサ１４２のバイアスノイズを示すノイズベクトルａｂ＝（ａｂｘ  ａｂｙ  ａｂｚ）に対する誤差ベクトルを含むとしてもよい。さらに、小型軽量ジャイロセンサ１４３、ジャイロセンサ１４４の検出信号を補間により組み合わせた検出信号のバイアスノイズを示すベクトルωｂ＝（ωｂｘ  ωｂｙ  ωｂｚ）に対する誤差ベクトルを含むとしてもよい。
【０１２０】
上述の実施形態における各構成は、ハードウェア又はソフトウェア、もしくはその両方によって構成され、１つのハードウェア又はソフトウェアから構成されてもよいし、複数のハードウェア又はソフトウェアから構成されてもよい。レーダ装置や位置姿勢推定装置の各機能（各処理）は、ＣＰＵやメモリ等を有するコンピュータにより実現されてもよい。例えば、記憶装置（記憶媒体）に実施形態における制御方法を行うための制御プログラムを格納し、各機能を、記憶装置に格納された制御プログラムをＣＰＵが実行することにより実現されてもよい。
【０１２１】
上記の実施形態の一部または全部は、以下の付記のようにも記載され得るが、以下には限られない。
（付記１）
飛翔体や宇宙機や車両など移動体に搭載する駆動機器の姿勢情報を生成する移動体搭載機器の姿勢推定装置において、前記移動体に搭載し前記移動体の位置情報を移動体絶対位置検出信号として検出するためのＧＰＳ受信機と、前記移動体に搭載し前記移動体の加速度情報を移動体加速度検出信号として検出するための高精度加速度センサと、前記移動体に搭載し前記移動体の角速度情報を移動体姿勢角速度検出信号として検出するための高精度ジャイロセンサと、前記駆動機器に搭載し前記駆動機器の加速度情報を小型軽量加速度センサ検出信号として検出するための小型軽量加速度センサと、前記移動体絶対位置検出信号と前記移動体加速度検出信号と前記移動体姿勢角速度検出信号とを組み合わせることで高精度位置姿勢伝播値を含む移動体高精度位置姿勢推定信号を生成する高精度位置姿勢推定装置と、前記駆動機器に搭載し前記駆動機器の角速度情報を小型軽量ジャイロセンサ検出信号として検出するための小型軽量ジャイロセンサと、前記駆動機器に搭載し前記駆動機器の角速度情報をジャイロセンサ検出信号として検出するためのジャイロセンサと、前記駆動機器に搭載し前記駆動機器の位置姿勢角情報を前記小型軽量加速度センサ検出信号と前記小型軽量ジャイロセンサ検出信号と前記ジャイロセンサ検出信号とを組み合わせることで位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号として生成する小型軽量位置姿勢推定演算器と、前記移動体に搭載し前記駆動機器の姿勢角情報を前記高精度位置姿勢伝播値を含む移動体高精度位置姿勢推定信号と前記位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号とを組み合わせることで姿勢補間推定値を生成する姿勢推定装置を有することを特徴とする移動体搭載機器の姿勢推定装置。
（付記２）
前記姿勢推定装置が、前記高精度位置姿勢伝播値を含む移動体高精度位置姿勢推定信号と前記位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号とを組み合わせることで構成する仮想的なカルマンフィルタ演算処理により前記姿勢補間推定値を生成することを特徴とする付記１記載の移動体搭載機器の姿勢推定装置。
（付記３）
前記姿勢推定装置が、前記位置姿勢伝播値を含む小型軽量位置姿勢推定演算器生成信号に含まれる角速度信号に対し、前回生成した１サンプリング前の角速度信号と今回生成した角速度信号との間でサンプリング間隔における角速度信号の変化率（角加速度信号）を用いて角速度の変化を推定し、その推定に基づいた角速度信号を入力とする仮想的なカルマンフィルタ演算処理により前記姿勢補間推定値を生成することを特徴とする付記１記載の移動体搭載機器の姿勢推定装置。
【０１２２】
この出願は、２０１４年４月２５日に出願された日本出願特願２０１４−０９１２７９を基礎とする優先権を主張し、その開示の全てをここに取り込む。
【符号の説明】
【０１２３】
１      第１の検出部
２      第１の位置姿勢推定部
３      第２の検出部
４      第２の位置姿勢推定部
５      位置姿勢出力部
１０    位置姿勢推定装置
１１    移動体絶対位置検出信号
１２    移動体加速度検出信号
１３    移動体姿勢角速度検出信号
１４    移動体高精度位置姿勢推定信号
１５    絶対位置検出信号
１６    小型軽量加速度センサ検出信号
１７    小型軽量ジャイロセンサ検出信号
１８    ジャイロセンサ検出信号
１９    小型軽量位置姿勢推定演算器生成信号
２０    位置姿勢補間推定値
３０    移動体
３１    機体
１００  レーダ装置
１０１  アンテナ部
１０２  信号処理部
１０３  アンテナセンサ
１０４  機体センサ
１１０  送信部
１１１  受信部
１１２  画像処理部
１１３  サーキュレータ
１１４  制御部
１１５  位置姿勢推定部
１２０  位置姿勢推定装置
１３０  高精度位置姿勢推定部
１３１  ＧＰＳ受信機
１３２  高精度加速度センサ
１３３  高精度ジャイロセンサ
１３５  高精度位置姿勢推定演算器
１３５ａ  伝播器
１３５ｂ  推定器
１４０  小型軽量位置姿勢推定部
１４１  ＧＰＳ受信機
１４２  小型軽量加速度センサ
１４３  小型軽量ジャイロセンサ
１４４  ジャイロセンサ
１４５  小型軽量位置姿勢推定演算器
１４５ａ  伝播器
１４５ｂ  推定器
１５０  位置姿勢出力部
１５０ａ  伝播器
１５０ｂ  推定器
２０１  アンテナ
２０２  アンテナ駆動機構
【図１】
0006471749.tif 000003
【図２】
0006471749.tif 000004
【図３】
0006471749.tif 000005
【図４】
0006471749.tif 000006
【図５】
0006471749.tif 000007
【図６】
0006471749.tif 000008
【図７】
0006471749.tif 000009