状態推定装置及びそのプログラム、並びに、統合状態推定システム及び統合状態推定装置
【課題】 複数の状態推定動作を並列動作させることで推定処理の高速性、信頼性、汎用性を高めた統合状態推定システムを提供する。
【解決手段】 統合状態推定システムＳは、通信路（ネットワークＮ）を介して、複数の状態推定装置１０を備え、各状態推定装置１０が、それぞれ推定した観測対象の状態（状態量Ｘ）を送受信し合うことで状態量Ｘを統合し、少なくとも一つの状態推定装置１０が、観測対象の状態を示す推定量Ｅを出力することを特徴とする。
【解決手段】 統合状態推定システムＳは、通信路（ネットワークＮ）を介して、複数の状態推定装置１０を備え、各状態推定装置１０が、それぞれ推定した観測対象の状態（状態量Ｘ）を送受信し合うことで状態量Ｘを統合し、少なくとも一つの状態推定装置１０が、観測対象の状態を示す推定量Ｅを出力することを特徴とする。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、観測対象の観測量に基づいて、当該観測対象の状態量を推定する状態推定装置及びそのプログラム、並びに、統合状態推定システム及び統合状態推定装置に関する。
【背景技術】
【０００２】
従来、観測対象の観測量から状態量を推定する手法として、カルマンフィルタによる推定手法が知られている。このカルマンフィルタは、複数の観測量を観測ベクトルとして一つにまとめ、観測時に混入する観測雑音の共分散行列とともにフィルタリング処理を行うことで、複数の観測量を融合して状態量を推定するものである。
【０００３】
また、他の手法としては、コンデンセーション法をはじめとする、パーティクルフィルタによる推定手法が知られている（例えば、非特許文献１参照）。このパーティクルフィルタは、有限のパーティクル（複数のベクトルの集合）によって確率密度関数を表現し、この確率密度関数を用いて、時系列に入力される観測量から状態量を推定するものである。このパーティクルフィルタによれば、カルマンフィルタでは推定することができない非ガウス性雑音が混入する場合であっても、任意の非ガウシアンな確率密度関数によって、状態量を推定することができる。
さらに、このパーティクルフィルタを、推定量の分散によって適応的に切り替える手法も提案されている（特許文献１参照）。
【非特許文献１】M.Isard and A.Blake, CONDENSATION - Conditional Density Propagation for Visual Tracking, International Journal of Computer Vision, vol.29, no.1, pp.5-28(1998)
【特許文献１】特開２００５−４４３５２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかし、前記した従来の状態推定手法によれば、複数の観測量をまとめて処理するため、装置内において処理が集中してしまう。このため、従来の手法を適用した状態推定装置において、本手法による処理は高速化を阻害する要因となっている。
また、従来の状態推定手法を適用した状態推定装置においては、処理が集中しているため、当該状態推定装置が故障した場合、状態推定動作が停止してしまう。このため、状態推定を安定して行うためには、同一の状態推定装置を複数動作させなければならず、システム全体として、高価なものとなってしまうという問題がある。
また、従来の状態推定手法を適用した状態推定装置では、予め定めた観測対象の観測量に基づいて状態を推定するため、観測対象や推定を行う対象を変更することが容易ではなく、汎用性に欠けるといった問題がある。
【０００５】
本発明は、以上のような問題点に鑑みてなされたものであり、複数の状態推定動作を並列動作させることで推定処理の高速性、信頼性、汎用性を高めた状態推定装置及びそのプログラム、並びに、統合状態推定システム及び統合状態推定装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明は、前記目的を達成するために創案されたものであり、まず、請求項１に記載の状態推定装置は、観測対象の観測量に基づいて、当該観測対象の状態を示す状態量を推定する状態推定装置において、状態量記憶手段と、状態更新手段と、状態受信手段と、状態統合手段と、状態送信手段とを備える構成とした。
【０００７】
かかる構成において、状態推定装置は、状態更新手段によって、外部から入力された観測量により、状態量記憶手段に記憶する状態量（内部状態量）を更新する。これによって、観測量に基づいて、逐次内部状態量が更新されることになる。なお、状態量の更新は、一般的なカルマンフィルタや拡張カルマンフィルタの観測更新方程式により更新することとしてもよいし、シーケンシャルモンテカルロ法により状態量を状態ベクトルの集合として更新することとしてもよい。
【０００８】
また、状態推定装置は、状態受信手段によって、通信路に接続された他の状態推定装置において記憶されている状態量を外部状態量として受信し、状態統合手段によって、外部状態量と状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する。これによって、状態推定装置は、他の状態推定装置において推定された状態量を取り込み、利用することができる。なお、状態量の統合とは、複数の状態推定装置に分散して記憶されている状態量を１つの状態量にまとめて加工することであって、例えば、複数の状態量の平均値をとることで新たな状態量としてもよいし、予め定めた確率分布となるように、それぞれの状態量を選択することで新たな状態量としてもよい。
また、状態推定装置は、状態送信手段によって、状態量記憶手段に記憶されている内部状態量を他の状態推定手段に送信する。これによって、状態推定装置は、自身が推定した状態量を、他の状態推定装置に利用させることが可能になる。
【０００９】
また、請求項２に記載の状態推定装置は、請求項１に記載の状態推定装置において、前記状態量記憶手段が、複数の前記内部状態量を時刻に対応付けて記憶する構成とした。
【００１０】
かかる構成において、状態推定装置は、内部状態量を時刻に対応付けて状態量記憶手段に記憶することで、他の状態推定装置から送信される状態量と内部で記憶する状態量とを時刻によって対応付けることができる。
【００１１】
さらに、請求項３に記載の状態推定装置は、請求項２に記載の状態推定装置において、状態予測手段を備える構成とした。
【００１２】
かかる構成において、状態推定装置は、状態予測手段によって、状態量記憶手段に記憶されている複数の内部状態量に基づいて、ある時刻における状態量を予測する。なお、状態量の予測については、一般的な状態遷移方程式あるいは現在の状態と未来の状態との間の遷移確率により予測することができる。
【００１３】
また、請求項４に記載の統合状態推定システムは、観測対象の観測量に基づいて、通信路に接続された複数の状態推定装置により、当該観測対象の状態を示す状態量を統合して推定する統合状態推定システムであって、状態推定装置が、状態量記憶手段と、状態受信手段と、状態統合手段と、状態送信手段とを備え、前記複数の状態推定装置の少なくとも一つが状態更新手段を備え、さらに、前記複数の状態推定装置の少なくとも一つが推定量出力手段を備える構成とした。
【００１４】
かかる構成において、統合状態推定システムは、当該システムを構成する状態推定装置が、状態量記憶手段に観測対象の状態量を内部状態量として記憶する。そして、状態推定装置は、状態受信手段によって、通信路に接続された他の状態推定装置において記憶されている状態量を外部状態量として受信し、状態統合手段によって、外部状態量と状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する。また、状態推定装置は、状態送信手段によって、状態量記憶手段に記憶されている内部状態量を他の状態推定装置に送信する。このように、統合状態推定装置システムは、各状態推定装置において、状態量を送受信することで、統合することができる。
【００１５】
なお、状態量の更新は、複数の状態推定装置の少なくとも一つが、状態更新手段によって、外部から入力された観測量により、状態量記憶手段に記憶する状態量（内部状態量）を更新することで行う。この状態更新手段の数は、観測量の数に基づいて決定されるものであって、少なくとも観測量の数が一つである場合は、複数の状態推定装置の中の一つに備えられればよい。
そして、複数の状態推定装置の少なくとも一つが、推定量出力手段によって、状態量記憶手段に記憶されている内部状態量を推定量として出力する。
【００１６】
さらに、請求項５に記載の統合状態推定装置は、観測対象の観測量に基づいて、当該観測対象の状態を示す状態量を統合して推定する統合状態推定装置であって、前記状態量を推定する状態推定手段を複数備え、前記状態推定手段が、状態量記憶手段と、状態受信手段と、状態統合手段と、状態送信手段とを備え、前記複数の状態推定手段の少なくとも一つが状態更新手段を備え、さらに、前記複数の状態推定手段の少なくとも一つが推定量出力手段を備える構成とした。
【００１７】
かかる構成において、統合状態推定装置は、状態推定手段が状態量記憶手段に観測対象の状態量を内部状態量として記憶する。そして、状態推定手段は、状態受信手段によって、通信路に接続された他の状態推定手段において記憶されている状態量を外部状態量として受信し、状態統合手段によって、外部状態量と状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する。また、状態推定手段は、状態送信手段によって、状態量記憶手段に記憶されている内部状態量を他の状態推定装置に送信する。このように、統合状態推定装置は、各状態推定手段において、状態量を送受信することで、統合することができる。
【００１８】
なお、状態量の更新は、複数の状態推定手段の少なくとも一つが、状態更新手段によって、外部から入力された観測量により、状態量記憶手段に記憶する状態量（内部状態量）を更新することで行う。そして、複数の状態推定手段の少なくとも一つが、推定量出力手段によって、状態量記憶手段に記憶されている内部状態量を推定量として出力する。
【００１９】
また、請求項６に記載の状態推定プログラムは、観測対象の観測量に基づいて、通信路に接続された複数の状態推定装置により、当該観測対象の状態を示す状態量を統合して推定するために、前記状態推定装置におけるコンピュータを、状態更新手段、状態受信手段、状態統合手段、状態送信手段、推定量出力手段として機能させる構成とした。
【００２０】
かかる構成において、状態推定プログラムは、状態更新手段によって、外部から入力された観測量により、状態量記憶手段に記憶する状態量（内部状態量）を更新する。そして、状態推定プログラムは、状態受信手段によって、通信路に接続された他の状態推定手段において記憶されている状態量を外部状態量として受信し、状態統合手段によって、外部状態量と状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する。
また、状態推定プログラムは、状態送信手段によって、状態量記憶手段に記憶されている内部状態量を他の状態推定手段に送信する。また、状態推定プログラムは、推定量出力手段によって、状態量記憶手段に記憶されている内部状態量を推定量として出力する。
【発明の効果】
【００２１】
本発明は、以下に示す優れた効果を奏するものである。
請求項１、請求項４又は請求項６に記載の発明によれば、複数の状態推定装置において推定される状態量を統合して観測対象の状態を推定することができる。これによって、推定処理を複数の状態推定装置に分散させることで、並列動作が可能となり、推定動作の高速化を図ることができる。また、本発明によれば、複数の状態推定装置のいずれかに障害が発生した場合であっても、他の状態推定装置における状態量を統合することで、状態推定のサービスを継続することができる。また、本発明によれば、状態推定装置単位で交換等が可能であるため、新しい観測量に基づいて状態を推定する場合であっても、その追加、変更が容易であり、システム全体の汎用性を高めることができる。
【００２２】
請求項２に記載の発明によれば、他の状態推定装置から送信される状態量と内部で記憶している状態量とを時刻によって対応付けているため、状態量の時刻同期をとることができる。これによって、状態推定装置間で推定する状態の時刻を一致させることができるため、時刻のずれがなくなり、状態推定の精度を高めることができる。
【００２３】
請求項３に記載の発明によれば、観測量が入力されない時点においても、状態を推定し、その推定量を出力することが可能になり、安定して状態推定を行うことができる。
請求項５に記載の発明によれば、複数の状態推定手段において推定される状態量を統合して観測対象の状態を推定することができる。これによって、並列動作が可能になり、推定動作の高速化を図ることができる。
【発明を実施するための最良の形態】
【００２４】
以下、本発明の実施の形態について図面を参照して説明する。
［統合状態推定システムの概要］
最初に、図１を参照して、統合状態推定システムの構成について説明する。図１は、本発明に係る統合状態推定システムの全体の構成を示す全体構成図である。
図１に示すように、統合状態推定システムＳは、通信路（ネットワークＮ）を介して、複数の状態推定装置１０を接続し、各状態推定装置１０で推定される観測対象の状態（状態量）を統合して、所望の状態量を推定し、推定値として出力するものである。
【００２５】
状態推定装置１０は、外部から入力される観測量Ｙに基づいて、観測対象の状態（状態量Ｘ）を推定するとともに、他の状態推定装置が推定した状態量と統合するものである。なお、状態推定装置１０は、いずれか一つが観測量Ｙを入力すればよく、また、いずれか一つが推定量Ｅを出力すればよい。例えば、状態推定装置１０３のように外部に推定量Ｅを出力しない形態、状態推定装置１０４のように外部から観測量Ｙを入力しない形態、状態推定装置１０５のように観測量Ｙの入力や推定量Ｅの出力を行わない形態であってもよい。
【００２６】
なお、ここで、観測対象とは、動き、変動等の状態の変化を伴うもの、あるいは、事象であって、動きを伴う人物、車両等や、変動を伴う株価等がその対象となりうる。例えば、状態推定装置１０は、人物を観測対象として、複数の位置からカメラで撮影した画像における人物の位置を観測量とすることで、人物の３次元位置や、速度等を推定することができる。
【００２７】
（状態量について）
ここで、統合状態推定システムＳにおいて使用する状態量について説明する。
状態量は、観測対象の動き、変動等の状態を示すデータ量であって、例えば、観測対象の速度等である。この状態量は、例えば、一つのベクトル（以下、状態ベクトルという）により表現することができる。例えば、物体の３次元位置及び３次元速度からなる６次元ベクトル量、あるいは、天体の軌道６要素等のベクトル量を状態量とすることができる。
【００２８】
また、例えば、状態量は、一つのベクトルと一つの行列との組により表現することができる。この場合、ベクトルは、先に説明した状態ベクトルと同様のものである。一方、行列は、状態ベクトルに含まれる誤差を表す共分散行列（以下、状態共分散行列という）により表現することができる。
【００２９】
さらに、例えば、状態量は、複数のベクトルの集合（以下、パーティクルという）として表現することができる。なお、パーティクルを構成する個々のベクトルは、先に説明した状態ベクトルと同様のものである。例えば、個々のベクトルとして、３次元位置及び３次元速度からなる６次元ベクトルを用いることができる。
【００３０】
（観測量について）
次に、統合状態推定システムＳに入力される観測量について説明する。
観測量は、観測対象を観測した結果のデータ量であって、例えば、観測対象の２次元位置等である。この観測量は、例えば、一つのベクトル（以下、観測ベクトルという）により表現することができる。なお、複数のセンサ等によって多次元の観測結果が得られる場合は、各センサ等から得られるデータを行ベクトルとして多重化することで、一つのベクトルとして表現することができる。
【００３１】
また、例えば、観測量は、観測ベクトルに加え、観測ベクトルの誤差や信頼度を表す量を付加することもできる。例えば、観測ベクトルの誤差を、共分散行列（以下、観測共分散行列という）により表現することができる。
さらに、例えば、観測量は、先に説明した状態ベクトルに対する確率密度として定義することとしてもよい。
【００３２】
なお、状態ベクトル又は観測ベクトルにおいて、スカラ量を扱う場合は、これを１次元のベクトル量とみなすこととする。また、行列やテンソル量を扱う場合は、その各要素を予め定めた一定の規則により１次元に並べたものをもってベクトル量とみなすものとする。
【００３３】
（システム例）
ここで、図２を参照して、統合状態推定システムの具体例であるカーナビゲーションシステムについて説明する。図２は、統合状態推定システムの具体例であるカーナビゲーションシステムの構成を示す全体構成図である。
カーナビゲーションシステムＳＮは、状態推定装置１０１〜１０３を、通信路（ネットワークＮ）を介して接続した構成となっている。このカーナビゲーションシステムＳＮは、自動車の３次元位置（例えば、緯度、経度及び高度）、速度ベクトル、姿勢角（例えば、ロール角、ピッチ角及びヨー角）、並びに、角速度ベクトルを状態量Ｘとする。
【００３４】
ここで、状態推定装置１０１は、ＧＰＳ（Global Positioning System）受信機２が観測した時刻データを観測量Ｙ１として入力する。また、状態推定装置１０２は、車速パルス計測器３が観測した自動車車輪の車速パルスの時間間隔データを観測量Ｙ２として入力する。さらに、状態推定装置１０３は、ジャイロセンサ４が観測した角加速度データを観測量Ｙ３として入力する。
そして、例えば、状態推定装置１０１が、状態量Ｘから自動車の位置情報のみを抽出し、推定量Ｅとして出力する。
【００３５】
このように、状態推定装置１０１〜１０３を通信路（ネットワークＮ）に接続することで、ＧＰＳ受信機２、車速パルス計測器３、ジャイロセンサ４の各観測手段により観測された観測量Ｙ１，Ｙ２，Ｙ３に基づく状態量Ｘが統合され、状態推定装置１０１において、自動車の位置を推定し、推定量Ｅとして出力することができる。
【００３６】
以上、説明したように、ここでは、統合状態推定システムＳ（図１参照）やカーナビゲーションシステムＳＮ（図２参照）を、ネットワークＮに接続した複数の状態推定装置１０で構成したが、各状態推定装置１０を、一つの手段として内部構成に組み込んで、図３に示すように、各状態推定手段１０をバスＢ等の通信路を介して接続することで、統合状態推定装置１として構成することとしてもよい。
すなわち、状態推定装置（状態推定手段）を接続する通信路は、ＬＡＮケーブル、電話線、シリアルケーブル等のメタル線路や、光ファイバ、赤外線、無線回線、基板内の配線、基板間の配線、ＩＣチップ内の配線等、任意の伝送媒体を用いることができる。また、通信路は、途中、交換機、ルータ、ハブ、リピータ等の中継装置を介しても構わない。
【００３７】
以下、統合状態推定システムＳを構成する状態推定装置１０の内部構成について詳細に説明を行うが、図３に示した統合状態推定装置１における状態推定手段１０においても、内部構成は同じ構成で実現することができる。
【００３８】
［第１実施形態：状態推定装置（状態推定手段）の構成］
まず、図４を参照して、状態推定装置１０の構成について説明する。図４は、本発明の第１実施形態に係る状態推定装置の内部構成を示すブロック構成図である。
状態推定装置（状態推定手段）１０は、外部から入力される観測対象の観測量に基づいて、ネットワークＮに接続した他の状態推定装置と協働して、観測対象の状態を示す状態量を推定するものである。ここでは、状態推定装置１０は、状態量記憶手段１１と、状態更新手段１２と、状態受信手段１３と、状態統合手段１４と、状態送信手段１５と、推定量出力手段１６とを備えている。
【００３９】
状態量記憶手段１１は、観測対象の状態を示す状態量のうち、当該状態推定装置１０内部で保持する状態量（内部状態量）を記憶するものであって、例えば、ＲＡＭ（Random Access Memory）等である。
この状態量記憶手段１１は、後記する状態更新手段１２や状態統合手段１４によって、状態量が読み出され、又は、書き込まれ、状態送信手段１５や推定量出力手段１６によって、読み出される。
【００４０】
状態更新手段１２は、外部から入力される観測量Ｙによって、状態量記憶手段１１に記憶されている状態量を更新するものである。ここでは、状態更新手段１２は、状態量記憶手段１１から状態量（内部状態量ＸＩ）を読み出し、観測量Ｙによって状態量を更新し、その更新された状態量（観測後状態量ＸＹ）を内部状態量ＸＩとして、状態量記憶手段１１に書き込む。
なお、状態更新手段１２は、観測量Ｙをそのまま観測後状態量ＸＹとして状態量記憶手段１１に書き込むこととしてもよいが、予め観測量Ｙにおけるノイズ、誤差等がモデル化されている場合は、そのモデルに基づいて、観測後状態量ＸＹを算出し、更新することが状態推定の精度を高める上で望ましい。
以下、数式を用いて、状態更新手段１２における状態量の更新手法について説明する。
【００４１】
（状態量及び観測量がベクトルの場合）
まず、状態量（内部状態量ＸＩ）として状態ベクトルｘＩ（すなわち、ＸＩ＝｛ｘＩ｝）、観測量Ｙとして観測ベクトルｙ（すなわち、Ｙ＝｛ｙ｝）を採用する場合における状態量の更新について説明する。なお、状態ベクトルｘＩから観測ベクトルｙへの関数ｈ及びノイズ（雑音）ｎにより、状態の更新が、以下の式（１）によりモデル化できるものとする。
【００４２】
【数１】
【００４３】
ここで、関数ｈに逆関数ｈ−１が存在する場合には、適当な実数値α（望ましくは０≦
α≦１の実数値）を用いて、以下の式（２）により観測後の状態ベクトルｘＹを得ることができる。
【００４４】
【数２】
【００４５】
すなわち、観測後状態量ＸＹは、ＸＹ＝｛ｘＹ｝である。なお、状態量記憶手段１１から、状態ベクトルｘＩが読み出せなかった場合（存在しなかった場合）には、例えば、前記式（２）でαの値を“１”とした、ｘＹ＝ｈ−１（ｙ）により観測後の状態ベクトルｘＹを求めることとしてもよい。
【００４６】
（状態量及び観測量がベクトル及び共分散行列の場合）
次に、状態量（内部状態量ＸＩ）として状態ベクトルｘＩ及び状態共分散行列ＰＩ（すなわち、ＸＩ＝｛ｘＩ，ＰＩ｝）、観測量Ｙとして観測ベクトルｙと観測共分散行列Ｒ（すなわち、Ｙ＝｛ｙ，Ｒ｝）を採用する場合における状態量の更新について説明する。なお、状態ベクトルｘＩから観測ベクトルｙへの関数ｈ及びノイズ（雑音）ｎにより、状態の更新が、以下の式（３）によりモデル化することができるものとする。
【００４７】
【数３】
【００４８】
ここで、Ｅ［］は期待値、上付き添え字Ｔは転置を表している。
このとき、例えば、以下の式（４）に示す拡張カルマンフィルタの観測更新方程式によって、観測後状態量ＸＹ＝｛ｘＹ，ＰＹ｝を決定することができる。
【００４９】
【数４】
【００５０】
ここで、Ｋはフィルタ感度、Ｈは観測行列、上付き添え字Ｔは転置を表している。
なお、状態量記憶手段１１から、状態ベクトルｘＩと状態共分散行列ＰＩとが読み出せなかった場合（存在しなかった場合）には、例えば、ｘＹ＝ｈ−１（ｙ）、ＰＹ＝ｄｉａｇ｛λ１，λ２，…，λＤ｝（ただし、ｄｉａｇは対角行列、λ１，λ２，…，λＤは非負の実数、ＤはＸＹの次元とする）としてもよい。
【００５１】
（状態量がパーティクル、観測量が確率密度関数の場合）
次に、状態量（内部状態量ＸＩ）としてパーティクル、観測量Ｙとして確率密度関数を採用する場合における状態量の更新について説明する。
まず、状態量記憶手段１１から読み出される内部状態量ＸＩに含まれる状態ベクトルの数をＮ個、各々の状態ベクトルをｘ（ｉ）（ただし、ｉ＝１，２，…，Ｎ）とする。すなわち、ＸＩ＝｛ｘ（１），ｘ（２），…，ｘ（Ｎ）｝とする。
【００５２】
一方、観測量Ｙである確率密度関数は、引数を状態ベクトルｘとし、Ｙ（ｘ）のように記述することとする。
最初に、状態更新手段１２は、１≦ｉ≦Ｎなるすべてのｉに対して、観測量Ｙ（ｘ（ｉ））に基づいて、以下の式（５）により、ｉ番目の状態ベクトルが発生する確率ｐ（ｉ）を算出する。
【００５３】
【数５】
【００５４】
続いて、状態更新手段１２は、観測後状態量ＸＹに、確率ｐ（ｉ）の大きい状態ベクトルｘ（ｉ）が多く含まれ、確率ｐ（ｉ）の小さい状態ベクトルｘ（ｉ）が少なく含まれる（又は含まれない）ように、状態量を再編成（再サンプリング）する。なお、状態ベクトルｘ（ｉ）をとるサンプル数は、精度を高めるため、確率ｐ（ｉ）に比例する頻度（個数）であることが望ましく、例えば、確率ｐ（ｉ）がｐ（１）＝１／１０、ｐ（２）＝６／１０、ｐ（３）＝３／１０で、状態量の総数が１０個の場合、状態ベクトルｘ（１）を１個、状態ベクトルｘ（２）を６個、状態ベクトルｘ（３）を３個のように、状態量（パーティクル）を生成し直す。
例えば、状態更新手段１２は、以下の手順により状態量を更新する。
まず、以下の式（６）に示すように、式（５）で示した確率ｐ（ｉ）を累積した累積確率ａ（ｉ）を算出するとともに、観測後状態量ＸＹを空集合（ＸＹ＝φ）とする。
【００５５】
【数６】
【００５６】
続けて、状態更新手段１２は、以下の式（７）に示すように、０＜ｒ≦１なる一様乱数ｒを生成する。
【００５７】
【数７】
【００５８】
さらに、状態更新手段１２は、以下の式（８）に示すように、累積確率ａ（ｉ）≧ｒを満たし、かつ、累積確率ａ（ｉ）が最小となる配列引数ｉを探索し、これをｊとおく。
【００５９】
【数８】
【００６０】
そして、状態更新手段１２は、以下の式（９）に示すように、観測後状態量ＸＹに状態ベクトルｘ（ｊ）を含める。
【００６１】
【数９】
【００６２】
そして、状態更新手段１２は、前記した式（７）〜式（９）の処理を所定回数（例えば、内部状態量ＸＩに含まれる状態ベクトルの数と同じＮ回）繰り返す。これによって、観測後状態量ＸＹにおいて、パーティクルの頻度によって、確率密度関数が近似表現されることになる。
なお、状態量記憶手段１１から、内部状態量ＸＩが読み出せなかった場合（存在しなかった場合）には、例えば、状態更新手段１２は、状態ベクトルを乱数によりＮ個生成し、これらをまとめたものを観測後状態量ＸＹとする。また、このときの乱数は、確率密度関数Ｙ（ｘ）に従うこととしてもよい。
図４に戻って、状態推定装置１０の構成について説明を続ける。
【００６３】
状態受信手段１３は、ネットワークＮに接続された他の状態推定装置が保持し、当該他の状態推定装置から送信される状態量（受信状態量ＸＲ；外部状態量）を受信するものである。この受信した受信状態量ＸＲは、状態統合手段１４に出力される。
ここで、他の状態推定装置は、状態量（受信状態量ＸＲ）を含んだメッセージを、ネットワークＮを経由して送信する。なお、このメッセージには、送信元に関する情報（例えば、ＩＰアドレス等）を含めることとしてもよい。
【００６４】
状態統合手段１４は、状態受信手段１３で受信した状態量（受信状態量ＸＲ）と、状態量記憶手段１１に記憶されている状態量（内部状態量ＸＩ）とを統合し統合状態量ＸＪを生成するものである。この統合状態量ＸＪは、内部状態量ＸＩとして状態量記憶手段１１に記憶され、内部状態量ＸＩが更新される。
なお、状態統合手段１４は、状態量記憶手段１１から内部状態量ＸＩが読み出せなかった場合（存在しなかった場合）には、受信した状態量（受信状態量ＸＲ）をそのまま統合状態量ＸＪとする。
以下、数式を用いて、状態統合手段１４における状態量の統合手法について説明する。
【００６５】
（状態量がベクトルの場合）
まず、状態量（内部状態量ＸＩ及び受信状態量ＸＲ）として、一つの状態ベクトルを採用する場合における状態量の統合について説明する。
状態量として状態ベクトルを採用する場合、状態統合手段１４は、内部状態量ＸＩの状態ベクトルをｘＩ、受信状態量ＸＲの状態ベクトルをｘＲとし、以下の式（１０）により統合後の状態ベクトルｘＪを算出する。
【００６６】
【数１０】
【００６７】
ここで、βは適当な実数値とし、望ましくは０≦β≦１の値をとるものとする。
なお、統合状態量ＸＪは、ＸＪ＝｛ｘＪ｝である。
【００６８】
（状態量がベクトル及び共分散行列の場合）
次に、状態量（内部状態量ＸＩ及び受信状態量ＸＲ）として、状態ベクトル及び状態共分散行列を採用する場合における状態量の統合について説明する。
状態量として、状態ベクトル及び状態共分散行列を採用する場合、状態統合手段１４は、内部状態量ＸＩの状態ベクトルｘＩ及び状態共分散行列ＰＩと、受信状態量ＸＲの状態ベクトルｘＲ及び状態共分散行列ＰＲとに基づいて、統合状態量ＸＪの状態ベクトルｘＪ及び状態共分散行列ＰＪを以下に示す手順により算出する。
まず、状態統合手段１４は、以下の式（１１）に示すように、状態共分散行列ＰＩ及び状態共分散行列ＰＲのｉ行ｉ列成分を抽出し、それぞれｖＩ（ｉ）及びｖＲ（ｉ）とする（ただし、ｉ＝１，２，…，Ｄ；Ｄは状態ベクトルの次元数）。
【００６９】
【数１１】
【００７０】
続いて、状態統合手段１４は、以下の式（１２）に示すように、状態ベクトルｘＩ及び状態ベクトルｘＲのｉ行成分を抽出し、それぞれｘＩ（ｉ）及びｘＲ（ｉ）とする。
【００７１】
【数１２】
【００７２】
そして、状態統合手段１４は、前記式（１１）で抽出したｖＩ（ｉ）及びｖＲ（ｉ）の大小に応じて、スカラ量ｘＴ（ｉ）を、以下に示す式（１３）により決定する。
【００７３】
【数１３】
【００７４】
そして、状態統合手段１４は、この式（１３）で決定されたスカラ量ｘＴ（ｉ）を、以下の式（１４）に示すように、配列引数ｉについて配列することで、状態ベクトルｘＪを構成する。
【００７５】
【数１４】
【００７６】
一方、状態統合手段１４は、状態共分散行列ＰＪを、以下の式（１５）に示すように、前記式（１１）で抽出したｖＩ（ｉ）及びｖＲ（ｉ）の小さい方を配列引数ｉについて並べた対角行列とする。
【００７７】
【数１５】
【００７８】
なお、統合状態量ＸＪは、ＸＪ＝｛ｘＪ，ＰＪ｝である。
【００７９】
（状態量がパーティクルの場合）
次に、状態量（内部状態量ＸＩ）としてパーティクルを採用する場合における状態量の統合について説明する。
状態量として、パーティクルを採用する場合、状態統合手段１４は、内部状態量ＸＩ及び受信状態量ＸＲの和集合（ＸＩ∪ＸＲ）に含まれる状態ベクトル“２Ｎ”個の中から、確率的にＮ´個の状態ベクトルを選択し、その選択されたＮ´個の状態ベクトルを統合状態量ＸＪとする。なお、統合状態量ＸＪに含まれる状態ベクトルの個数Ｎ´は、内部状態量ＸＩの個数Ｎと同数であってもよい。
【００８０】
ここでは、内部状態量ＸＩ及び受信状態量ＸＲに含まれる状態ベクトルの数をＮ個とし、各々の状態ベクトルをｘＩ（ｉ）及びｘＲ（ｉ）（ただし、ｉ＝１，２，…，Ｎ）とする。すなわち、ＸＩ＝｛ｘＩ（１），ｘＩ（２），…，ｘＩ（Ｎ）｝、ＸＲ＝｛ｘＲ（１），ｘＲ（２），…，ｘＲ（Ｎ）｝とする。
まず、状態統合手段１４は、統合状態量ＸＪを空集合（ＸＪ＝φ）とする。
続けて、状態統合手段１４は、以下の式（１６）に示すように、０＜ｒ≦１なる一様乱数ｒを生成する。
【００８１】
【数１６】
【００８２】
そして、状態統合手段１４は、一様乱数ｒが予め定めた定数（閾値γ〔０＜γ≦１の実数値〕）よりも大きいか小さいかによって、以下の式（１７）に示すように、状態ベクトルｘＩ（ｉ）又はｘＲ（ｉ）を、統合状態量ＸＪに含める。
【００８３】
【数１７】
【００８４】
そして、状態統合手段１４は、前記した式（１６）及び式（１７）の処理を状態ベクトルの回数（Ｎ）分繰り返す。これによって、内部状態量ＸＩ及び受信状態量ＸＲに含まれる状態ベクトルから確率的に選択された状態ベクトル群により統合状態量ＸＪを構成することができる。このとき、例えば、閾値γを“０．５”とすると、内部状態量ＸＩ及び受信状態量ＸＲから同等の確率で状態ベクトルが選択されることになる。また、γの値が大きいほど、内部状態量ＸＩに比べて、受信状態量ＸＲを重視することができる。
図４に戻って、状態推定装置１０の構成について説明を続ける。
【００８５】
状態送信手段１５は、状態量記憶手段１１に記憶されている状態量（内部状態量ＸＩ）を読み出して、ネットワークＮに接続された他の状態推定装置に送信するものである。
なお、状態送信手段１５は、状態量（内部状態量ＸＩ）を含んだメッセージを他の状態推定装置に送信する際に、送信先や送信元を示すアドレス情報（ＩＰアドレス等）を含めることとしてもよい。
また、状態量がまだ記憶されていない等の理由によって、状態量記憶手段１１から状態量（内部状態量ＸＩ）を読み出せない場合、状態送信手段１５は、送信を行わないこととしてもよいし、送信不可能であるというメッセージを送信することとしてもよい。
また、状態送信手段１５は、定期的に内部状態量ＸＩを送信することとしてもよいし、他の状態推定装置からの要求に基づいて送信することとしてもよい。
【００８６】
推定量出力手段１６は、状態量記憶手段１１に記憶されている状態量（内部状態量ＸＩ）を読み出して、推定量Ｅとして外部に出力するものである。
なお、推定量出力手段１６は、内部状態量ＸＩをそのまま推定量Ｅとして出力することとしてもよいし、一部の状態量のみを出力したり、状態量に所定の変換を施して出力したりすることとしてもよい。
【００８７】
例えば、状態量が状態ベクトルである場合、推定量出力手段１６は、状態ベクトルの一部成分を抽出して、推定量Ｅを新たなベクトル量やスカラ量とすることができる。具体的には、状態ベクトルが３次元位置及び３次元速度で構成される場合に、３次元位置のみを推定量Ｅとする。あるいは、推定量出力手段１６は、状態ベクトルに演算処理、例えば、線形変換を施した結果を推定量Ｅとして出力することとしてもよい。
【００８８】
また、例えば、状態量がパーティクルの場合、推定量出力手段１６は、パーティクルを構成する一部の状態ベクトルを取り出して推定量Ｅとすることができる。あるいは、推定量出力手段１６は、パーティクルを構成する状態ベクトルすべてについて平均ベクトルを算出し、推定量Ｅとして出力することとしてもよい。
なお、状態量がまだ記憶されていない等の理由によって、状態量記憶手段１１から状態量（内部状態量ＸＩ）を読み出せない場合、推定量出力手段１６は、推定量Ｅを出力しないこととしてもよいし、出力することができない旨を出力することとしてもよい。
【００８９】
以上、状態推定装置１０の構成について説明したが、本発明はこの構成に限定されるものではない。例えば、推定量Ｅの出力を他の状態推定装置で行うこととし、状態推定装置１０からは出力を行わない構成としてもよい。この場合、状態推定装置１０の構成から推定量出力手段１６を省略することができる。また、外部からの観測量Ｙの入力を他の状態推定装置で行うこととし、状態推定装置１０では入力を行わない構成としてもよい。この場合、状態推定装置１０の構成から状態更新手段１２を省略することができる。
【００９０】
また、図５に示すように、状態更新手段１２を外部の観測装置２０に備え、状態更新手段１２を状態推定装置１０から省略し、データの入出力のみを行う入出力手段１７を備えた状態推定装置１０Ｂの構成としてもよい。例えば、観測装置２０は、図示を省略したセンサ等を備えた位置観測手段と、状態更新手段１２とを備えている。そして、観測装置２０の状態更新手段１２が、位置観測手段（図示せず）により観測した結果である観測対象の位置（観測量）に基づいて状態量（観測後状態量ＸＹ）を算出し、状態量記憶手段１１に記憶されている内部状態量ＸＩを更新することとしてもよい。
【００９１】
この場合、観測装置２０は、観測後状態量ＸＹとともに、観測後状態量ＸＹに対する評価値を状態推定装置１０Ｂに出力することとしてもよい。例えば、観測装置２０は、センサであるカメラによって入力した画像に観測対象が存在するか（撮影されているか）否かを判定し、存在すると判定した場合は評価値に高い値を、存在しないと判定した場合は評価値に低い値を設定する。この評価値を参照することで、状態推定装置１０Ｂは、状態統合手段１４において、状態量の統合を行う際に、内部状態量ＸＩと受信状態量ＸＲとの統合の度合いを決定することとしてもよい。
【００９２】
また、状態推定装置１０（１０Ｂ）は、一般的なコンピュータを、前記した各手段として機能させることで実現することができる。このプログラム（状態推定プログラム）は、通信回線を介して配布することも可能であるし、ＣＤ−ＲＯＭ等の記録媒体に書き込んで配布することも可能である。
【００９３】
以上説明したように、状態推定装置１０（１０Ｂ）は、他の状態推定装置で推定した状態量と、当該状態推定装置１０で推定した状態量とを統合し、観測対象の状態を推定することができる。これによって、状態推定装置１０は、他の状態推定装置と協働し、状態の推定を分散して行うことができる。また、例えば、他の状態推定装置に状態推定装置１０と共通の状態量を記憶させておくことで、状態推定装置１０に障害が発生した場合であっても、状態の推定を継続することが可能になる。
【００９４】
［第１実施形態：状態推定装置（状態推定手段）の動作］
次に、図６〜図８を参照して、状態推定装置１０の動作について説明する。図６は、状態推定装置の全体動作を示すフローチャートである。図７は、状態推定装置における状態更新手段の動作を示すフローチャートである。図８は、状態推定装置における状態統合手段の動作を示すフローチャートである。
【００９５】
（全体動作）
まず、図６を参照（適宜図４参照）して、状態推定装置１０の動作について説明する。
状態推定装置１０は、状態更新手段１２によって、外部から観測量Ｙが入力されたか否かを判定する（ステップＳ１）。そして、観測量Ｙが入力された場合（ステップＳ１でＹｅｓ）、状態更新手段１２は、観測量Ｙによって、状態量記憶手段１１に記憶されている内部状態量ＸＩを更新する（ステップＳ２）。なお、このステップＳ２における内部状態量更新処理の詳細については、図７を参照して後で説明を行うこととする。一方、観測量Ｙが入力されなかった場合（ステップＳ１でＮｏ）、状態更新手段１２は、そのままステップＳ３の動作に進む。
【００９６】
また、状態推定装置１０は、状態統合手段１４によって、状態受信手段１３において、ネットワークＮに接続された他の状態推定装置から状態量（受信状態量ＸＲ）を受信したか否かを判定する（ステップＳ３）。そして、受信状態量ＸＲを受信した場合（ステップＳ３でＹｅｓ）、状態統合手段１４は、受信状態量ＸＲと状態量記憶手段１１に記憶されている内部状態量ＸＩとを統合する（ステップＳ４）。なお、このステップＳ４における内部状態量統合処理の詳細については、図８を参照して後で説明を行うこととする。一方、受信状態量ＸＲが受信されなかった場合（ステップＳ３でＮｏ）、状態統合手段１４は、そのままステップＳ５の動作に進む。
【００９７】
そして、状態推定装置１０は、状態送信手段１５によって、状態量記憶手段１１に記憶されている内部状態量ＸＩの一部又は全部を読み出して、送信状態量ＸＳとして、ネットワークＮに接続された他の状態推定装置に送信する（ステップＳ５）。なお、状態送信手段１５は、状態量記憶手段１１から内部状態量ＸＩが読み出せない場合、送信を行わない、又は、送信不可能であるというメッセージを送信する。
また、状態推定装置１０は、推定量出力手段１６によって、状態量記憶手段１１に記憶されている内部状態量ＸＩを読み出して、推定量Ｅとして外部に出力する（ステップＳ６）。なお、推定量出力手段１６は、一部の内部状態量ＸＩのみを出力したり、内部状態量ＸＩに所定の変換を施して出力したりすることとしてもよい。
【００９８】
以上、状態推定装置１０の動作について説明したが、本動作はこの手順に限定されるものではない。ここでは、説明の都合上時系列に動作の説明を行ったが、内部状態量更新処理（ステップＳ２）や内部状態量統合処理（ステップＳ４）を並列に動作させることとしてもよいし、観測量Ｙの入力や受信状態量ＸＲの受信が不規則に入力される場合は、そのタイミングに合わせて動作することとしてもよい。
【００９９】
（内部状態量更新処理）
次に、図７を参照（適宜図４参照）して、状態推定装置１０の状態更新手段１２における状態量更新処理の動作について説明する。なお、本動作は、図６のステップＳ２の動作に相当する。また、ここでは、状態量更新処理の一例として、状態量がパーティクル、観測量が確率密度関数である場合の動作について説明する。
【０１００】
まず、状態更新手段１２は、状態量記憶手段１１に内部状態量ＸＩ（ＸＩ＝｛ｘ（１），ｘ（２），…，ｘ（Ｎ）｝）が記憶されているか否かを判定する（ステップＳ２０）。そして、内部状態量ＸＩが記憶されている場合（ステップＳ２０でＹｅｓ）、状態更新手段１２は、内部状態量ＸＩに含まれるＮ個の状態ベクトルｘ（ｉ）（ただし、ｉ＝１，２，…，Ｎ）に対して、観測量Ｙ（ｘ（ｉ））に基づいて、状態ベクトルが発生する確率ｐ（ｉ）を算出する（ステップＳ２１：前記式（５）参照）。さらに、状態更新手段１２は、ステップＳ２１で算出した確率ｐ（ｉ）をｉ番目まで累積した累積確率ａ（ｉ）を、ｉごとにＮ個分算出する（ステップＳ２２）。
【０１０１】
ここで、状態更新手段１２は、観測後の状態量（観測後状態量ＸＹ）を空集合（ＸＹ＝φ）、ループ回数に用いる変数ｍを“１”にそれぞれ初期化する（ステップＳ２３）。
そして、状態更新手段１２は、０＜ｒ≦１なる実数値の一様乱数ｒを生成し（ステップＳ２４）、ステップＳ２２で算出した累積確率ａ（ｉ）の中でａ（ｉ）≧ｒを満たし、かつ、ａ（ｉ）が最小となる配列引数ｉを探索しｊとする（ステップＳ２５）。続けて、状態更新手段１２は、ステップＳ２５で探索された配列引数ｊに対応する状態ベクトルｘ（ｊ）を、観測後状態量ＸＹに追加する（ステップＳ２６）。
【０１０２】
その後、状態更新手段１２は、ループ回数の変数ｍをインクリメント（＋１）し（ステップＳ２７）、変数ｍの値がＮ以下である場合（ステップＳ２８でＹｅｓ）、ステップＳ２４に戻って動作を続ける。一方、変数ｍの値がＮを超過した場合（ステップＳ２８でＮｏ）、動作を終了する。
なお、ステップＳ２０において、状態量記憶手段１１に内部状態量ＸＩが記憶されていない場合（ステップＳ２０でＮｏ）、状態更新手段１２は、状態ベクトルを乱数によりＮ個（ｒ１，ｒ２，…，ｒＮ）生成することで、観測後状態量ＸＹを生成し（ステップＳ２９）、動作を終了する。
以上の動作を行うことで、状態更新手段１２は、状態量記憶手段１１に記憶されている内部状態量ＸＩを、確率の高いパーティクルをより高頻度にした確率密度関数の近似によって表現することができる。
【０１０３】
（内部状態量統合処理）
次に、図８を参照（適宜図４参照）して、状態推定装置１０の状態統合手段１４における内部状態量統合処理の動作について説明する。なお、本動作は、図６のステップＳ４の動作に相当する。また、ここでは、内部状態量統合処理の一例として、状態量がパーティクルである場合の動作について説明する。
なお、内部状態量ＸＩ及び受信状態量ＸＲに含まれる状態ベクトルの数をＮ個とし、各々の状態ベクトルをｘＩ（ｉ）及びｘＲ（ｉ）（ただし、ｉ＝１，２，…，Ｎ）とする。すなわち、ＸＩ＝｛ｘＩ（１），ｘＩ（２），…，ｘＩ（Ｎ）｝、ＸＲ＝｛ｘＲ（１），ｘＲ（２），…，ｘＲ（Ｎ）｝とする。
【０１０４】
まず、状態統合手段１４は、統合後の状態量（統合状態量ＸＹ）を空集合（ＸＹ＝φ）、ループ回数に用いる変数ｍを“１”にそれぞれ初期化する（ステップＳ４０）。
そして、状態統合手段１４は、０＜ｒ≦１なる実数値の一様乱数ｒを生成し（ステップＳ４１）、その一様乱数ｒが予め定めた定数（閾値γ〔０＜γ≦１の実数値〕）以下であるか否かを判定する（ステップＳ４２）。
【０１０５】
ここで、一様乱数ｒが閾値γ以下の場合（ステップＳ４２でＹｅｓ）、状態統合手段１４は、統合状態量ＸＪに、受信状態量ＸＲに含まれるｍ番目の状態ベクトルｘＲ（ｍ）を追加する（ステップＳ４３）。一方、一様乱数ｒが閾値γ未満の場合（ステップＳ４２でＮｏ）、状態統合手段１４は、統合状態量ＸＪに、内部状態量ＸＩに含まれるｍ番目の状態ベクトルｘＩ（ｍ）を追加する（ステップＳ４４）。
その後、状態統合手段１４は、ループ回数の変数ｍをインクリメント（＋１）し（ステップＳ４５）、変数ｍの値がＮ以下である場合（ステップＳ４６でＹｅｓ）、ステップＳ４１に戻って動作を続ける。一方、変数ｍの値がＮを超過した場合（ステップＳ４６でＮｏ）、動作を終了する。
【０１０６】
以上の動作を行うことで、状態統合手段１４は、内部状態量ＸＩ及び受信状態量ＸＲに含まれる状態ベクトルから、予め定めた閾値γを基準として選択された状態ベクトル群により統合状態量ＸＪを構成することができる。すなわち、閾値γによって、内部状態量又は受信状態量のいずれを重視するのかを設定することができる。
【０１０７】
［第２実施形態：状態推定装置（状態推定手段）の構成］
次に、図９を参照して、状態推定装置の他の構成について説明する。図９は、本発明の第２実施形態に係る状態推定装置の内部構成を示すブロック構成図である。
状態推定装置１０Ｃは、外部から入力される観測対象の観測量に基づいて、ネットワークＮに接続した他の状態推定装置と協働して、観測対象の状態を示す状態量を推定するものである。なお、状態推定装置１０Ｃは、複数の時点における状態量（内部状態量）を保持することで、複数の状態推定装置間での処理の違いや、観測時刻の違いに起因する時間ずれの影響を吸収する。ここでは、状態推定装置１０Ｃは、状態量記憶手段１１Ｃと、状態更新手段１２Ｃと、状態受信手段１３Ｃと、状態統合手段１４Ｃと、状態送信手段１５Ｃと、推定量出力手段１６Ｃと、状態予測手段１８とを備えている。
【０１０８】
状態量記憶手段１１Ｃは、観測対象の状態を示す状態量のうち、当該状態推定装置１０Ｃ内部で保持する状態量（内部状態量）を記憶するものである。なお、状態量記憶手段１１Ｃは、複数の時点における内部状態量を記憶することが、状態量記憶手段１１（図４）とは異なっている。
例えば、状態量記憶手段１１Ｃには、時刻“０”から時刻“Ｍ−１”に対応したＭ個の要素を有する配列を確保する。
【０１０９】
ここで、状態量記憶手段１１Ｃに対する書き込み又は読み出しを行う各手段（状態更新手段１２Ｃ、状態統合手段１４Ｃ、状態送信手段１５Ｃ、推定量出力手段１６Ｃ及び状態予測手段１８）は、０≦ｔ＜Ｍなる整数値ｔで示される時刻における内部状態量ＸＩ（ｔ）を参照する場合、Ｍ個の要素を有する配列のｔ番目の要素を参照すればよい。また、０≦ｔ＜Ｍなる整数値以外で時刻が示される場合は、状態量記憶手段１１Ｃに対する書き込み時には動作を行わず、読み出し時には何も読み出さない（空集合φを読み出す）ものとする。
また、状態量記憶手段１１Ｃは、Ｍ個の要素を有する配列を確保し、その各要素に時刻情報（時刻ｔ）と内部状態量ＸＩ（ｔ）とを記憶する構成としてもよい。
【０１１０】
ここで、図１０を参照して、状態量記憶手段１１Ｃに記憶されている内部状態量へのアクセス（読み出し／書き込み）手法について説明する。図１０は、状態量記憶手段の記憶内容の一例を示すデータ構造図である。図１０では、一例として、状態量記憶手段１１Ｃに１０個の要素を有する配列を確保し、アドレス０からアドレス９にそれぞれ時刻情報（時刻ｔ）と内部状態量ＸＩ（ｔ）とを対応付けて記憶している状態を示している。
まず、時刻ｔに内部状態量ＸＩ（ｔ）を書き込む手法について説明する。
ここで、時刻ｔを配列の要素数Ｔで除算した場合の剰余をアドレスＡ（Ａ番目のアドレス）とする。すなわち、アドレスＡは、以下の式（１８）により算出される。
【０１１１】
【数１８】
【０１１２】
ここで、演算子％は剰余を示し、ｔ％ＭはｔをＭで除算した剰余を表すものとする。
このように算出されたアドレスＡに対して、時刻ｔと内部状態量ＸＩ（ｔ）との対を書き込むこととする。
例えば、図１０に示すように、時刻ｔ＝“１０２５”に内部状態量ＸＩ（１０２５）を書き込む場合、前記式（１８）により、アドレスＡ＝“１０２５％１０”＝“５”を得る。そこで、アドレスＡ＝“５”に対し、ｔ＝“１０２５”なる時刻情報と、ｔ＝“１０２５”における内部状態量ＸＩ（１０２５）との対を書き込む。
【０１１３】
次に、時刻ｔに内部状態量ＸＩ（ｔ）を読み出す手法について説明する。なお、アドレスは、前記式（１８）により算出される。
ここで、アドレスＡ（Ａ番目のアドレス）には、時刻τが時刻情報として書き込まれているとする。もし、τ＝ｔであれば、アドレスＡに書き込まれている状態量は所望の時刻ｔにおける内部状態量ＸＩ（ｔ）であるため、そのまま内部状態量ＸＩ（ｔ）を読み出せばよい。もし、τ≠ｔであれば、アドレスＡに書き込まれている状態量は所望の時刻ｔにおけるものではないため、例えば、読み出しデータを空集合φとする。
【０１１４】
例えば、図１０に示すように、時刻ｔ＝“１０３２”の内部状態量ＸＩ（１０３２）を読み出す場合、前記式（１８）により、アドレスＡ＝“１０３２％１０”＝“２”を得る。そして、アドレスＡ＝“２”に書き込まれている時刻情報（時刻τ）を読み出す。この場合、τ＝“１０３２”である。すなわち、τ＝ｔ＝“１０３２”であるため、アドレスＡ＝“２”に書き込まれている状態量は所望の時刻ｔ＝“１０３２”における内部状態量ＸＩ（１０３２）であると判断することができ、このアドレスＡ＝“２”に書き込まれている内部状態量ＸＩ（１０３２）を読み出せばよい。
【０１１５】
また、時刻ｔ＝“１０３３”の内部状態量ＸＩ（１０３３）を読み出す場合、前記式（１８）により、アドレスＡ＝“１０３３％１０”＝“３”を得る。そして、アドレスＡ＝“３”に書き込まれている時刻情報（時刻τ）を読み出す。この場合、τ＝“１０２３”である。すなわち、τ≠ｔ＝“１０３３”であるため、アドレスＡ＝“３”に書き込まれている状態量は所望の時刻ｔ＝“１０３３”における内部状態量ＸＩ（１０３３）ではないと判断することができる。そこで、読み出しデータを空集合φとする。
これによって、時刻ｔの内部状態量ＸＩ（ｔ）の読み出し／書き込みにおける時刻のずれを防止することができる。
図９に戻って、状態推定装置１０Ｃの構成について説明を続ける。
【０１１６】
状態更新手段１２Ｃは、外部から入力される時刻ｔＹの観測量Ｙ（ｔＹ）によって、状態量記憶手段１１Ｃに記憶されている内部状態量ＸＩ（ｔＹ）を更新するものである。
状態受信手段１３Ｃは、ネットワークＮに接続された他の状態推定装置が保持し、当該他の状態推定装置から送信される時刻ｔＲの受信状態量ＸＲ（ｔＲ）を受信し、状態統合手段１４Ｃに出力するものである。
【０１１７】
状態統合手段１４Ｃは、状態受信手段１３Ｃで受信した時刻ｔＲの受信状態量ＸＲ（ｔＲ）と、状態量記憶手段１１Ｃに記憶されている時刻ｔＲの内部状態量ＸＩ（ｔＲ）とを統合し時刻ｔＲの統合状態量ＸＪ（ｔＲ）を生成するものである。
状態送信手段１５Ｃは、状態量記憶手段１１Ｃに記憶されている、ある時刻ｔＳの内部状態量ＸＩ（ｔＳ）を読み出して、ネットワークＮに接続された他の状態推定装置に送信するものである。
【０１１８】
推定量出力手段１６Ｃは、状態量記憶手段１１Ｃに記憶されている、ある時刻ｔＥの内部状態量ＸＩ（ｔＥ）を読み出して、推定量Ｅ（ｔＥ）として外部に出力するものである。
なお、状態更新手段１２Ｃ、状態受信手段１３Ｃ、状態統合手段１４Ｃ、状態送信手段１５Ｃ及び推定量出力手段１６Ｃは、時刻（時刻ｔＹ、時刻ｔＲ、時刻ｔＳ及び時刻ｔＥ）に基づいて動作し、内部状態量ＸＩ（ｔ）の更新等を行うとともに、その時刻に同期した制御信号を状態予測手段１８に出力することを除き、図４で説明した状態更新手段１２、状態受信手段１３、状態統合手段１４、状態送信手段１５及び推定量出力手段１６と同一の機能を有しているため、各機能については説明を省略する。
【０１１９】
状態予測手段１８は、制御信号で通知されるタイミングに基づいて、ある時刻ｔＰにおける状態量（予測状態量）ＸＰ（ｔＰ）を予測するものである。
ここで、状態予測手段１８は、ある時刻ｔＯにおける内部状態量ＸＩ（ｔＯ）と、ある時刻ｔＰにおける内部状態量ＸＩ（ｔＰ）とを状態量記憶手段１１Ｃから読み出し、予測処理を行うことで、時刻ｔＰにおける予測状態量ＸＰ（ｔＰ）を予測し、内部状態量ＸＩ（ｔＰ）として状態量記憶手段１１Ｃに書き込むこととする。
この状態予測手段１８は、まず、時刻ｔＯから時刻ｔＰへの状態遷移を表す予め定めた変換則Ｆ（ｔＯ，ｔＰ；Ｘ）を用いて、以下の式（１９）により、状態遷移後の状態量（遷移後状態量ＸＱ（ｔＰ））を算出する。
【０１２０】
【数１９】
【０１２１】
また、状態予測手段１８は、算出した遷移後状態量ＸＱ（ｔＰ）と、時刻ｔＯにおける内部状態量ＸＩ（ｔＯ）とに基づいて、時刻ｔＰにおける予測状態量ＸＰ（ｔＰ）を算出する。なお、遷移後状態量ＸＱ（ｔＰ）と内部状態量ＸＩ（ｔＯ）とから予測状態量ＸＰ（ｔＰ）を算出する手法は、図４で説明した状態統合手段１４と同様の手法を用いることができる。この場合、状態統合手段１４における内部状態量ＸＩ、受信状態量ＸＲ及び統合状態量ＸＪを、それぞれ内部状態量ＸＩ（ｔＯ）、遷移後状態量ＸＱ（ｔＰ）及び予測状態量ＸＰ（ｔＰ）と読み替えればよく、説明を省略する。
【０１２２】
なお、状態予測手段１８は、遷移後状態量ＸＱ（ｔＰ）をそのまま予測状態量ＸＰ（ｔＰ）とすることとしてもよく、この場合は、状態予測手段１８は、状態量記憶手段１１Ｃから、時刻ｔＯにおける内部状態量ＸＩ（ｔＯ）を読み出す必要はない。
【０１２３】
また、状態予測手段１８は、例えば、状態統合手段１４Ｃから制御信号として時刻ｔＲが通知された場合、時刻ｔＯ＝“ｔＲ”、時刻ｔＰ＝“ｔＲ＋１”として動作する。また、状態予測手段１８は、時刻ｔＯ及び時刻ｔＰを、（ｔＯ，ｔＰ）＝（ｔＲ，ｔＲ±１），（ｔＲ，ｔＲ±２），…，（ｔＲ，ｔＲ±ＴＲ）のように、時刻ｔＲの前後の時間ＴＲにわたって適用することとしてもよい。
あるいは、状態予測手段１８は、状態更新手段１２Ｃの動作の直後、状態受信手段１３Ｃの動作の前後、状態送信手段１５Ｃの動作の直前、推定量出力手段１６Ｃの動作の直前等、状態統合手段１４Ｃと同様に、制御信号に基づいて動作を行うこととしてもよい。
また、状態予測手段１８は、制御信号によらず、タイマ（図示せず）等に基づいて、時刻ｔＯ及び時刻ｔＰを設定することとしてもよい。
以下、数式を用いて、前記式（１９）における遷移後の状態量の演算について、さらに説明を付加することにする。
【０１２４】
（状態量がベクトルの場合）
まず、状態量（内部状態量ＸＩ）として、一つの状態ベクトルを採用する場合における遷移後状態量の演算について説明する。
この場合、状態予測手段１８は、前記式（１９）の変換則Ｆとして、単位時間あたりの状態遷移行列Φを用いることができる。
すなわち、内部状態量ＸＩ（ｔＯ）を表す状態ベクトルをｘＩ（ｔＯ）としたとき、状態遷移後の状態量である遷移後状態量ＸＱ（ｔＰ）を表す状態ベクトルｘＱ（ｔＰ）は、状態遷移行列Φを用いて、以下の式（２０）により算出することができる。
【０１２５】
【数２０】
【０１２６】
このとき、遷移後状態量ＸＱ（ｔＰ）は、ＸＱ（ｔＰ）＝｛ｘＱ（ｔＰ）｝となる。
【０１２７】
（状態量がベクトル及び共分散行列の場合）
次に、状態量（内部状態量ＸＩ）として、状態ベクトル及び状態共分散行列を採用する場合における遷移後状態量の演算について説明する。
この場合、状態予測手段１８は、前記式（１９）の変換則Ｆとして、単位時間あたりの状態遷移行列Φ及び単位時間あたりのプロセス雑音の共分散行列Ｑを用いることができる。
すなわち、内部状態量ＸＩ（ｔＯ）を表す状態ベクトルをｘＩ（ｔＯ）、その状態共分散行列をＰＩ（ｔＯ）としたとき、遷移後状態量ＸＱ（ｔＰ）を表す状態ベクトルｘＱ（ｔＰ）、その状態共分散行列ＰＱ（ｔＰ）は、単位時間あたりの状態遷移行列Φ及び単位時間あたりのプロセス雑音の共分散行列Ｑを用いて、以下の式（２１）により算出することができる。なお、上付き添え字Ｔは転置を表している。
【０１２８】
【数２１】
【０１２９】
このとき、遷移後状態量ＸＱ（ｔＰ）は、ＸＱ（ｔＰ）＝｛ｘＱ（ｔＰ），ＰＱ（ｔＰ）｝となる。
【０１３０】
（状態量がパーティクルの場合）
次に、状態量（内部状態量ＸＩ）としてパーティクルを採用する場合における遷移後状態量の演算について説明する。
この場合、状態予測手段１８は、前記式（１９）の変換則Ｆとして、単位時間あたりの状態遷移行列Φ及び単位時間あたりのプロセス雑音の共分散行列Ｑを用いることができる。
すなわち、内部状態量ＸＩ（ｔＯ）に含まれる状態ベクトルの数をＮ個、各々の状態ベクトルをｘＩ（ｉ）（ｔＯ）（ただし、ｉ＝１，２，…，Ｎ）としたとき、遷移後状態量ＸＱ（ｔＰ）に含まれる状態ベクトルｘＱ（ｉ）（ｔＰ）は、単位時間あたりの状態遷移行列Φ及び単位時間あたりのプロセス雑音の共分散行列Ｑを用いて、以下の式（２２）により算出することができる。なお、ｎはノイズ（雑音）、Ｅ［］は期待値、上付き添え字Ｔは転置を表している。
【０１３１】
【数２２】
【０１３２】
このとき、遷移後状態量ＸＱ（ｔＰ）は、ＸＱ（ｔＰ）＝｛ｘＱ（１）（ｔＰ），ｘＱ（２）（ｔＰ），…，ｘＱ（Ｎ）（ｔＰ）｝となる。
以上、状態推定装置１０Ｃの構成について説明したが、本発明はこの構成に限定されるものではない。例えば、図１１に示すように、状態更新手段１２Ｃを外部の観測装置２０Ｃに備え、状態更新手段１２Ｃを状態推定装置１０Ｃから省略し、データの入出力のみを行う入出力手段１７Ｃを備えた状態推定装置１０Ｄの構成としてもよい。この場合、センサ等の位置観測手段（図示せず）により観測した結果である観測対象の位置（観測量）が時刻ｔＹに対応付けられて、観測装置２０の状態更新手段１２Ｃから、状態推定装置１０Ｄに入力される。
【０１３３】
また、状態推定装置１０Ｃ（１０Ｄ）は、一般的なコンピュータを、前記した各手段として機能させることで実現することができる。このプログラム（状態推定プログラム）は、通信回線を介して配布することも可能であるし、ＣＤ−ＲＯＭ等の記録媒体に書き込んで配布することも可能である。
【０１３４】
以上説明したように、状態推定装置１０Ｃ（１０Ｄ）は、他の状態推定装置で推定した状態量と、当該状態推定装置１０Ｃで推定した状態量とを統合し、観測対象の状態を推定することができる。これによって、状態推定装置１０Ｃは、他の状態推定装置と協働し、状態の推定を分散して行うことができる。また、例えば、他の状態推定装置に状態推定装置１０Ｃと共通の状態量を記憶させておくことで、状態推定装置１０Ｃに障害が発生した場合であっても、状態の推定を継続することが可能になる。さらに、状態推定装置１０Ｃは、時間の経過に伴った予測を行うため、状態推定の精度を高めることができる。
なお、状態推定装置１０Ｃの動作は、複数の時点における状態量（内部状態量）を保持して状態量の予測を行うことを除き、図６〜図８で説明した状態推定装置１０（図４参照）と、基本的な動作は同一であるため説明を省略する。
【図面の簡単な説明】
【０１３５】
【図１】本発明に係る統合状態推定システムの全体の構成を示す全体構成図である。
【図２】統合状態推定システムの具体例であるカーナビゲーションシステムの構成を示す全体構成図である。
【図３】本発明に係る統合状態推定装置の構成を示す構成図である。
【図４】本発明の第１実施形態に係る状態推定装置の内部構成を示すブロック構成図である。
【図５】本発明の第１実施形態に係る状態推定装置の変形例を示すブロック構成図である。
【図６】本発明に係る状態推定装置の全体動作を示すフローチャートである。
【図７】本発明に係る状態推定装置における状態更新手段の動作を示すフローチャートである。
【図８】本発明に係る状態推定装置における状態統合手段の動作を示すフローチャートである。
【図９】本発明の第２実施形態に係る状態推定装置の内部構成を示すブロック構成図である。
【図１０】状態量記憶手段の記憶内容の一例を示すデータ構造図である。
【図１１】本発明の第２実施形態に係る状態推定装置の変形例を示すブロック構成図である。
【符号の説明】
【０１３６】
Ｓ 統合状態推定システム
１ 統合状態推定装置
１０ 状態推定装置
１１ 状態量記憶手段
１２ 状態更新手段
１３ 状態受信手段
１４ 状態統合手段
１５ 状態送信手段
１６ 推定量出力手段
１７ 入出力手段
１８ 状態予測手段
【技術分野】
【０００１】
本発明は、観測対象の観測量に基づいて、当該観測対象の状態量を推定する状態推定装置及びそのプログラム、並びに、統合状態推定システム及び統合状態推定装置に関する。
【背景技術】
【０００２】
従来、観測対象の観測量から状態量を推定する手法として、カルマンフィルタによる推定手法が知られている。このカルマンフィルタは、複数の観測量を観測ベクトルとして一つにまとめ、観測時に混入する観測雑音の共分散行列とともにフィルタリング処理を行うことで、複数の観測量を融合して状態量を推定するものである。
【０００３】
また、他の手法としては、コンデンセーション法をはじめとする、パーティクルフィルタによる推定手法が知られている（例えば、非特許文献１参照）。このパーティクルフィルタは、有限のパーティクル（複数のベクトルの集合）によって確率密度関数を表現し、この確率密度関数を用いて、時系列に入力される観測量から状態量を推定するものである。このパーティクルフィルタによれば、カルマンフィルタでは推定することができない非ガウス性雑音が混入する場合であっても、任意の非ガウシアンな確率密度関数によって、状態量を推定することができる。
さらに、このパーティクルフィルタを、推定量の分散によって適応的に切り替える手法も提案されている（特許文献１参照）。
【非特許文献１】M.Isard and A.Blake, CONDENSATION - Conditional Density Propagation for Visual Tracking, International Journal of Computer Vision, vol.29, no.1, pp.5-28(1998)
【特許文献１】特開２００５−４４３５２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかし、前記した従来の状態推定手法によれば、複数の観測量をまとめて処理するため、装置内において処理が集中してしまう。このため、従来の手法を適用した状態推定装置において、本手法による処理は高速化を阻害する要因となっている。
また、従来の状態推定手法を適用した状態推定装置においては、処理が集中しているため、当該状態推定装置が故障した場合、状態推定動作が停止してしまう。このため、状態推定を安定して行うためには、同一の状態推定装置を複数動作させなければならず、システム全体として、高価なものとなってしまうという問題がある。
また、従来の状態推定手法を適用した状態推定装置では、予め定めた観測対象の観測量に基づいて状態を推定するため、観測対象や推定を行う対象を変更することが容易ではなく、汎用性に欠けるといった問題がある。
【０００５】
本発明は、以上のような問題点に鑑みてなされたものであり、複数の状態推定動作を並列動作させることで推定処理の高速性、信頼性、汎用性を高めた状態推定装置及びそのプログラム、並びに、統合状態推定システム及び統合状態推定装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明は、前記目的を達成するために創案されたものであり、まず、請求項１に記載の状態推定装置は、観測対象の観測量に基づいて、当該観測対象の状態を示す状態量を推定する状態推定装置において、状態量記憶手段と、状態更新手段と、状態受信手段と、状態統合手段と、状態送信手段とを備える構成とした。
【０００７】
かかる構成において、状態推定装置は、状態更新手段によって、外部から入力された観測量により、状態量記憶手段に記憶する状態量（内部状態量）を更新する。これによって、観測量に基づいて、逐次内部状態量が更新されることになる。なお、状態量の更新は、一般的なカルマンフィルタや拡張カルマンフィルタの観測更新方程式により更新することとしてもよいし、シーケンシャルモンテカルロ法により状態量を状態ベクトルの集合として更新することとしてもよい。
【０００８】
また、状態推定装置は、状態受信手段によって、通信路に接続された他の状態推定装置において記憶されている状態量を外部状態量として受信し、状態統合手段によって、外部状態量と状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する。これによって、状態推定装置は、他の状態推定装置において推定された状態量を取り込み、利用することができる。なお、状態量の統合とは、複数の状態推定装置に分散して記憶されている状態量を１つの状態量にまとめて加工することであって、例えば、複数の状態量の平均値をとることで新たな状態量としてもよいし、予め定めた確率分布となるように、それぞれの状態量を選択することで新たな状態量としてもよい。
また、状態推定装置は、状態送信手段によって、状態量記憶手段に記憶されている内部状態量を他の状態推定手段に送信する。これによって、状態推定装置は、自身が推定した状態量を、他の状態推定装置に利用させることが可能になる。
【０００９】
また、請求項２に記載の状態推定装置は、請求項１に記載の状態推定装置において、前記状態量記憶手段が、複数の前記内部状態量を時刻に対応付けて記憶する構成とした。
【００１０】
かかる構成において、状態推定装置は、内部状態量を時刻に対応付けて状態量記憶手段に記憶することで、他の状態推定装置から送信される状態量と内部で記憶する状態量とを時刻によって対応付けることができる。
【００１１】
さらに、請求項３に記載の状態推定装置は、請求項２に記載の状態推定装置において、状態予測手段を備える構成とした。
【００１２】
かかる構成において、状態推定装置は、状態予測手段によって、状態量記憶手段に記憶されている複数の内部状態量に基づいて、ある時刻における状態量を予測する。なお、状態量の予測については、一般的な状態遷移方程式あるいは現在の状態と未来の状態との間の遷移確率により予測することができる。
【００１３】
また、請求項４に記載の統合状態推定システムは、観測対象の観測量に基づいて、通信路に接続された複数の状態推定装置により、当該観測対象の状態を示す状態量を統合して推定する統合状態推定システムであって、状態推定装置が、状態量記憶手段と、状態受信手段と、状態統合手段と、状態送信手段とを備え、前記複数の状態推定装置の少なくとも一つが状態更新手段を備え、さらに、前記複数の状態推定装置の少なくとも一つが推定量出力手段を備える構成とした。
【００１４】
かかる構成において、統合状態推定システムは、当該システムを構成する状態推定装置が、状態量記憶手段に観測対象の状態量を内部状態量として記憶する。そして、状態推定装置は、状態受信手段によって、通信路に接続された他の状態推定装置において記憶されている状態量を外部状態量として受信し、状態統合手段によって、外部状態量と状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する。また、状態推定装置は、状態送信手段によって、状態量記憶手段に記憶されている内部状態量を他の状態推定装置に送信する。このように、統合状態推定装置システムは、各状態推定装置において、状態量を送受信することで、統合することができる。
【００１５】
なお、状態量の更新は、複数の状態推定装置の少なくとも一つが、状態更新手段によって、外部から入力された観測量により、状態量記憶手段に記憶する状態量（内部状態量）を更新することで行う。この状態更新手段の数は、観測量の数に基づいて決定されるものであって、少なくとも観測量の数が一つである場合は、複数の状態推定装置の中の一つに備えられればよい。
そして、複数の状態推定装置の少なくとも一つが、推定量出力手段によって、状態量記憶手段に記憶されている内部状態量を推定量として出力する。
【００１６】
さらに、請求項５に記載の統合状態推定装置は、観測対象の観測量に基づいて、当該観測対象の状態を示す状態量を統合して推定する統合状態推定装置であって、前記状態量を推定する状態推定手段を複数備え、前記状態推定手段が、状態量記憶手段と、状態受信手段と、状態統合手段と、状態送信手段とを備え、前記複数の状態推定手段の少なくとも一つが状態更新手段を備え、さらに、前記複数の状態推定手段の少なくとも一つが推定量出力手段を備える構成とした。
【００１７】
かかる構成において、統合状態推定装置は、状態推定手段が状態量記憶手段に観測対象の状態量を内部状態量として記憶する。そして、状態推定手段は、状態受信手段によって、通信路に接続された他の状態推定手段において記憶されている状態量を外部状態量として受信し、状態統合手段によって、外部状態量と状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する。また、状態推定手段は、状態送信手段によって、状態量記憶手段に記憶されている内部状態量を他の状態推定装置に送信する。このように、統合状態推定装置は、各状態推定手段において、状態量を送受信することで、統合することができる。
【００１８】
なお、状態量の更新は、複数の状態推定手段の少なくとも一つが、状態更新手段によって、外部から入力された観測量により、状態量記憶手段に記憶する状態量（内部状態量）を更新することで行う。そして、複数の状態推定手段の少なくとも一つが、推定量出力手段によって、状態量記憶手段に記憶されている内部状態量を推定量として出力する。
【００１９】
また、請求項６に記載の状態推定プログラムは、観測対象の観測量に基づいて、通信路に接続された複数の状態推定装置により、当該観測対象の状態を示す状態量を統合して推定するために、前記状態推定装置におけるコンピュータを、状態更新手段、状態受信手段、状態統合手段、状態送信手段、推定量出力手段として機能させる構成とした。
【００２０】
かかる構成において、状態推定プログラムは、状態更新手段によって、外部から入力された観測量により、状態量記憶手段に記憶する状態量（内部状態量）を更新する。そして、状態推定プログラムは、状態受信手段によって、通信路に接続された他の状態推定手段において記憶されている状態量を外部状態量として受信し、状態統合手段によって、外部状態量と状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する。
また、状態推定プログラムは、状態送信手段によって、状態量記憶手段に記憶されている内部状態量を他の状態推定手段に送信する。また、状態推定プログラムは、推定量出力手段によって、状態量記憶手段に記憶されている内部状態量を推定量として出力する。
【発明の効果】
【００２１】
本発明は、以下に示す優れた効果を奏するものである。
請求項１、請求項４又は請求項６に記載の発明によれば、複数の状態推定装置において推定される状態量を統合して観測対象の状態を推定することができる。これによって、推定処理を複数の状態推定装置に分散させることで、並列動作が可能となり、推定動作の高速化を図ることができる。また、本発明によれば、複数の状態推定装置のいずれかに障害が発生した場合であっても、他の状態推定装置における状態量を統合することで、状態推定のサービスを継続することができる。また、本発明によれば、状態推定装置単位で交換等が可能であるため、新しい観測量に基づいて状態を推定する場合であっても、その追加、変更が容易であり、システム全体の汎用性を高めることができる。
【００２２】
請求項２に記載の発明によれば、他の状態推定装置から送信される状態量と内部で記憶している状態量とを時刻によって対応付けているため、状態量の時刻同期をとることができる。これによって、状態推定装置間で推定する状態の時刻を一致させることができるため、時刻のずれがなくなり、状態推定の精度を高めることができる。
【００２３】
請求項３に記載の発明によれば、観測量が入力されない時点においても、状態を推定し、その推定量を出力することが可能になり、安定して状態推定を行うことができる。
請求項５に記載の発明によれば、複数の状態推定手段において推定される状態量を統合して観測対象の状態を推定することができる。これによって、並列動作が可能になり、推定動作の高速化を図ることができる。
【発明を実施するための最良の形態】
【００２４】
以下、本発明の実施の形態について図面を参照して説明する。
［統合状態推定システムの概要］
最初に、図１を参照して、統合状態推定システムの構成について説明する。図１は、本発明に係る統合状態推定システムの全体の構成を示す全体構成図である。
図１に示すように、統合状態推定システムＳは、通信路（ネットワークＮ）を介して、複数の状態推定装置１０を接続し、各状態推定装置１０で推定される観測対象の状態（状態量）を統合して、所望の状態量を推定し、推定値として出力するものである。
【００２５】
状態推定装置１０は、外部から入力される観測量Ｙに基づいて、観測対象の状態（状態量Ｘ）を推定するとともに、他の状態推定装置が推定した状態量と統合するものである。なお、状態推定装置１０は、いずれか一つが観測量Ｙを入力すればよく、また、いずれか一つが推定量Ｅを出力すればよい。例えば、状態推定装置１０３のように外部に推定量Ｅを出力しない形態、状態推定装置１０４のように外部から観測量Ｙを入力しない形態、状態推定装置１０５のように観測量Ｙの入力や推定量Ｅの出力を行わない形態であってもよい。
【００２６】
なお、ここで、観測対象とは、動き、変動等の状態の変化を伴うもの、あるいは、事象であって、動きを伴う人物、車両等や、変動を伴う株価等がその対象となりうる。例えば、状態推定装置１０は、人物を観測対象として、複数の位置からカメラで撮影した画像における人物の位置を観測量とすることで、人物の３次元位置や、速度等を推定することができる。
【００２７】
（状態量について）
ここで、統合状態推定システムＳにおいて使用する状態量について説明する。
状態量は、観測対象の動き、変動等の状態を示すデータ量であって、例えば、観測対象の速度等である。この状態量は、例えば、一つのベクトル（以下、状態ベクトルという）により表現することができる。例えば、物体の３次元位置及び３次元速度からなる６次元ベクトル量、あるいは、天体の軌道６要素等のベクトル量を状態量とすることができる。
【００２８】
また、例えば、状態量は、一つのベクトルと一つの行列との組により表現することができる。この場合、ベクトルは、先に説明した状態ベクトルと同様のものである。一方、行列は、状態ベクトルに含まれる誤差を表す共分散行列（以下、状態共分散行列という）により表現することができる。
【００２９】
さらに、例えば、状態量は、複数のベクトルの集合（以下、パーティクルという）として表現することができる。なお、パーティクルを構成する個々のベクトルは、先に説明した状態ベクトルと同様のものである。例えば、個々のベクトルとして、３次元位置及び３次元速度からなる６次元ベクトルを用いることができる。
【００３０】
（観測量について）
次に、統合状態推定システムＳに入力される観測量について説明する。
観測量は、観測対象を観測した結果のデータ量であって、例えば、観測対象の２次元位置等である。この観測量は、例えば、一つのベクトル（以下、観測ベクトルという）により表現することができる。なお、複数のセンサ等によって多次元の観測結果が得られる場合は、各センサ等から得られるデータを行ベクトルとして多重化することで、一つのベクトルとして表現することができる。
【００３１】
また、例えば、観測量は、観測ベクトルに加え、観測ベクトルの誤差や信頼度を表す量を付加することもできる。例えば、観測ベクトルの誤差を、共分散行列（以下、観測共分散行列という）により表現することができる。
さらに、例えば、観測量は、先に説明した状態ベクトルに対する確率密度として定義することとしてもよい。
【００３２】
なお、状態ベクトル又は観測ベクトルにおいて、スカラ量を扱う場合は、これを１次元のベクトル量とみなすこととする。また、行列やテンソル量を扱う場合は、その各要素を予め定めた一定の規則により１次元に並べたものをもってベクトル量とみなすものとする。
【００３３】
（システム例）
ここで、図２を参照して、統合状態推定システムの具体例であるカーナビゲーションシステムについて説明する。図２は、統合状態推定システムの具体例であるカーナビゲーションシステムの構成を示す全体構成図である。
カーナビゲーションシステムＳＮは、状態推定装置１０１〜１０３を、通信路（ネットワークＮ）を介して接続した構成となっている。このカーナビゲーションシステムＳＮは、自動車の３次元位置（例えば、緯度、経度及び高度）、速度ベクトル、姿勢角（例えば、ロール角、ピッチ角及びヨー角）、並びに、角速度ベクトルを状態量Ｘとする。
【００３４】
ここで、状態推定装置１０１は、ＧＰＳ（Global Positioning System）受信機２が観測した時刻データを観測量Ｙ１として入力する。また、状態推定装置１０２は、車速パルス計測器３が観測した自動車車輪の車速パルスの時間間隔データを観測量Ｙ２として入力する。さらに、状態推定装置１０３は、ジャイロセンサ４が観測した角加速度データを観測量Ｙ３として入力する。
そして、例えば、状態推定装置１０１が、状態量Ｘから自動車の位置情報のみを抽出し、推定量Ｅとして出力する。
【００３５】
このように、状態推定装置１０１〜１０３を通信路（ネットワークＮ）に接続することで、ＧＰＳ受信機２、車速パルス計測器３、ジャイロセンサ４の各観測手段により観測された観測量Ｙ１，Ｙ２，Ｙ３に基づく状態量Ｘが統合され、状態推定装置１０１において、自動車の位置を推定し、推定量Ｅとして出力することができる。
【００３６】
以上、説明したように、ここでは、統合状態推定システムＳ（図１参照）やカーナビゲーションシステムＳＮ（図２参照）を、ネットワークＮに接続した複数の状態推定装置１０で構成したが、各状態推定装置１０を、一つの手段として内部構成に組み込んで、図３に示すように、各状態推定手段１０をバスＢ等の通信路を介して接続することで、統合状態推定装置１として構成することとしてもよい。
すなわち、状態推定装置（状態推定手段）を接続する通信路は、ＬＡＮケーブル、電話線、シリアルケーブル等のメタル線路や、光ファイバ、赤外線、無線回線、基板内の配線、基板間の配線、ＩＣチップ内の配線等、任意の伝送媒体を用いることができる。また、通信路は、途中、交換機、ルータ、ハブ、リピータ等の中継装置を介しても構わない。
【００３７】
以下、統合状態推定システムＳを構成する状態推定装置１０の内部構成について詳細に説明を行うが、図３に示した統合状態推定装置１における状態推定手段１０においても、内部構成は同じ構成で実現することができる。
【００３８】
［第１実施形態：状態推定装置（状態推定手段）の構成］
まず、図４を参照して、状態推定装置１０の構成について説明する。図４は、本発明の第１実施形態に係る状態推定装置の内部構成を示すブロック構成図である。
状態推定装置（状態推定手段）１０は、外部から入力される観測対象の観測量に基づいて、ネットワークＮに接続した他の状態推定装置と協働して、観測対象の状態を示す状態量を推定するものである。ここでは、状態推定装置１０は、状態量記憶手段１１と、状態更新手段１２と、状態受信手段１３と、状態統合手段１４と、状態送信手段１５と、推定量出力手段１６とを備えている。
【００３９】
状態量記憶手段１１は、観測対象の状態を示す状態量のうち、当該状態推定装置１０内部で保持する状態量（内部状態量）を記憶するものであって、例えば、ＲＡＭ（Random Access Memory）等である。
この状態量記憶手段１１は、後記する状態更新手段１２や状態統合手段１４によって、状態量が読み出され、又は、書き込まれ、状態送信手段１５や推定量出力手段１６によって、読み出される。
【００４０】
状態更新手段１２は、外部から入力される観測量Ｙによって、状態量記憶手段１１に記憶されている状態量を更新するものである。ここでは、状態更新手段１２は、状態量記憶手段１１から状態量（内部状態量ＸＩ）を読み出し、観測量Ｙによって状態量を更新し、その更新された状態量（観測後状態量ＸＹ）を内部状態量ＸＩとして、状態量記憶手段１１に書き込む。
なお、状態更新手段１２は、観測量Ｙをそのまま観測後状態量ＸＹとして状態量記憶手段１１に書き込むこととしてもよいが、予め観測量Ｙにおけるノイズ、誤差等がモデル化されている場合は、そのモデルに基づいて、観測後状態量ＸＹを算出し、更新することが状態推定の精度を高める上で望ましい。
以下、数式を用いて、状態更新手段１２における状態量の更新手法について説明する。
【００４１】
（状態量及び観測量がベクトルの場合）
まず、状態量（内部状態量ＸＩ）として状態ベクトルｘＩ（すなわち、ＸＩ＝｛ｘＩ｝）、観測量Ｙとして観測ベクトルｙ（すなわち、Ｙ＝｛ｙ｝）を採用する場合における状態量の更新について説明する。なお、状態ベクトルｘＩから観測ベクトルｙへの関数ｈ及びノイズ（雑音）ｎにより、状態の更新が、以下の式（１）によりモデル化できるものとする。
【００４２】
【数１】
【００４３】
ここで、関数ｈに逆関数ｈ−１が存在する場合には、適当な実数値α（望ましくは０≦
α≦１の実数値）を用いて、以下の式（２）により観測後の状態ベクトルｘＹを得ることができる。
【００４４】
【数２】
【００４５】
すなわち、観測後状態量ＸＹは、ＸＹ＝｛ｘＹ｝である。なお、状態量記憶手段１１から、状態ベクトルｘＩが読み出せなかった場合（存在しなかった場合）には、例えば、前記式（２）でαの値を“１”とした、ｘＹ＝ｈ−１（ｙ）により観測後の状態ベクトルｘＹを求めることとしてもよい。
【００４６】
（状態量及び観測量がベクトル及び共分散行列の場合）
次に、状態量（内部状態量ＸＩ）として状態ベクトルｘＩ及び状態共分散行列ＰＩ（すなわち、ＸＩ＝｛ｘＩ，ＰＩ｝）、観測量Ｙとして観測ベクトルｙと観測共分散行列Ｒ（すなわち、Ｙ＝｛ｙ，Ｒ｝）を採用する場合における状態量の更新について説明する。なお、状態ベクトルｘＩから観測ベクトルｙへの関数ｈ及びノイズ（雑音）ｎにより、状態の更新が、以下の式（３）によりモデル化することができるものとする。
【００４７】
【数３】
【００４８】
ここで、Ｅ［］は期待値、上付き添え字Ｔは転置を表している。
このとき、例えば、以下の式（４）に示す拡張カルマンフィルタの観測更新方程式によって、観測後状態量ＸＹ＝｛ｘＹ，ＰＹ｝を決定することができる。
【００４９】
【数４】
【００５０】
ここで、Ｋはフィルタ感度、Ｈは観測行列、上付き添え字Ｔは転置を表している。
なお、状態量記憶手段１１から、状態ベクトルｘＩと状態共分散行列ＰＩとが読み出せなかった場合（存在しなかった場合）には、例えば、ｘＹ＝ｈ−１（ｙ）、ＰＹ＝ｄｉａｇ｛λ１，λ２，…，λＤ｝（ただし、ｄｉａｇは対角行列、λ１，λ２，…，λＤは非負の実数、ＤはＸＹの次元とする）としてもよい。
【００５１】
（状態量がパーティクル、観測量が確率密度関数の場合）
次に、状態量（内部状態量ＸＩ）としてパーティクル、観測量Ｙとして確率密度関数を採用する場合における状態量の更新について説明する。
まず、状態量記憶手段１１から読み出される内部状態量ＸＩに含まれる状態ベクトルの数をＮ個、各々の状態ベクトルをｘ（ｉ）（ただし、ｉ＝１，２，…，Ｎ）とする。すなわち、ＸＩ＝｛ｘ（１），ｘ（２），…，ｘ（Ｎ）｝とする。
【００５２】
一方、観測量Ｙである確率密度関数は、引数を状態ベクトルｘとし、Ｙ（ｘ）のように記述することとする。
最初に、状態更新手段１２は、１≦ｉ≦Ｎなるすべてのｉに対して、観測量Ｙ（ｘ（ｉ））に基づいて、以下の式（５）により、ｉ番目の状態ベクトルが発生する確率ｐ（ｉ）を算出する。
【００５３】
【数５】
【００５４】
続いて、状態更新手段１２は、観測後状態量ＸＹに、確率ｐ（ｉ）の大きい状態ベクトルｘ（ｉ）が多く含まれ、確率ｐ（ｉ）の小さい状態ベクトルｘ（ｉ）が少なく含まれる（又は含まれない）ように、状態量を再編成（再サンプリング）する。なお、状態ベクトルｘ（ｉ）をとるサンプル数は、精度を高めるため、確率ｐ（ｉ）に比例する頻度（個数）であることが望ましく、例えば、確率ｐ（ｉ）がｐ（１）＝１／１０、ｐ（２）＝６／１０、ｐ（３）＝３／１０で、状態量の総数が１０個の場合、状態ベクトルｘ（１）を１個、状態ベクトルｘ（２）を６個、状態ベクトルｘ（３）を３個のように、状態量（パーティクル）を生成し直す。
例えば、状態更新手段１２は、以下の手順により状態量を更新する。
まず、以下の式（６）に示すように、式（５）で示した確率ｐ（ｉ）を累積した累積確率ａ（ｉ）を算出するとともに、観測後状態量ＸＹを空集合（ＸＹ＝φ）とする。
【００５５】
【数６】
【００５６】
続けて、状態更新手段１２は、以下の式（７）に示すように、０＜ｒ≦１なる一様乱数ｒを生成する。
【００５７】
【数７】
【００５８】
さらに、状態更新手段１２は、以下の式（８）に示すように、累積確率ａ（ｉ）≧ｒを満たし、かつ、累積確率ａ（ｉ）が最小となる配列引数ｉを探索し、これをｊとおく。
【００５９】
【数８】
【００６０】
そして、状態更新手段１２は、以下の式（９）に示すように、観測後状態量ＸＹに状態ベクトルｘ（ｊ）を含める。
【００６１】
【数９】
【００６２】
そして、状態更新手段１２は、前記した式（７）〜式（９）の処理を所定回数（例えば、内部状態量ＸＩに含まれる状態ベクトルの数と同じＮ回）繰り返す。これによって、観測後状態量ＸＹにおいて、パーティクルの頻度によって、確率密度関数が近似表現されることになる。
なお、状態量記憶手段１１から、内部状態量ＸＩが読み出せなかった場合（存在しなかった場合）には、例えば、状態更新手段１２は、状態ベクトルを乱数によりＮ個生成し、これらをまとめたものを観測後状態量ＸＹとする。また、このときの乱数は、確率密度関数Ｙ（ｘ）に従うこととしてもよい。
図４に戻って、状態推定装置１０の構成について説明を続ける。
【００６３】
状態受信手段１３は、ネットワークＮに接続された他の状態推定装置が保持し、当該他の状態推定装置から送信される状態量（受信状態量ＸＲ；外部状態量）を受信するものである。この受信した受信状態量ＸＲは、状態統合手段１４に出力される。
ここで、他の状態推定装置は、状態量（受信状態量ＸＲ）を含んだメッセージを、ネットワークＮを経由して送信する。なお、このメッセージには、送信元に関する情報（例えば、ＩＰアドレス等）を含めることとしてもよい。
【００６４】
状態統合手段１４は、状態受信手段１３で受信した状態量（受信状態量ＸＲ）と、状態量記憶手段１１に記憶されている状態量（内部状態量ＸＩ）とを統合し統合状態量ＸＪを生成するものである。この統合状態量ＸＪは、内部状態量ＸＩとして状態量記憶手段１１に記憶され、内部状態量ＸＩが更新される。
なお、状態統合手段１４は、状態量記憶手段１１から内部状態量ＸＩが読み出せなかった場合（存在しなかった場合）には、受信した状態量（受信状態量ＸＲ）をそのまま統合状態量ＸＪとする。
以下、数式を用いて、状態統合手段１４における状態量の統合手法について説明する。
【００６５】
（状態量がベクトルの場合）
まず、状態量（内部状態量ＸＩ及び受信状態量ＸＲ）として、一つの状態ベクトルを採用する場合における状態量の統合について説明する。
状態量として状態ベクトルを採用する場合、状態統合手段１４は、内部状態量ＸＩの状態ベクトルをｘＩ、受信状態量ＸＲの状態ベクトルをｘＲとし、以下の式（１０）により統合後の状態ベクトルｘＪを算出する。
【００６６】
【数１０】
【００６７】
ここで、βは適当な実数値とし、望ましくは０≦β≦１の値をとるものとする。
なお、統合状態量ＸＪは、ＸＪ＝｛ｘＪ｝である。
【００６８】
（状態量がベクトル及び共分散行列の場合）
次に、状態量（内部状態量ＸＩ及び受信状態量ＸＲ）として、状態ベクトル及び状態共分散行列を採用する場合における状態量の統合について説明する。
状態量として、状態ベクトル及び状態共分散行列を採用する場合、状態統合手段１４は、内部状態量ＸＩの状態ベクトルｘＩ及び状態共分散行列ＰＩと、受信状態量ＸＲの状態ベクトルｘＲ及び状態共分散行列ＰＲとに基づいて、統合状態量ＸＪの状態ベクトルｘＪ及び状態共分散行列ＰＪを以下に示す手順により算出する。
まず、状態統合手段１４は、以下の式（１１）に示すように、状態共分散行列ＰＩ及び状態共分散行列ＰＲのｉ行ｉ列成分を抽出し、それぞれｖＩ（ｉ）及びｖＲ（ｉ）とする（ただし、ｉ＝１，２，…，Ｄ；Ｄは状態ベクトルの次元数）。
【００６９】
【数１１】
【００７０】
続いて、状態統合手段１４は、以下の式（１２）に示すように、状態ベクトルｘＩ及び状態ベクトルｘＲのｉ行成分を抽出し、それぞれｘＩ（ｉ）及びｘＲ（ｉ）とする。
【００７１】
【数１２】
【００７２】
そして、状態統合手段１４は、前記式（１１）で抽出したｖＩ（ｉ）及びｖＲ（ｉ）の大小に応じて、スカラ量ｘＴ（ｉ）を、以下に示す式（１３）により決定する。
【００７３】
【数１３】
【００７４】
そして、状態統合手段１４は、この式（１３）で決定されたスカラ量ｘＴ（ｉ）を、以下の式（１４）に示すように、配列引数ｉについて配列することで、状態ベクトルｘＪを構成する。
【００７５】
【数１４】
【００７６】
一方、状態統合手段１４は、状態共分散行列ＰＪを、以下の式（１５）に示すように、前記式（１１）で抽出したｖＩ（ｉ）及びｖＲ（ｉ）の小さい方を配列引数ｉについて並べた対角行列とする。
【００７７】
【数１５】
【００７８】
なお、統合状態量ＸＪは、ＸＪ＝｛ｘＪ，ＰＪ｝である。
【００７９】
（状態量がパーティクルの場合）
次に、状態量（内部状態量ＸＩ）としてパーティクルを採用する場合における状態量の統合について説明する。
状態量として、パーティクルを採用する場合、状態統合手段１４は、内部状態量ＸＩ及び受信状態量ＸＲの和集合（ＸＩ∪ＸＲ）に含まれる状態ベクトル“２Ｎ”個の中から、確率的にＮ´個の状態ベクトルを選択し、その選択されたＮ´個の状態ベクトルを統合状態量ＸＪとする。なお、統合状態量ＸＪに含まれる状態ベクトルの個数Ｎ´は、内部状態量ＸＩの個数Ｎと同数であってもよい。
【００８０】
ここでは、内部状態量ＸＩ及び受信状態量ＸＲに含まれる状態ベクトルの数をＮ個とし、各々の状態ベクトルをｘＩ（ｉ）及びｘＲ（ｉ）（ただし、ｉ＝１，２，…，Ｎ）とする。すなわち、ＸＩ＝｛ｘＩ（１），ｘＩ（２），…，ｘＩ（Ｎ）｝、ＸＲ＝｛ｘＲ（１），ｘＲ（２），…，ｘＲ（Ｎ）｝とする。
まず、状態統合手段１４は、統合状態量ＸＪを空集合（ＸＪ＝φ）とする。
続けて、状態統合手段１４は、以下の式（１６）に示すように、０＜ｒ≦１なる一様乱数ｒを生成する。
【００８１】
【数１６】
【００８２】
そして、状態統合手段１４は、一様乱数ｒが予め定めた定数（閾値γ〔０＜γ≦１の実数値〕）よりも大きいか小さいかによって、以下の式（１７）に示すように、状態ベクトルｘＩ（ｉ）又はｘＲ（ｉ）を、統合状態量ＸＪに含める。
【００８３】
【数１７】
【００８４】
そして、状態統合手段１４は、前記した式（１６）及び式（１７）の処理を状態ベクトルの回数（Ｎ）分繰り返す。これによって、内部状態量ＸＩ及び受信状態量ＸＲに含まれる状態ベクトルから確率的に選択された状態ベクトル群により統合状態量ＸＪを構成することができる。このとき、例えば、閾値γを“０．５”とすると、内部状態量ＸＩ及び受信状態量ＸＲから同等の確率で状態ベクトルが選択されることになる。また、γの値が大きいほど、内部状態量ＸＩに比べて、受信状態量ＸＲを重視することができる。
図４に戻って、状態推定装置１０の構成について説明を続ける。
【００８５】
状態送信手段１５は、状態量記憶手段１１に記憶されている状態量（内部状態量ＸＩ）を読み出して、ネットワークＮに接続された他の状態推定装置に送信するものである。
なお、状態送信手段１５は、状態量（内部状態量ＸＩ）を含んだメッセージを他の状態推定装置に送信する際に、送信先や送信元を示すアドレス情報（ＩＰアドレス等）を含めることとしてもよい。
また、状態量がまだ記憶されていない等の理由によって、状態量記憶手段１１から状態量（内部状態量ＸＩ）を読み出せない場合、状態送信手段１５は、送信を行わないこととしてもよいし、送信不可能であるというメッセージを送信することとしてもよい。
また、状態送信手段１５は、定期的に内部状態量ＸＩを送信することとしてもよいし、他の状態推定装置からの要求に基づいて送信することとしてもよい。
【００８６】
推定量出力手段１６は、状態量記憶手段１１に記憶されている状態量（内部状態量ＸＩ）を読み出して、推定量Ｅとして外部に出力するものである。
なお、推定量出力手段１６は、内部状態量ＸＩをそのまま推定量Ｅとして出力することとしてもよいし、一部の状態量のみを出力したり、状態量に所定の変換を施して出力したりすることとしてもよい。
【００８７】
例えば、状態量が状態ベクトルである場合、推定量出力手段１６は、状態ベクトルの一部成分を抽出して、推定量Ｅを新たなベクトル量やスカラ量とすることができる。具体的には、状態ベクトルが３次元位置及び３次元速度で構成される場合に、３次元位置のみを推定量Ｅとする。あるいは、推定量出力手段１６は、状態ベクトルに演算処理、例えば、線形変換を施した結果を推定量Ｅとして出力することとしてもよい。
【００８８】
また、例えば、状態量がパーティクルの場合、推定量出力手段１６は、パーティクルを構成する一部の状態ベクトルを取り出して推定量Ｅとすることができる。あるいは、推定量出力手段１６は、パーティクルを構成する状態ベクトルすべてについて平均ベクトルを算出し、推定量Ｅとして出力することとしてもよい。
なお、状態量がまだ記憶されていない等の理由によって、状態量記憶手段１１から状態量（内部状態量ＸＩ）を読み出せない場合、推定量出力手段１６は、推定量Ｅを出力しないこととしてもよいし、出力することができない旨を出力することとしてもよい。
【００８９】
以上、状態推定装置１０の構成について説明したが、本発明はこの構成に限定されるものではない。例えば、推定量Ｅの出力を他の状態推定装置で行うこととし、状態推定装置１０からは出力を行わない構成としてもよい。この場合、状態推定装置１０の構成から推定量出力手段１６を省略することができる。また、外部からの観測量Ｙの入力を他の状態推定装置で行うこととし、状態推定装置１０では入力を行わない構成としてもよい。この場合、状態推定装置１０の構成から状態更新手段１２を省略することができる。
【００９０】
また、図５に示すように、状態更新手段１２を外部の観測装置２０に備え、状態更新手段１２を状態推定装置１０から省略し、データの入出力のみを行う入出力手段１７を備えた状態推定装置１０Ｂの構成としてもよい。例えば、観測装置２０は、図示を省略したセンサ等を備えた位置観測手段と、状態更新手段１２とを備えている。そして、観測装置２０の状態更新手段１２が、位置観測手段（図示せず）により観測した結果である観測対象の位置（観測量）に基づいて状態量（観測後状態量ＸＹ）を算出し、状態量記憶手段１１に記憶されている内部状態量ＸＩを更新することとしてもよい。
【００９１】
この場合、観測装置２０は、観測後状態量ＸＹとともに、観測後状態量ＸＹに対する評価値を状態推定装置１０Ｂに出力することとしてもよい。例えば、観測装置２０は、センサであるカメラによって入力した画像に観測対象が存在するか（撮影されているか）否かを判定し、存在すると判定した場合は評価値に高い値を、存在しないと判定した場合は評価値に低い値を設定する。この評価値を参照することで、状態推定装置１０Ｂは、状態統合手段１４において、状態量の統合を行う際に、内部状態量ＸＩと受信状態量ＸＲとの統合の度合いを決定することとしてもよい。
【００９２】
また、状態推定装置１０（１０Ｂ）は、一般的なコンピュータを、前記した各手段として機能させることで実現することができる。このプログラム（状態推定プログラム）は、通信回線を介して配布することも可能であるし、ＣＤ−ＲＯＭ等の記録媒体に書き込んで配布することも可能である。
【００９３】
以上説明したように、状態推定装置１０（１０Ｂ）は、他の状態推定装置で推定した状態量と、当該状態推定装置１０で推定した状態量とを統合し、観測対象の状態を推定することができる。これによって、状態推定装置１０は、他の状態推定装置と協働し、状態の推定を分散して行うことができる。また、例えば、他の状態推定装置に状態推定装置１０と共通の状態量を記憶させておくことで、状態推定装置１０に障害が発生した場合であっても、状態の推定を継続することが可能になる。
【００９４】
［第１実施形態：状態推定装置（状態推定手段）の動作］
次に、図６〜図８を参照して、状態推定装置１０の動作について説明する。図６は、状態推定装置の全体動作を示すフローチャートである。図７は、状態推定装置における状態更新手段の動作を示すフローチャートである。図８は、状態推定装置における状態統合手段の動作を示すフローチャートである。
【００９５】
（全体動作）
まず、図６を参照（適宜図４参照）して、状態推定装置１０の動作について説明する。
状態推定装置１０は、状態更新手段１２によって、外部から観測量Ｙが入力されたか否かを判定する（ステップＳ１）。そして、観測量Ｙが入力された場合（ステップＳ１でＹｅｓ）、状態更新手段１２は、観測量Ｙによって、状態量記憶手段１１に記憶されている内部状態量ＸＩを更新する（ステップＳ２）。なお、このステップＳ２における内部状態量更新処理の詳細については、図７を参照して後で説明を行うこととする。一方、観測量Ｙが入力されなかった場合（ステップＳ１でＮｏ）、状態更新手段１２は、そのままステップＳ３の動作に進む。
【００９６】
また、状態推定装置１０は、状態統合手段１４によって、状態受信手段１３において、ネットワークＮに接続された他の状態推定装置から状態量（受信状態量ＸＲ）を受信したか否かを判定する（ステップＳ３）。そして、受信状態量ＸＲを受信した場合（ステップＳ３でＹｅｓ）、状態統合手段１４は、受信状態量ＸＲと状態量記憶手段１１に記憶されている内部状態量ＸＩとを統合する（ステップＳ４）。なお、このステップＳ４における内部状態量統合処理の詳細については、図８を参照して後で説明を行うこととする。一方、受信状態量ＸＲが受信されなかった場合（ステップＳ３でＮｏ）、状態統合手段１４は、そのままステップＳ５の動作に進む。
【００９７】
そして、状態推定装置１０は、状態送信手段１５によって、状態量記憶手段１１に記憶されている内部状態量ＸＩの一部又は全部を読み出して、送信状態量ＸＳとして、ネットワークＮに接続された他の状態推定装置に送信する（ステップＳ５）。なお、状態送信手段１５は、状態量記憶手段１１から内部状態量ＸＩが読み出せない場合、送信を行わない、又は、送信不可能であるというメッセージを送信する。
また、状態推定装置１０は、推定量出力手段１６によって、状態量記憶手段１１に記憶されている内部状態量ＸＩを読み出して、推定量Ｅとして外部に出力する（ステップＳ６）。なお、推定量出力手段１６は、一部の内部状態量ＸＩのみを出力したり、内部状態量ＸＩに所定の変換を施して出力したりすることとしてもよい。
【００９８】
以上、状態推定装置１０の動作について説明したが、本動作はこの手順に限定されるものではない。ここでは、説明の都合上時系列に動作の説明を行ったが、内部状態量更新処理（ステップＳ２）や内部状態量統合処理（ステップＳ４）を並列に動作させることとしてもよいし、観測量Ｙの入力や受信状態量ＸＲの受信が不規則に入力される場合は、そのタイミングに合わせて動作することとしてもよい。
【００９９】
（内部状態量更新処理）
次に、図７を参照（適宜図４参照）して、状態推定装置１０の状態更新手段１２における状態量更新処理の動作について説明する。なお、本動作は、図６のステップＳ２の動作に相当する。また、ここでは、状態量更新処理の一例として、状態量がパーティクル、観測量が確率密度関数である場合の動作について説明する。
【０１００】
まず、状態更新手段１２は、状態量記憶手段１１に内部状態量ＸＩ（ＸＩ＝｛ｘ（１），ｘ（２），…，ｘ（Ｎ）｝）が記憶されているか否かを判定する（ステップＳ２０）。そして、内部状態量ＸＩが記憶されている場合（ステップＳ２０でＹｅｓ）、状態更新手段１２は、内部状態量ＸＩに含まれるＮ個の状態ベクトルｘ（ｉ）（ただし、ｉ＝１，２，…，Ｎ）に対して、観測量Ｙ（ｘ（ｉ））に基づいて、状態ベクトルが発生する確率ｐ（ｉ）を算出する（ステップＳ２１：前記式（５）参照）。さらに、状態更新手段１２は、ステップＳ２１で算出した確率ｐ（ｉ）をｉ番目まで累積した累積確率ａ（ｉ）を、ｉごとにＮ個分算出する（ステップＳ２２）。
【０１０１】
ここで、状態更新手段１２は、観測後の状態量（観測後状態量ＸＹ）を空集合（ＸＹ＝φ）、ループ回数に用いる変数ｍを“１”にそれぞれ初期化する（ステップＳ２３）。
そして、状態更新手段１２は、０＜ｒ≦１なる実数値の一様乱数ｒを生成し（ステップＳ２４）、ステップＳ２２で算出した累積確率ａ（ｉ）の中でａ（ｉ）≧ｒを満たし、かつ、ａ（ｉ）が最小となる配列引数ｉを探索しｊとする（ステップＳ２５）。続けて、状態更新手段１２は、ステップＳ２５で探索された配列引数ｊに対応する状態ベクトルｘ（ｊ）を、観測後状態量ＸＹに追加する（ステップＳ２６）。
【０１０２】
その後、状態更新手段１２は、ループ回数の変数ｍをインクリメント（＋１）し（ステップＳ２７）、変数ｍの値がＮ以下である場合（ステップＳ２８でＹｅｓ）、ステップＳ２４に戻って動作を続ける。一方、変数ｍの値がＮを超過した場合（ステップＳ２８でＮｏ）、動作を終了する。
なお、ステップＳ２０において、状態量記憶手段１１に内部状態量ＸＩが記憶されていない場合（ステップＳ２０でＮｏ）、状態更新手段１２は、状態ベクトルを乱数によりＮ個（ｒ１，ｒ２，…，ｒＮ）生成することで、観測後状態量ＸＹを生成し（ステップＳ２９）、動作を終了する。
以上の動作を行うことで、状態更新手段１２は、状態量記憶手段１１に記憶されている内部状態量ＸＩを、確率の高いパーティクルをより高頻度にした確率密度関数の近似によって表現することができる。
【０１０３】
（内部状態量統合処理）
次に、図８を参照（適宜図４参照）して、状態推定装置１０の状態統合手段１４における内部状態量統合処理の動作について説明する。なお、本動作は、図６のステップＳ４の動作に相当する。また、ここでは、内部状態量統合処理の一例として、状態量がパーティクルである場合の動作について説明する。
なお、内部状態量ＸＩ及び受信状態量ＸＲに含まれる状態ベクトルの数をＮ個とし、各々の状態ベクトルをｘＩ（ｉ）及びｘＲ（ｉ）（ただし、ｉ＝１，２，…，Ｎ）とする。すなわち、ＸＩ＝｛ｘＩ（１），ｘＩ（２），…，ｘＩ（Ｎ）｝、ＸＲ＝｛ｘＲ（１），ｘＲ（２），…，ｘＲ（Ｎ）｝とする。
【０１０４】
まず、状態統合手段１４は、統合後の状態量（統合状態量ＸＹ）を空集合（ＸＹ＝φ）、ループ回数に用いる変数ｍを“１”にそれぞれ初期化する（ステップＳ４０）。
そして、状態統合手段１４は、０＜ｒ≦１なる実数値の一様乱数ｒを生成し（ステップＳ４１）、その一様乱数ｒが予め定めた定数（閾値γ〔０＜γ≦１の実数値〕）以下であるか否かを判定する（ステップＳ４２）。
【０１０５】
ここで、一様乱数ｒが閾値γ以下の場合（ステップＳ４２でＹｅｓ）、状態統合手段１４は、統合状態量ＸＪに、受信状態量ＸＲに含まれるｍ番目の状態ベクトルｘＲ（ｍ）を追加する（ステップＳ４３）。一方、一様乱数ｒが閾値γ未満の場合（ステップＳ４２でＮｏ）、状態統合手段１４は、統合状態量ＸＪに、内部状態量ＸＩに含まれるｍ番目の状態ベクトルｘＩ（ｍ）を追加する（ステップＳ４４）。
その後、状態統合手段１４は、ループ回数の変数ｍをインクリメント（＋１）し（ステップＳ４５）、変数ｍの値がＮ以下である場合（ステップＳ４６でＹｅｓ）、ステップＳ４１に戻って動作を続ける。一方、変数ｍの値がＮを超過した場合（ステップＳ４６でＮｏ）、動作を終了する。
【０１０６】
以上の動作を行うことで、状態統合手段１４は、内部状態量ＸＩ及び受信状態量ＸＲに含まれる状態ベクトルから、予め定めた閾値γを基準として選択された状態ベクトル群により統合状態量ＸＪを構成することができる。すなわち、閾値γによって、内部状態量又は受信状態量のいずれを重視するのかを設定することができる。
【０１０７】
［第２実施形態：状態推定装置（状態推定手段）の構成］
次に、図９を参照して、状態推定装置の他の構成について説明する。図９は、本発明の第２実施形態に係る状態推定装置の内部構成を示すブロック構成図である。
状態推定装置１０Ｃは、外部から入力される観測対象の観測量に基づいて、ネットワークＮに接続した他の状態推定装置と協働して、観測対象の状態を示す状態量を推定するものである。なお、状態推定装置１０Ｃは、複数の時点における状態量（内部状態量）を保持することで、複数の状態推定装置間での処理の違いや、観測時刻の違いに起因する時間ずれの影響を吸収する。ここでは、状態推定装置１０Ｃは、状態量記憶手段１１Ｃと、状態更新手段１２Ｃと、状態受信手段１３Ｃと、状態統合手段１４Ｃと、状態送信手段１５Ｃと、推定量出力手段１６Ｃと、状態予測手段１８とを備えている。
【０１０８】
状態量記憶手段１１Ｃは、観測対象の状態を示す状態量のうち、当該状態推定装置１０Ｃ内部で保持する状態量（内部状態量）を記憶するものである。なお、状態量記憶手段１１Ｃは、複数の時点における内部状態量を記憶することが、状態量記憶手段１１（図４）とは異なっている。
例えば、状態量記憶手段１１Ｃには、時刻“０”から時刻“Ｍ−１”に対応したＭ個の要素を有する配列を確保する。
【０１０９】
ここで、状態量記憶手段１１Ｃに対する書き込み又は読み出しを行う各手段（状態更新手段１２Ｃ、状態統合手段１４Ｃ、状態送信手段１５Ｃ、推定量出力手段１６Ｃ及び状態予測手段１８）は、０≦ｔ＜Ｍなる整数値ｔで示される時刻における内部状態量ＸＩ（ｔ）を参照する場合、Ｍ個の要素を有する配列のｔ番目の要素を参照すればよい。また、０≦ｔ＜Ｍなる整数値以外で時刻が示される場合は、状態量記憶手段１１Ｃに対する書き込み時には動作を行わず、読み出し時には何も読み出さない（空集合φを読み出す）ものとする。
また、状態量記憶手段１１Ｃは、Ｍ個の要素を有する配列を確保し、その各要素に時刻情報（時刻ｔ）と内部状態量ＸＩ（ｔ）とを記憶する構成としてもよい。
【０１１０】
ここで、図１０を参照して、状態量記憶手段１１Ｃに記憶されている内部状態量へのアクセス（読み出し／書き込み）手法について説明する。図１０は、状態量記憶手段の記憶内容の一例を示すデータ構造図である。図１０では、一例として、状態量記憶手段１１Ｃに１０個の要素を有する配列を確保し、アドレス０からアドレス９にそれぞれ時刻情報（時刻ｔ）と内部状態量ＸＩ（ｔ）とを対応付けて記憶している状態を示している。
まず、時刻ｔに内部状態量ＸＩ（ｔ）を書き込む手法について説明する。
ここで、時刻ｔを配列の要素数Ｔで除算した場合の剰余をアドレスＡ（Ａ番目のアドレス）とする。すなわち、アドレスＡは、以下の式（１８）により算出される。
【０１１１】
【数１８】
【０１１２】
ここで、演算子％は剰余を示し、ｔ％ＭはｔをＭで除算した剰余を表すものとする。
このように算出されたアドレスＡに対して、時刻ｔと内部状態量ＸＩ（ｔ）との対を書き込むこととする。
例えば、図１０に示すように、時刻ｔ＝“１０２５”に内部状態量ＸＩ（１０２５）を書き込む場合、前記式（１８）により、アドレスＡ＝“１０２５％１０”＝“５”を得る。そこで、アドレスＡ＝“５”に対し、ｔ＝“１０２５”なる時刻情報と、ｔ＝“１０２５”における内部状態量ＸＩ（１０２５）との対を書き込む。
【０１１３】
次に、時刻ｔに内部状態量ＸＩ（ｔ）を読み出す手法について説明する。なお、アドレスは、前記式（１８）により算出される。
ここで、アドレスＡ（Ａ番目のアドレス）には、時刻τが時刻情報として書き込まれているとする。もし、τ＝ｔであれば、アドレスＡに書き込まれている状態量は所望の時刻ｔにおける内部状態量ＸＩ（ｔ）であるため、そのまま内部状態量ＸＩ（ｔ）を読み出せばよい。もし、τ≠ｔであれば、アドレスＡに書き込まれている状態量は所望の時刻ｔにおけるものではないため、例えば、読み出しデータを空集合φとする。
【０１１４】
例えば、図１０に示すように、時刻ｔ＝“１０３２”の内部状態量ＸＩ（１０３２）を読み出す場合、前記式（１８）により、アドレスＡ＝“１０３２％１０”＝“２”を得る。そして、アドレスＡ＝“２”に書き込まれている時刻情報（時刻τ）を読み出す。この場合、τ＝“１０３２”である。すなわち、τ＝ｔ＝“１０３２”であるため、アドレスＡ＝“２”に書き込まれている状態量は所望の時刻ｔ＝“１０３２”における内部状態量ＸＩ（１０３２）であると判断することができ、このアドレスＡ＝“２”に書き込まれている内部状態量ＸＩ（１０３２）を読み出せばよい。
【０１１５】
また、時刻ｔ＝“１０３３”の内部状態量ＸＩ（１０３３）を読み出す場合、前記式（１８）により、アドレスＡ＝“１０３３％１０”＝“３”を得る。そして、アドレスＡ＝“３”に書き込まれている時刻情報（時刻τ）を読み出す。この場合、τ＝“１０２３”である。すなわち、τ≠ｔ＝“１０３３”であるため、アドレスＡ＝“３”に書き込まれている状態量は所望の時刻ｔ＝“１０３３”における内部状態量ＸＩ（１０３３）ではないと判断することができる。そこで、読み出しデータを空集合φとする。
これによって、時刻ｔの内部状態量ＸＩ（ｔ）の読み出し／書き込みにおける時刻のずれを防止することができる。
図９に戻って、状態推定装置１０Ｃの構成について説明を続ける。
【０１１６】
状態更新手段１２Ｃは、外部から入力される時刻ｔＹの観測量Ｙ（ｔＹ）によって、状態量記憶手段１１Ｃに記憶されている内部状態量ＸＩ（ｔＹ）を更新するものである。
状態受信手段１３Ｃは、ネットワークＮに接続された他の状態推定装置が保持し、当該他の状態推定装置から送信される時刻ｔＲの受信状態量ＸＲ（ｔＲ）を受信し、状態統合手段１４Ｃに出力するものである。
【０１１７】
状態統合手段１４Ｃは、状態受信手段１３Ｃで受信した時刻ｔＲの受信状態量ＸＲ（ｔＲ）と、状態量記憶手段１１Ｃに記憶されている時刻ｔＲの内部状態量ＸＩ（ｔＲ）とを統合し時刻ｔＲの統合状態量ＸＪ（ｔＲ）を生成するものである。
状態送信手段１５Ｃは、状態量記憶手段１１Ｃに記憶されている、ある時刻ｔＳの内部状態量ＸＩ（ｔＳ）を読み出して、ネットワークＮに接続された他の状態推定装置に送信するものである。
【０１１８】
推定量出力手段１６Ｃは、状態量記憶手段１１Ｃに記憶されている、ある時刻ｔＥの内部状態量ＸＩ（ｔＥ）を読み出して、推定量Ｅ（ｔＥ）として外部に出力するものである。
なお、状態更新手段１２Ｃ、状態受信手段１３Ｃ、状態統合手段１４Ｃ、状態送信手段１５Ｃ及び推定量出力手段１６Ｃは、時刻（時刻ｔＹ、時刻ｔＲ、時刻ｔＳ及び時刻ｔＥ）に基づいて動作し、内部状態量ＸＩ（ｔ）の更新等を行うとともに、その時刻に同期した制御信号を状態予測手段１８に出力することを除き、図４で説明した状態更新手段１２、状態受信手段１３、状態統合手段１４、状態送信手段１５及び推定量出力手段１６と同一の機能を有しているため、各機能については説明を省略する。
【０１１９】
状態予測手段１８は、制御信号で通知されるタイミングに基づいて、ある時刻ｔＰにおける状態量（予測状態量）ＸＰ（ｔＰ）を予測するものである。
ここで、状態予測手段１８は、ある時刻ｔＯにおける内部状態量ＸＩ（ｔＯ）と、ある時刻ｔＰにおける内部状態量ＸＩ（ｔＰ）とを状態量記憶手段１１Ｃから読み出し、予測処理を行うことで、時刻ｔＰにおける予測状態量ＸＰ（ｔＰ）を予測し、内部状態量ＸＩ（ｔＰ）として状態量記憶手段１１Ｃに書き込むこととする。
この状態予測手段１８は、まず、時刻ｔＯから時刻ｔＰへの状態遷移を表す予め定めた変換則Ｆ（ｔＯ，ｔＰ；Ｘ）を用いて、以下の式（１９）により、状態遷移後の状態量（遷移後状態量ＸＱ（ｔＰ））を算出する。
【０１２０】
【数１９】
【０１２１】
また、状態予測手段１８は、算出した遷移後状態量ＸＱ（ｔＰ）と、時刻ｔＯにおける内部状態量ＸＩ（ｔＯ）とに基づいて、時刻ｔＰにおける予測状態量ＸＰ（ｔＰ）を算出する。なお、遷移後状態量ＸＱ（ｔＰ）と内部状態量ＸＩ（ｔＯ）とから予測状態量ＸＰ（ｔＰ）を算出する手法は、図４で説明した状態統合手段１４と同様の手法を用いることができる。この場合、状態統合手段１４における内部状態量ＸＩ、受信状態量ＸＲ及び統合状態量ＸＪを、それぞれ内部状態量ＸＩ（ｔＯ）、遷移後状態量ＸＱ（ｔＰ）及び予測状態量ＸＰ（ｔＰ）と読み替えればよく、説明を省略する。
【０１２２】
なお、状態予測手段１８は、遷移後状態量ＸＱ（ｔＰ）をそのまま予測状態量ＸＰ（ｔＰ）とすることとしてもよく、この場合は、状態予測手段１８は、状態量記憶手段１１Ｃから、時刻ｔＯにおける内部状態量ＸＩ（ｔＯ）を読み出す必要はない。
【０１２３】
また、状態予測手段１８は、例えば、状態統合手段１４Ｃから制御信号として時刻ｔＲが通知された場合、時刻ｔＯ＝“ｔＲ”、時刻ｔＰ＝“ｔＲ＋１”として動作する。また、状態予測手段１８は、時刻ｔＯ及び時刻ｔＰを、（ｔＯ，ｔＰ）＝（ｔＲ，ｔＲ±１），（ｔＲ，ｔＲ±２），…，（ｔＲ，ｔＲ±ＴＲ）のように、時刻ｔＲの前後の時間ＴＲにわたって適用することとしてもよい。
あるいは、状態予測手段１８は、状態更新手段１２Ｃの動作の直後、状態受信手段１３Ｃの動作の前後、状態送信手段１５Ｃの動作の直前、推定量出力手段１６Ｃの動作の直前等、状態統合手段１４Ｃと同様に、制御信号に基づいて動作を行うこととしてもよい。
また、状態予測手段１８は、制御信号によらず、タイマ（図示せず）等に基づいて、時刻ｔＯ及び時刻ｔＰを設定することとしてもよい。
以下、数式を用いて、前記式（１９）における遷移後の状態量の演算について、さらに説明を付加することにする。
【０１２４】
（状態量がベクトルの場合）
まず、状態量（内部状態量ＸＩ）として、一つの状態ベクトルを採用する場合における遷移後状態量の演算について説明する。
この場合、状態予測手段１８は、前記式（１９）の変換則Ｆとして、単位時間あたりの状態遷移行列Φを用いることができる。
すなわち、内部状態量ＸＩ（ｔＯ）を表す状態ベクトルをｘＩ（ｔＯ）としたとき、状態遷移後の状態量である遷移後状態量ＸＱ（ｔＰ）を表す状態ベクトルｘＱ（ｔＰ）は、状態遷移行列Φを用いて、以下の式（２０）により算出することができる。
【０１２５】
【数２０】
【０１２６】
このとき、遷移後状態量ＸＱ（ｔＰ）は、ＸＱ（ｔＰ）＝｛ｘＱ（ｔＰ）｝となる。
【０１２７】
（状態量がベクトル及び共分散行列の場合）
次に、状態量（内部状態量ＸＩ）として、状態ベクトル及び状態共分散行列を採用する場合における遷移後状態量の演算について説明する。
この場合、状態予測手段１８は、前記式（１９）の変換則Ｆとして、単位時間あたりの状態遷移行列Φ及び単位時間あたりのプロセス雑音の共分散行列Ｑを用いることができる。
すなわち、内部状態量ＸＩ（ｔＯ）を表す状態ベクトルをｘＩ（ｔＯ）、その状態共分散行列をＰＩ（ｔＯ）としたとき、遷移後状態量ＸＱ（ｔＰ）を表す状態ベクトルｘＱ（ｔＰ）、その状態共分散行列ＰＱ（ｔＰ）は、単位時間あたりの状態遷移行列Φ及び単位時間あたりのプロセス雑音の共分散行列Ｑを用いて、以下の式（２１）により算出することができる。なお、上付き添え字Ｔは転置を表している。
【０１２８】
【数２１】
【０１２９】
このとき、遷移後状態量ＸＱ（ｔＰ）は、ＸＱ（ｔＰ）＝｛ｘＱ（ｔＰ），ＰＱ（ｔＰ）｝となる。
【０１３０】
（状態量がパーティクルの場合）
次に、状態量（内部状態量ＸＩ）としてパーティクルを採用する場合における遷移後状態量の演算について説明する。
この場合、状態予測手段１８は、前記式（１９）の変換則Ｆとして、単位時間あたりの状態遷移行列Φ及び単位時間あたりのプロセス雑音の共分散行列Ｑを用いることができる。
すなわち、内部状態量ＸＩ（ｔＯ）に含まれる状態ベクトルの数をＮ個、各々の状態ベクトルをｘＩ（ｉ）（ｔＯ）（ただし、ｉ＝１，２，…，Ｎ）としたとき、遷移後状態量ＸＱ（ｔＰ）に含まれる状態ベクトルｘＱ（ｉ）（ｔＰ）は、単位時間あたりの状態遷移行列Φ及び単位時間あたりのプロセス雑音の共分散行列Ｑを用いて、以下の式（２２）により算出することができる。なお、ｎはノイズ（雑音）、Ｅ［］は期待値、上付き添え字Ｔは転置を表している。
【０１３１】
【数２２】
【０１３２】
このとき、遷移後状態量ＸＱ（ｔＰ）は、ＸＱ（ｔＰ）＝｛ｘＱ（１）（ｔＰ），ｘＱ（２）（ｔＰ），…，ｘＱ（Ｎ）（ｔＰ）｝となる。
以上、状態推定装置１０Ｃの構成について説明したが、本発明はこの構成に限定されるものではない。例えば、図１１に示すように、状態更新手段１２Ｃを外部の観測装置２０Ｃに備え、状態更新手段１２Ｃを状態推定装置１０Ｃから省略し、データの入出力のみを行う入出力手段１７Ｃを備えた状態推定装置１０Ｄの構成としてもよい。この場合、センサ等の位置観測手段（図示せず）により観測した結果である観測対象の位置（観測量）が時刻ｔＹに対応付けられて、観測装置２０の状態更新手段１２Ｃから、状態推定装置１０Ｄに入力される。
【０１３３】
また、状態推定装置１０Ｃ（１０Ｄ）は、一般的なコンピュータを、前記した各手段として機能させることで実現することができる。このプログラム（状態推定プログラム）は、通信回線を介して配布することも可能であるし、ＣＤ−ＲＯＭ等の記録媒体に書き込んで配布することも可能である。
【０１３４】
以上説明したように、状態推定装置１０Ｃ（１０Ｄ）は、他の状態推定装置で推定した状態量と、当該状態推定装置１０Ｃで推定した状態量とを統合し、観測対象の状態を推定することができる。これによって、状態推定装置１０Ｃは、他の状態推定装置と協働し、状態の推定を分散して行うことができる。また、例えば、他の状態推定装置に状態推定装置１０Ｃと共通の状態量を記憶させておくことで、状態推定装置１０Ｃに障害が発生した場合であっても、状態の推定を継続することが可能になる。さらに、状態推定装置１０Ｃは、時間の経過に伴った予測を行うため、状態推定の精度を高めることができる。
なお、状態推定装置１０Ｃの動作は、複数の時点における状態量（内部状態量）を保持して状態量の予測を行うことを除き、図６〜図８で説明した状態推定装置１０（図４参照）と、基本的な動作は同一であるため説明を省略する。
【図面の簡単な説明】
【０１３５】
【図１】本発明に係る統合状態推定システムの全体の構成を示す全体構成図である。
【図２】統合状態推定システムの具体例であるカーナビゲーションシステムの構成を示す全体構成図である。
【図３】本発明に係る統合状態推定装置の構成を示す構成図である。
【図４】本発明の第１実施形態に係る状態推定装置の内部構成を示すブロック構成図である。
【図５】本発明の第１実施形態に係る状態推定装置の変形例を示すブロック構成図である。
【図６】本発明に係る状態推定装置の全体動作を示すフローチャートである。
【図７】本発明に係る状態推定装置における状態更新手段の動作を示すフローチャートである。
【図８】本発明に係る状態推定装置における状態統合手段の動作を示すフローチャートである。
【図９】本発明の第２実施形態に係る状態推定装置の内部構成を示すブロック構成図である。
【図１０】状態量記憶手段の記憶内容の一例を示すデータ構造図である。
【図１１】本発明の第２実施形態に係る状態推定装置の変形例を示すブロック構成図である。
【符号の説明】
【０１３６】
Ｓ 統合状態推定システム
１ 統合状態推定装置
１０ 状態推定装置
１１ 状態量記憶手段
１２ 状態更新手段
１３ 状態受信手段
１４ 状態統合手段
１５ 状態送信手段
１６ 推定量出力手段
１７ 入出力手段
１８ 状態予測手段
【特許請求の範囲】
【請求項１】
観測対象の観測量に基づいて、当該観測対象の状態を示す状態量を推定する状態推定装置において、
前記観測対象の状態量を内部状態量として記憶する状態量記憶手段と、
外部から入力された前記観測量に基づいて、前記状態量記憶手段に記憶されている内部状態量を更新する状態更新手段と、
通信路に接続された他の状態推定装置において記憶されている状態量を外部状態量として受信する状態受信手段と、
この状態受信手段で受信した外部状態量と、前記状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する状態統合手段と、
前記状態量記憶手段に記憶されている内部状態量を前記他の状態推定手段に送信する状態送信手段と、
を備えていることを特徴とする状態推定装置。
【請求項２】
前記状態量記憶手段は、複数の前記内部状態量を時刻に対応付けて記憶することを特徴とする請求項１に記載の状態推定装置。
【請求項３】
前記状態量記憶手段に記憶されている複数の内部状態量に基づいて、ある時刻における状態量を予測する状態予測手段を備えていることを特徴とする請求項２に記載の状態推定装置。
【請求項４】
観測対象の観測量に基づいて、通信路に接続された複数の状態推定装置により、当該観測対象の状態を示す状態量を統合して推定する統合状態推定システムであって、
前記状態推定装置は、
前記観測対象の状態量を内部状態量として記憶する状態量記憶手段と、
前記通信路に接続された他の状態推定装置において記憶されている状態量を外部状態量として受信する状態受信手段と、
この状態受信手段で受信した外部状態量と、前記状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する状態統合手段と、
前記状態量記憶手段に記憶されている内部状態量を前記他の状態推定手段に送信する状態送信手段とを備え、
前記複数の状態推定装置の少なくとも一つが、外部から入力された前記観測量に基づいて、当該状態推定装置の状態量記憶手段に記憶されている内部状態量を更新する状態更新手段を備え、
前記複数の状態推定装置の少なくとも一つが、当該状態推定装置の状態量記憶手段に記憶されている内部状態量を推定量として出力する推定量出力手段を備えていることを特徴とする統合状態推定システム。
【請求項５】
観測対象の観測量に基づいて、当該観測対象の状態を示す状態量を統合して推定する統合状態推定装置であって、
前記状態量を推定する状態推定手段を複数備え、
前記状態推定手段は、
前記観測対象の状態量を内部状態量として記憶する状態量記憶手段と、
通信路に接続された他の状態推定手段において記憶されている状態量を、外部状態量として受信する状態受信手段と、
この状態受信手段で受信した外部状態量と、前記状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する状態統合手段と、
前記状態量記憶手段に記憶されている内部状態量を前記他の状態推定手段に送信する状態送信手段とを備え、
前記複数の状態推定手段の少なくとも一つが、外部から入力された前記観測量に基づいて、当該状態推定手段の状態量記憶手段に記憶されている内部状態量を更新する状態更新手段を備え、
前記複数の状態推定手段の少なくとも一つが、当該状態推定手段の状態量記憶手段に記憶されている内部状態量を推定量として出力する推定量出力手段を備えていることを特徴とする統合状態推定装置。
【請求項６】
観測対象の観測量に基づいて、通信路に接続された複数の状態推定装置により、当該観測対象の状態を示す状態量を統合して推定するために、前記状態推定装置におけるコンピュータを、
外部から入力される前記観測量に基づいて、前記状態量を内部状態量として記憶する状態量記憶手段において、当該内部状態量を更新する状態更新手段、
前記通信路に接続された他の状態推定装置において記憶されている状態量を外部状態量として受信する状態受信手段、
この状態受信手段で受信した外部状態量と、前記状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する状態統合手段、
前記状態量記憶手段に記憶されている内部状態量を前記他の状態推定手段に送信する状態送信手段、
前記状態量記憶手段に記憶されている内部状態量を推定量として出力する推定量出力手段、
として機能させることを特徴とする状態推定プログラム。
【請求項１】
観測対象の観測量に基づいて、当該観測対象の状態を示す状態量を推定する状態推定装置において、
前記観測対象の状態量を内部状態量として記憶する状態量記憶手段と、
外部から入力された前記観測量に基づいて、前記状態量記憶手段に記憶されている内部状態量を更新する状態更新手段と、
通信路に接続された他の状態推定装置において記憶されている状態量を外部状態量として受信する状態受信手段と、
この状態受信手段で受信した外部状態量と、前記状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する状態統合手段と、
前記状態量記憶手段に記憶されている内部状態量を前記他の状態推定手段に送信する状態送信手段と、
を備えていることを特徴とする状態推定装置。
【請求項２】
前記状態量記憶手段は、複数の前記内部状態量を時刻に対応付けて記憶することを特徴とする請求項１に記載の状態推定装置。
【請求項３】
前記状態量記憶手段に記憶されている複数の内部状態量に基づいて、ある時刻における状態量を予測する状態予測手段を備えていることを特徴とする請求項２に記載の状態推定装置。
【請求項４】
観測対象の観測量に基づいて、通信路に接続された複数の状態推定装置により、当該観測対象の状態を示す状態量を統合して推定する統合状態推定システムであって、
前記状態推定装置は、
前記観測対象の状態量を内部状態量として記憶する状態量記憶手段と、
前記通信路に接続された他の状態推定装置において記憶されている状態量を外部状態量として受信する状態受信手段と、
この状態受信手段で受信した外部状態量と、前記状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する状態統合手段と、
前記状態量記憶手段に記憶されている内部状態量を前記他の状態推定手段に送信する状態送信手段とを備え、
前記複数の状態推定装置の少なくとも一つが、外部から入力された前記観測量に基づいて、当該状態推定装置の状態量記憶手段に記憶されている内部状態量を更新する状態更新手段を備え、
前記複数の状態推定装置の少なくとも一つが、当該状態推定装置の状態量記憶手段に記憶されている内部状態量を推定量として出力する推定量出力手段を備えていることを特徴とする統合状態推定システム。
【請求項５】
観測対象の観測量に基づいて、当該観測対象の状態を示す状態量を統合して推定する統合状態推定装置であって、
前記状態量を推定する状態推定手段を複数備え、
前記状態推定手段は、
前記観測対象の状態量を内部状態量として記憶する状態量記憶手段と、
通信路に接続された他の状態推定手段において記憶されている状態量を、外部状態量として受信する状態受信手段と、
この状態受信手段で受信した外部状態量と、前記状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する状態統合手段と、
前記状態量記憶手段に記憶されている内部状態量を前記他の状態推定手段に送信する状態送信手段とを備え、
前記複数の状態推定手段の少なくとも一つが、外部から入力された前記観測量に基づいて、当該状態推定手段の状態量記憶手段に記憶されている内部状態量を更新する状態更新手段を備え、
前記複数の状態推定手段の少なくとも一つが、当該状態推定手段の状態量記憶手段に記憶されている内部状態量を推定量として出力する推定量出力手段を備えていることを特徴とする統合状態推定装置。
【請求項６】
観測対象の観測量に基づいて、通信路に接続された複数の状態推定装置により、当該観測対象の状態を示す状態量を統合して推定するために、前記状態推定装置におけるコンピュータを、
外部から入力される前記観測量に基づいて、前記状態量を内部状態量として記憶する状態量記憶手段において、当該内部状態量を更新する状態更新手段、
前記通信路に接続された他の状態推定装置において記憶されている状態量を外部状態量として受信する状態受信手段、
この状態受信手段で受信した外部状態量と、前記状態量記憶手段に記憶されている内部状態量とを統合して、当該内部状態量を更新する状態統合手段、
前記状態量記憶手段に記憶されている内部状態量を前記他の状態推定手段に送信する状態送信手段、
前記状態量記憶手段に記憶されている内部状態量を推定量として出力する推定量出力手段、
として機能させることを特徴とする状態推定プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２００７−７２７０９（Ｐ２００７−７２７０９Ａ）
【公開日】平成１９年３月２２日（２００７．３．２２）
【国際特許分類】
【出願番号】特願２００５−２５８２０６（Ｐ２００５−２５８２０６）
【出願日】平成１７年９月６日（２００５．９．６）
【出願人】（０００００４３５２）日本放送協会 (2,206)
【Ｆターム（参考）】
【公開日】平成１９年３月２２日（２００７．３．２２）
【国際特許分類】
【出願日】平成１７年９月６日（２００５．９．６）
【出願人】（０００００４３５２）日本放送協会 (2,206)
【Ｆターム（参考）】
[ Back to top ]