(57)【要約】【課題】精度よく推定対象の状態を推定することができる。【解決手段】最適フィルタリング部２０により、各時刻ｔについて、線形な状態更新式、及び非線形な観測方程式をＵＳＬにより近似した線形な観測方程式を用いて予め定められた状態空間モデルで表わされるフィルタに従って、時刻ｔの状態の予測推定値及び状態推定誤差を算出し、時刻ｔの観測値と、前回算出された時刻ｔの状態推定値と、算出された時刻ｔの状態の予測推定値及び状態推定誤差と、前回の繰り返しで算出された時刻ｔの状態推定値を用いて算出される、近似した線形な観測方程式で用いられる傾きの値とに基づいて、予め定められた反復終了条件を満たすまで時刻ｔの状態推定値を繰り返し算出することにより、時刻ｔの状態を推定する。【選択図】図１
2015219714.tif 000002
【特許請求の範囲】
【請求項１】
入力された推定対象の各時刻ｔの観測値に基づいて、前記推定対象の各時刻ｔの状態を推定する状態推定装置であって、
各時刻ｔについて、システムノイズを用いて状態の推定値を時間更新するための線形な状態更新式、及び観測ノイズを用いて状態の推定値と観測値との関係を示す非線形な観測方程式をｕｎｓｃｅｎｔｅｄ  ｓｔａｔｉｓｔｉｃａｌ  ｌｉｎｅａｒｉｚａｔｉｏｎ（ＵＳＬ）により近似した線形な観測方程式を用いて予め定められた状態空間モデルで表わされるフィルタに従って、前記状態の初期値又は時刻ｔ−１について推定された前記状態と、状態推定誤差の初期値又は時刻ｔ−１について算出された状態推定誤差とに基づいて、時刻ｔの状態の予測推定値及び前記予測推定値の状態推定誤差を算出し、
入力された時刻ｔの前記観測値と、前回算出された時刻ｔの状態推定値と、算出された時刻ｔの状態の予測推定値及び前記予測推定値の状態推定誤差と、前回の繰り返しで算出された時刻ｔの状態推定値を用いて算出される、前記近似した線形な観測方程式で用いられる傾きの値とに基づいて、予め定められた反復終了条件を満たすまで時刻ｔの状態推定値を繰り返し算出することにより、時刻ｔの状態を推定し、前記近似した線形な観測方程式で用いられる傾きの値と、前記算出された時刻ｔの前記予測推定値の状態推定誤差とに基づいて、時刻ｔの状態推定誤差を算出する最適フィルタリング部と、
前記最適フィルタリング部によって各時刻ｔについて推定された前記状態を出力する出力部と、
を含む、状態推定装置。
【請求項２】
前記最適フィルタリング部は、以下の（１）式、（２）式に従って、時刻ｔの状態の予測推定値￣Ｘｔ及び前記予測推定値￣Ｘｔの状態推定誤差￣Ｐｔを算出し、以下の（３）式〜（７）式に従って、繰り返し回数ｊにおける時刻ｔの状態推定値＾Ｘｔ,j+1を算出し、以下の（８）式に従って、時刻ｔの状態＾Ｘｔを推定し、以下の（９）式に従って、時刻ｔの状態推定誤差＾Ｐｔを算出する請求項１記載の状態推定装置。
【数１】
2015219714.tif 000012
【数２】
2015219714.tif 000013
ただし、Ｆは予め定められた状態遷移行列であり、ｈ（・）は非線形観測変換であり、Ｑ及びＲは予め定められた共分散行列であり、Ｙｔは時刻ｔにおける前記観測値であり、Ａｔ，ｊは繰り返し回数ｊ回目における前記近似した線形な観測方程式で用いられる傾きの値であり、ｍは前記予め定められた反復終了条件を満たしたときの繰り返し回数であり、χｉ，（ｉ＝０，１，…，２ｎ）は２ｎ＋１個のシグマ点列であり、ｗｉはχｉに対応する重みであり、Ｃ（〜ｉ）は−Ｐｔのコレスキー行列の〜ｉ列であり、αはシグマ点列の発生のスケールに関するパラメータである。
【請求項３】
最適フィルタリング部と、出力部とを含み、入力された推定対象の各時刻ｔの観測値に基づいて、前記推定対象の各時刻ｔの状態を推定する状態推定装置における状態推定方法において、
前記最適フィルタリング部は、各時刻ｔについて、システムノイズを用いて状態の推定値を時間更新するための線形な状態更新式、及び観測ノイズを用いて状態の推定値と観測値との関係を示す非線形な観測方程式をｕｎｓｃｅｎｔｅｄ  ｓｔａｔｉｓｔｉｃａｌ  ｌｉｎｅａｒｉｚａｔｉｏｎ（ＵＳＬ）により近似した線形な観測方程式を用いて予め定められた状態空間モデルで表わされるフィルタに従って、前記状態の初期値又は時刻ｔ−１について推定された前記状態と、状態推定誤差の初期値又は時刻ｔ−１について算出された状態推定誤差とに基づいて、時刻ｔの状態の予測推定値及び前記予測推定値の状態推定予測誤差を算出し、
入力された時刻ｔの前記観測値と、前回算出された時刻ｔの状態推定値と、算出された時刻ｔの状態の予測推定値及び前記予測推定値の状態推定誤差と、前回の繰り返しで算出された時刻ｔの状態推定値を用いて算出される、前記近似した線形な観測方程式で用いられる傾きの値とに基づいて、予め定められた反復終了条件を満たすまで時刻ｔの状態推定値を繰り返し算出することにより、時刻ｔの状態を推定し、前記近似した線形な観測方程式で用いられる傾きの値と、前記算出された時刻ｔの前記予測推定値の状態推定誤差とに基づいて、時刻ｔの状態推定誤差を算出し、
前記出力部は、前記最適フィルタリング部によって各時刻ｔについて推定された前記状態を出力する
状態推定方法。
【請求項４】
コンピュータを、請求項１又は２記載の状態推定装置を構成する各部として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、状態推定装置、方法、及びプログラムに係り、特に、推定対象の状態を観測値から推定する状態推定装置、方法、及びプログラムに関する。
【背景技術】
【０００２】
従来、unscented transformation(UT)を用いて、古典的な繰り返し型のフィルタアルゴリズムであるiterative extended Kalman filter(iEKF)の微分不可能な関数への拡張を行う方法が知られている（非特許文献１）。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】G. Sibley, G. Sukhatme and L. Matthies. “The Iterated Sigma Point Kalman Filter with Applications to Long Range Stereo” In Proc. Robotics: Science and Systems, Vol. 8, No. 1, pp. 235-244, 2006.
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、非特許文献１の方法においては、任意の非線形関数に対する非線形性を考慮に入れた近似線形モデルを実現することができないという問題がある。
【０００５】
本発明では、上記問題点を解決するために成されたものであり、精度よく推定対象の状態を推定することができる状態推定装置、方法、及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するために、第１の発明に係る状態推定装置は、入力された推定対象の各時刻ｔの観測値に基づいて、前記推定対象の各時刻ｔの状態を推定する状態推定装置であって、各時刻ｔについて、システムノイズを用いて状態の推定値を時間更新するための線形な状態更新式、及び観測ノイズを用いて状態の推定値と観測値との関係を示す非線形な観測方程式をｕｎｓｃｅｎｔｅｄ  ｓｔａｔｉｓｔｉｃａｌ  ｌｉｎｅａｒｉｚａｔｉｏｎ（ＵＳＬ）により近似した線形な観測方程式を用いて予め定められた状態空間モデルで表わされるフィルタに従って、前記状態の初期値又は時刻ｔ−１について推定された前記状態と、状態推定誤差の初期値又は時刻ｔ−１について算出された状態推定誤差とに基づいて、時刻ｔの状態の予測推定値及び前記予測推定値の状態推定誤差を算出し、入力された時刻ｔの前記観測値と、前回算出された時刻ｔの状態推定値と、算出された時刻ｔの状態の予測推定値及び前記予測推定値の状態推定誤差と、前回の繰り返しで算出された時刻ｔの状態推定値を用いて算出される、前記近似した線形な観測方程式で用いられる傾きの値とに基づいて、予め定められた反復終了条件を満たすまで時刻ｔの状態推定値を繰り返し算出することにより、時刻ｔの状態を推定し、前記近似した線形な観測方程式で用いられる傾きの値と、前記算出された時刻ｔの前記予測推定値の状態推定誤差とに基づいて、時刻ｔの状態推定誤差を算出する最適フィルタリング部と、前記最適フィルタリング部によって各時刻ｔについて推定された前記状態を出力する出力部と、を含んで構成されている。
【０００７】
第２の発明に係る状態推定方法は、最適フィルタリング部と、出力部とを含み、入力された推定対象の各時刻ｔの観測値に基づいて、前記推定対象の各時刻ｔの状態を推定する状態推定装置における状態推定方法において、前記最適フィルタリング部は、各時刻ｔについて、システムノイズを用いて状態の推定値を時間更新するための線形な状態更新式、及び観測ノイズを用いて状態の推定値と観測値との関係を示す非線形な観測方程式をｕｎｓｃｅｎｔｅｄ  ｓｔａｔｉｓｔｉｃａｌ  ｌｉｎｅａｒｉｚａｔｉｏｎ（ＵＳＬ）により近似した線形な観測方程式を用いて予め定められた状態空間モデルで表わされるフィルタに従って、前記状態の初期値又は時刻ｔ−１について推定された前記状態と、状態推定誤差の初期値又は時刻ｔ−１について算出された状態推定誤差とに基づいて、時刻ｔの状態の予測推定値及び前記予測推定値の状態推定予測誤差を算出し、入力された時刻ｔの前記観測値と、前回算出された時刻ｔの状態推定値と、算出された時刻ｔの状態の予測推定値及び前記予測推定値の状態推定誤差と、前回の繰り返しで算出された時刻ｔの状態推定値を用いて算出される、前記近似した線形な観測方程式で用いられる傾きの値とに基づいて、予め定められた反復終了条件を満たすまで時刻ｔの状態推定値を繰り返し算出することにより、時刻ｔの状態を推定し、前記近似した線形な観測方程式で用いられる傾きの値と、前記算出された時刻ｔの前記予測推定値の状態推定誤差とに基づいて、時刻ｔの状態推定誤差を算出し、前記出力部は、前記最適フィルタリング部によって各時刻ｔについて推定された前記状態を出力する。
【０００８】
第１及び第２の発明によれば、最適フィルタリング部により、各時刻ｔについて、線形な状態更新式、及び非線形な観測方程式をＵＳＬにより近似した線形な観測方程式を用いて予め定められた状態空間モデルで表わされるフィルタに従って、時刻ｔの状態の予測推定値及び予測推定値の状態推定誤差を算出し、予め定められた反復終了条件を満たすまで時刻ｔの状態推定値を繰り返し算出することにより、時刻ｔの状態を推定し、前回の繰り返しで算出された事項ｔの状態推定値を用いて算出される、近似した線形な観測方程式で用いられる傾きの値と、算出された時刻ｔの予測推定値の状態推定誤差とに基づいて、時刻ｔの状態推定誤差を算出し、出力部により、各時刻ｔについて推定された状態を出力する。
【０００９】
このように、各時刻ｔについて、線形な状態更新式、及び非線形な観測方程式をＵＳＬにより近似した線形な観測方程式を用いて予め定められた状態空間モデルで表わされるフィルタに従って、時刻ｔの状態予測値及び状態予測誤差を算出し、予め定められた反復終了条件を満たすまで、前回の繰り返しで算出された時刻ｔの状態推定値を用いて算出される、近似した線形な観測方程式で用いられる傾きの値に基づいて時刻ｔの状態推定値を繰り返し算出することにより、時刻ｔの状態を推定するため、精度よく推定対象の状態を推定することができる。
【００１０】
また、本発明のプログラムは、コンピュータを、上記の状態推定装置を構成する各部として機能させるためのプログラムである。
【発明の効果】
【００１１】
以上説明したように、本発明の状態推定装置、方法、及びプログラムによれば、各時刻ｔについて、線形な状態更新式、及び非線形な観測方程式をＵＳＬにより近似した線形な観測方程式を用いて予め定められた状態空間モデルで表わされるフィルタに従って、時刻ｔの状態予測値及び状態予測誤差を算出し、予め定められた反復終了条件を満たすまで、前回の繰り返しで算出された時刻ｔの状態推定値を用いて算出される、近似した線形な観測方程式で用いられる傾きの値に基づいて時刻ｔの状態推定値を繰り返し算出することにより、時刻ｔの状態を推定するため、精度よく推定対象の状態を推定することができる。
【図面の簡単な説明】
【００１２】
【図１】本発明の実施の形態に係る状態推定装置の機能的構成を示すブロック図である。
【図２】データ構造の例を示す図である。
【図３】本発明の実施の形態に係る状態推定装置における状態推定処理ルーチンを示すフローチャート図である。
【図４】シミュレーション結果の例を示す図である。
【図５】シミュレーション結果の例を示す図である。
【発明を実施するための形態】
【００１３】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００１４】
＜本発明の原理＞
本発明の実施の形態において、観測機器によって推定対象(現象)の時刻ｔにおける観測データが得られているものとする。この観測データから推定対象の隠れた状態を推定することが問題である。状態空間モデルは推定対象の背後にある支配方程式を解析者がモデル化したものであるが、本実施の形態では、下記（１）式及び（２）式の様な線形なシステムモデルと非線形な観測モデルから成る状態空間モデルを想定する。
【００１５】
【数１】
2015219714.tif 000003
【００１６】
ここで確率変数Ｘｔを時刻ｔにおける推定対象のｎ次元の状態、確率変数Ｙｔを時刻ｔにおける観測値と呼ぶことにすると、上記（１）式が状態Ｘｔの時間発展を表し、上記（２）式が状態Ｘｔの観測機構を表す連立(確率) 方程式となる。Ｆ、ｈ（・）はそれぞれ状態遷移行列、非線形観測変換と呼ばれる。μｔ−１、ωｔはシステムノイズ、及び観測ノイズと呼ばれ、それぞれ平均０、共分散行列Ｑ、及びＲの正規分布に従う白色ノイズである。なお、状態空間モデルでこれらＦ、ｈ（・）、Ｑ、Ｒが解析者により設定される。
【００１７】
フィルタの問題は観測時系列｛Ｙｔ｝＝｛Ｙｔ,Ｙｔ−１,・・・,Ｙ１｝が得られた時の尤も確からしい状態の推定値を求めるものであるが、上記（２）式の観測モデルは状態に対して非線形なため、最適フィルタは一般的に実現しない。そこで、本実施の形態においては、下記（３）式に示す線形近似モデルを使用する。
【００１８】
【数２】
2015219714.tif 000004
【００１９】
上記（３）式の線形近似モデルは非線形観測変換ｈ（・）のHermitian展開の２次以降を無視することで得られる。もしＸｔが正規分布Ｎ（￣Ｘｔ,￣Ｐｔ）に従うとすると、上記（３）式は下記（４）式となる。
【００２０】
【数３】
2015219714.tif 000005
【００２１】
ここで￣Ｘｔは時刻ｔにおける状態の予測推定値（観測時系列｛Ｙｔ−１｝を使用した時刻ｔにおける状態の予測推定値)であり、下記（５）式によって算出される。また、￣Ｐｔは当該￣Ｘｔの状態推定誤差共分散行列であり、下記（６）式によって算出される。上記（３）式は線形近似モデルの傾き及び切片が（条件付き）期待値で計算されることを意味しているため、Taylor展開の高次項の切断による線形近似の自然な拡張になっていることがわかる。つまり、状態の推定誤差共分散行列も考慮に入れた線形近似モデルになっているため、通常の状態推定値周りのTaylor展開に基づく標準的な線形近似モデルと比べて近似誤差が小さくなることが期待される。
【００２２】
【数４】
2015219714.tif 000006
【００２３】
しかしながら、上記（３）式の線形近似モデルには期待値の計算が残っており、一般の非線形観測変換に対して解析的な表式は存在しない。つまり上記（３）式に対する最適フィルタはこのままでは実現しない。そこで、本実施の形態においては、下記（７）式、及び（８）式に示すように、これらの期待値を近似する。
【００２４】
【数５】
2015219714.tif 000007
【００２５】
χｉ,（ｉ＝０，１,・・・，２ｎ）は２ｎ＋１個のシグマ点列と呼ばれ、このシグマ点列を非線形観測変換に基づき変換し、対応する重みｗｉを掛け合わせることでそれぞれの期待値を近似する。Ｃ（〜ｉ）は￣Ｐｔのコレスキー行列の〜ｉ列を表す。αはシグマ点列の発生のスケールに関するパラメータであるが、通常√３が設定される。この設定により状態が正規分布に従いスカラーの場合、状態推定値周りでのテイラー展開の意味で４次の項まで、ベクトルの場合、期待値は３次まで、共分散行列は２次の項までの近似精度が保証される。上記の様なシグマ点列による状態の期待値及び共分散行列の計算はunscented transformation(UT)と呼ばれるため、上記（３）式、（７）式、及び（８）式によって得られる線形近似式をunscented statistical linearization(USL)とする。
【００２６】
上記（３）式、（７）式、及び（８）式からなるＵＳＬによる線形近似モデルに対して下記（９）式、及び（１０）式のフィルタアルゴリズムを適用する。
【００２７】
【数６】
2015219714.tif 000008
【００２８】
上記（９）式が繰り返し回数ｊにおける線形近似観測モデル、上記（１０）式が最適フィルタリングを表す。上記（１０）式の＾Ｘｔ,ｊ＋１が時刻ｔの状態推定結果(繰り返し回数ｊ)である。上記アルゴリズムのｍ回の繰り返し後、下記（１２）式、及び（１３）式のように算出することで、時刻ｔの状態推定結果及び状態推定誤差共分散行列を得る。上記アルゴリズムの期待値は全てＮ（＾Ｘｔ,ｊ,￣Ｐｔ）に対して定義され、実際はシグマ点列を用いて近似的に計算される（上記（７）式、及び（８）式）。なお、傾きＡｔ，ｊは、上記（９）式により下記（１４）式で表わされる。
【００２９】
【数７】
2015219714.tif 000009
【００３０】
この繰り返し線形近似によって非線形観測モデルの線形近似精度が逐次的に改善されるため、iEKFに比べて、当該アルゴリズムの使用により上記（１）式、及び（２）式で表される状態空間モデルに対して高い状態フィルタ精度が得られることが期待できる。当該フィルタをiterative unscented statistically linearized filter(iUSLF)とする。
【００３１】
＜本発明の実施の形態に係る状態推定装置の構成＞
次に、本発明の実施の形態に係る状態推定装置の構成について説明する。図１に示すように、本発明の実施の形態に係る状態推定装置１００は、ＣＰＵと、ＲＡＭと、後述する状態推定処理ルーチンを実行するためのプログラムや各種データを記憶したＲＯＭと、を含むコンピュータで構成することが出来る。この状態推定装置１００は、機能的には図１に示すように入力部１０と、最適フィルタリング部２０と、出力部９０とを備えている。
【００３２】
入力部１０は、観測機器（図示省略）によって得られた、推定対象の各時刻ｔの観測データＹｔの入力を受け付ける。
【００３３】
入力部１０は、解析者により入力される状態遷移行列Ｆと、非線形観測変換ｈ（・）と、共分散行列Ｑ及びＲと、状態の推定値の初期値＾Ｘ０と、状態推定誤差共分散行列の初期値＾Ｐ０と、を受け付ける。なお、状態の推定値の初期値＾Ｘ０、及び状態推定誤差共分散行列の初期値＾Ｐ０は、予め定められた適切な値とする。
【００３４】
最適フィルタリング部２０は、時間更新部２２と、観測更新部２４とを含んで構成されている。
【００３５】
時間更新部２２は、まず、入力部１０において受け付けた状態遷移行列Ｆと、共分散行列Ｑと、状態の推定値の初期値＾Ｘ０及び、状態推定誤差共分散行列の初期値＾Ｐ０又は観測更新部２４において前回算出された状態の推定値＾Ｘｔ-1及び状態推定誤差共分散行列＾Ｐｔ-1とに基づいて、状態の予測推定値￣Ｘｔ及び当該予測推定値￣Ｘｔの状態推定誤差共分散行列￣Ｐｔを算出する。
【００３６】
具体的には、上記（５）式と、（６）式とにより時間更新における状態の予測推定値￣Ｘｔ及び当該予測推定値￣Ｘｔの状態推定誤差共分散行列￣Ｐｔを算出する。
【００３７】
観測更新部２４は、時間更新部２２において算出された状態の予測推定値￣Ｘｔ、及び観測更新部２４において前回の繰り返しで算出される状態推定値＾Ｘｔ，ｊと、時間更新部２２において算出された予測推定値￣Ｘｔの状態推定誤差共分散行列￣Ｐｔと、入力部１０において受け付けた各時刻ｔにおける観測データＹｔ、共分散行列Ｒ、及び非線形観測変換ｈ（・）と、に基づいて、状態推定値＾Ｘｔ，ｊ+1を算出することを繰り返し、状態の推定値＾Ｘｔ、及び当該状態の推定値＾Ｘｔの状態推定誤差共分散行列＾Ｐｔを算出し、時間更新部２２及び出力部９０に出力する。
【００３８】
具体的には、状態の予測推定値￣Ｘｔ又は状態推定値＾Ｘｔ，ｊと、予測推定値￣Ｘｔの状態推定誤差共分散行列￣Ｐｔと、に基づいて、上記（８）式、上記（１４）式に従って、傾きＡｔ，ｊを算出する。次に、算出された傾きＡｔ，ｊと、状態推定誤差共分散行列￣Ｐｔと、共分散行列Ｒと、に基づいて、上記（１１）式に従って、Ｋｔ，ｊを算出する。次に、算出されたＫｔ，ｊ及び傾きＡｔ，ｊと、状態の予測推定値￣Ｘｔ、及び状態推定値＾Ｘｔ，ｊと、非線形観測変換ｈ（・）と、に基づいて、上記（７）式、上記（１０）式に従って、繰り返し回数ｊ回目の状態推定値＾Ｘｔ，ｊ＋１を算出する。そして、予め定められた反復終了条件を満たすまで、傾きＡｔ，ｊの算出処理と、Ｋｔ，ｊの算出処理と、状態推定値＾Ｘｔ，ｊ＋１の算出処理とを繰り返し、繰り返し終了後の状態推定値＾Ｘｔ，ｊ＋１を用いて、上記（１２）式に従って、時刻ｔの状態の推定値＾Ｘｔを算出し、繰り返し終了後のＫｔ，ｊと、Ａｋ，ｊと、状態推定誤差共分散行列￣Ｐｔとに基づいて、上記（１３）式に従って、状態の推定値＾Ｘｔの状態推定誤差共分散行列＾Ｐｔを算出する。なお、本実施の形態においては、予め定められた反復終了条件を繰り返し回数ｍ回に到達したこととする。また、図２に観測データのデータ構造、繰り返し回数ｊ回におけるデータ構造、及び出力のデータ構造の例を示す。
【００３９】
＜本発明の実施の形態に係る状態推定装置の作用＞
次に、本発明の実施の形態に係る状態推定装置１００の作用について説明する。図３に示す状態推定処理ルーチンを実行する前に、入力部１０において、解析者により入力される状態遷移行列Ｆと、非線形観測変換ｈ（・）と、共分散行列Ｑ及びＲと、状態の推定値の初期値＾Ｘ０と、状態推定誤差共分散行列の初期値＾Ｐ０と、を受け付ける。そして、入力部１０により、各時刻ｔの観測データＹｔを受け付けているときに、状態推定装置１００は、図３に示す状態推定処理ルーチンを実行する。
【００４０】
まず、ステップＳ１００では、入力部１０において受け付けた状態遷移行列Ｆと、入力部１０において受け付けた状態の推定値の初期値＾Ｘ０又はステップＳ１１４において前回算出された状態の推定値＾Ｘｔ-1とに基づいて、上記（５）式に従って、時刻ｔの状態の予測推定値￣Ｘｔを算出する。
【００４１】
次に、ステップＳ１０２では、入力部１０において受け付けた状態遷移行列Ｆ、及び共分散行列Ｑと、入力部１０において受け付けた状態推定誤差共分散行列の初期値＾Ｐ０又は、ステップＳ１１６において前回算出された状態推定誤差共分散行列＾Ｐt-1とに基づいて、上記（６）式に従って、ステップＳ１００において算出された予測推定値￣Ｘｔの状態推定誤差共分散行列￣Ｐｔを算出する。
【００４２】
次に、ステップＳ１０３では、変数ｊの値を０と設定する。
【００４３】
次に、ステップＳ１０４では、ステップＳ１００において取得した状態の予測推定値￣Ｘｔ又はステップＳ１０８において前回の繰り返しで算出された状態推定値＾Ｘｔ,ｊと、ステップＳ１０２において取得した状態推定誤差共分散行列￣Ｐｔと、に基づいて、上記（８）式、上記（１４）式に従って、傾きＡｔ，ｊを算出する。
【００４４】
次に、ステップＳ１０６では、ステップＳ１０４において取得した傾きＡｔ，ｊと、ステップＳ１０２において取得した状態推定誤差共分散行列￣Ｐｔと、入力部１０において受け付けた共分散行列Ｒと、に基づいて、上記（１１）式に従って、Ｋｔ，ｊを算出する。
【００４５】
次に、ステップＳ１０８では、ステップＳ１０４において取得した傾きＡｔ，ｊと、ステップＳ１０６において取得したＫｔ，ｊと、ステップＳ１００において取得した状態の予測推定値￣Ｘｔと、ステップＳ１０８において前回の繰り返しで算出された状態推定値＾Ｘｔ，ｊと、入力部１０において受け付けた非線形観測変換ｈ（・）と、に基づいて、上記（７）式、上記（１０）式に従って、状態推定値＾Ｘｔ，ｊ＋１を算出する。
【００４６】
次に、ステップＳ１１０では、反復終了条件として、変数ｊの値が、繰り返し回数ｍの値と同一か否か判定する。変数ｊの値がｍの値と同一である場合には、ステップＳ１１４へ移行し、変数ｊの値がｍの値と同一でない場合には、ステップＳ１１２へ移行する。
【００４７】
ステップＳ１１２では、変数ｊの値に１を加算した値を変数ｊの値として設定して、ステップＳ１０４へ移行し、ステップＳ１０４からステップＳ１１０の処理を繰り返す。
【００４８】
ステップＳ１１４では、ステップＳ１０８において取得した状態推定値＾Ｘｔ，ｊ＋１を用いて、上記（１２）式に従って、時刻ｔの状態の推定値＾Ｘｔを算出する。
【００４９】
次に、ステップＳ１１６では、ステップＳ１０４において取得した傾きＡｔ，ｊと、ステップＳ１０６において取得したＫｔ，ｊと、ステップＳ１０２において取得した状態推定誤差共分散行列￣Ｐｔと、に基づいて、上記（１３）式に従って、ステップＳ１１４において取得した状態の推定値＾Ｘｔの状態推定誤差共分散行列＾Ｐｔを算出する。
【００５０】
次に、ステップＳ１１８では、ステップＳ１１４において取得した状態の推定値＾Ｘｔと、ステップＳ１１６において取得した状態推定誤差共分散行列＾Ｐｔと、を出力部９０に出力する。そして、ステップＳ１２０において、時刻ｔを１時刻進めて、次の時刻ｔの観測データＹｔを読み込んで、ステップＳ１００へ戻り、ステップＳ１００〜ステップＳ１１８の処理を繰り返す。
【００５１】
＜実験例＞
本発明の状態推定装置におけるフィルタの有効性を下記１〜３の手法と比較して検証する。
１．ＵＫＦ（従来法１）
２．ｉＥＫＦ（従来法２）
３．ｉＳＰＫＦ（非特許文献１）
４．ｉＵＳＬＦ（本発明のフィルタ）
【００５２】
ここで、ｉＳＰＫＦ(iterative sigma point Kalman filter）のアルゴリズムは下記（１５）式のアルゴリズムである。
【００５３】
【数８】
2015219714.tif 000010
【００５４】
上記（１５）式のアルゴリズムをｍ回繰り返した後、＾Ｘｔ：＝＾Ｘｔ，ｍ＋１、＾Ｐｔ＝￣Ｐｔ−Ｃｏｖ［Ｘｔ，Ｙｔ］Ｖ［Ｙｔ］−１Ｃｏｖ［Ｘｔ，Ｙｔ］Ｔとする。アルゴリズムの期待値はすべてＮ（＾Ｘｔ，ｊ，￣Ｐｔ）に対して計算され、ｉＵＳＬＦと同様にこれらの値はシグマ点列を用いて計算される。ｉＳＰＫＦはｉＥＫＦの微分不可能な非線形変換への拡張と解釈することができ、非線形観測変換のヤコビアンを必要としないderivative-freeな逐次フィルタリング手法である。実際、繰り返しが無い場合の状態の予測推定値はＥＫＦと同じである。ｉＵＳＬＦも同じ意味でderivative-freeなアルゴリズムであるが、その状態の予測推定値はＵＫＦと同等になる。
【００５５】
ｉＵＳＬＦとの決定的な違いは、Ｖ［Ｙｔ］の計算にある。ｉＳＰＫＦではこの計算をシグマ点列で行うため、状態変換の非線形性がそのまま考慮される。一方、ｉＵＳＬＦでは、線形近似モデルから決定される値を使用するため、非線形性が直接には考慮されない（線形近似モデルの傾きに非線形性が考慮されるが）。言い替えるとｉＳＰＫＦは逐次線形化のアルゴリズムではなく、まさしく名前の通りシグマ点列を使用した逐次的なフィルタリング法である。
【００５６】
実験例で使用する状態空間モデルは下記（１６）式である。
【００５７】
【数９】
2015219714.tif 000011
【００５８】
簡単のため状態及び観測データはスカラーとする。下記にシミュレーション結果を示す。シミュレーションは３０回のMonte Carlo runs で行った。図４のプロットはそれぞれ５００≦ｔ≦５３０における一つの観測時系列Ｙｔ＝｛ｙ１０００,ｙ９９９, ・ ・ ・ ,ｙ１｝と状態の真値の時歴(図４左)、及び各手法の状態フィルタ推定値の平均RMSE(root mean square error)を示す(図４右。横軸は繰り返し回数)。
【００５９】
UKFは繰り返しアルゴリズムではないため、繰り返し回数に対して同じ値をプロットした。まずiEKFは、この問題に対しては推定に失敗する。理由はｘｔ＝０付近における非線形観測変換の平坦性にある。iEKFの線形近似モデルの傾きがこの付近で０に近づき、結果として観測値と状態の線形相関が小さくなることでカルマンゲインが小さくなり、状態の観測更新が行われなくなる。従って状態は素早く０に収束し、iEKFは、以後観測値の情報を取り込むことなく０を出力し続ける結果になる。図４右の結果はiUSLFの有効性を表しており、少ない繰り返し回数でUKFの状態フィルタ推定精度を上回っている。iSPKFも同様に少ない繰り返し回数で推定精度が収束しているが、その値はUKFの結果とほぼ同じになっている。図５にiUSLFによる５００≦ｔ≦５３０における状態フィルタ推定結果を示す。この結果からｔ＝５０３付近における状態に対する素早い追従性を確認することができる、またその付近における推定誤差分散も小さいことから、状態の大きな時間変化に対する追従性を提案フィルタが保持していることがわかる。
【００６０】
以上説明したように、本発明の実施の形態に係る状態推定装置によれば、各時刻ｔについて、線形な状態更新式、及び非線形な観測方程式をＵＳＬにより近似した線形な観測方程式を用いて予め定められた状態空間モデルで表わされるフィルタに従って、時刻ｔの状態の予測推定値及び状態推定誤差を算出し、予め定められた反復終了条件を満たすまで、前回の繰り返しで算出された時刻ｔの状態推定値を用いて算出される、近似した線形な観測方程式で用いられる傾きの値に基づいて時刻ｔの状態推定値を繰り返し算出することにより、時刻ｔの状態を推定するため、精度よく推定対象の状態を推定することができる。
【００６１】
また、解析対象（現象）の状態を観測時系列から推定するフィルタリングの問題において、特にガウス型観測モデルが状態に対して非線形になる系に対して従来法より推定精度の高い繰り返し型のフィルタアルゴリズムを提案することができる。
【００６２】
また、通常のモデル線形化法より線形近似精度が高いunscented statistical linearization（ＵＳＬ）のフィルタを使用していることから、最新の観測データに基づく繰り返しの線形化により、高い状態推定精度を実現することができる。
【００６３】
また、本実施の形態において用いるフィルタでは、非特許文献１において用いるフィルタアルゴリズムでは実現しない、非線形モデルを逐次線形化するため、逐次線形化により、フィルタのカルマンゲインが大きく評価されるため、フィルタの観測値に値する敏感性（sensitivity）が向上し、素早い状態追随性を実現することができる。
【００６４】
また、観測データの予測精度も高いため、非特許文献１において用いるフィルタを用いるよりも、状態のフィルタ推定精度を全体的に改善することができる。
【００６５】
また、本実施の形態においては、非線形観測モデルをＵＳＬによって線形化するが、１期前までの観測データだけではなく、現在の観測データの情報も使用することで線形近似モデルの精度を繰り返し向上させることができる。これにより、従来法では実現することができなかった、任意の非線形関数に対する非線形性を考慮にいれた近似線形モデルを実現することができ、当該近似モデルに対して最適フィルタを適用することで、高い状態フィルタ推定精度を実現することができる。
【００６６】
また、本実施の形態においては、フィルタは関数のヤコビアンを必要としないderivative-freeなアルゴリズムであるため実装が容易である。
【００６７】
また、本実施の形態においては、フィルタは非線形モデルの逐次線形化をベースにしているため、その他の手法と比べてフィルタの観測値に値する敏感性が高く、状態の大きな変化に対する追従性が高いといった特徴をもつ。
【００６８】
また、本実施の形態においては、非線形ガウス型観測モデルに対する、新しいタイプの繰り返しフィルタリング法を用いている。当該フィルタアルゴリズムは、iterative unscented statistically linearized filter（ｉＵＳＬＦ）と呼ばれ、数値実験により従来法の状態フィルタ推定精度を上回る結果となる。また、フィルタは少ない繰り返し回数で優れた推定精度を実現するため、非線形性が強いオンラインの問題に対する新たな有効なフィルタリング法として期待できる。
【００６９】
なお、本発明は、上述した実施形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【００７０】
例えば、本実施の形態における状態推定装置においては、予め定められた反復終了条件を、繰り返し回数ｍ回に到達することとする場合について説明したが、これに限定されるものではなく、予め定められた反復終了条件を、算出される状態推定値＾Ｘｔ，ｊ＋１の値が収束することとしてもよい。
【００７１】
また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能であるし、ネットワークを介して提供することも可能である。
【符号の説明】
【００７２】
１０   入力部
２０   最適フィルタリング部
２２   時間更新部
２４   観測更新部
９０   出力部
１００ 状態推定装置
【図１】
2015219714.tif 000014
【図２】
2015219714.tif 000015
【図３】
2015219714.tif 000016
【図４】
2015219714.tif 000017
【図５】
2015219714.tif 000018