JP6820204B2 - 状態推定器、及びプログラム - Google Patents

Description

本発明は、所定の状態ベクトルの時間変化を規定するシステムモデルを用いて状態推定を行う状態推定器、及びプログラムに関する。

時系列データ等の所定の状態ベクトルに関する状態推定を行う状態推定器として、大別すると、線形或いはガウス型の時系列データを扱うものと、非線形或いは非ガウス型の時系列データを扱うものがある。

特に、非線形或いは非ガウス型の時系列データを扱う状態推定器は、ＩＰ（Internet Protocol）ネットワークを介して時刻を同期させるシステムの時刻オフセット計測に用いることができる。

まず、この時刻を同期させる技法として、一般的には、ＩＥＥＥ１５８８ＰＴＰ（precision time protocol）が知られており（例えば、特許文献１参照）、この技法に基づいて時刻同期を確立するための時刻制御装置を構成する例も知られている（例えば、特許文献２参照）。

特許文献２には、その時刻制御装置として、マスター装置とスレーブ装置で交換されるパケットの時刻情報から時刻差を算出する減算部及び除算部、時刻差から平均値を算出する平均値算出部、ＰＩＤ（Proportional-Integral-Differential）制御部、及び時刻調整部を備えるよう構成する例が示されている。

ＰＴＰではマスター装置からスレーブ装置に定期的に送信されるSyncメッセージの送信間隔と受信間隔の差が等しくなるように、スレーブ装置の時計の発振周波数を調整することで発振周波数を同期させる。そして、発振周波数同期が確立した後で、スレーブ装置からDelay_reqを送信し、その返信であるDelay_resを受信し、それらのパケットの送信時刻及び受信時刻を用いて、マスター装置の時刻とスレーブ装置の時刻差を計測し、時刻差が０となるようにスレーブ装置の時刻を調整する。この時刻差の計測では、Delay_reqの伝送遅延、Delay_reqの伝送遅延が等しいことを想定している。

ＩＰネットワークは、ルータやスイッチなどが次の装置へとパケットを中継することで、ＩＰネットワークに接続するどの装置に対してもパケットを送ることができるネットワークであるが、一時的にパケットが集中するとパケットが中継されるまでの時間が長くなる。このため伝送遅延時間が変動するパケットジッタと呼ばれる現象が発生する。

この結果、Syncメッセージの送信間隔と受信間隔の差や、計測したマスター装置の時刻とスレーブ装置の時刻差は、パケットジッタの影響を受け観測誤差が多く含まれる。

当該ＰＩＤ制御部は、入力値、入力の積分値、入力の微分値のそれぞれに係数をかけて合計した値を用いて目標の値に制御を行うものであるが、パケットジッタや雑音など不規則に変化する成分を多く含む信号は、高周波成分を多く含み、微分値が過大になりＰＩＤ制御に悪影響を及ぼすことがある。

（ローパスフィルタの利用）
そこで、ＩＰネットワークを介して時刻を同期させるシステムでは、観測した時刻差をそのまま時刻制御に使用するのではなく、平均値を用いたり、ローパスフィルタを用いて観測誤差や雑音の制御への影響を軽減することが行われている。

ただし、ローパスフィルタでは、振幅の大きなパルス状の雑音が加わった場合に、その低周波成分が信号として加味され、わずかであるが出力に悪影響を与えることがある。

このため、ローパスフィルタのように時系列データと雑音の周波数分布の違いを利用して信号の観測精度を高める代わりに、時系列データを扱うシステムの状態空間モデルを作成し、観測信号から状態空間モデルの状態を予測し、予測した状態空間モデルの状態からの観測信号を抽出することで、信号の観測精度を高めたり、観測遅延を短くしたり、予測した状態そのものを直接制御に利用することが行われている。

そして、状態及び観測値の統計的性質を利用することによって時系列モデルの状態を推定する状態推定器として、カルマンフィルタや粒子フィルタが知られている。

（カルマンフィルタの利用）
カルマンフィルタは、状態や観測値に含まれる雑音が正規分布に従う線形のシステムに適用できる技法であり広く利用されている。しかし、カルマンフィルタは、正規分布に従わないシステムや非線形なシステム、或いは時系列に急激な構造変化がみられるような場合では良い結果が得られない。

（粒子フィルタの利用）
そこで、非線形或いは非ガウス型の時系列データを扱うシステムに適用できる状態推定器として、粒子フィルタが知られており（例えば、非特許文献１）、粒子フィルタは画像中における被写体の追尾などにも利用されている（例えば、特許文献３参照）。

粒子フィルタは、１次元もしくは複数次元ベクトルで表現された状態ベクトルの時間変化を規定する所定のシステムモデルを用いて状態推定を行う状態推定器であり、状態ベクトルとその状態ベクトルに対応する重みの集合によって状態の確率分布を表現する。予測対象の状態の確率分布を確率変数をｘとする確率密度関数ｐ（ｘ）とすると、これを或る状態ベクトルｘ_ｉ（以下、状態ｘ_ｉとする）とその状態ｘ_ｉの重みｗ_ｉを粒子とする集合によって表現することが特徴である。重みｗ_ｉは、個々の状態ｘ_ｉの尤度を示す。

状態ｘの確率分布をＮ個の状態ベクトルｘ_i（ｉ＝１，２．．．Ｎ）とその状態ベクトルの重みｗ_ｉ（ｉ＝１，２．．．Ｎ）を用いて、確率密度関数ｐ（ｘ）は式（１）のように表すことができる。

ここでδはディラックのデルタ関数であり、状態ベクトルとその状態ベクトルの重みを粒子と呼ぶ。尚、粒子フィルタでは、フィルタ分布計算によりｗ_ｉの値が１以外の値をとるが、直後のリサンプル処理を行うことでｗ_ｉの値は１となる。

例えば、図８に示す粒子フィルタの処理手順を基に、図９を参照して、移動物体（図示する例では車両）の位置と速度を推定する例を説明する。図８は、従来からの粒子フィルタによる処理を示すフローチャートである。また、図９は、従来からの粒子フィルタにおける全体的な動作例を示す説明図である。

ここで、ｘ（ｔ）, ｙ（ｔ）, ｖ（ｔ）, ｕ（ｔ）を、それぞれ時刻ｔにおける状態ベクトル（以下、状態ｘ（ｔ）とする）、観測値のベクトル（以下、観測値ｙ（ｔ）とする）、システム雑音のベクトル（以下、雑音ベクトルｖ（ｔ）とする）、観測雑音のベクトル（以下、観測雑音ｕ（ｔ）とする）を表すものとする。そして、時系列データの状態空間モデルが、状態ｘ（ｔ）＝Ｆ（ｘ（ｔ−１），ｖ（ｔ））のシステムモデルと、観測値ｙ（ｔ）＝Ｈ（ｘ（ｔ），ｕ（ｔ））の観測モデルに従うものとする。

尚、図９に示す動物体の時系列データの状態推定システムの例では、状態ｘ（ｔ）をシステム雑音を加えた時刻ｔにおける位置Ｘａ（［ｋｍ］）と速度Ｘｂ（［ｋｍ／ｈ］）で構成される２次元のベクトル（Ｘａ，Ｘｂ）とし、観測値ｙ（ｔ）については、時刻ｔにて観測する位置Ｘａ（ｔ）に観測雑音ｕ（ｔ）を加えたものとしている。

まず、図８に示すように、粒子フィルタの処理は、最初に重みが１の粒子を用いて初期状態ｘ（ｔ＝０）の分布に対応する粒子を設定する （ステップＳ１１０）。粒子フィルタの処理は、初期時刻ｔ＝０以後、時刻ｔ＝１，２，…として単位時間ステップ毎にサンプリングされて時系列に処理し、図９に示すように、前回時刻ｔ−１の状態推定結果を粒子で近似して示す分布（以下、ｘ（ｔ−１｜ｔ−１）と記す）とシステムモデルから、今回時刻ｔの状態を予測し（以下、観測値ｙ（ｔ）を用いずに予測した、状態の予測結果を予測分布ｘ（ｔ｜ｔ−１）と記す）、この予測分布ｘ（ｔ｜ｔ−１）と観測値ｙ（ｔ）を用いて今回時刻ｔの状態を推定する処理を繰り返すことになる。以下、予測分布ｘ（ｔ｜ｔ−１）と観測値ｙ（ｔ）を用いて推定した状態の分布をフィルタ分布ｘ（ｔ｜ｔ）と記す。

続いて、図８及び図９に示すように、粒子フィルタの処理は、前回時刻ｔ−１の状態の推定結果のフィルタ分布ｘ（ｔ−１｜ｔ−１）を構成する粒子を基に、その粒子毎に、擬似乱数を計算し、システムモデルｘ（ｔ）＝Ｆ（ｘ（ｔ−１），ｖ（ｔ））に、各粒子の状態ベクトルｘ_ｉをｘ（ｔ−１）に、疑似乱数から生成したシステム雑音をｖ（ｔ）に代入して、各粒子の新たな状態を更新することにより、時刻ｔの状態の予測分布ｘ（ｔ｜ｔ−１）を計算する（ステップＳ１２０）。

図１０を参照して、より具体的に、本例における時刻ｔの状態の予測分布ｘ（ｔ｜ｔ−１）の計算について説明する。図１０は、従来からの粒子フィルタにおける予測分布計算の動作例を示す説明図である。

予測分布の計算にあたって、粒子フィルタの処理は、時刻ｔ−１の各粒子に対して、各粒子の状態ベクトルと疑似乱数から生成したシステム雑音ｖ（ｔ）をシステムモデルｘ（ｔ）＝Ｆ（ｘ（ｔ−１），ｖ（ｔ））に与えることで計算する。

粒子フィルタでは非線形のシステムモデルを扱うことができるが、簡単のためシステムモデルをｘ（ｔ）＝Ｆｘ（ｘ（ｔ−１））＋Ｆｖ（ｖ（ｔ））と表せる線形モデルを例として説明する。時刻ｔにおける各粒子の状態ベクトルは、システム雑音が無ければ、時刻ｔ−１における各粒子の状態ベクトル（図１０（ａ）のｘ（ｔ−１）の各点の値を参照）とｖ（ｔ）＝０をシステムモデルに与えて得られた状態Ｆｘ（ｘ（ｔ−１））となる（図１０（ｂ）参照）。システムモデルでは雑音の影響がＦｖ（ｖ（ｔ））の項として加わるため、Ｆｘ（ｘ（ｔ−１））の値に、擬似乱数を用いてｖ（ｔ）を生成して計算したＦｖ（ｖ（ｔ））を加える。これが時刻ｔにおける各粒子の状態ベクトルの予測値となる（図１０（ｃ）参照）。これらの予測した粒子で表現される分布が予測分布ｔ（ｔ｜ｔ−１）である。

図１０（ｃ）のように、システム雑音有りとして計算することで、時刻ｔにおける粒子の状態ベクトルの予測値に意図的に拡がりを持たせていることが分かる。システムモデルのモデル化誤差や、状態推定の誤差の影響が蓄積すると、粒子の状態ベクトルの予測値の誤差が大きくなる。粒子の状態ベクトルに拡がりがなくなると、実際のシステムの状態ベクトルと一致する状態ベクトルを持つ粒子が無くなってしまう。このようなシステム雑音を付与することで、状態推定誤差やモデル化誤差による大きな予測ずれに伴う推定誤りを予防する作用がある。

続いて、図８及び図９に示すように、粒子フィルタの処理は、当該予測分布ｘ（ｔ｜ｔ−１）における各粒子に対して、観測値ｙ（ｔ）の尤度ｐ（ｙ｜ｘ_ｉ）を計算し、その粒子の重みをｗ_ｉとしたフィルタ分布ｘ（ｔ｜ｔ）を計算する（ステップＳ１３０）。

ここで、各粒子が表す状態ベクトルは、動物体（本例では車両）の予測位置と予測速度を表している。各粒子の状態ベクトルの予測値は粒子毎に異なるが、実際に観測して得られた位置の観測値ｙ（ｔ）が１０．２ｋｍであったと仮定して（図９参照）、図１１を参照して、より具体的に、本例における状態のフィルタ分布ｘ（ｔ｜ｔ）の計算について説明する。図１１は、従来からの粒子フィルタにおけるフィルタ分布計算の動作例を示す説明図である。

時刻ｔの観測値ｙ（ｔ）＝１０．２ｋｍが得られている場合でも、その観測結果には観測雑音が含まれるため、観測値と実際の位置は必ずしも一致しない。そこで、フィルタ分布の計算にあたって、粒子フィルタの処理は、予測分布ｘ（ｔ｜ｔ−１）における各粒子に対して（図１１（ａ）参照）、時刻ｔの観測値ｙ（ｔ）＝１０．２ｋｍと一致に対応する粒子に最も高い尤度を割り当て、この観測値ｙ（ｔ）に対応する粒子から遠ざかるに従い各粒子に低い尤度を割り当てる（図１１（ｂ）参照）。尤度計算には種々の技法があるが、ここでは単純な正規分布曲線に近似して割り当てる例とする。このように、粒子で表される各状態に対して、観測値が得られる尤度を計算しその値を各粒子の重みｗ_ｉとする（図１１（ｃ）参照）。図９及び図１１（ｃ）では、粒子の重みｗ_ｉを黒丸の大きさで表している。

続いて、図８及び図９に示すように、粒子フィルタの処理は、フィルタ分布の各粒子に対し、粒子の重みの大きさに比例するように粒子を割り当て直すリサンプル処理を行う（ステップＳ１４０）。リサンプル処理は、状態ｘ_ｉを持つ粒子の数が、ステップＳ１３０で計算した重みｗ_ｉに比例した数となるように重み１の新たな粒子を割り当て、或いは削除する処理であり、重みの大きな粒子は重み１の新たな粒子で複製し、重みの小さな粒子は削除する。このリサンプル処理の後、ｔに１を加算して次の時刻における予測を行うようステップＳ１２０に移行する。

尤度に基づく重みづけの結果、リサンプル処理（ステップＳ１４０）後の粒子は、図９に例示するように、予測分布の計算（ステップＳ１２０）で得られる当該予測分布における粒子より集中し、高い密度の粒子分布が得られる。これらの粒子の集合が、時刻ｔにおける状態分布の推定結果を表すことから、粒子の状態ベクトルの期待値や、中央値、信頼区間を計算することで、時刻ｔの状態ベクトルの期待値や中央値、信頼区間を導出することができる（例えば位置の期待値として１０．３ｋｍ、速度の期待値として３９ｋｍ／ｈ）。

このように粒子フィルタでは、直前の状態分布及びシステムモデル、観測値を用いて次の時間ステップの状態分布の推定を行い、その結果得られた状態分布を次の時刻の予測に用いる。この処理を逐次繰り返すとともに、粒子の集合に期待値計算など統計処理を行うことで、状態推定を行うことができる。

特開２０１０−１９０６３５号公報 特開２０１３−８３４５０号公報 特開２０１６−５８８３６号公報

樋口 知之, "粒子フィルタ", 電子情報通信学会誌, Vol.88, No.12, pp.989-994, 2005年12月01日発行

ＩＰネットワークを介して時刻を同期させるシステムにおいて、時計や発信器の時刻を高い精度で同期させるためには、非常に低い周波数変化によるわずかな位相変化を検出し補正する必要がある。このため、低い周波数成分をもつ観測誤差や雑音まで十分に除去するフィルタ処理が必要になる。

そこで、ローパスフィルタを利用し、低い周波数成分の誤差や雑音を十分低減させるためには、フィルタのタップ数を増やす必要がある。ローパスフィルタのタップ数を長くすると遅延が増加し、観測に長い時間を要し、時刻を同期させるために要する時間が長くなる。また、パケットジッタは一時的なパケットの集中で発生し、パケットの集中度合いによってその大きさが変化するため、観測期間中に遅延が大きく変動し、観測精度が悪化したり、制御が不安定になる問題がある。

また、ローパスフィルタでは、振幅の大きなパルス状の雑音が加わった場合に、その低周波成分が信号として加味され、出力に影響を与える問題がある。

また、カルマンフィルタは、状態及び観測誤差が正規分布に従うことを前提としている。しかし、パケットの時刻情報から算出される時刻差に含まれる雑音の多くは、ＩＰネットワークの伝送遅延の変化に起因するものであり、これはＩＰネットワークの混雑状況に応じて複雑に変化し、誤差の分布が変化してしまう。カルマンフィルタは、時系列に急激な構造変化がみられるような場合や、正規分布に従わないシステムや非線形なシステムでは良い結果が得られない問題があった。

また、粒子フィルタは、複雑な分布に対応でき、尤度分布の変化に柔軟に対応できる利点がある。しかし、予測分布計算に粒子の数に比例した数の擬似乱数を作成する必要があり、また、リサンプル処理に大きな計算負荷が発生する。このため、粒子フィルタは計算負荷が大きく、高速に実時間で動作する用途に利用することが困難であった。

つまり、ＩＰネットワークを介して時刻を同期させるシステムに限らず、動物体の状態推定を行う制御装置などにおいても、高速に実時間で動作することが要望されるときには、少ない計算量且つ短い遅延にて精度よく状態を推定し、観測誤差の影響を除去可能とする状態推定器の利用が望まれる。

本発明の目的は、上述の問題に鑑みて、所定の状態ベクトルの時間変化を規定するシステムモデルを用いて状態推定を行う際に、少ない計算量且つ短い遅延にて精度よく状態を推定し、観測誤差の影響を除去可能とする状態推定器、及びプログラムを提供することにある。

本発明の状態推定器は、１次元もしくは複数次元ベクトルの粒子で表現された状態ベクトルの時間変化を規定する所定のシステムモデルを用いて状態推定を行う状態推定器であって、状態ベクトルとその状態ベクトルの重みの集合によって表現される状態の確率分布に従い、入力される観測値に基づいて、前記所定のシステムモデルに従う複数の粒子の状態ベクトルにそれぞれ対応する尤度を計算する尤度計算部と、前記観測値の観測時刻毎に、前回計算したフィルタ分布を構成する各粒子の状態ベクトルに対し、システム雑音が無いものとして前記所定のシステムモデルに従って各粒子の状態ベクトルと該各粒子の状態ベクトルの重みを更新し、該重みに拡散処理を施して予測分布を構成する粒子を追加もしくは各粒子の重みを更新し、該重みに該予測分布を構成する各状態ベクトルに対し前記尤度計算部によって計算される尤度を乗じた値を新たな重みとして更新し、各粒子の状態ベクトルにおける該重みを一定の範囲に収めるレンジ調整処理を施し、該レンジ調整処理を施して得られる各粒子の状態ベクトルと該各粒子の状態ベクトルの重みで構成されるフィルタ分布を今回のフィルタ分布として計算するフィルタ分布更新部と、前記フィルタ分布更新部により当該フィルタ分布を計算する毎に、各粒子の状態ベクトルの重みを更新して保持する重みレジスタと、当該今回のフィルタ分布を構成する各状態ベクトルとその状態ベクトルの重みから所定の推定値を導出する推定値計算部と、を備えることを特徴とする。

また、本発明の状態推定器において、前記フィルタ分布更新部は、前記拡散処理として、当該前回計算したフィルタ分布を構成する各状態ベクトルに対し前記所定のシステムモデルに従って更新した各粒子の状態ベクトルと該各粒子の状態ベクトルの重みを基にして加重平均又はローパスフィルタ処理、もしくは全部もしくは一定範囲の状態ベクトルの重みに対し均等に一定値を加算する処理により新たな重みを計算する処理、を含むことを特徴とする。

また、本発明の状態推定器において、前記フィルタ分布更新部は、前記レンジ調整処理として、当該フィルタ分布内の複数の状態ベクトルの全ての重みを一定の比率で増減させ、最も大きな重みの値を一定の範囲に収める処理を含むことを特徴とする。

また、本発明の状態推定器において、前記フィルタ分布更新部は、前記レンジ調整処理として、前記重みレジスタへと更新する重みの最大値が第１閾値を超えた場合、その全ての重みを一定の比率で小さくし、前記重みレジスタへと更新する重みの最大値が第２閾値以下となった場合に、全ての重みを一定の比率で大きくする処理を含むことを特徴とする。

また、本発明の状態推定器において、前記フィルタ分布更新部は、前記レンジ調整処理として、前記重みレジスタへと更新する重みの最大値が前記第１閾値を超える場合には、その全ての重みを１／２倍の比率で小さくし、前記重みレジスタへと更新する重みの最大値が前記第２閾値以下となる場合には、全ての重みを２倍の比率で大きくする処理を含むことを特徴とする。

また、本発明の状態推定器において、前記尤度計算部は、各粒子の状態ベクトルに対する尤度の合計を１とする正規化処理を省略して尤度を計算し、該尤度の値、若しくは該尤度の定数倍の値を前記フィルタ分布更新部に出力することを特徴とする。

更に、本発明のプログラムは、コンピュータを、本発明の状態推定器として機能させるためのプログラムとする。

本発明によれば、所定の状態ベクトルの時間変化を規定するシステムモデルを用いて状態推定を行う際に、少ない計算量且つ短い遅延にて精度よく状態を推定し、観測誤差の影響を除去可能とする状態推定器を構成することができる。

本発明による一実施形態の状態推定器の概略構成を示すブロック図である。 本発明による一実施形態の状態推定器におけるフィルタ分布更新処理を示すフローチャートである。 本発明による一実施形態の状態推定器におけるフィルタ分布更新部の全体的な動作例を示す説明図である。 本発明による一実施形態の状態推定器におけるフィルタ分布更新部のフィルタ分布計算処理に関する動作例を示す説明図である。 本発明による一実施形態の状態推定器におけるフィルタ分布更新部の処理に係る重みレジスタに関する動作例を示す説明図である。 本発明による一実施形態の状態推定器におけるフィルタ分布更新部の処理に係る尤度計算に関する動作例を示す説明図である。 本発明による一実施形態の状態推定器におけるフィルタ分布更新部のレンジ調整処理に関する動作例を示す説明図である。 従来からの粒子フィルタによる処理を示すフローチャートである。 従来からの粒子フィルタにおける全体的な動作例を示す説明図である。 従来からの粒子フィルタにおける予測分布計算の動作例を示す説明図である。 従来からの粒子フィルタにおけるフィルタ分布計算の動作例を示す説明図である。

以下、図１乃至図７を参照して、本発明による一実施形態の状態推定器１について説明する。

（装置構成）
まず、図１に、本発明による一実施形態の状態推定器１の概略構成を示している。状態推定器１は、１次元もしくは複数次元ベクトルの粒子で表現された状態ベクトルの時間変化を規定する所定のシステムモデルを用いて、入力される観測値を基に状態推定を行う装置として構成され、状態ベクトルとその状態ベクトルの重みの集合によって状態の確率分布を表現することを特徴とし、尤度計算部１１、フィルタ分布更新部１２、重みレジスタ１３、及び推定値計算部１４を備える。

本発明に係る状態推定器１において、状態ｘの確率分布をＮ個の状態ベクトルｘ_ｉ（ｉ＝１，２．．．Ｎ）とその状態ベクトルの重みｗ_ｉ（ｉ＝１，２．．．Ｎ）を用いて、確率密度関数ｐ（ｘ）は上述した式（１）のように表すことができる。

尤度計算部１１は、入力された観測値から予測分布ｘ（ｔ｜ｔ−１）を構成する各粒子の状態ベクトルに対応する尤度を計算し、その値、若しくはその定数倍の値をフィルタ分布更新部１２に出力する機能部である。尤度計算部１１は、状態推定対象のシステムが状態ベクトルで表される状態であるときに観測値が得られる尤度を計算する。尤度の計算方法は、粒子フィルタで用いる尤度計算方法をそのまま用いることができる。

ただし、本発明に係る尤度計算部１１によって計算される尤度は、各状態ベクトルに対する尤度の合計が１となるよう必ずしも正規化する必要はない。即ち、割算回路（割算処理）を設けて尤度を正規化する処理を省略し処理の高速化に寄与させるよう構成することができる。この正規化処理を省略しても、後述するレンジ調整処理が設けられていることから動作上の不具合が生じないようになっている。

フィルタ分布更新部１２は、観測時刻毎に、前回計算したフィルタ分布ｘ（ｔ−１｜ｔ−１）を構成する各粒子の状態ベクトルに対し、システム雑音が無いとして、１次元もしくは複数次元ベクトルで表現された状態ベクトルの時間変化を規定する所定のシステムモデルに従って、各粒子の状態ベクトル及び重みレジスタ１３に保持されている各粒子の重みを更新し、更新した各粒子の状態ベクトル及びこの各粒子の状態ベクトルに対する重みに拡散処理を施し更新して予測分布ｘ（ｔ｜ｔ−１）を計算し、この予測分布ｘ（ｔ｜ｔ−１）を構成する各粒子に対して、当該入力された観測値を基に尤度計算部１１によって計算される当該粒子の状態ベクトルに対する尤度を当該粒子の重みに乗じた値を新たな重みとすることにより更新して、レンジ調整前のフィルタ分布ｘ’（ｔ｜ｔ）を計算する。そして、フィルタ分布更新部１２は、当該レンジ調整前のフィルタ分布ｘ’（ｔ｜ｔ）を構成する各粒子の重みについてレンジ調整処理を施し、当該重みレジスタ１３に保持されていた重みを当該レンジ調整処理後の重みへ更新し、レンジ調整処理を施した重みが付与されている各粒子で表現されるフィルタ分布ｘ（ｔ｜ｔ）を、推定値計算部１４へ出力する。

より具体的には、フィルタ分布更新部１２は、まず、観測値を得た時刻毎に前回計算したフィルタ分布ｘ（ｔ−１｜ｔ−１）を構成する各粒子の状態ベクトルに対して、重みレジスタ１３に保持されている重みを基に、システム雑音が無いとして、観測値を得た時刻における状態ベクトルを計算して各粒子の状態ベクトルを更新し、重みレジスタ１３に保持されている重みをこの更新後の各粒子の状態ベクトルに対応づけて更新する処理を行う。尚、フィルタ分布更新部１２は、２つ以上の粒子の状態ベクトルの更新後の状態ベクトルが一致する場合には、粒子を１つにまとめ、それらの更新前の粒子の重みを加算した値をまとめた粒子の重みとする処理を行う。続いて、フィルタ分布更新部１２は、このシステム雑音無しとして計算した各状態ベクトルに対応する重みに対し、加重平均又はローパスフィルタ処理、もしくは一定範囲の状態ベクトルの重みに均等に一定値を加算する処理により、予測分布の分散を拡大する拡散処理を行い、予測分布ｘ（ｔ｜ｔ−１）とする。

一定値を加算する一定範囲としては、例えば、前回推定した状態ベクトルの期待値を中心とする前回推定した状態ベクトルの標準偏差の定数倍の範囲内や、状態ベクトルの範囲を限定する場合はその範囲内全てとすることができる。

これにより、擬似乱数を用いてシステム雑音ｖ（ｔ）を計算することなしに、前回のフィルタ分布ｘ（ｔ−１｜ｔ−１）から計算した観測時刻における状態予測分布を拡散し、状態の予測範囲を意図的に拡がりを持たせることができる。このように、フィルタ分布更新部１２は、前回計算したフィルタ分布ｘ（ｔ−１｜ｔ−１）を構成する各粒子の状態ベクトルと重み、システムモデルを用いて観測時刻におけるシステム雑音無しの状態予測分布を計算しこれに拡散処理を施すことで、擬似乱数を計算することなしに拡散された予測分布ｘ（ｔ｜ｔ−１）を計算する。

続いて、フィルタ分布更新部１２は、拡散処理を施した予測分布ｘ（ｔ｜ｔ−１）を構成する各状態ベクトルに対して当該入力された観測値を基に尤度計算部１１によって計算されている尤度を当該粒子の重みに乗じた値を新たな重みとすることにより、フィルタ分布ｘ（ｔ｜ｔ）を計算する。

続いて、フィルタ分布更新部１２は、当該フィルタ分布ｘ（ｔ｜ｔ）を構成する各状態ベクトルの重みに対して一定の範囲に収めるレンジ調整処理を施し、当該重みレジスタ１３に保持されていた重みを当該レンジ調整処理後の重みで更新する。従って、フィルタ分布更新部１２は、重みレジスタ１３に対し、フィルタ分布を計算する毎に当該フィルタ分布を構成する各粒子の状態ベクトルの各々の重みを更新して保持させる。

レンジ調整処理は、当該フィルタ分布内の複数の状態ベクトルの全ての重みを一定の比率で増減させ、最も大きな重みの値を一定の範囲に収める処理である。特に、このレンジ調整処理は、重みレジスタ１３へと更新する重みの最大値が第１閾値Ｔｈ１を超える場合には、その全ての重みを一定の比率で小さくし、重みレジスタ１３へと更新する重みの最大値が第２閾値Ｔｈ２以下となる場合には、全ての重みを一定の比率で大きくする。このレンジ調整処理があるため、フィルタ分布を計算するために用いる尤度の正規化処理を省略し、全体的な動作を高速化させることができる。

そして、フィルタ分布更新部１２は、状態ベクトル及びレンジ調整処理を施した重みが付与されている粒子で表現されるフィルタ分布を推定値計算部１４へ出力する。

重みレジスタ１３は、初期値として所定のシステムモデルに従う予め定められた複数の粒子の状態ベクトルにそれぞれ対応する重みを記憶しているが、フィルタ分布更新部１２によりフィルタ分布を計算する毎に、当該フィルタ分布を構成する複数の粒子の状態ベクトルの重みを更新して保持する機能部である。つまり、重みレジスタ１３は、今回計算するフィルタ分布を構成する粒子の状態ベクトルに対応づけるために前回値の重みを読み出し、今回計算後のフィルタ分布内の複数の状態ベクトルに対応づけて今回値の重みを書き込むことで、フィルタ分布を計算する毎に、当該フィルタ分布内の複数の状態ベクトルの重みを更新して保持する機能部である。尚、重みレジスタ１３は、フィルタ分布更新部１２が予測分布ｘ（ｔ｜ｔ−１）を計算する際にも用いられる。

推定値計算部１４は、フィルタ分布更新部１２から得られるレンジ調整処理を施した重みが付与されている各粒子の状態ベクトル及び重みで表現される今回のフィルタ分布ｘ（ｔ｜ｔ）から、粒子の状態ベクトルの期待値や中央値或いは信頼区間などを計算することで推定した状態の期待値や中央値、或いは信頼区間を導出し、推定値として外部に出力する機能部である。

（装置動作）
以下、図１に示す状態推定器１の動作について、従来技法の粒子フィルタと対比可能にするために、図２及び図３に示すフィルタ分布更新処理を中心に詳細に説明する。図２は、本発明による一実施形態の状態推定器１におけるフィルタ分布更新部１２の処理（フィルタ分布更新処理）を示すフローチャートである。図３は、本発明による一実施形態の状態推定器１におけるフィルタ分布更新部１２の全体的な動作例を示す説明図である。

図２及び図３を参照して説明する前に、まず、本実施形態に係る状態推定器１は、予測対象の状態（確率変数）Ｘの確率密度関数ｐ（Ｘ）を、状態ｘ_ｉとその状態の重みｗ_ｉを粒子とする集合によって表現する点で粒子フィルタと同様であるが、大きな相違点として、疑似乱数ではなく拡散処理によりシステム雑音の付与に相当する予測分布の拡散を行う点と、フィルタ分布に基づく状態推定結果を得るにあたり、リサンプル処理を行うのではなくレンジ調整を行う点で相違している。

また、ｘ（ｔ）, ｙ（ｔ）, ｖ（ｔ）, ｕ（ｔ）は、それぞれ時刻ｔにおける状態のベクトル、観測値のベクトル、システム雑音のベクトル、観測雑音のベクトルを表すものとする。そして、時系列データの状態空間モデルが、状態ｘ（ｔ）＝Ｆ（ｘ（ｔ−１），ｖ（ｔ））のシステムモデルと、観測値ｙ（ｔ）＝Ｈ（ｘ（ｔ），ｕ（ｔ））の観測モデルに従うものとする。

そして、図３に示す動物体の時系列データの状態推定システムの例では、状態ｘ（ｔ）をシステム雑音を加えた時刻ｔにおける位置Ｘａ（［ｋｍ］）と速度Ｘｂ（［ｋｍ／ｈ］）で構成される２次元のベクトル（Ｘａ，Ｘｂ）とし、観測値ｙ（ｔ）については、時刻ｔにて観測する位置Ｘａ（ｔ）に観測雑音ｕ（ｔ）を加えたものとしている。時刻ｔにおける状態ｘ（ｔ）の取りうる状態ｘ_ｉを仮にＸａ＝｛１０．１, １０．２, １０．３, １０．４, １０．５, １０．６｝, Ｘｂ＝｛３９，４０，４１｝に限定して、その動作を説明する。この場合、状態（Ｘａ，Ｘｂ）はＸａとＸｂを組みあわた１８の状態をとりうる。本例の処理上では、時刻ｔにおいてはこの１８通りのいずれかの状態をとるよう構成する例を説明するが、時間経過とともに取りうる状態は更新されるべきものであり、また、内挿補間するよう構成することもできる。

ここで、状態ｘ（ｔ）の取りうる各状態をビット列や数値に対応させるのが好適である。例えば、Ｘａに対して｛“００１”, “０１０”, “０１１”, “１００”, “１０１”, “１１０”, “１１１” ｝を、Ｘｂに対して｛“００”, “０１”, “１０”｝を対応させることで、（Ｘａ，Ｘｂ）の状態を、これら２進数を結合した５ビットの２進数に対応づけることができ、処理構成を容易化し高速処理に寄与させることができる。このほか、１８種類の各状態に順番に１から１８までの整数を対応させてもよい。

図１に示す状態推定器１におけるフィルタ分布更新部１２は、尤度計算部１１から得られる尤度と、重みレジスタ１３に保持されている重みを基に、本例では２次元で表される状態の変化を規定するシステムモデルに従って、観測値ｙ（ｔ）を得る毎に前回計算したフィルタ分布ｘ（ｔ−１｜ｔ−１）から、推定値を導出可能な今回のフィルタ分布ｘ（ｔ｜ｔ）を求め、今回のフィルタ分布とするよう動作する。

尤度計算部１１は、観測値ｙ（ｔ）＝ ｙを入力し、状態ｘ（ｔ）がｘ_ｉの時に観測値ｙを観測する尤度ｐ（ｙ｜ｘ_ｉ）を計算し、その値、若しくはその定数倍の値をフィルタ分布更新部１２に出力する。

尤度計算部１１とフィルタ分布更新部１２との接続は、例えばフィルタ分布更新部１２がフィルタ分布を計算する過程において、その計算に必要となった状態ｘ_ｉの尤度を尤度計算部１１に対し逐次要求し、尤度計算部１１は、その要求された状態ｘ_ｉについての観測値ｙに基づく尤度を計算してフィルタ分布更新部１２に出力する構成とすることができる。この状態ｘ_ｉの尤度の要求は、状態ｘ（ｔ）の取りうる各状態ｘ_ｉを対応させた当該ビット列や数値を用いることで指定することができる。このほか、尤度計算部１１は、当該取りうる状態ｘ_ｉの尤度を一度に計算して、当該状態ｘ_ｉに対応するビット列や数値を対応する順序で、一括してフィルタ分布更新部１２に出力する構成とすることもできる。

そして、重みレジスタ１３は、特定の状態ｘ（ｔ）＝ｘ_ｉを持つ粒子の重みｗ_ｉを記憶し、その重みｗ_ｉを状態ｘ（ｔ）＝ｘ_ｉに対応づけて読み出す機能及び書き込む機能を持つ記憶部である。以下、時刻ｔの状態ｘ_ｉに対応する重みレジスタをｗ_ｉとする。また、図３に例示する動物体の位置予測を行うシステムにおける、状態（Ｘａ，Ｘｂ）の重みをＷ（Ｘａ，Ｘｂ）とする。

上述したように、状態ｘ（ｔ）の取りうる状態は、本例では、ＸａとＸｂの組み合わせ１８通りである。これらの各状態に対応する重みがあることから、重みレジスタ１３は、１８通りの重みＷを記憶し、フィルタ分布更新部１２の制御によって各状態に対応する重みＷを指定して読み出し、書き込みを行うよう動作する。そして、前述したように、各状態をビット列や数値に対応させることで、重みレジスタ１３においても各状態を図示しない記憶装置のアドレスに対応づけることができ、容易に実装でき、処理の高効率化を図ることができる。

（フィルタ分布更新処理）
まず、図２に示すように、状態推定器１におけるフィルタ分布更新部１２は、その処理に先立ち、重みレジスタ１３に初期重みを設定する（ステップＳ１）。ここで、各状態の初期分布を予測できる場合にはその値を設置するが、予測できない場合は、均等な重みを設定してもよい。以後、重みレジスタ１３に保持されている重みは、フィルタ分布更新部１２の処理によって、以下に説明する前回のフィルタ分布ｘ（ｔ−１｜ｔ−１）に基づくシステム雑音を無しとした予測分布ｘ’（ｔ｜ｔ−１）の計算時、続いて拡散処理を施して得られる予測分布ｘ（ｔ｜ｔ−１）の計算時、続いて尤度を加味したフィルタ分布ｘ（ｔ｜ｔ）の計算時、最後にレンジ調整処理を施したフィルタ分布ｘ（ｔ｜ｔ）の計算時にて、各粒子の状態ベクトルの更新に対応付けて更新される。

そして、フィルタ分布更新部１２は、「前回のフィルタ分布ｘ（ｔ−１｜ｔ−１）」における各粒子（状態ベクトル及びその重み）に対して重みレジスタ１３に保持されている重みを基に拡散処理を施して予測分布ｘ（ｔ｜ｔ−１）を計算し（ステップＳ２）、この予測分布における各粒子に対して当該入力された観測値ｙ（ｔ）を基に尤度計算部１１によって計算される尤度を当該粒子の重みに乗じた値を新たな重みとすることにより、フィルタ分布ｘ（ｔ｜ｔ）を計算する（ステップＳ３）。

そして、フィルタ分布更新部１２は、当該フィルタ分布ｘ（ｔ｜ｔ）における各粒子における重みについてレンジ調整処理を施し（ステップＳ４）、当該重みレジスタ１３に保持されていた重みを当該レンジ調整処理後の重みへ更新し、レンジ調整処理を施した重みが付与されている各粒子で表現される今回のフィルタ分布ｘ（ｔ｜ｔ）を推定値計算部１４へ出力する。このステップＳ２乃至Ｓ４の繰り返しで、フィルタ分布が更新される。

以下、図３を参照しながら、フィルタ分布更新部１２におけるステップＳ２乃至Ｓ４の各処理を順に説明する。

まず、フィルタ分布更新部１２は、前回のフィルタ分布ｘ（ｔ−１｜ｔ−１）から拡散処理に基づく予測分布ｘ（ｔ｜ｔ−１）を計算する（ステップＳ２）。

ここで、前述した粒子フィルタの処理では擬似乱数ｖ（ｔ）を作成した後、システムモデルｘ（ｔ）＝Ｆ（ｘ（ｔ−１），ｖ（ｔ））に適用することで予測分布ｘ（ｔ｜ｔ−１）を計算するものとなっているが、この擬似乱数の計算に多くの計算資源が必要となり、高速処理の妨げとなる。

そこで、本発明に係るフィルタ分布更新部１２は、擬似乱数の生成を行うことなく、重みレジスタ１３に保持されている重みを基に状態変数空間上で近傍に存在する粒子に拡散するよう構成している。

この拡散処理は例えば、近傍に存在する粒子の重みを用いた加重平均やローパスフィルタ、もしくは一定範囲の状態ベクトルの重みに均等に一定値を加算する処理を用いることができる。以後、拡散処理を行った各粒子の新たな重みが予測分布ｘ（ｔ｜ｔ−１）に用いられる。

この拡散処理に加重平均やローパスフィルタを用いると、粒子の重みが状態変数空間上で近傍にある粒子に拡散されるため、従来技法の粒子フィルタにおける擬似乱数によって各粒子を拡散する処理と類似の効果を与えることができる。

より具体的には、従来技法の粒子フィルタでは、予測分布の計算を粒子毎に行うために粒子の数×システム雑音の次元数の擬似乱数を計算する必要があったが、本発明に係るフィルタ分布更新部１２では、擬似乱数を用いる代わりに拡散処理とすることで、大幅な計算量の削減が可能である。拡散処理は、例えば、時刻（ｔ）におけるシステム雑音を無しとして予測した各粒子について、それぞれの注目粒子の周囲（例えば４点）に粒子がなければ重み０の新たな粒子を追加し、各粒子に対してその粒子の周囲の重みを加重平均した値を新たな重みとして与えることにより拡散する。

また、別の例としては、図４（ｂ）における全ての格子点に相当する、前回推定したフィルタ分布ｘ（ｔ−１｜ｔ−１）の期待値の近傍の状態ベクトルを持つ粒子について、時刻ｔにおける状態ベクトルの予測値を含む一定範囲の等間隔の状態ベクトルに対し新たな重みを与えて拡散する際に、時刻ｔの時点で既に粒子が存在しなければ、重み０の粒子を追加した後、全ての粒子に対して、その周囲の重みを加重平均した値、もしくはローパスフィルタ処理を施した値、もしくは元の重みに一定の値を加算した値を、新たな重みとして与えることにより拡散する。

より具体的には、予測分布の計算にあたって、フィルタ分布更新部１２は、新たな観測値を得る毎に、まず、前回計算したフィルタ分布ｘ（ｔ−１｜ｔ−１）（図４（ａ）参照）から、重みレジスタ１３に保持されている重みを基に、システムモデルｘ（ｔ）＝Ｆ（ｘ（ｔ−１），ｖ（ｔ））におけるシステム雑音無しとして、ｘ’（ｔ）＝Ｆ（ｘ（ｔ−１），０）の予測分布ｘ’（ｔ｜ｔ−１）を計算する（図４（ｂ）参照）。図４（ａ）等では、粒子の重みｗ_ｉを黒丸の大きさで表している。

例として、状態推定の対象とするシステムが次式で表せる２次元の状態ベクトルをもつ線形のシステムモデルであるとして説明する。
Ｘａ（ｔ）＝Ｘａ（ｔ−１）＋ Ｘｂ（ｔ−１）＊Δｔ ＋ｖ１（ｔ）
Ｘｂ（ｔ）＝Ｘｂ（ｔ−１）＋ｖ２（ｔ）
ここで、２次元の状態ベクトルはＸａとＸｂの要素をもち、時刻ｔにおける状態ベクトルがＸａ（ｔ），Ｘｂ（ｔ）とで構成され、Δｔは前回の観測時刻から今回の観測時刻までの経過時間であり、ｖ１（ｔ），ｖ２（ｔ）は時刻ｔのシステム雑音とする。

従来の粒子フィルタでは、各粒子の予測値を計算する毎に疑似乱数を計算して、時刻ｔ−１の粒子の状態ベクトルと、疑似乱数から計算したｖ１（ｔ）、ｖ２（ｔ）を当該システムモデルに代入することにより状態ベクトルの予測値を計算していた。従来の粒子フィルタでは、リサンプル処理によって粒子が複製されるため、同じ状態ベクトルをもつ粒子が多数存在するが、疑似乱数の項があるため、時刻ｔ−１の状態ベクトルが同じ値をもつ粒子であっても、状態ベクトルの予測値は同一の値とならない。このように、システム雑音は予測分布の分散を大きくする効果をもっている。

一方、本発明に係る状態推定器１では、疑似乱数の計算を行わないものとしている。このため本発明に係る状態推定器１は、システム雑音を無しとして、すなわちｖ１（ｔ），ｖ２（ｔ）を０として、各粒子のｔ−１の状態を当該システムモデルに代入することで、粒子の状態ベクトルの予測値を計算した後、状態ベクトルの拡散処理を行う。このようにして、本発明に係る状態推定器１では、拡散処理により予測分布の分散を大きくすることで、システム雑音の付与と同様の効果を得ることができる。

本発明に係る状態推定器１では、図５に示すように、システム雑音を無しとして、すなわちｖ１（ｔ），ｖ２（ｔ）を０として、各粒子の状態（Ｘａ，Ｘｂ）をこのシステムモデルに代入し、システム雑音を無しとした状態予測値を計算し、予測前の各粒子の重みをそのまま対応付けて設定する。尚、２つ以上の粒子の予測後の状態が一致する場合には、粒子を１つにまとめ、それらの更新前の粒子の重みを加算した値をまとめた粒子の重みとする。このようにして、図４（ａ）に示すフィルタ分布ｘ（ｔ−１｜ｔ−１）から図４（ｂ）に示す予測分布ｘ’（ｔ｜ｔ−１）を計算する。

図５に示す例ではＷ’（Ｘａ＋Ｘｂ＊Δｔ，Ｘｂ）＝Ｗ（Ｘａ, Ｘｂ）となる。ここで、Ｗ’（Ｘａ，Ｘｂ）はシステム雑音を無しとしたときの重みである。

続いて、フィルタ分布更新部１２は、重みレジスタ１３に保持されている重みを基に、当該フィルタ分布における各粒子に対する近傍位置に新たな粒子を追加する拡散処理を行うことにより、システム雑音ｖ１（ｔ），ｖ２（ｔ）を与え、各粒子の状態ｘ（ｔ）を示す予測分布ｘ（ｔ｜ｔ−１）を計算する（図４（ｃ）参照）。

例えば、加重平均で拡散処理を行う例として、
Ｗ’’（Ｘａ，Ｘｂ）＝（Ｗ’（Ｘａ−０．１，Ｘｂ）
＋Ｗ’（Ｘａ，Ｘｂ）＊１２
＋Ｗ’（Ｘａ＋０．１, Ｘｂ）
＋Ｗ’（Ｘａ，Ｘｂ−１）
＋Ｗ’（Ｘａ，Ｘｂ＋１））／１６
とするように、注目粒子の周囲４点を加えたシステム雑音を無しとしたときの各粒子の重みＷ’（Ｘａ，Ｘｂ）の加重平均で、拡散後の各粒子の重みＷ’’（Ｘａ，Ｘｂ）を計算することができる（図５参照）。

続いて、フィルタ分布更新部１２は、拡散処理を施した予測分布ｘ（ｔ｜ｔ−１）における各粒子の状態ｘ_ｉに対して（図６（ａ）参照）、当該入力された観測値ｙ（ｔ）＝ｙを基に尤度計算部１１によって計算されている尤度ｐ（ｙ|ｘ_ｉ）を割り当て（図６（ｂ）参照）、図６（ｃ）に示すフィルタ分布を計算する（ステップＳ３）。このとき、フィルタ分布更新部１２は、状態ｘ（ｔ）＝ｘ_ｉの重みｗ_ｉに尤度ｐ（ｙ|ｘ_ｉ）を乗じた値を、状態ｘ（ｔ）＝ｘ_ｉの新たな重みｗ_ｉ（図５に示すＷ’’’）として、重みレジスタ１３に保持されている重みを更新する（図４（ｄ）及び図５参照）。

続いて、フィルタ分布更新部１２は、当該フィルタ分布における各粒子における重みを一定の範囲に収めるレンジ調整処理を施し、当該重みレジスタ１３に保持されていた重みを当該フィルタ分布における各粒子に対応付けたレンジ調整処理後の重みへ更新する（ステップＳ４）。従って、フィルタ分布更新部１２は、重みレジスタ１３に対し、フィルタ分布を計算する毎に当該フィルタ分布内の複数の粒子の各々の重みを更新して保持させる。

このように、従来技法の粒子フィルタでは、前述したようにフィルタ分布の計算後にリサンプル処理を行うが、本発明に係るフィルタ分布更新部１２は、リサンプル処理を行わずにレンジ調整を行うよう動作する。

フィルタ分布の計算後では、重みｗ_ｉの値が全体的に小さくなることがあり、その場合には計算精度が低下する。また、重みｗ_ｉの値が極端に大きくなることもあり、オーバーフローが発生して正しい重みから大きく外れてしまい、その場合にも計算精度が低下する。例えば、観測値における観測雑音がその観測真値に対してプラス方向に極めて大きいとき、フィルタ分布内の全体の粒子の重みが影響を受けてしまい、その全ての重みが全体的に小さくなってしまうことがある。また、尤度計算部１１の出力は尤度の定数倍としているため、定数の値によっては重みｗ_ｉの値が極端に大きくなる場合もある。

そこで、フィルタ分布更新部１２は、フィルタ分布の計算後に、当該フィルタ分布内の複数の粒子の全ての重みを一定の比率で増減させ、最も大きな重みの値を一定の範囲に収めるレンジ調整処理を行う。

特に、本実施形態のフィルタ分布更新部１２では、図７に示すように、このレンジ調整処理として、重みレジスタ１３へと更新する重みの最大値が第１閾値Ｔｈ１を超えた場合、その全ての重みを一定の比率で小さくし、重みレジスタ１３へと更新する重みの最大値が第２閾値Ｔｈ２以下となった場合に、全ての重みを一定の比率で大きくする。このレンジ調整処理があるため、フィルタ分布を計算するために用いる尤度の正規化処理を省略し、全体的な動作を高速化させることができる。

更に、重みレジスタ１３へと更新する重みの最大値が第１閾値Ｔｈ１を超えた場合、その全ての重みを「１／２倍」の比率で小さくし、重みレジスタ１３へと更新する重みの最大値が第２閾値Ｔｈ２以下となった場合に、全ての重みを「２倍」の比率で大きくするよう構成すると、１動作クロックによるビットシフトにより実現されるため、より一層、高速化に寄与するものとなる。

そして、フィルタ分布更新部１２は、レンジ調整処理を施した重みが付与されている各粒子で表現された今回のフィルタ分布を推定値計算部１４へ出力する。

推定値計算部１４は、フィルタ分布更新部１２から得られるレンジ調整処理を施した重みが付与されている各粒子で表現された今回のフィルタ分布から、粒子の期待値、中央値、或いは信頼区間を導出し、状態推定結果として外部に出力する。

例えば、図３に示す動物体の状態推定システムの例では、推定値計算部１４は、位置Ｘａの期待値Ｅａは、重みレジスタ１３に保持される時刻ｔの全ての粒子（状態ｘ（ｔ））の重みＷ（Ｘａ，Ｘｂ）を全て読み出し、ΣＸａ＊Ｗ（Ｘａ，Ｘｂ）／ ΣＷ（Ｘａ，Ｘｂ）により計算することができる。尚、Σは全ての状態に対する加算処理を表す。

以上のように、本発明に係る状態推定器１は、フィルタ分布更新部１２により、複数次元で表される状態の変化を規定する所定のシステムモデルに従って観測値を得る毎に前回計算したフィルタ分布における各状態ベクトルに対して重みレジスタ１３に保持されている重みを基に拡散処理を施して予測分布を計算するよう構成しているため、精度よく高速処理が可能である。

また、本発明に係る状態推定器１は、１つの状態に対する粒子が１つにまとめられて、予測分布からフィルタ分布を算出し、リサンプル処理を行うことなくレンジ調整処理を施すよう構成しているため、より一層、高速処理を可能にしている。

一方で、従来技法の粒子フィルタでは、予測分布の計算を粒子毎に行うため、粒子の数に比例した数の擬似乱数を計算する必要となり、粒子の状態の分布が集中する状態推定システムなど、膨大な計算量を要し、高速に実時間で動作する用途に利用することが困難であった。

そして、従来技法の粒子フィルタでは、擬似乱数が関与する処理以外の処理は状態ごとに処理をまとめられるが、リサンプル処理によって粒子のコピーが作られるため、同一の状態を持つ粒子が複数存在することになり、状態と粒子やその重みを１対１で対応づけることができなくなる。このため、従来技法の粒子フィルタは、汎用性が高まるものの非常に処理効率が悪いものとなる。

本発明に係る状態推定器１は、これらの粒子フィルタの問題を解決し、少ない計算量且つ短い遅延にて精度よく状態を推定し、尚且つ観測誤差の影響を除去可能とすることができる。

そして、本発明に係る状態推定器１は、粒子フィルタの利点を生かしつつ、計算負荷の大きい擬似乱数の計算やリサンプル処理を必要としない構成としているため、少ない計算負荷によって、線形或いはガウス型の時系列データを扱うシステムに限らず、正規分布に従わないモデルや非線形なモデルに基づく時系列データの状態推定システムや、尤度分布の変化が大きなシステムの状態推定にも実用的な構成となっている。

尚、本発明に係る状態推定器１は、コンピュータとして機能させることができ、当該コンピュータに、各構成要素を実現させるためのプログラムは、当該コンピュータのメモリに記憶される。当該コンピュータに備えられる中央演算処理装置（ＣＰＵ）などの制御で、各構成要素の機能を実現するための処理内容が記述されたプログラムを、適宜、当該メモリから読み込んで各構成要素の機能を当該コンピュータに実現させることができる。

本発明に係る状態推定器１及びプログラムは、上述した実施形態の例に限定されるものではなく、特許請求の範囲の記載によってのみ制限される。

本発明によれば、時系列データの状態推定を行う際に、少ない計算量且つ短い遅延にて精度よく状態を推定し、尚且つ観測誤差の影響を除去可能とするので、状態空間モデルに基づく時系列データの制御装置、時系列解析装置、或いは動物体追跡等の信号処理装置の用途に有用である。

１ 状態推定器
１１ 尤度計算部
１２ フィルタ分布更新部
１３ 重みレジスタ
１４ 推定値計算部

Claims (7)

1. １次元もしくは複数次元ベクトルの粒子で表現された状態ベクトルの時間変化を規定する所定のシステムモデルを用いて状態推定を行う状態推定器であって、
   状態ベクトルとその状態ベクトルの重みの集合によって表現される状態の確率分布に従い、入力される観測値に基づいて、前記所定のシステムモデルに従う複数の粒子の状態ベクトルにそれぞれ対応する尤度を計算する尤度計算部と、
   前記観測値の観測時刻毎に、前回計算したフィルタ分布を構成する各粒子の状態ベクトルに対し、システム雑音が無いものとして前記所定のシステムモデルに従って各粒子の状態ベクトルと該各粒子の状態ベクトルの重みを更新し、該重みに拡散処理を施して予測分布を構成する粒子を追加もしくは各粒子の重みを更新し、該重みに該予測分布を構成する各状態ベクトルに対し前記尤度計算部によって計算される尤度を乗じた値を新たな重みとして更新し、各粒子の状態ベクトルにおける該重みを一定の範囲に収めるレンジ調整処理を施し、該レンジ調整処理を施して得られる各粒子の状態ベクトルと該各粒子の状態ベクトルの重みで構成されるフィルタ分布を今回のフィルタ分布として計算するフィルタ分布更新部と、
   前記フィルタ分布更新部により当該フィルタ分布を計算する毎に、各粒子の状態ベクトルの重みを更新して保持する重みレジスタと、
   当該今回のフィルタ分布を構成する各状態ベクトルとその状態ベクトルの重みから所定の推定値を導出する推定値計算部と、
   を備えることを特徴とする状態推定器。
2. 前記フィルタ分布更新部は、前記拡散処理として、当該前回計算したフィルタ分布を表現する各状態ベクトルに対し前記所定のシステムモデルに従って更新した状態ベクトルとその状態ベクトルの重みを基にして加重平均又はローパスフィルタ処理、もしくは全部もしくは一定範囲の状態ベクトルの重みに対し均等に一定値を加算する処理により新たな重みを計算する処理、を含むことを特徴とする、請求項１に記載の状態推定器。
3. 前記フィルタ分布更新部は、前記レンジ調整処理として、当該フィルタ分布内の複数の状態ベクトルの全ての重みを一定の比率で増減させ、最も大きな重みの値を一定の範囲に収める処理を含むことを特徴とする、請求項１又は２に記載の状態推定器。
4. 前記フィルタ分布更新部は、前記レンジ調整処理として、前記重みレジスタへと更新する重みの最大値が第１閾値を超えた場合、その全ての重みを一定の比率で小さくし、前記重みレジスタへと更新する重みの最大値が第２閾値以下となった場合に、全ての重みを一定の比率で大きくする処理を含むことを特徴とする、請求項１から３のいずれか一項に記載の状態推定器。
5. 前記フィルタ分布更新部は、前記レンジ調整処理として、前記重みレジスタへと更新する重みの最大値が前記第１閾値を超える場合には、その全ての重みを１／２倍の比率で小さくし、前記重みレジスタへと更新する重みの最大値が前記第２閾値以下となる場合には、全ての重みを２倍の比率で大きくする処理を含むことを特徴とする、請求項４に記載の状態推定器。
6. 前記尤度計算部は、各状態ベクトルに対する尤度の合計を１とする正規化処理を省略して尤度を計算し、該尤度の値、若しくは該尤度の定数倍の値を前記フィルタ分布更新部に出力することを特徴とする、請求項１から５のいずれか一項に記載の状態推定器。
7. コンピュータを、請求項１から６のいずれか一項に記載の状態推定器として機能させるためのプログラム。