状態推定装置及び状態推定方法
【課題】推定処理の途中で、必要なモデルを判別して、不要なモデルのカルマンフィルタの処理結果がフィルタ全体の推定精度に与える悪影響を抑制することができるようにする。
【解決手段】統合処理部１０がカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）のうち、モデル構成判定部８により使用すると判別されたモデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）を、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた統合加重で統合する。
【解決手段】統合処理部１０がカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）のうち、モデル構成判定部８により使用すると判別されたモデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）を、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた統合加重で統合する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、観測装置により観測された観測値に基づいて、動特性を有するシステムの状態値を推定する状態推定装置及び状態推定方法に関し、特に、化学、鉄鋼などのプラント、自動車、各種モータなどの制御機器、画像、音声、レーダなどの信号処理機器、医療分野における生体計測機器、衛星や飛翔体などの移動体追跡システム、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）や慣性航法装置を応用した測位システム、通信システム、経済予測システムなどの広範な分野に応用される状態推定装置及び状態推定方法に関するものである。
【背景技術】
【０００２】
システムの状態量の動特性を表す運動方程式と、状態量と観測量の関係を表す観測方程式（以下、上記の運動方程式と観測方程式を併せて、「状態方程式」と称する）とに基づいて、観測量の時系列データからシステムの状態量を推定する最適フィルタ方式として、カルマンフィルタがよく知られている。
上記の状態方程式は、システムの動特性と観測系の性質を数学的に表現した数式モデルに相当する。
ただし、カルマンフィルタは、この数式モデルが時間的に変動せず一定であるか、もしくは、その変動が確定的で事前に既知であることを前提としている。
【０００３】
実際の応用場面では、モデルが時間とともに変動し、また、その変動の程度やタイミングが未知である場合も多い。
したがって、このような場面では使用することができないカルマンフィルタの弱点を克服するフィルタ方式として、事前に想定される複数のモデルに対応するカルマンフィルタを並列に動作させる多重モデル方式のフィルタが各種提案されている。
その中でも、推定精度と演算負荷のバランスから最も有効とされるのが、ＩＭＭ（Ｉｎｔｅｒａｃｔｉｎｇ Ｍｕｌｔｉｐｌｅ Ｍｏｄｅｌ）フィルタである（例えば、非特許文献１を参照）。
【０００４】
ＩＭＭフィルタでは、例えば、以下に示すような状態方程式を使用する。アンダーラインを付している変数はベクトルであることを示している。
【数１】
【０００５】
式（１）はシステムの運動方程式であり、式（２）は観測方程式である。
また、ｘkはｐ次元状態ベクトル、ｙkはｑ次元観測ベクトル、ｒkはシステムのモデル変数、ｗkはシステム雑音ベクトル、ｖkは観測雑音ベクトルを表している。
また、係数行列Ａ（），Ｂ（），Ｈ（），Ｇ（）はｒkの既知の行列関数である。なお、ｋはサンプリング時刻（以下、単に時刻と称する）ｔkを表すインデックスである。
【０００６】
状態ベクトルｘkは、システムの動的挙動を規定する複数の状態変数を含むベクトルである。
例えば、モータの制御機器においては、コイルに流れる電流値や、モータシャフトの回転位置、速度などで構成される。
また、衛星の追跡システムにおいては、衛星の位置、速度、姿勢角などで構成される。
また、ある種の経済予測システムにおいては、収入、税率、失業率、経済成長率などが含まれ、生体計測機器の例では、血糖値、脈拍、呼吸数、体温などが含まれる。
【０００７】
観測ベクトルｙkは、何らかの観測系で得られる複数の観測量で構成されたベクトルであり、状態ベクトルｘkと式（２）の関係で関連付けられている。
観測ベクトルｙkの時系列データに基づいて状態ベクトルｘkを推定することがフィルタ処理の目的となる。
また、システム雑音ベクトルｗk、観測雑音ベクトルｖkは、互いに独立な白色ガウス雑音であると仮定する。
【０００８】
モデル変数ｒkは、複数の数式モデル（状態方程式）を識別する変数に相当する。即ち、モデル変数ｒkが変わると、係数行列Ａ（），Ｂ（），Ｈ（），Ｇ（）の値が変わることによって、式（１）、式（２）は別の運動方程式、観測方程式を表すことになる。
モデル変数ｒkは、時刻ｔkにおいて、ｓ個の離散的状態（Ｓ＝｛１，２，・・・，ｓ｝で表す）のいずれかをとり、その値は、確率的に推移すると仮定する。
【０００９】
この推移においては、一次マルコフ連鎖を仮定し、時刻ｔkにおいて、システムがいずれの離散的状態にあるかの確率は、時刻ｔk-1の離散的状態のみに依存するものとする。
そこで、下記の式（３）に示すように、時刻ｔk-1でシステムが離散的状態ｉであったときに時刻ｔkで状態ｊに推移する確率ｐijを予め規定する。ただし、定義により、下記の式（４）が成立する。
【数２】
【００１０】
ＩＭＭフィルタは、上記の状態方程式の記述に基づいて、ｓ個のカルマンフィルタからなるフィルタバンクを構成し、以下のように処理を実行する。
まず、初期時刻ｔ0に対するｓ個のモデルｊ（ｊ＝１，２，・・・，ｓ）のモデル確率の初期値μ0（ｊ）と、各モデルに対応するカルマンフィルタの状態ベクトル推定値の初期値ｘ0|0（ｊ）ハットと、推定誤差共分散行列の初期値Ｐ0|0（ｊ）とを設定して、フィルタ処理を開始する。
サンプリング時刻毎の処理ループに入ると、最初に、以下の混合処理を行う。ただし、以下では、時刻ｔkにおける処理を示すものとする。
【００１１】
混合処理では、各モデルのカルマンフィルタが前時刻ｔk-1で算出した状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐk-1|k-1（ｊ）を互いに交換して、次式のような重み付け統合を行う。式（７）のｍijは混合加重である。また、Ｔは行列の転置を表している。
【数３】
【００１２】
次に、式（５）のｘmk-1|k-1（ｊ）ハットと、式（６）のＰmk-1|k-1（ｊ）をモデルｊ（ｊ＝１，２，・・・，ｓ）のフィルタの入力とし、カルマンフィルタのアルゴリズムにしたがって予測処理と更新処理を実行する。
これにより、各モデルの状態ベクトル予測値ｘk|k-1（ｊ）ハットと、予測誤差共分散行列Ｐk|k-1（ｊ）と、更新後の状態ベクトル推定値ｘk|k（ｊ）ハットと、推定誤差共分散行列Ｐk|k（ｊ）とが得られる。
【００１３】
次に、上記の状態ベクトル予測値ｘk|k-1（ｊ）ハットと、予測誤差共分散行列Ｐk|k-1（ｊ）を使用し、以下の手順にしたがって各モデルのモデル確率を更新する。
まず、以下の式（９）、式（１０）にしたがって、各モデルの残差ベクトルｙk（ｊ）チルダと、残差共分散行列Ｄk（ｊ）を算出する。
【数４】
【００１４】
式（１０）の右辺第２項のＲkは観測誤差共分散行列である。
残差ベクトルｙk（ｊ）チルダは、状態ベクトル予測値から求められた観測ベクトルの予測値と、実際に取得された観測ベクトルとの差を表すベクトルである。
この残差ベクトルｙk（ｊ）チルダは、平均零ベクトルと残差共分散行列Ｄk（ｊ）の多変量ガウス分布にしたがって実現すると仮定すれば、式（１１）により算出される確率密度Λk（ｊ）はモデルｊの尤度を表すものとなる。
なお、モデルｊの尤度Λk（ｊ）は現時刻ｔkの観測ベクトルｙkを受けた結果として、各モデルのフィルタがどの程度適合しているかを測る指標値となる。
この尤度Λk（ｊ）を使用する式（１２）にしたがって、現時刻ｔkに対応するモデル確率μk（ｊ）が算出される。
式（１２）と式（８）により逐次的に計算されるモデル確率μk（ｊ）は、前時刻ｔk-1のモデル確率μk-1（ｊ）及びモデル間の推移ｐijと、現時刻ｔkの尤度Λk（ｊ）から推論された「モデルｊが正しい確率」であり、下記の式（１３）が成立する。
【数５】
【００１５】
最後に、ＩＭＭフィルタは、各モデルのカルマンフィルタの出力である状態ベクトル推定値ｘk|k（ｊ）ハットと、推定誤差共分散行列Ｐk|k（ｊ）とを上記のモデル確率μk（ｊ）で重み付け統合した値を時刻ｔkの処理結果として外部に出力し、時刻ｔkの処理を終了する。
【数６】
【００１６】
上記のＩＭＭフィルタを搭載している従来の状態推定装置は、以下のように構成されている。
（１）予め用意されているモデルのカルマンフィルタ処理を実施して、当該モデルの状態ベクトル推定値ｘk|k（ｊ）ハットと推定誤差共分散行列Ｐk|k（ｊ）を更新する複数のカルマンフィルタ処理部
（２）複数のカルマンフィルタ処理部により更新された状態ベクトル推定値ｘk|k（ｊ）ハットと推定誤差共分散行列Ｐk|k（ｊ）から、式（９）〜式（１１）によって、各モデルのモデル尤度Λk（ｊ）を計算するモデル尤度計算部
（３）モデル尤度計算部により計算されたモデル尤度Λk（ｊ）から、式（１２）によって、各モデルのモデル確率μk（ｊ）を計算するモデル確率計算部
（４）複数のカルマンフィルタ処理部により更新された状態ベクトル推定値ｘk|k（ｊ）ハットと推定誤差共分散行列Ｐk|k（ｊ）を、式（１４）〜式（１５）によって、モデル確率計算部により計算されたモデル確率μk（ｊ）に応じた統合加重で統合する統合処理部
（５）複数のカルマンフィルタ処理部により更新された状態ベクトル推定値ｘk|k（ｊ）ハットと推定誤差共分散行列Ｐk|k（ｊ）を、式（５）〜式（８）によって、モデル確率計算部により計算されたモデル確率μk（ｊ）に応じた混合加重で混合し、混合後の状態ベクトル推定値ｘmk-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐmk-1|k-1（ｊ）を複数のカルマンフィルタ処理部に出力する混合処理部
【００１７】
上記のＩＭＭフィルタを搭載している従来の状態推定装置は、複数のモデルに対応するカルマンフィルタを並列動作させることによって、システムの動特性や観測系の性質が変動する場合でも、状態量を推定することができる。
また、事前に設定したモデルの中に、システムや観測系の性質を正確に描写したものが存在しなくても、それに近いモデルのフィルタ出力ほど高いモデル確率で重み付けされて統合処理されることにより、近似的にシステム状態量を計算することができる。
【００１８】
【非特許文献１】ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎ ｏｎ Ａｕｔｏｍａｔｉｃ Ｃｏｎｔｒｏｌ，Ｖｏｌ． ３３，Ｎｏ．８，ｐｐ．７８０−７８３，Ａｕｇｕｓｔ １９８８
【発明の開示】
【発明が解決しようとする課題】
【００１９】
従来の状態推定装置は以上のように構成されているので、推定処理の開始から終了に至るまで、予め固定的に設定されたｓ個のモデルのカルマンフィルタの処理結果が統合され、その統合結果がフィルタ全体の推定結果として得られる。しかし、実際には、推定処理に必要となるモデルの数や種類が時間の経過に伴って変動する場合があり、ある時刻において、無用なモデルのカルマンフィルタを使用すると、観測雑音の影響で、そのモデルのモデル確率が零より大きな値となるため、式（１４）、式（１５）の統合処理に影響を与えて、推定精度の劣化を招くことがある。また、式（５）〜式（８）の混合処理において、無用なモデルのカルマンフィルタの処理結果が、他のモデルのカルマンフィルタの入力に影響を与えるため、結果的にフィルタ全体の推定精度の劣化を引き起こすことがある課題があった。
【００２０】
この発明は上記のような課題を解決するためになされたもので、推定処理の途中で、必要なモデルを判別して、不要なモデルのカルマンフィルタの処理結果がフィルタ全体の推定精度に与える悪影響を抑制することができる状態推定装置及び状態推定方法を得ることを目的とする。
【課題を解決するための手段】
【００２１】
この発明に係る状態推定装置は、モデル確率算出手段により算出されたモデル尤度から必要なモデルを判別するモデル判別手段を設け、統合処理手段が複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列のうち、モデル判別手段により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列をモデル確率算出手段により算出されたモデル確率に応じた統合加重で統合するようにしたものである。
【発明の効果】
【００２２】
この発明によれば、モデル確率算出手段により算出されたモデル尤度から必要なモデルを判別するモデル判別手段を設け、統合処理手段が複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列のうち、モデル判別手段により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列をモデル確率算出手段により算出されたモデル確率に応じた統合加重で統合するように構成したので、不要なモデルのカルマンフィルタの処理結果がフィルタ全体の推定精度に与える悪影響を抑制することができる効果がある。
【発明を実施するための最良の形態】
【００２３】
実施の形態１．
図１はこの発明の実施の形態１による状態推定装置を示す構成図である。図１では、モデル数がｓ＝３の場合の例を示している。言うまでもないが、他のモデル数の場合にも同様に構成することができる。
図において、観測装置１は例えばセンサなどの計測手段であり、時刻ｔkの観測ベクトルｙkを状態推定装置に出力する。
【００２４】
混合処理部２は推定値記憶部４−１〜４−３から前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐk-1|k-1（ｊ）を読み出し、式（５）〜式（８）にしたがって、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐk-1|k-1（ｊ）を前時刻ｔk-1のモデル確率μk-1（ｊ）に応じた混合加重で混合する処理を実施する。ただし、ｊ＝１，２，・・・，ｓである。
なお、混合処理部２は混合処理手段を構成している。
【００２５】
カルマンフィルタ処理部３−１は混合処理部２により混合されたモデル（１）の状態ベクトル推定値ｘmk-1|k-1（１）ハット及び推定誤差共分散行列Ｐmk-1|k-1（１）と、観測装置１から出力された観測ベクトルｙkとを入力して、モデル（１）のカルマンフィルタ処理を実施し、モデル（１）の状態ベクトル推定値の更新値ｘk|k（１）ハット及び推定誤差共分散行列の更新値Ｐk|k（１）を推定値記憶部４−１及び統合処理部１０に出力する。
カルマンフィルタ処理部３−２は混合処理部２により混合されたモデル（２）の状態ベクトル推定値ｘmk-1|k-1（２）ハット及び推定誤差共分散行列Ｐmk-1|k-1（２）と、観測装置１から出力された観測ベクトルｙkとを入力して、モデル（２）のカルマンフィルタ処理を実施し、モデル（２）の状態ベクトル推定値の更新値ｘk|k（２）ハット及び推定誤差共分散行列の更新値Ｐk|k（２）を推定値記憶部４−２及び統合処理部１０に出力する。
【００２６】
カルマンフィルタ処理部３−３は混合処理部２により混合されたモデル（３）の状態ベクトル推定値ｘmk-1|k-1（３）ハット及び推定誤差共分散行列Ｐmk-1|k-1（３）と、観測装置１から出力された観測ベクトルｙkとを入力して、モデル（３）のカルマンフィルタ処理を実施し、モデル（３）の状態ベクトル推定値の更新値ｘk|k（３）ハット及び推定誤差共分散行列の更新値Ｐk|k（３）を推定値記憶部４−３及び統合処理部１０に出力する。
なお、カルマンフィルタ処理部３−１〜３−３はカルマンフィルタ処理手段を構成している。
推定値記憶部４−１〜４−３はカルマンフィルタ処理部３−１〜３−３から出力された状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）を記憶するメモリである。
【００２７】
モデル尤度計算部５はカルマンフィルタ処理部３−１〜３−３から状態ベクトル予測値ｘk|k-1（ｊ）ハット、予測誤差共分散行列Ｐk|k-1（ｊ）及び観測ベクトルｙkを収集して、式（９）〜式（１１）によって、各モデルのモデル尤度Λk（ｊ）を計算する処理を実施する。
モデル確率計算部６はモデル尤度計算部５により計算されたモデル尤度Λk（ｊ）から、式（１２）によって、各モデルのモデル確率μk（ｊ）を計算する処理を実施する。
なお、モデル尤度計算部５及びモデル確率計算部６からモデル確率算出手段が構成されている。
モデル確率記憶部７はモデル確率計算部６により計算された各モデルのモデル確率μk（ｊ）を記憶するメモリである。
【００２８】
モデル構成判定部８はモデル尤度計算部５により計算されたモデル尤度Λk（ｊ）と統合処理部１０より統合された状態ベクトル推定値ｘk|kハットから、次時刻ｔk+1で使用するモデル（必要なモデル）の集合Ｓ’（Ｓ’⊂Ｓ＝｛１，２，・・・，ｓ｝）を決定し、そのモデルの集合Ｓ’をモデル構成判定結果としてモデル構成記憶部９に格納する処理を実施する。
なお、モデル構成判定部８はモデル判別手段を構成している。
モデル構成記憶部９はモデル構成判定結果Ｓ’を記憶するメモリである。
【００２９】
統合処理部１０はカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）のうち、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すると判別されたモデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）を、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた統合加重で統合する処理を実施する。
なお、統合処理部１０は統合処理手段を構成している。
【００３０】
図２はこの発明の実施の形態１による状態推定装置の混合処理部２を示す構成図であり、図において、混合加重計算部１１はモデル確率記憶部７に記憶されている前時刻ｔk-1のモデル確率μk-1（ｊ）から、式（７）〜式（８）によって、混合加重ｍijを計算する処理を実施する。
混合計算部１２−１は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを混合加重ｍijで混合して、モデル（１）の状態ベクトル推定値ｘmk-1|k-1（１）ハットをカルマンフィルタ処理部３−１に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合加重ｍijで混合して、モデル（１）の推定誤差共分散行列Ｐmk-1|k-1（１）をカルマンフィルタ処理部３−１に出力する。
【００３１】
混合計算部１２−２は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを混合加重ｍijで混合して、モデル（２）の状態ベクトル推定値ｘmk-1|k-1（２）ハットをカルマンフィルタ処理部３−２に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合加重ｍijで混合して、モデル（２）の推定誤差共分散行列Ｐmk-1|k-1（２）をカルマンフィルタ処理部３−２に出力する。
混合計算部１２−３は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを混合加重ｍijで混合して、モデル（３）の状態ベクトル推定値ｘmk-1|k-1（３）ハットをカルマンフィルタ処理部３−３に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合加重ｍijで混合して、モデル（３）の推定誤差共分散行列Ｐmk-1|k-1（３）をカルマンフィルタ処理部３−３に出力する。
【００３２】
図３はこの発明の実施の形態１による状態推定装置の統合処理部１０を示す構成図であり、図において、統合加重調整部２１はモデル構成記憶部９に記憶されている前時刻ｔk-1のモデル構成判定結果Ｓ’を参照して、現時刻ｔkで使用するモデルを認識し、現時刻ｔkで使用するモデルの統合加重の総和が１になるように（使用しないモデルの統合加重は０とする）、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた各モデルの統合加重μ’（ｊ）を計算する処理を実施する。
統合計算部２２はカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハットを統合加重μ’（ｊ）で統合して、統合後の状態ベクトル推定値ｘk|kを出力するとともに、カルマンフィルタ処理部３−１〜３−３から出力された各モデルの推定誤差共分散行列の更新値Ｐk|k（ｊ）を統合加重μ’（ｊ）で統合して、統合後の推定誤差共分散行列Ｐk|kを出力する。
図４はこの発明の実施の形態１による状態推定方法を示すフローチャートである。
【００３３】
次に動作について説明する。
最初に、モデル確率記憶部７にモデル確率の初期値μ0（ｊ）（ｊ＝１，２，・・・，ｓ）が設定され、また、推定値記憶部４−１〜４−３に各モデルの状態ベクトル推定値の初期値ｘ0|0（ｊ）ハットと、推定誤差共分散行列の初期値Ｐ0|0（ｊ）が設定される。
その後、観測装置１から時刻ｔk（ｋ≧１とする）の観測ベクトルｙkが得られると、以下のフィルタ処理を開始する。
【００３４】
まず、混合処理部２の混合加重計算部１１は、モデル確率記憶部７に記憶されている前時刻ｔk-1のモデル確率μk-1（ｊ）を式（７）〜式（８）に代入して、混合加重ｍijを計算する（ステップＳＴ１）。ただし、ｉ＝１，２，・・・，ｓである。
混合処理部２の混合計算部１２−１〜１２−３は、混合加重計算部１１が混合加重ｍijを計算すると、推定値記憶部４−１〜４−３から前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐk-1|k-1（ｊ）の読み出しを行う。
【００３５】
混合計算部１２−１〜１２−３は、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと混合加重ｍijを式（５）に代入して、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを混合し、混合後の状態ベクトル推定値ｘmk-1|k-1（ｊ）ハットをカルマンフィルタ処理部３−１〜３−３に出力する（ステップＳＴ２）。
また、混合計算部１２−１〜１２−３は、前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）と、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと、混合後の状態ベクトル推定値ｘmk-1|k-1（ｊ）ハットと、混合加重ｍijとを式（６）に代入して、前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合し、混合後の推定誤差共分散行列Ｐmk-1|k-1（ｊ）をカルマンフィルタ処理部３−１〜３−３に出力する（ステップＳＴ３）。
【００３６】
カルマンフィルタ処理部３−１は、混合処理部２により混合されたモデル（１）の状態ベクトル推定値ｘmk-1|k-1（１）ハット及び推定誤差共分散行列Ｐmk-1|k-1（１）と、観測装置１から出力された観測ベクトルｙkとを入力して、モデル（１）のカルマンフィルタ処理を実施し、モデル（１）の状態ベクトル推定値の更新値ｘk|k（１）ハット及び推定誤差共分散行列の更新値Ｐk|k（１）を推定値記憶部４−１及び統合処理部１０に出力する（ステップＳＴ４）。
また、カルマンフィルタ処理部３−１は、モデル（１）の状態ベクトル予測値ｘk|k-1（１）ハット及び予測誤差共分散行列Ｐk|k-1（１）と、観測ベクトルｙkとをモデル尤度計算部５に出力する。
【００３７】
カルマンフィルタ処理部３−２は、カルマンフィルタ処理部３−１と並列にモデル（２）のカルマンフィルタ処理を実施する。
即ち、カルマンフィルタ処理部３−２は、混合処理部２により混合されたモデル（２）の状態ベクトル推定値ｘmk-1|k-1（２）ハット及び推定誤差共分散行列Ｐmk-1|k-1（２）と、観測装置１から出力された観測ベクトルｙkとを入力して、モデル（２）のカルマンフィルタ処理を実施し、モデル（２）の状態ベクトル推定値の更新値ｘk|k（２）ハット及び推定誤差共分散行列の更新値Ｐk|k（２）を推定値記憶部４−２及び統合処理部１０に出力する（ステップＳＴ４）。
また、カルマンフィルタ処理部３−２は、モデル（２）の状態ベクトル予測値ｘk|k-1（２）ハット及び予測誤差共分散行列Ｐk|k-1（２）と、観測ベクトルｙkとをモデル尤度計算部５に出力する。
【００３８】
カルマンフィルタ処理部３−３は、カルマンフィルタ処理部３−１，３−２と並列にモデル（３）のカルマンフィルタ処理を実施する。
即ち、カルマンフィルタ処理部３−３は、混合処理部２により混合されたモデル（３）の状態ベクトル推定値ｘmk-1|k-1（３）ハット及び推定誤差共分散行列Ｐmk-1|k-1（３）と、観測装置１から出力された観測ベクトルｙkとを入力して、モデル（３）のカルマンフィルタ処理を実施し、モデル（３）の状態ベクトル推定値の更新値ｘk|k（３）ハット及び推定誤差共分散行列の更新値Ｐk|k（３）を推定値記憶部４−３及び統合処理部１０に出力する（ステップＳＴ４）。
また、カルマンフィルタ処理部３−３は、モデル（３）の状態ベクトル予測値ｘk|k-1（３）ハット及び予測誤差共分散行列Ｐk|k-1（３）と、観測ベクトルｙkとをモデル尤度計算部５に出力する。
【００３９】
モデル尤度計算部５は、カルマンフィルタ処理部３−１〜３−３から状態ベクトル予測値ｘk|k-1（ｊ）ハット、予測誤差共分散行列Ｐk|k-1（ｊ）及び観測ベクトルｙkを受けると、その状態ベクトル予測値ｘk|k-1（ｊ）ハットと観測ベクトルｙkを式（９）に代入して、各モデルの残差ベクトルｙk（ｊ）チルダを算出する。
また、モデル尤度計算部５は、その予測誤差共分散行列Ｐk|k-1（ｊ）を式（１０）に代入して、各モデルの残差共分散行列Ｄk（ｊ）を算出する。
モデル尤度計算部５は、各モデルの残差ベクトルｙk（ｊ）チルダと残差共分散行列Ｄk（ｊ）を算出すると、各モデルの残差ベクトルｙk（ｊ）チルダと残差共分散行列Ｄk（ｊ）を式（１１）に代入して、各モデルのモデル尤度Λk（ｊ）を計算する（ステップＳＴ５）。
【００４０】
モデル確率計算部６は、モデル尤度計算部５が各モデルのモデル尤度Λk（ｊ）を計算すると、そのモデル尤度Λk（ｊ）を式（１２）に代入して、各モデルのモデル確率μk（ｊ）を計算し、そのモデル確率μk（ｊ）をモデル確率記憶部７に格納するとともに、そのモデル確率μk（ｊ）を統合処理部１０に出力する（ステップＳＴ６）。
【００４１】
モデル構成判定部８の処理内容は、下記の実施の形態３，４で詳述するが、モデル構成判定部８は、モデル尤度計算部５により計算されたモデル尤度Λk（ｊ）と統合処理部１０より統合された状態ベクトル推定値ｘk|kハットから、次時刻ｔk+1で使用するモデル（必要なモデル）の集合Ｓ’（Ｓ’⊂Ｓ＝｛１，２，・・・，ｓ｝）を決定し、そのモデルの集合Ｓ’をモデル構成判定結果としてモデル構成記憶部９に格納する（ステップＳＴ７）。
【００４２】
統合処理部１０は、カルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）のうち、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すると判別されたモデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）を、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた統合加重で統合する。
【００４３】
即ち、統合処理部１０の統合加重調整部２１は、モデル構成記憶部９に記憶されている前時刻ｔk-1のモデル構成判定結果Ｓ’を参照して、現時刻ｔkで使用するモデルを認識する。
統合加重調整部２１は、下記の式（１６）に示すように、現時刻ｔkで使用しないモデルの統合加重を０とし、現時刻ｔkで使用するモデルの統合加重の総和が１になるように、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた各モデルの統合加重μ’（ｊ）を計算する（ステップＳＴ８）。
【数７】
【００４４】
統合処理部１０の統合計算部２２は、統合加重調整部２１が各モデルの統合加重μ’（ｊ）を計算すると、その統合加重μ’（ｊ）とカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハットを下記の式（１７）に代入して、各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハットを統合し、統合後の状態ベクトル推定値ｘk|kを出力する（ステップＳＴ９）。
また、統合計算部２２は、各モデルの統合加重μ’（ｊ）と、各モデルの推定誤差共分散行列の更新値Ｐk|k（ｊ）と、各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハットと、統合後の状態ベクトル推定値ｘk|kとを下記の式（１８）に代入して、各モデルの推定誤差共分散行列の更新値Ｐk|k（ｊ）を統合し、統合後の推定誤差共分散行列Ｐk|kを出力する（ステップＳＴ１０）。
【数８】
【００４５】
なお、この実施の形態１では、現時刻ｔkで使用しないモデルのカルマンフィルタの処理結果を統合処理部１０での統合処理に使用しないようにしているが、使用しないモデルのカルマンフィルタの処理自体は継続して実行している。
その理由は、上記カルマンフィルタの出力より得られるモデル尤度を継続的に監視するなどして、使用再開の判断を行えるよう配慮したものである。
また、一旦処理を停止してしまうと、処理を再開する際、初期値から動作を開始する必要があり、処理結果が収束するまでの間に、フィルタ全体の推定精度に影響を与えることがあるからである。
【００４６】
以上で明らかなように、この実施の形態１によれば、モデル確率計算部６により計算されたモデル尤度Λk（ｊ）と統合処理部１０より統合された状態ベクトル推定値ｘk|kハットから、次時刻ｔk+1で使用するモデルを判別するモデル構成判定部８を設け、統合処理部１０がカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）のうち、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すると判別されたモデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）を、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた統合加重で統合するように構成したので、不要なモデルのカルマンフィルタの処理結果がフィルタ全体の推定精度に与える悪影響を抑制することができる効果を奏する。
【００４７】
実施の形態２．
図５はこの発明の実施の形態２による状態推定装置を示す構成図であり、図において、図１と同一符号は同一または相当部分を示すので説明を省略する。
混合処理部３０は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハット及び推定誤差共分散行列Ｐk-1|k-1（ｊ）のうち、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すると判別されたモデルの状態ベクトル推定値ｘk-1|k-1（ｊ）ハット及び推定誤差共分散行列Ｐk-1|k-1（ｊ）を、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた混合加重で混合する処理を実施する。
なお、混合処理部３０は混合処理手段を構成している。
【００４８】
図６はこの発明の実施の形態２による状態推定装置の混合処理部３０を示す構成図であり、図において、混合加重計算部３１はモデル確率記憶部７に記憶されている前時刻ｔk-1のモデル確率μk-1（ｊ）から、式（７）〜式（８）によって、混合加重ｍijを計算する処理を実施する。
混合加重調整部３２はモデル構成記憶部９に記憶されている前時刻ｔk-1のモデル構成判定結果Ｓ’を参照して、現時刻ｔkで使用するモデルを認識し、現時刻ｔkで使用するモデルの混合加重の総和が１になるように（使用しないモデルの統合加重は０とする）、混合加重計算部３１により計算された混合加重ｍijを調整する処理を実施する。
混合計算部３３−１は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを調整後の混合加重ｍ’ijで混合して、モデル（１）の状態ベクトル推定値ｘmk-1|k-1（１）ハットをカルマンフィルタ処理部３−１に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を調整後の混合加重ｍ’ijで混合して、モデル（１）の推定誤差共分散行列Ｐmk-1|k-1（１）をカルマンフィルタ処理部３−１に出力する。
【００４９】
混合計算部３３−２は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを調整後の混合加重ｍ’ijで混合して、モデル（２）の状態ベクトル推定値ｘmk-1|k-1（２）ハットをカルマンフィルタ処理部３−２に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を調整後の混合加重ｍ’ijで混合して、モデル（２）の推定誤差共分散行列Ｐmk-1|k-1（２）をカルマンフィルタ処理部３−２に出力する。
混合計算部３３−３は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを調整後の混合加重ｍ’ijで混合して、モデル（３）の状態ベクトル推定値ｘmk-1|k-1（３）ハットをカルマンフィルタ処理部３−３に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を調整後の混合加重ｍ’ijで混合して、モデル（３）の推定誤差共分散行列Ｐmk-1|k-1（３）をカルマンフィルタ処理部３−３に出力する。
【００５０】
次に動作について説明する。
混合処理部３０以外は、上記実施の形態１と同様であるため、混合処理部３０の処理内容のみを説明する。
混合処理部３０の混合加重計算部３１は、モデル確率記憶部７に記憶されている前時刻ｔk-1のモデル確率μk-1（ｊ）を式（７）〜式（８）に代入して、混合加重ｍijを計算する。
【００５１】
混合処理部３０の混合加重調整部３２は、混合加重計算部３１が混合加重ｍijを計算すると、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すべきでないと判別されたモデルの前時刻ｔk-1におけるカルマンフィルタの処理結果が、現時刻ｔkにおけるカルマンフィルタの混合計算に入力されるのを阻止するため、混合加重計算部３１により計算された混合加重ｍijを調整する。
【００５２】
即ち、混合加重調整部３２は、モデル構成記憶部９に記憶されている前時刻ｔk-1のモデル構成判定結果Ｓ’を参照して、現時刻ｔkで使用するモデルを認識する。
混合加重調整部３２は、下記の式（１９）に示すように、現時刻ｔkで使用しないモデルの混合加重を０とし、現時刻ｔkで使用するモデルの混合加重の総和が１になるように、混合加重計算部３１により計算された混合加重ｍijを調整する。
混合加重調整部３２は、調整後の混合加重ｍ’ijを混合計算部３３−１〜３３−３に出力する。ただし、ｉ＝１，２，・・・，ｓである。
【数９】
【００５３】
混合計算部３３−１〜３３−３は、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと、混合加重調整部３２により調整された混合加重ｍ’ijを下記の式（２０）に代入して、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを混合し、混合後の状態ベクトル推定値ｘmk-1|k-1（ｊ）ハットをカルマンフィルタ処理部３−１〜３−３に出力する。
また、混合計算部３３−１〜３３−３は、前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）と、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと、混合後の状態ベクトル推定値ｘmk-1|k-1（ｊ）ハットと、調整後の混合加重ｍ’ijとを下記の式（２１）に代入して、前時刻ｔk-1の推定誤差共分散行列Ｐmk-1|k-1（ｊ）を混合し、混合後の推定誤差共分散行列Ｐmk-1|k-1（ｊ）をカルマンフィルタ処理部３−１〜３−３に出力する。
【数１０】
【００５４】
なお、この実施の形態２では、現時刻ｔkで使用しないモデルの前時刻ｔk-1におけるカルマンフィルタの処理結果を混合処理部３０での混合処理に使用しないようにしているが、使用しないモデルのカルマンフィルタの処理自体は継続して実行している。
その理由は、上記カルマンフィルタの出力より得られるモデル尤度を継続的に監視するなどして、使用再開の判断を行えるよう配慮したものである。
また、一旦処理を停止してしまうと、処理を再開する際、初期値から動作を開始する必要があり、処理結果が収束するまでの間に、フィルタ全体の推定精度に影響を与えることがあるからである。
【００５５】
以上で明らかなように、この実施の形態２によれば、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハット及び推定誤差共分散行列Ｐk-1|k-1（ｊ）のうち、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すると判別されたモデルの状態ベクトル推定値ｘk-1|k-1（ｊ）ハット及び推定誤差共分散行列Ｐk-1|k-1（ｊ）を、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた混合加重で混合するように構成したので、上記実施の形態１よりも更に、不要なモデルのカルマンフィルタの処理結果がフィルタ全体の推定精度に与える悪影響を抑制することができる効果を奏する。
【００５６】
実施の形態３．
図７はこの発明の実施の形態３による状態推定装置のモデル構成判定部８を示す構成図であり、図において、状態量によるモデル構成判定部４１は統合処理部１０より統合された状態ベクトル推定値ｘk|kハットに含まれている状態変数の値を調べて、使用するモデルを判別する処理を実施する。
モデル尤度によるモデル構成判定部４２はモデル尤度計算部５により計算されたモデル尤度Λk（ｊ）に基づいて使用するモデルを判別する処理を実施する。
モデル構成決定部４３はモデル構成判定部４１，４２の判別結果を統合して、最終的に使用するモデルを判別する処理を実施する。
【００５７】
次に動作について説明する。
状態量によるモデル構成判定部４１は、統合処理部１０から統合後の状態ベクトル推定値ｘk|kハットを入力し、その状態ベクトル推定値ｘk|kハットに含まれている状態変数の値を調べて、使用するモデルを判別する。
モデル構成判定部４１の判別処理は、特に状態変数の値に応じて、どのようなモデルのカルマンフィルタが必要であるかを事前知識として有している場合に有効である。
【００５８】
図８は状態ベクトルｘkが２つの状態変数ｘ1k，ｘ2kで構成されているとき、２つの状態変数ｘ1k，ｘ2kの値に応じて、使用するモデルを判別している例を示す説明図である。
図８では、以下に示すようなモデルの選択ルールを表している。ただし、ａ，ｂは閾値である。
ｘ1k＜ａ ＡＮＤ ｘ2k＜ｂ → モデル（１）（２）（３）を使用
ｘ1k＜ａ ＡＮＤ ｘ2k≧ｂ → モデル（１）（２）を使用
ｘ1k≧ａ ＡＮＤ ｘ2k＜ｂ → モデル（２）（３）を使用
ｘ1k≧ａ ＡＮＤ ｘ2k≧ｂ → モデル（２）を使用
なお、上記の選択ルールは、一例であり、ＡＮＤ条件の代わりにＯＲ条件を使用したり、あるいは、例えば、状態変数ｘ1k，ｘ2kの和や積をとるなど、状態変数ｘ1k，ｘ2kに対して、何らかの演算を施したりして、その結果の範囲に対する条件などでルールを作成してもよい。
【００５９】
モデル尤度によるモデル構成判定部４２は、モデル尤度計算部５から各モデルのモデル尤度Λk（ｊ）を入力し、そのモデル尤度Λk（ｊ）が所定の閾値より低い場合、当該モデルを不要なモデルと判別し、そのモデル尤度Λk（ｊ）が所定の閾値より高い場合、当該モデルを必要なモデルと判別する。
ただし、モデル尤度Λk（ｊ）は、観測雑音の影響を受けて変動するため、時間平滑化を行ってから使用するものとする。即ち、時間平滑化後のモデル尤度Λk（ｊ）バーを所定の閾値と比較して、必要なモデルか否かを判別する。
【００６０】
この時間平滑化の処理としては、例えば、下記の式（２２）のように、過去Ｎ時刻分のモデル尤度Λk（ｊ）の移動平均を求めたり、あるいは、式（２３）のように、一次の低域通過フィルタを通すことによって実現することができる。ただし、これらの平滑化処理は、当然ながら、モデル番号毎に独立に実行する。式（２３）の右辺のαは、０＜α＜１を満たすように選択された平滑化ゲインである。
【数１１】
【００６１】
モデル構成決定部４３は、モデル構成判定部４１，４２の判別結果を入力し、モデル番号毎に、そのモデルのカルマンフィルタを次時刻において使用するか否かの最終判定を行う。
判定基準としては、状態量によるモデル構成判定部４１と、モデル尤度によるモデル構成判定部４２の両方で使用すると判別されたモデルを、最終的に使用するモデルとするＡＮＤ条件や、いずれか一方で使用すると判別されたモデルを、最終的に使用するモデルとするＯＲ条件を用いることが考えられる。
また、状態量によるモデル構成判定部４１の判別結果と、モデル尤度によるモデル構成判定部４２の判別結果に優先度を設定し、優先度の高い方の判別結果が使用するモデルとされていれば、そのモデルを使用するようにしてもよい。
さらに、上記優先度が時刻や運用条件によって変動せず、予め固定的に設定できる場合は、当初から状態量によるモデル構成判定部４１、モデル尤度によるモデル構成判定部４２のいずれか必要な方のみを動作させ、モデル構成決定部４３の処理を省略することにより装置を簡素化できることは言うまでもない。
【００６２】
なお、この判定基準は、各モデルに共通でもよいし、また、各モデル毎に独立に設定してもよい。
さらに、あるモデルについての判別結果が、時刻毎に頻繁に入れ替わるのを防ぐ目的で、連続して何回か同じ判別結果が得られた場合に限り、「使用／不使用」の判別結果を反転させることができるとする条件を加味することも考えられる。
【００６３】
以上で明らかなように、この実施の形態３によれば、モデル尤度計算部５により計算されたモデル尤度Λk（ｊ）と統合処理部１０より統合された状態ベクトル推定値ｘk|kハットから、使用するモデルを判別するように構成したので、的確なモデルを使用することができるようになり、その結果、状態推定精度を高めることができる効果を奏する。
【００６４】
実施の形態４．
図９はこの発明の実施の形態４による状態推定装置のモデル構成判定部８を示す構成図であり、図において、図７と同一符号は同一または相当部分を示すので説明を省略する。
モデル確率調整部４４はモデル構成決定部４３により最終的に使用すると判別されたモデルのモデル尤度Λk（ｊ）（または、時間平滑化後のモデル尤度Λk（ｊ）バー）を正規化し、正規化後のモデル尤度Λk（ｊ）にしたがってモデル確率記憶部７に記憶されているモデル確率μk（ｊ）を変更する処理を実施する。
【００６５】
次に動作について説明する。
モデル確率調整部４４は、モデル構成決定部４３により決定された現時刻のモデル構成判定結果が前時刻のモデル構成判定結果と異なり、あるモデルの使用が中止されたり、逆にあるモデルの使用が新たに開始された場合に、モデル確率記憶部７に記憶されているモデル確率μk（ｊ）を変更するものである。
ここで、式（１２）と式（８）によって逐次的に計算されるモデル確率μk（ｊ）は、過去から現時刻に至るまでのモデル尤度Λk（ｊ）の情報を含んでいる。このため、モデル構成が変更された場合には、過去の情報を極力排除し、最近のモデル尤度の情報のみでモデル確率を再設定することが、フィルタ全体の収束性を高めるために有効である。
ただし、各時刻のモデル尤度は、観測雑音の影響を受けて変動する可能性があるため、式（２２）や式（２３）によって時間平滑化されたモデル尤度Λk（ｊ）バーを用いることが望ましい。
【００６６】
そこで、モデル確率調整部４４では、モデル尤度によるモデル構成判定部４２から時間平滑化されたモデル尤度Λk（ｊ）バーを入力し、下記の式（２４）に示すように、モデル構成決定部４３により最終的に使用すると決定されたモデル群に対して、モデル尤度Λk（ｊ）バーを正規化することよってモデル確率μk（ｊ）を計算し、そのモデル確率μk（ｊ）をモデル確率記憶部７に格納する。
【数１２】
【００６７】
以上で明らかなように、この実施の形態４によれば、モデル構成決定部４３により最終的に使用すると判別されたモデルのモデル尤度Λk（ｊ）を正規化し、正規化後のモデル尤度Λk（ｊ）にしたがってモデル確率記憶部７に記憶されているモデル確率μk（ｊ）を変更するように構成したので、モデル構成の変更が生じた場合にも、フィルタ出力に過渡応答が生じて収束が遅れるのを抑制することができるようになり、この結果、上記実施の形態１，２よりも、更に、推定精度を高めることができる効果を奏する。
【図面の簡単な説明】
【００６８】
【図１】この発明の実施の形態１による状態推定装置を示す構成図である。
【図２】この発明の実施の形態１による状態推定装置の混合処理部２を示す構成図である。
【図３】この発明の実施の形態１による状態推定装置の統合処理部１０を示す構成図である。
【図４】この発明の実施の形態１による状態推定方法を示すフローチャートである。
【図５】この発明の実施の形態２による状態推定装置を示す構成図である。
【図６】この発明の実施の形態２による状態推定装置の混合処理部３０を示す構成図である。
【図７】この発明の実施の形態３による状態推定装置のモデル構成判定部８を示す構成図である。
【図８】状態ベクトルｘkが２つの状態変数ｘ1k，ｘ2kで構成されているとき、２つの状態変数ｘ1k，ｘ2kの値に応じて、使用するモデルを判別している例を示す説明図である。
【図９】この発明の実施の形態４による状態推定装置のモデル構成判定部８を示す構成図である。
【符号の説明】
【００６９】
１ 観測装置、２ 混合処理部（混合処理手段）、３−１〜３−３ カルマンフィルタ処理部（カルマンフィルタ処理手段）、４−１〜４−３ 推定値記憶部、５ モデル尤度計算部（モデル確率算出手段）、６ モデル確率計算部（モデル確率算出手段）、７ モデル確率記憶部、８ モデル構成判定部（モデル判別手段）、９ モデル構成記憶部、１０ 統合処理部（統合処理手段）、１１ 混合加重計算部、１２−１〜１２−３ 混合計算部、２１ 統合加重調整部、２２ 統合計算部、３０ 混合処理部（混合処理手段）、３１ 混合加重計算部、３２ 混合加重調整部、３３−１〜３３−３ 混合計算部、４１ 状態量によるモデル構成判定部、４２ モデル尤度によるモデル構成判定部、４３ モデル構成決定部、４４ モデル確率調整部。
【技術分野】
【０００１】
この発明は、観測装置により観測された観測値に基づいて、動特性を有するシステムの状態値を推定する状態推定装置及び状態推定方法に関し、特に、化学、鉄鋼などのプラント、自動車、各種モータなどの制御機器、画像、音声、レーダなどの信号処理機器、医療分野における生体計測機器、衛星や飛翔体などの移動体追跡システム、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）や慣性航法装置を応用した測位システム、通信システム、経済予測システムなどの広範な分野に応用される状態推定装置及び状態推定方法に関するものである。
【背景技術】
【０００２】
システムの状態量の動特性を表す運動方程式と、状態量と観測量の関係を表す観測方程式（以下、上記の運動方程式と観測方程式を併せて、「状態方程式」と称する）とに基づいて、観測量の時系列データからシステムの状態量を推定する最適フィルタ方式として、カルマンフィルタがよく知られている。
上記の状態方程式は、システムの動特性と観測系の性質を数学的に表現した数式モデルに相当する。
ただし、カルマンフィルタは、この数式モデルが時間的に変動せず一定であるか、もしくは、その変動が確定的で事前に既知であることを前提としている。
【０００３】
実際の応用場面では、モデルが時間とともに変動し、また、その変動の程度やタイミングが未知である場合も多い。
したがって、このような場面では使用することができないカルマンフィルタの弱点を克服するフィルタ方式として、事前に想定される複数のモデルに対応するカルマンフィルタを並列に動作させる多重モデル方式のフィルタが各種提案されている。
その中でも、推定精度と演算負荷のバランスから最も有効とされるのが、ＩＭＭ（Ｉｎｔｅｒａｃｔｉｎｇ Ｍｕｌｔｉｐｌｅ Ｍｏｄｅｌ）フィルタである（例えば、非特許文献１を参照）。
【０００４】
ＩＭＭフィルタでは、例えば、以下に示すような状態方程式を使用する。アンダーラインを付している変数はベクトルであることを示している。
【数１】
【０００５】
式（１）はシステムの運動方程式であり、式（２）は観測方程式である。
また、ｘkはｐ次元状態ベクトル、ｙkはｑ次元観測ベクトル、ｒkはシステムのモデル変数、ｗkはシステム雑音ベクトル、ｖkは観測雑音ベクトルを表している。
また、係数行列Ａ（），Ｂ（），Ｈ（），Ｇ（）はｒkの既知の行列関数である。なお、ｋはサンプリング時刻（以下、単に時刻と称する）ｔkを表すインデックスである。
【０００６】
状態ベクトルｘkは、システムの動的挙動を規定する複数の状態変数を含むベクトルである。
例えば、モータの制御機器においては、コイルに流れる電流値や、モータシャフトの回転位置、速度などで構成される。
また、衛星の追跡システムにおいては、衛星の位置、速度、姿勢角などで構成される。
また、ある種の経済予測システムにおいては、収入、税率、失業率、経済成長率などが含まれ、生体計測機器の例では、血糖値、脈拍、呼吸数、体温などが含まれる。
【０００７】
観測ベクトルｙkは、何らかの観測系で得られる複数の観測量で構成されたベクトルであり、状態ベクトルｘkと式（２）の関係で関連付けられている。
観測ベクトルｙkの時系列データに基づいて状態ベクトルｘkを推定することがフィルタ処理の目的となる。
また、システム雑音ベクトルｗk、観測雑音ベクトルｖkは、互いに独立な白色ガウス雑音であると仮定する。
【０００８】
モデル変数ｒkは、複数の数式モデル（状態方程式）を識別する変数に相当する。即ち、モデル変数ｒkが変わると、係数行列Ａ（），Ｂ（），Ｈ（），Ｇ（）の値が変わることによって、式（１）、式（２）は別の運動方程式、観測方程式を表すことになる。
モデル変数ｒkは、時刻ｔkにおいて、ｓ個の離散的状態（Ｓ＝｛１，２，・・・，ｓ｝で表す）のいずれかをとり、その値は、確率的に推移すると仮定する。
【０００９】
この推移においては、一次マルコフ連鎖を仮定し、時刻ｔkにおいて、システムがいずれの離散的状態にあるかの確率は、時刻ｔk-1の離散的状態のみに依存するものとする。
そこで、下記の式（３）に示すように、時刻ｔk-1でシステムが離散的状態ｉであったときに時刻ｔkで状態ｊに推移する確率ｐijを予め規定する。ただし、定義により、下記の式（４）が成立する。
【数２】
【００１０】
ＩＭＭフィルタは、上記の状態方程式の記述に基づいて、ｓ個のカルマンフィルタからなるフィルタバンクを構成し、以下のように処理を実行する。
まず、初期時刻ｔ0に対するｓ個のモデルｊ（ｊ＝１，２，・・・，ｓ）のモデル確率の初期値μ0（ｊ）と、各モデルに対応するカルマンフィルタの状態ベクトル推定値の初期値ｘ0|0（ｊ）ハットと、推定誤差共分散行列の初期値Ｐ0|0（ｊ）とを設定して、フィルタ処理を開始する。
サンプリング時刻毎の処理ループに入ると、最初に、以下の混合処理を行う。ただし、以下では、時刻ｔkにおける処理を示すものとする。
【００１１】
混合処理では、各モデルのカルマンフィルタが前時刻ｔk-1で算出した状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐk-1|k-1（ｊ）を互いに交換して、次式のような重み付け統合を行う。式（７）のｍijは混合加重である。また、Ｔは行列の転置を表している。
【数３】
【００１２】
次に、式（５）のｘmk-1|k-1（ｊ）ハットと、式（６）のＰmk-1|k-1（ｊ）をモデルｊ（ｊ＝１，２，・・・，ｓ）のフィルタの入力とし、カルマンフィルタのアルゴリズムにしたがって予測処理と更新処理を実行する。
これにより、各モデルの状態ベクトル予測値ｘk|k-1（ｊ）ハットと、予測誤差共分散行列Ｐk|k-1（ｊ）と、更新後の状態ベクトル推定値ｘk|k（ｊ）ハットと、推定誤差共分散行列Ｐk|k（ｊ）とが得られる。
【００１３】
次に、上記の状態ベクトル予測値ｘk|k-1（ｊ）ハットと、予測誤差共分散行列Ｐk|k-1（ｊ）を使用し、以下の手順にしたがって各モデルのモデル確率を更新する。
まず、以下の式（９）、式（１０）にしたがって、各モデルの残差ベクトルｙk（ｊ）チルダと、残差共分散行列Ｄk（ｊ）を算出する。
【数４】
【００１４】
式（１０）の右辺第２項のＲkは観測誤差共分散行列である。
残差ベクトルｙk（ｊ）チルダは、状態ベクトル予測値から求められた観測ベクトルの予測値と、実際に取得された観測ベクトルとの差を表すベクトルである。
この残差ベクトルｙk（ｊ）チルダは、平均零ベクトルと残差共分散行列Ｄk（ｊ）の多変量ガウス分布にしたがって実現すると仮定すれば、式（１１）により算出される確率密度Λk（ｊ）はモデルｊの尤度を表すものとなる。
なお、モデルｊの尤度Λk（ｊ）は現時刻ｔkの観測ベクトルｙkを受けた結果として、各モデルのフィルタがどの程度適合しているかを測る指標値となる。
この尤度Λk（ｊ）を使用する式（１２）にしたがって、現時刻ｔkに対応するモデル確率μk（ｊ）が算出される。
式（１２）と式（８）により逐次的に計算されるモデル確率μk（ｊ）は、前時刻ｔk-1のモデル確率μk-1（ｊ）及びモデル間の推移ｐijと、現時刻ｔkの尤度Λk（ｊ）から推論された「モデルｊが正しい確率」であり、下記の式（１３）が成立する。
【数５】
【００１５】
最後に、ＩＭＭフィルタは、各モデルのカルマンフィルタの出力である状態ベクトル推定値ｘk|k（ｊ）ハットと、推定誤差共分散行列Ｐk|k（ｊ）とを上記のモデル確率μk（ｊ）で重み付け統合した値を時刻ｔkの処理結果として外部に出力し、時刻ｔkの処理を終了する。
【数６】
【００１６】
上記のＩＭＭフィルタを搭載している従来の状態推定装置は、以下のように構成されている。
（１）予め用意されているモデルのカルマンフィルタ処理を実施して、当該モデルの状態ベクトル推定値ｘk|k（ｊ）ハットと推定誤差共分散行列Ｐk|k（ｊ）を更新する複数のカルマンフィルタ処理部
（２）複数のカルマンフィルタ処理部により更新された状態ベクトル推定値ｘk|k（ｊ）ハットと推定誤差共分散行列Ｐk|k（ｊ）から、式（９）〜式（１１）によって、各モデルのモデル尤度Λk（ｊ）を計算するモデル尤度計算部
（３）モデル尤度計算部により計算されたモデル尤度Λk（ｊ）から、式（１２）によって、各モデルのモデル確率μk（ｊ）を計算するモデル確率計算部
（４）複数のカルマンフィルタ処理部により更新された状態ベクトル推定値ｘk|k（ｊ）ハットと推定誤差共分散行列Ｐk|k（ｊ）を、式（１４）〜式（１５）によって、モデル確率計算部により計算されたモデル確率μk（ｊ）に応じた統合加重で統合する統合処理部
（５）複数のカルマンフィルタ処理部により更新された状態ベクトル推定値ｘk|k（ｊ）ハットと推定誤差共分散行列Ｐk|k（ｊ）を、式（５）〜式（８）によって、モデル確率計算部により計算されたモデル確率μk（ｊ）に応じた混合加重で混合し、混合後の状態ベクトル推定値ｘmk-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐmk-1|k-1（ｊ）を複数のカルマンフィルタ処理部に出力する混合処理部
【００１７】
上記のＩＭＭフィルタを搭載している従来の状態推定装置は、複数のモデルに対応するカルマンフィルタを並列動作させることによって、システムの動特性や観測系の性質が変動する場合でも、状態量を推定することができる。
また、事前に設定したモデルの中に、システムや観測系の性質を正確に描写したものが存在しなくても、それに近いモデルのフィルタ出力ほど高いモデル確率で重み付けされて統合処理されることにより、近似的にシステム状態量を計算することができる。
【００１８】
【非特許文献１】ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎ ｏｎ Ａｕｔｏｍａｔｉｃ Ｃｏｎｔｒｏｌ，Ｖｏｌ． ３３，Ｎｏ．８，ｐｐ．７８０−７８３，Ａｕｇｕｓｔ １９８８
【発明の開示】
【発明が解決しようとする課題】
【００１９】
従来の状態推定装置は以上のように構成されているので、推定処理の開始から終了に至るまで、予め固定的に設定されたｓ個のモデルのカルマンフィルタの処理結果が統合され、その統合結果がフィルタ全体の推定結果として得られる。しかし、実際には、推定処理に必要となるモデルの数や種類が時間の経過に伴って変動する場合があり、ある時刻において、無用なモデルのカルマンフィルタを使用すると、観測雑音の影響で、そのモデルのモデル確率が零より大きな値となるため、式（１４）、式（１５）の統合処理に影響を与えて、推定精度の劣化を招くことがある。また、式（５）〜式（８）の混合処理において、無用なモデルのカルマンフィルタの処理結果が、他のモデルのカルマンフィルタの入力に影響を与えるため、結果的にフィルタ全体の推定精度の劣化を引き起こすことがある課題があった。
【００２０】
この発明は上記のような課題を解決するためになされたもので、推定処理の途中で、必要なモデルを判別して、不要なモデルのカルマンフィルタの処理結果がフィルタ全体の推定精度に与える悪影響を抑制することができる状態推定装置及び状態推定方法を得ることを目的とする。
【課題を解決するための手段】
【００２１】
この発明に係る状態推定装置は、モデル確率算出手段により算出されたモデル尤度から必要なモデルを判別するモデル判別手段を設け、統合処理手段が複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列のうち、モデル判別手段により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列をモデル確率算出手段により算出されたモデル確率に応じた統合加重で統合するようにしたものである。
【発明の効果】
【００２２】
この発明によれば、モデル確率算出手段により算出されたモデル尤度から必要なモデルを判別するモデル判別手段を設け、統合処理手段が複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列のうち、モデル判別手段により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列をモデル確率算出手段により算出されたモデル確率に応じた統合加重で統合するように構成したので、不要なモデルのカルマンフィルタの処理結果がフィルタ全体の推定精度に与える悪影響を抑制することができる効果がある。
【発明を実施するための最良の形態】
【００２３】
実施の形態１．
図１はこの発明の実施の形態１による状態推定装置を示す構成図である。図１では、モデル数がｓ＝３の場合の例を示している。言うまでもないが、他のモデル数の場合にも同様に構成することができる。
図において、観測装置１は例えばセンサなどの計測手段であり、時刻ｔkの観測ベクトルｙkを状態推定装置に出力する。
【００２４】
混合処理部２は推定値記憶部４−１〜４−３から前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐk-1|k-1（ｊ）を読み出し、式（５）〜式（８）にしたがって、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐk-1|k-1（ｊ）を前時刻ｔk-1のモデル確率μk-1（ｊ）に応じた混合加重で混合する処理を実施する。ただし、ｊ＝１，２，・・・，ｓである。
なお、混合処理部２は混合処理手段を構成している。
【００２５】
カルマンフィルタ処理部３−１は混合処理部２により混合されたモデル（１）の状態ベクトル推定値ｘmk-1|k-1（１）ハット及び推定誤差共分散行列Ｐmk-1|k-1（１）と、観測装置１から出力された観測ベクトルｙkとを入力して、モデル（１）のカルマンフィルタ処理を実施し、モデル（１）の状態ベクトル推定値の更新値ｘk|k（１）ハット及び推定誤差共分散行列の更新値Ｐk|k（１）を推定値記憶部４−１及び統合処理部１０に出力する。
カルマンフィルタ処理部３−２は混合処理部２により混合されたモデル（２）の状態ベクトル推定値ｘmk-1|k-1（２）ハット及び推定誤差共分散行列Ｐmk-1|k-1（２）と、観測装置１から出力された観測ベクトルｙkとを入力して、モデル（２）のカルマンフィルタ処理を実施し、モデル（２）の状態ベクトル推定値の更新値ｘk|k（２）ハット及び推定誤差共分散行列の更新値Ｐk|k（２）を推定値記憶部４−２及び統合処理部１０に出力する。
【００２６】
カルマンフィルタ処理部３−３は混合処理部２により混合されたモデル（３）の状態ベクトル推定値ｘmk-1|k-1（３）ハット及び推定誤差共分散行列Ｐmk-1|k-1（３）と、観測装置１から出力された観測ベクトルｙkとを入力して、モデル（３）のカルマンフィルタ処理を実施し、モデル（３）の状態ベクトル推定値の更新値ｘk|k（３）ハット及び推定誤差共分散行列の更新値Ｐk|k（３）を推定値記憶部４−３及び統合処理部１０に出力する。
なお、カルマンフィルタ処理部３−１〜３−３はカルマンフィルタ処理手段を構成している。
推定値記憶部４−１〜４−３はカルマンフィルタ処理部３−１〜３−３から出力された状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）を記憶するメモリである。
【００２７】
モデル尤度計算部５はカルマンフィルタ処理部３−１〜３−３から状態ベクトル予測値ｘk|k-1（ｊ）ハット、予測誤差共分散行列Ｐk|k-1（ｊ）及び観測ベクトルｙkを収集して、式（９）〜式（１１）によって、各モデルのモデル尤度Λk（ｊ）を計算する処理を実施する。
モデル確率計算部６はモデル尤度計算部５により計算されたモデル尤度Λk（ｊ）から、式（１２）によって、各モデルのモデル確率μk（ｊ）を計算する処理を実施する。
なお、モデル尤度計算部５及びモデル確率計算部６からモデル確率算出手段が構成されている。
モデル確率記憶部７はモデル確率計算部６により計算された各モデルのモデル確率μk（ｊ）を記憶するメモリである。
【００２８】
モデル構成判定部８はモデル尤度計算部５により計算されたモデル尤度Λk（ｊ）と統合処理部１０より統合された状態ベクトル推定値ｘk|kハットから、次時刻ｔk+1で使用するモデル（必要なモデル）の集合Ｓ’（Ｓ’⊂Ｓ＝｛１，２，・・・，ｓ｝）を決定し、そのモデルの集合Ｓ’をモデル構成判定結果としてモデル構成記憶部９に格納する処理を実施する。
なお、モデル構成判定部８はモデル判別手段を構成している。
モデル構成記憶部９はモデル構成判定結果Ｓ’を記憶するメモリである。
【００２９】
統合処理部１０はカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）のうち、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すると判別されたモデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）を、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた統合加重で統合する処理を実施する。
なお、統合処理部１０は統合処理手段を構成している。
【００３０】
図２はこの発明の実施の形態１による状態推定装置の混合処理部２を示す構成図であり、図において、混合加重計算部１１はモデル確率記憶部７に記憶されている前時刻ｔk-1のモデル確率μk-1（ｊ）から、式（７）〜式（８）によって、混合加重ｍijを計算する処理を実施する。
混合計算部１２−１は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを混合加重ｍijで混合して、モデル（１）の状態ベクトル推定値ｘmk-1|k-1（１）ハットをカルマンフィルタ処理部３−１に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合加重ｍijで混合して、モデル（１）の推定誤差共分散行列Ｐmk-1|k-1（１）をカルマンフィルタ処理部３−１に出力する。
【００３１】
混合計算部１２−２は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを混合加重ｍijで混合して、モデル（２）の状態ベクトル推定値ｘmk-1|k-1（２）ハットをカルマンフィルタ処理部３−２に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合加重ｍijで混合して、モデル（２）の推定誤差共分散行列Ｐmk-1|k-1（２）をカルマンフィルタ処理部３−２に出力する。
混合計算部１２−３は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを混合加重ｍijで混合して、モデル（３）の状態ベクトル推定値ｘmk-1|k-1（３）ハットをカルマンフィルタ処理部３−３に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合加重ｍijで混合して、モデル（３）の推定誤差共分散行列Ｐmk-1|k-1（３）をカルマンフィルタ処理部３−３に出力する。
【００３２】
図３はこの発明の実施の形態１による状態推定装置の統合処理部１０を示す構成図であり、図において、統合加重調整部２１はモデル構成記憶部９に記憶されている前時刻ｔk-1のモデル構成判定結果Ｓ’を参照して、現時刻ｔkで使用するモデルを認識し、現時刻ｔkで使用するモデルの統合加重の総和が１になるように（使用しないモデルの統合加重は０とする）、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた各モデルの統合加重μ’（ｊ）を計算する処理を実施する。
統合計算部２２はカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハットを統合加重μ’（ｊ）で統合して、統合後の状態ベクトル推定値ｘk|kを出力するとともに、カルマンフィルタ処理部３−１〜３−３から出力された各モデルの推定誤差共分散行列の更新値Ｐk|k（ｊ）を統合加重μ’（ｊ）で統合して、統合後の推定誤差共分散行列Ｐk|kを出力する。
図４はこの発明の実施の形態１による状態推定方法を示すフローチャートである。
【００３３】
次に動作について説明する。
最初に、モデル確率記憶部７にモデル確率の初期値μ0（ｊ）（ｊ＝１，２，・・・，ｓ）が設定され、また、推定値記憶部４−１〜４−３に各モデルの状態ベクトル推定値の初期値ｘ0|0（ｊ）ハットと、推定誤差共分散行列の初期値Ｐ0|0（ｊ）が設定される。
その後、観測装置１から時刻ｔk（ｋ≧１とする）の観測ベクトルｙkが得られると、以下のフィルタ処理を開始する。
【００３４】
まず、混合処理部２の混合加重計算部１１は、モデル確率記憶部７に記憶されている前時刻ｔk-1のモデル確率μk-1（ｊ）を式（７）〜式（８）に代入して、混合加重ｍijを計算する（ステップＳＴ１）。ただし、ｉ＝１，２，・・・，ｓである。
混合処理部２の混合計算部１２−１〜１２−３は、混合加重計算部１１が混合加重ｍijを計算すると、推定値記憶部４−１〜４−３から前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐk-1|k-1（ｊ）の読み出しを行う。
【００３５】
混合計算部１２−１〜１２−３は、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと混合加重ｍijを式（５）に代入して、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを混合し、混合後の状態ベクトル推定値ｘmk-1|k-1（ｊ）ハットをカルマンフィルタ処理部３−１〜３−３に出力する（ステップＳＴ２）。
また、混合計算部１２−１〜１２−３は、前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）と、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと、混合後の状態ベクトル推定値ｘmk-1|k-1（ｊ）ハットと、混合加重ｍijとを式（６）に代入して、前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合し、混合後の推定誤差共分散行列Ｐmk-1|k-1（ｊ）をカルマンフィルタ処理部３−１〜３−３に出力する（ステップＳＴ３）。
【００３６】
カルマンフィルタ処理部３−１は、混合処理部２により混合されたモデル（１）の状態ベクトル推定値ｘmk-1|k-1（１）ハット及び推定誤差共分散行列Ｐmk-1|k-1（１）と、観測装置１から出力された観測ベクトルｙkとを入力して、モデル（１）のカルマンフィルタ処理を実施し、モデル（１）の状態ベクトル推定値の更新値ｘk|k（１）ハット及び推定誤差共分散行列の更新値Ｐk|k（１）を推定値記憶部４−１及び統合処理部１０に出力する（ステップＳＴ４）。
また、カルマンフィルタ処理部３−１は、モデル（１）の状態ベクトル予測値ｘk|k-1（１）ハット及び予測誤差共分散行列Ｐk|k-1（１）と、観測ベクトルｙkとをモデル尤度計算部５に出力する。
【００３７】
カルマンフィルタ処理部３−２は、カルマンフィルタ処理部３−１と並列にモデル（２）のカルマンフィルタ処理を実施する。
即ち、カルマンフィルタ処理部３−２は、混合処理部２により混合されたモデル（２）の状態ベクトル推定値ｘmk-1|k-1（２）ハット及び推定誤差共分散行列Ｐmk-1|k-1（２）と、観測装置１から出力された観測ベクトルｙkとを入力して、モデル（２）のカルマンフィルタ処理を実施し、モデル（２）の状態ベクトル推定値の更新値ｘk|k（２）ハット及び推定誤差共分散行列の更新値Ｐk|k（２）を推定値記憶部４−２及び統合処理部１０に出力する（ステップＳＴ４）。
また、カルマンフィルタ処理部３−２は、モデル（２）の状態ベクトル予測値ｘk|k-1（２）ハット及び予測誤差共分散行列Ｐk|k-1（２）と、観測ベクトルｙkとをモデル尤度計算部５に出力する。
【００３８】
カルマンフィルタ処理部３−３は、カルマンフィルタ処理部３−１，３−２と並列にモデル（３）のカルマンフィルタ処理を実施する。
即ち、カルマンフィルタ処理部３−３は、混合処理部２により混合されたモデル（３）の状態ベクトル推定値ｘmk-1|k-1（３）ハット及び推定誤差共分散行列Ｐmk-1|k-1（３）と、観測装置１から出力された観測ベクトルｙkとを入力して、モデル（３）のカルマンフィルタ処理を実施し、モデル（３）の状態ベクトル推定値の更新値ｘk|k（３）ハット及び推定誤差共分散行列の更新値Ｐk|k（３）を推定値記憶部４−３及び統合処理部１０に出力する（ステップＳＴ４）。
また、カルマンフィルタ処理部３−３は、モデル（３）の状態ベクトル予測値ｘk|k-1（３）ハット及び予測誤差共分散行列Ｐk|k-1（３）と、観測ベクトルｙkとをモデル尤度計算部５に出力する。
【００３９】
モデル尤度計算部５は、カルマンフィルタ処理部３−１〜３−３から状態ベクトル予測値ｘk|k-1（ｊ）ハット、予測誤差共分散行列Ｐk|k-1（ｊ）及び観測ベクトルｙkを受けると、その状態ベクトル予測値ｘk|k-1（ｊ）ハットと観測ベクトルｙkを式（９）に代入して、各モデルの残差ベクトルｙk（ｊ）チルダを算出する。
また、モデル尤度計算部５は、その予測誤差共分散行列Ｐk|k-1（ｊ）を式（１０）に代入して、各モデルの残差共分散行列Ｄk（ｊ）を算出する。
モデル尤度計算部５は、各モデルの残差ベクトルｙk（ｊ）チルダと残差共分散行列Ｄk（ｊ）を算出すると、各モデルの残差ベクトルｙk（ｊ）チルダと残差共分散行列Ｄk（ｊ）を式（１１）に代入して、各モデルのモデル尤度Λk（ｊ）を計算する（ステップＳＴ５）。
【００４０】
モデル確率計算部６は、モデル尤度計算部５が各モデルのモデル尤度Λk（ｊ）を計算すると、そのモデル尤度Λk（ｊ）を式（１２）に代入して、各モデルのモデル確率μk（ｊ）を計算し、そのモデル確率μk（ｊ）をモデル確率記憶部７に格納するとともに、そのモデル確率μk（ｊ）を統合処理部１０に出力する（ステップＳＴ６）。
【００４１】
モデル構成判定部８の処理内容は、下記の実施の形態３，４で詳述するが、モデル構成判定部８は、モデル尤度計算部５により計算されたモデル尤度Λk（ｊ）と統合処理部１０より統合された状態ベクトル推定値ｘk|kハットから、次時刻ｔk+1で使用するモデル（必要なモデル）の集合Ｓ’（Ｓ’⊂Ｓ＝｛１，２，・・・，ｓ｝）を決定し、そのモデルの集合Ｓ’をモデル構成判定結果としてモデル構成記憶部９に格納する（ステップＳＴ７）。
【００４２】
統合処理部１０は、カルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）のうち、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すると判別されたモデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）を、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた統合加重で統合する。
【００４３】
即ち、統合処理部１０の統合加重調整部２１は、モデル構成記憶部９に記憶されている前時刻ｔk-1のモデル構成判定結果Ｓ’を参照して、現時刻ｔkで使用するモデルを認識する。
統合加重調整部２１は、下記の式（１６）に示すように、現時刻ｔkで使用しないモデルの統合加重を０とし、現時刻ｔkで使用するモデルの統合加重の総和が１になるように、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた各モデルの統合加重μ’（ｊ）を計算する（ステップＳＴ８）。
【数７】
【００４４】
統合処理部１０の統合計算部２２は、統合加重調整部２１が各モデルの統合加重μ’（ｊ）を計算すると、その統合加重μ’（ｊ）とカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハットを下記の式（１７）に代入して、各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハットを統合し、統合後の状態ベクトル推定値ｘk|kを出力する（ステップＳＴ９）。
また、統合計算部２２は、各モデルの統合加重μ’（ｊ）と、各モデルの推定誤差共分散行列の更新値Ｐk|k（ｊ）と、各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハットと、統合後の状態ベクトル推定値ｘk|kとを下記の式（１８）に代入して、各モデルの推定誤差共分散行列の更新値Ｐk|k（ｊ）を統合し、統合後の推定誤差共分散行列Ｐk|kを出力する（ステップＳＴ１０）。
【数８】
【００４５】
なお、この実施の形態１では、現時刻ｔkで使用しないモデルのカルマンフィルタの処理結果を統合処理部１０での統合処理に使用しないようにしているが、使用しないモデルのカルマンフィルタの処理自体は継続して実行している。
その理由は、上記カルマンフィルタの出力より得られるモデル尤度を継続的に監視するなどして、使用再開の判断を行えるよう配慮したものである。
また、一旦処理を停止してしまうと、処理を再開する際、初期値から動作を開始する必要があり、処理結果が収束するまでの間に、フィルタ全体の推定精度に影響を与えることがあるからである。
【００４６】
以上で明らかなように、この実施の形態１によれば、モデル確率計算部６により計算されたモデル尤度Λk（ｊ）と統合処理部１０より統合された状態ベクトル推定値ｘk|kハットから、次時刻ｔk+1で使用するモデルを判別するモデル構成判定部８を設け、統合処理部１０がカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）のうち、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すると判別されたモデルの状態ベクトル推定値の更新値ｘk|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）を、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた統合加重で統合するように構成したので、不要なモデルのカルマンフィルタの処理結果がフィルタ全体の推定精度に与える悪影響を抑制することができる効果を奏する。
【００４７】
実施の形態２．
図５はこの発明の実施の形態２による状態推定装置を示す構成図であり、図において、図１と同一符号は同一または相当部分を示すので説明を省略する。
混合処理部３０は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハット及び推定誤差共分散行列Ｐk-1|k-1（ｊ）のうち、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すると判別されたモデルの状態ベクトル推定値ｘk-1|k-1（ｊ）ハット及び推定誤差共分散行列Ｐk-1|k-1（ｊ）を、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた混合加重で混合する処理を実施する。
なお、混合処理部３０は混合処理手段を構成している。
【００４８】
図６はこの発明の実施の形態２による状態推定装置の混合処理部３０を示す構成図であり、図において、混合加重計算部３１はモデル確率記憶部７に記憶されている前時刻ｔk-1のモデル確率μk-1（ｊ）から、式（７）〜式（８）によって、混合加重ｍijを計算する処理を実施する。
混合加重調整部３２はモデル構成記憶部９に記憶されている前時刻ｔk-1のモデル構成判定結果Ｓ’を参照して、現時刻ｔkで使用するモデルを認識し、現時刻ｔkで使用するモデルの混合加重の総和が１になるように（使用しないモデルの統合加重は０とする）、混合加重計算部３１により計算された混合加重ｍijを調整する処理を実施する。
混合計算部３３−１は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを調整後の混合加重ｍ’ijで混合して、モデル（１）の状態ベクトル推定値ｘmk-1|k-1（１）ハットをカルマンフィルタ処理部３−１に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を調整後の混合加重ｍ’ijで混合して、モデル（１）の推定誤差共分散行列Ｐmk-1|k-1（１）をカルマンフィルタ処理部３−１に出力する。
【００４９】
混合計算部３３−２は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを調整後の混合加重ｍ’ijで混合して、モデル（２）の状態ベクトル推定値ｘmk-1|k-1（２）ハットをカルマンフィルタ処理部３−２に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を調整後の混合加重ｍ’ijで混合して、モデル（２）の推定誤差共分散行列Ｐmk-1|k-1（２）をカルマンフィルタ処理部３−２に出力する。
混合計算部３３−３は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを調整後の混合加重ｍ’ijで混合して、モデル（３）の状態ベクトル推定値ｘmk-1|k-1（３）ハットをカルマンフィルタ処理部３−３に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を調整後の混合加重ｍ’ijで混合して、モデル（３）の推定誤差共分散行列Ｐmk-1|k-1（３）をカルマンフィルタ処理部３−３に出力する。
【００５０】
次に動作について説明する。
混合処理部３０以外は、上記実施の形態１と同様であるため、混合処理部３０の処理内容のみを説明する。
混合処理部３０の混合加重計算部３１は、モデル確率記憶部７に記憶されている前時刻ｔk-1のモデル確率μk-1（ｊ）を式（７）〜式（８）に代入して、混合加重ｍijを計算する。
【００５１】
混合処理部３０の混合加重調整部３２は、混合加重計算部３１が混合加重ｍijを計算すると、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すべきでないと判別されたモデルの前時刻ｔk-1におけるカルマンフィルタの処理結果が、現時刻ｔkにおけるカルマンフィルタの混合計算に入力されるのを阻止するため、混合加重計算部３１により計算された混合加重ｍijを調整する。
【００５２】
即ち、混合加重調整部３２は、モデル構成記憶部９に記憶されている前時刻ｔk-1のモデル構成判定結果Ｓ’を参照して、現時刻ｔkで使用するモデルを認識する。
混合加重調整部３２は、下記の式（１９）に示すように、現時刻ｔkで使用しないモデルの混合加重を０とし、現時刻ｔkで使用するモデルの混合加重の総和が１になるように、混合加重計算部３１により計算された混合加重ｍijを調整する。
混合加重調整部３２は、調整後の混合加重ｍ’ijを混合計算部３３−１〜３３−３に出力する。ただし、ｉ＝１，２，・・・，ｓである。
【数９】
【００５３】
混合計算部３３−１〜３３−３は、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと、混合加重調整部３２により調整された混合加重ｍ’ijを下記の式（２０）に代入して、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットを混合し、混合後の状態ベクトル推定値ｘmk-1|k-1（ｊ）ハットをカルマンフィルタ処理部３−１〜３−３に出力する。
また、混合計算部３３−１〜３３−３は、前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）と、前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハットと、混合後の状態ベクトル推定値ｘmk-1|k-1（ｊ）ハットと、調整後の混合加重ｍ’ijとを下記の式（２１）に代入して、前時刻ｔk-1の推定誤差共分散行列Ｐmk-1|k-1（ｊ）を混合し、混合後の推定誤差共分散行列Ｐmk-1|k-1（ｊ）をカルマンフィルタ処理部３−１〜３−３に出力する。
【数１０】
【００５４】
なお、この実施の形態２では、現時刻ｔkで使用しないモデルの前時刻ｔk-1におけるカルマンフィルタの処理結果を混合処理部３０での混合処理に使用しないようにしているが、使用しないモデルのカルマンフィルタの処理自体は継続して実行している。
その理由は、上記カルマンフィルタの出力より得られるモデル尤度を継続的に監視するなどして、使用再開の判断を行えるよう配慮したものである。
また、一旦処理を停止してしまうと、処理を再開する際、初期値から動作を開始する必要があり、処理結果が収束するまでの間に、フィルタ全体の推定精度に影響を与えることがあるからである。
【００５５】
以上で明らかなように、この実施の形態２によれば、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘk-1|k-1（ｊ）ハット及び推定誤差共分散行列Ｐk-1|k-1（ｊ）のうち、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すると判別されたモデルの状態ベクトル推定値ｘk-1|k-1（ｊ）ハット及び推定誤差共分散行列Ｐk-1|k-1（ｊ）を、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた混合加重で混合するように構成したので、上記実施の形態１よりも更に、不要なモデルのカルマンフィルタの処理結果がフィルタ全体の推定精度に与える悪影響を抑制することができる効果を奏する。
【００５６】
実施の形態３．
図７はこの発明の実施の形態３による状態推定装置のモデル構成判定部８を示す構成図であり、図において、状態量によるモデル構成判定部４１は統合処理部１０より統合された状態ベクトル推定値ｘk|kハットに含まれている状態変数の値を調べて、使用するモデルを判別する処理を実施する。
モデル尤度によるモデル構成判定部４２はモデル尤度計算部５により計算されたモデル尤度Λk（ｊ）に基づいて使用するモデルを判別する処理を実施する。
モデル構成決定部４３はモデル構成判定部４１，４２の判別結果を統合して、最終的に使用するモデルを判別する処理を実施する。
【００５７】
次に動作について説明する。
状態量によるモデル構成判定部４１は、統合処理部１０から統合後の状態ベクトル推定値ｘk|kハットを入力し、その状態ベクトル推定値ｘk|kハットに含まれている状態変数の値を調べて、使用するモデルを判別する。
モデル構成判定部４１の判別処理は、特に状態変数の値に応じて、どのようなモデルのカルマンフィルタが必要であるかを事前知識として有している場合に有効である。
【００５８】
図８は状態ベクトルｘkが２つの状態変数ｘ1k，ｘ2kで構成されているとき、２つの状態変数ｘ1k，ｘ2kの値に応じて、使用するモデルを判別している例を示す説明図である。
図８では、以下に示すようなモデルの選択ルールを表している。ただし、ａ，ｂは閾値である。
ｘ1k＜ａ ＡＮＤ ｘ2k＜ｂ → モデル（１）（２）（３）を使用
ｘ1k＜ａ ＡＮＤ ｘ2k≧ｂ → モデル（１）（２）を使用
ｘ1k≧ａ ＡＮＤ ｘ2k＜ｂ → モデル（２）（３）を使用
ｘ1k≧ａ ＡＮＤ ｘ2k≧ｂ → モデル（２）を使用
なお、上記の選択ルールは、一例であり、ＡＮＤ条件の代わりにＯＲ条件を使用したり、あるいは、例えば、状態変数ｘ1k，ｘ2kの和や積をとるなど、状態変数ｘ1k，ｘ2kに対して、何らかの演算を施したりして、その結果の範囲に対する条件などでルールを作成してもよい。
【００５９】
モデル尤度によるモデル構成判定部４２は、モデル尤度計算部５から各モデルのモデル尤度Λk（ｊ）を入力し、そのモデル尤度Λk（ｊ）が所定の閾値より低い場合、当該モデルを不要なモデルと判別し、そのモデル尤度Λk（ｊ）が所定の閾値より高い場合、当該モデルを必要なモデルと判別する。
ただし、モデル尤度Λk（ｊ）は、観測雑音の影響を受けて変動するため、時間平滑化を行ってから使用するものとする。即ち、時間平滑化後のモデル尤度Λk（ｊ）バーを所定の閾値と比較して、必要なモデルか否かを判別する。
【００６０】
この時間平滑化の処理としては、例えば、下記の式（２２）のように、過去Ｎ時刻分のモデル尤度Λk（ｊ）の移動平均を求めたり、あるいは、式（２３）のように、一次の低域通過フィルタを通すことによって実現することができる。ただし、これらの平滑化処理は、当然ながら、モデル番号毎に独立に実行する。式（２３）の右辺のαは、０＜α＜１を満たすように選択された平滑化ゲインである。
【数１１】
【００６１】
モデル構成決定部４３は、モデル構成判定部４１，４２の判別結果を入力し、モデル番号毎に、そのモデルのカルマンフィルタを次時刻において使用するか否かの最終判定を行う。
判定基準としては、状態量によるモデル構成判定部４１と、モデル尤度によるモデル構成判定部４２の両方で使用すると判別されたモデルを、最終的に使用するモデルとするＡＮＤ条件や、いずれか一方で使用すると判別されたモデルを、最終的に使用するモデルとするＯＲ条件を用いることが考えられる。
また、状態量によるモデル構成判定部４１の判別結果と、モデル尤度によるモデル構成判定部４２の判別結果に優先度を設定し、優先度の高い方の判別結果が使用するモデルとされていれば、そのモデルを使用するようにしてもよい。
さらに、上記優先度が時刻や運用条件によって変動せず、予め固定的に設定できる場合は、当初から状態量によるモデル構成判定部４１、モデル尤度によるモデル構成判定部４２のいずれか必要な方のみを動作させ、モデル構成決定部４３の処理を省略することにより装置を簡素化できることは言うまでもない。
【００６２】
なお、この判定基準は、各モデルに共通でもよいし、また、各モデル毎に独立に設定してもよい。
さらに、あるモデルについての判別結果が、時刻毎に頻繁に入れ替わるのを防ぐ目的で、連続して何回か同じ判別結果が得られた場合に限り、「使用／不使用」の判別結果を反転させることができるとする条件を加味することも考えられる。
【００６３】
以上で明らかなように、この実施の形態３によれば、モデル尤度計算部５により計算されたモデル尤度Λk（ｊ）と統合処理部１０より統合された状態ベクトル推定値ｘk|kハットから、使用するモデルを判別するように構成したので、的確なモデルを使用することができるようになり、その結果、状態推定精度を高めることができる効果を奏する。
【００６４】
実施の形態４．
図９はこの発明の実施の形態４による状態推定装置のモデル構成判定部８を示す構成図であり、図において、図７と同一符号は同一または相当部分を示すので説明を省略する。
モデル確率調整部４４はモデル構成決定部４３により最終的に使用すると判別されたモデルのモデル尤度Λk（ｊ）（または、時間平滑化後のモデル尤度Λk（ｊ）バー）を正規化し、正規化後のモデル尤度Λk（ｊ）にしたがってモデル確率記憶部７に記憶されているモデル確率μk（ｊ）を変更する処理を実施する。
【００６５】
次に動作について説明する。
モデル確率調整部４４は、モデル構成決定部４３により決定された現時刻のモデル構成判定結果が前時刻のモデル構成判定結果と異なり、あるモデルの使用が中止されたり、逆にあるモデルの使用が新たに開始された場合に、モデル確率記憶部７に記憶されているモデル確率μk（ｊ）を変更するものである。
ここで、式（１２）と式（８）によって逐次的に計算されるモデル確率μk（ｊ）は、過去から現時刻に至るまでのモデル尤度Λk（ｊ）の情報を含んでいる。このため、モデル構成が変更された場合には、過去の情報を極力排除し、最近のモデル尤度の情報のみでモデル確率を再設定することが、フィルタ全体の収束性を高めるために有効である。
ただし、各時刻のモデル尤度は、観測雑音の影響を受けて変動する可能性があるため、式（２２）や式（２３）によって時間平滑化されたモデル尤度Λk（ｊ）バーを用いることが望ましい。
【００６６】
そこで、モデル確率調整部４４では、モデル尤度によるモデル構成判定部４２から時間平滑化されたモデル尤度Λk（ｊ）バーを入力し、下記の式（２４）に示すように、モデル構成決定部４３により最終的に使用すると決定されたモデル群に対して、モデル尤度Λk（ｊ）バーを正規化することよってモデル確率μk（ｊ）を計算し、そのモデル確率μk（ｊ）をモデル確率記憶部７に格納する。
【数１２】
【００６７】
以上で明らかなように、この実施の形態４によれば、モデル構成決定部４３により最終的に使用すると判別されたモデルのモデル尤度Λk（ｊ）を正規化し、正規化後のモデル尤度Λk（ｊ）にしたがってモデル確率記憶部７に記憶されているモデル確率μk（ｊ）を変更するように構成したので、モデル構成の変更が生じた場合にも、フィルタ出力に過渡応答が生じて収束が遅れるのを抑制することができるようになり、この結果、上記実施の形態１，２よりも、更に、推定精度を高めることができる効果を奏する。
【図面の簡単な説明】
【００６８】
【図１】この発明の実施の形態１による状態推定装置を示す構成図である。
【図２】この発明の実施の形態１による状態推定装置の混合処理部２を示す構成図である。
【図３】この発明の実施の形態１による状態推定装置の統合処理部１０を示す構成図である。
【図４】この発明の実施の形態１による状態推定方法を示すフローチャートである。
【図５】この発明の実施の形態２による状態推定装置を示す構成図である。
【図６】この発明の実施の形態２による状態推定装置の混合処理部３０を示す構成図である。
【図７】この発明の実施の形態３による状態推定装置のモデル構成判定部８を示す構成図である。
【図８】状態ベクトルｘkが２つの状態変数ｘ1k，ｘ2kで構成されているとき、２つの状態変数ｘ1k，ｘ2kの値に応じて、使用するモデルを判別している例を示す説明図である。
【図９】この発明の実施の形態４による状態推定装置のモデル構成判定部８を示す構成図である。
【符号の説明】
【００６９】
１ 観測装置、２ 混合処理部（混合処理手段）、３−１〜３−３ カルマンフィルタ処理部（カルマンフィルタ処理手段）、４−１〜４−３ 推定値記憶部、５ モデル尤度計算部（モデル確率算出手段）、６ モデル確率計算部（モデル確率算出手段）、７ モデル確率記憶部、８ モデル構成判定部（モデル判別手段）、９ モデル構成記憶部、１０ 統合処理部（統合処理手段）、１１ 混合加重計算部、１２−１〜１２−３ 混合計算部、２１ 統合加重調整部、２２ 統合計算部、３０ 混合処理部（混合処理手段）、３１ 混合加重計算部、３２ 混合加重調整部、３３−１〜３３−３ 混合計算部、４１ 状態量によるモデル構成判定部、４２ モデル尤度によるモデル構成判定部、４３ モデル構成決定部、４４ モデル確率調整部。
【特許請求の範囲】
【請求項１】
予め用意されているモデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値を入力して当該モデルのカルマンフィルタ処理を実施して、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列から各モデルのモデル尤度を算出し、上記モデル尤度から各モデルのモデル確率を算出するモデル確率算出手段と、上記モデル確率算出手段により算出されたモデル尤度から必要なモデルを判別するモデル判別手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列のうち、上記モデル判別手段により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列を上記モデル確率算出手段により算出されたモデル確率に応じた統合加重で統合する統合処理手段とを備えた状態推定装置。
【請求項２】
予め用意されているモデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値を入力して当該モデルのカルマンフィルタ処理を実施して、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列から各モデルのモデル尤度を算出し、上記モデル尤度から各モデルのモデル確率を算出するモデル確率算出手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出手段により算出されたモデル確率に応じた統合加重で統合する統合処理手段とを備えた状態推定装置において、上記統合処理手段により統合された状態推定値から必要なモデルを判別するモデル判別手段を設け、上記統合処理手段が上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を統合する際、上記モデル判別手段により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを統合することを特徴とする状態推定装置。
【請求項３】
予め用意されているモデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値を入力して当該モデルのカルマンフィルタ処理を実施して、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列から各モデルのモデル尤度を算出し、上記モデル尤度から各モデルのモデル確率を算出するモデル確率算出手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出手段により算出されたモデル確率に応じた統合加重で統合する統合処理手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出手段により算出されたモデル確率に応じた混合加重で混合し、混合後の状態推定値及び推定誤差共分散行列を上記複数のカルマンフィルタ処理手段に出力する混合処理手段とを備えた状態推定装置において、上記モデル確率算出手段により算出されたモデル尤度から必要なモデルを判別するモデル判別手段を設け、上記混合処理手段が上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を混合する際、上記モデル判別手段により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを混合することを特徴とする状態推定装置。
【請求項４】
予め用意されているモデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値を入力して当該モデルのカルマンフィルタ処理を実施して、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列から各モデルのモデル尤度を算出し、上記モデル尤度から各モデルのモデル確率を算出するモデル確率算出手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出手段により算出されたモデル確率に応じた統合加重で統合する統合処理手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出手段により算出されたモデル確率に応じた混合加重で混合し、混合後の状態推定値及び推定誤差共分散行列を上記複数のカルマンフィルタ処理手段に出力する混合処理手段とを備えた状態推定装置において、上記統合処理手段により統合された状態推定値から必要なモデルを判別するモデル判別手段を設け、上記混合処理手段が上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を混合する際、上記モデル判別手段により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを混合することを特徴とする状態推定装置。
【請求項５】
モデル判別手段は、モデル確率算出手段により算出されたモデル尤度と統合処理手段により統合された状態推定値から必要なモデルを判別することを特徴とする請求項１から請求項４のうちのいずれか１項記載の状態推定装置。
【請求項６】
モデル判別手段は、必要なモデルのモデル尤度を正規化し、正規化後のモデル尤度にしたがってモデル確率算出手段により算出されたモデル確率を変更することを特徴とする請求項１、請求項３または請求項５記載の状態推定装置。
【請求項７】
予め用意されているモデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値を入力して当該モデルのカルマンフィルタ処理を実施して、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理ステップと、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列から各モデルのモデル尤度を算出し、上記モデル尤度から各モデルのモデル確率を算出するモデル確率算出ステップと、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出ステップで算出されたモデル確率に応じた統合加重で統合する統合処理ステップとを備えた状態推定方法において、上記モデル確率算出ステップで算出されたモデル尤度と上記統合処理ステップで統合された状態推定値から必要なモデルを判別するモデル判別ステップを設け、上記統合処理ステップが上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を統合する際、上記モデル判別ステップで必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを統合することを特徴とする状態推定方法。
【請求項８】
予め用意されているモデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値を入力して当該モデルのカルマンフィルタ処理を実施して、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理ステップと、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列から各モデルのモデル尤度を算出し、上記モデル尤度から各モデルのモデル確率を算出するモデル確率算出ステップと、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出ステップで算出されたモデル確率に応じた統合加重で統合する統合処理ステップと、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出ステップで算出されたモデル確率に応じた混合加重で混合し、混合後の状態推定値及び推定誤差共分散行列を上記複数のカルマンフィルタ処理ステップに出力する混合処理ステップとを備えた状態推定方法において、上記モデル確率算出ステップで算出されたモデル尤度と上記統合処理ステップで統合された状態推定値から必要なモデルを判別するモデル判別ステップを設け、上記混合処理ステップが上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を混合する際、上記モデル判別ステップで必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを混合することを特徴とする状態推定方法。
【請求項１】
予め用意されているモデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値を入力して当該モデルのカルマンフィルタ処理を実施して、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列から各モデルのモデル尤度を算出し、上記モデル尤度から各モデルのモデル確率を算出するモデル確率算出手段と、上記モデル確率算出手段により算出されたモデル尤度から必要なモデルを判別するモデル判別手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列のうち、上記モデル判別手段により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列を上記モデル確率算出手段により算出されたモデル確率に応じた統合加重で統合する統合処理手段とを備えた状態推定装置。
【請求項２】
予め用意されているモデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値を入力して当該モデルのカルマンフィルタ処理を実施して、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列から各モデルのモデル尤度を算出し、上記モデル尤度から各モデルのモデル確率を算出するモデル確率算出手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出手段により算出されたモデル確率に応じた統合加重で統合する統合処理手段とを備えた状態推定装置において、上記統合処理手段により統合された状態推定値から必要なモデルを判別するモデル判別手段を設け、上記統合処理手段が上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を統合する際、上記モデル判別手段により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを統合することを特徴とする状態推定装置。
【請求項３】
予め用意されているモデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値を入力して当該モデルのカルマンフィルタ処理を実施して、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列から各モデルのモデル尤度を算出し、上記モデル尤度から各モデルのモデル確率を算出するモデル確率算出手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出手段により算出されたモデル確率に応じた統合加重で統合する統合処理手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出手段により算出されたモデル確率に応じた混合加重で混合し、混合後の状態推定値及び推定誤差共分散行列を上記複数のカルマンフィルタ処理手段に出力する混合処理手段とを備えた状態推定装置において、上記モデル確率算出手段により算出されたモデル尤度から必要なモデルを判別するモデル判別手段を設け、上記混合処理手段が上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を混合する際、上記モデル判別手段により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを混合することを特徴とする状態推定装置。
【請求項４】
予め用意されているモデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値を入力して当該モデルのカルマンフィルタ処理を実施して、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列から各モデルのモデル尤度を算出し、上記モデル尤度から各モデルのモデル確率を算出するモデル確率算出手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出手段により算出されたモデル確率に応じた統合加重で統合する統合処理手段と、上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出手段により算出されたモデル確率に応じた混合加重で混合し、混合後の状態推定値及び推定誤差共分散行列を上記複数のカルマンフィルタ処理手段に出力する混合処理手段とを備えた状態推定装置において、上記統合処理手段により統合された状態推定値から必要なモデルを判別するモデル判別手段を設け、上記混合処理手段が上記複数のカルマンフィルタ処理手段により更新された状態推定値及び推定誤差共分散行列を混合する際、上記モデル判別手段により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを混合することを特徴とする状態推定装置。
【請求項５】
モデル判別手段は、モデル確率算出手段により算出されたモデル尤度と統合処理手段により統合された状態推定値から必要なモデルを判別することを特徴とする請求項１から請求項４のうちのいずれか１項記載の状態推定装置。
【請求項６】
モデル判別手段は、必要なモデルのモデル尤度を正規化し、正規化後のモデル尤度にしたがってモデル確率算出手段により算出されたモデル確率を変更することを特徴とする請求項１、請求項３または請求項５記載の状態推定装置。
【請求項７】
予め用意されているモデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値を入力して当該モデルのカルマンフィルタ処理を実施して、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理ステップと、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列から各モデルのモデル尤度を算出し、上記モデル尤度から各モデルのモデル確率を算出するモデル確率算出ステップと、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出ステップで算出されたモデル確率に応じた統合加重で統合する統合処理ステップとを備えた状態推定方法において、上記モデル確率算出ステップで算出されたモデル尤度と上記統合処理ステップで統合された状態推定値から必要なモデルを判別するモデル判別ステップを設け、上記統合処理ステップが上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を統合する際、上記モデル判別ステップで必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを統合することを特徴とする状態推定方法。
【請求項８】
予め用意されているモデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値を入力して当該モデルのカルマンフィルタ処理を実施して、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理ステップと、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列から各モデルのモデル尤度を算出し、上記モデル尤度から各モデルのモデル確率を算出するモデル確率算出ステップと、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出ステップで算出されたモデル確率に応じた統合加重で統合する統合処理ステップと、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を上記モデル確率算出ステップで算出されたモデル確率に応じた混合加重で混合し、混合後の状態推定値及び推定誤差共分散行列を上記複数のカルマンフィルタ処理ステップに出力する混合処理ステップとを備えた状態推定方法において、上記モデル確率算出ステップで算出されたモデル尤度と上記統合処理ステップで統合された状態推定値から必要なモデルを判別するモデル判別ステップを設け、上記混合処理ステップが上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を混合する際、上記モデル判別ステップで必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを混合することを特徴とする状態推定方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２００９−３１０９６（Ｐ２００９−３１０９６Ａ）
【公開日】平成２１年２月１２日（２００９．２．１２）
【国際特許分類】
【出願番号】特願２００７−１９４５７０（Ｐ２００７−１９４５７０）
【出願日】平成１９年７月２６日（２００７．７．２６）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２１年２月１２日（２００９．２．１２）
【国際特許分類】
【出願日】平成１９年７月２６日（２００７．７．２６）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]