JP5089281B2 - 状態推定装置及び状態推定方法 - Google Patents
状態推定装置及び状態推定方法 Download PDFInfo
- Publication number
- JP5089281B2 JP5089281B2 JP2007194570A JP2007194570A JP5089281B2 JP 5089281 B2 JP5089281 B2 JP 5089281B2 JP 2007194570 A JP2007194570 A JP 2007194570A JP 2007194570 A JP2007194570 A JP 2007194570A JP 5089281 B2 JP5089281 B2 JP 5089281B2
- Authority
- JP
- Japan
- Prior art keywords
- model
- covariance matrix
- state
- likelihood
- unit
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Filters That Use Time-Delay Elements (AREA)
- Radar Systems Or Details Thereof (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Description
この発明は、観測装置により観測された観測値に基づいて、動特性を有するシステムの状態値を推定する状態推定装置及び状態推定方法に関し、特に、化学、鉄鋼などのプラント、自動車、各種モータなどの制御機器、画像、音声、レーダなどの信号処理機器、医療分野における生体計測機器、衛星や飛翔体などの移動体追跡システム、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）や慣性航法装置を応用した測位システム、通信システム、経済予測システムなどの広範な分野に応用される状態推定装置及び状態推定方法に関するものである。
システムの状態量の動特性を表す運動方程式と、状態量と観測量の関係を表す観測方程式（以下、上記の運動方程式と観測方程式を併せて、「状態方程式」と称する）とに基づいて、観測量の時系列データからシステムの状態量を推定する最適フィルタ方式として、カルマンフィルタがよく知られている。
上記の状態方程式は、システムの動特性と観測系の性質を数学的に表現した数式モデルに相当する。
ただし、カルマンフィルタは、この数式モデルが時間的に変動せず一定であるか、もしくは、その変動が確定的で事前に既知であることを前提としている。
上記の状態方程式は、システムの動特性と観測系の性質を数学的に表現した数式モデルに相当する。
ただし、カルマンフィルタは、この数式モデルが時間的に変動せず一定であるか、もしくは、その変動が確定的で事前に既知であることを前提としている。
実際の応用場面では、モデルが時間とともに変動し、また、その変動の程度やタイミングが未知である場合も多い。
したがって、このような場面では使用することができないカルマンフィルタの弱点を克服するフィルタ方式として、事前に想定される複数のモデルに対応するカルマンフィルタを並列に動作させる多重モデル方式のフィルタが各種提案されている。
その中でも、推定精度と演算負荷のバランスから最も有効とされるのが、ＩＭＭ（Ｉｎｔｅｒａｃｔｉｎｇ Ｍｕｌｔｉｐｌｅ Ｍｏｄｅｌ）フィルタである（例えば、非特許文献１を参照）。
したがって、このような場面では使用することができないカルマンフィルタの弱点を克服するフィルタ方式として、事前に想定される複数のモデルに対応するカルマンフィルタを並列に動作させる多重モデル方式のフィルタが各種提案されている。
その中でも、推定精度と演算負荷のバランスから最も有効とされるのが、ＩＭＭ（Ｉｎｔｅｒａｃｔｉｎｇ Ｍｕｌｔｉｐｌｅ Ｍｏｄｅｌ）フィルタである（例えば、非特許文献１を参照）。
ＩＭＭフィルタでは、例えば、以下に示すような状態方程式を使用する。アンダーラインを付している変数はベクトルであることを示している。
式（１）はシステムの運動方程式であり、式（２）は観測方程式である。
また、ｘ 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を予め規定する。ただし、定義により、下記の式（４）が成立する。
そこで、下記の式（３）に示すように、時刻ｔk-1でシステムが離散的状態ｉであったときに時刻ｔkで状態ｊに推移する確率ｐijを予め規定する。ただし、定義により、下記の式（４）が成立する。
ＩＭＭフィルタは、上記の状態方程式の記述に基づいて、ｓ個のカルマンフィルタからなるフィルタバンクを構成し、以下のように処理を実行する。
まず、初期時刻ｔ0に対するｓ個のモデルｊ（ｊ＝１，２，・・・，ｓ）のモデル確率の初期値μ0（ｊ）と、各モデルに対応するカルマンフィルタの状態ベクトル推定値の初期値ｘ 0|0（ｊ）ハットと、推定誤差共分散行列の初期値Ｐ0|0（ｊ）とを設定して、フィルタ処理を開始する。
サンプリング時刻毎の処理ループに入ると、最初に、以下の混合処理を行う。ただし、以下では、時刻ｔkにおける処理を示すものとする。
まず、初期時刻ｔ0に対するｓ個のモデルｊ（ｊ＝１，２，・・・，ｓ）のモデル確率の初期値μ0（ｊ）と、各モデルに対応するカルマンフィルタの状態ベクトル推定値の初期値ｘ 0|0（ｊ）ハットと、推定誤差共分散行列の初期値Ｐ0|0（ｊ）とを設定して、フィルタ処理を開始する。
サンプリング時刻毎の処理ループに入ると、最初に、以下の混合処理を行う。ただし、以下では、時刻ｔkにおける処理を示すものとする。
混合処理では、各モデルのカルマンフィルタが前時刻ｔk-1で算出した状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐk-1|k-1（ｊ）を互いに交換して、次式のような重み付け統合を行う。式（７）のｍijは混合加重である。また、Ｔは行列の転置を表している。
次に、式（５）のｘ m k-1|k-1（ｊ）ハットと、式（６）のＰm k-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-1（ｊ）ハットと、予測誤差共分散行列Ｐk|k-1（ｊ）を使用し、以下の手順にしたがって各モデルのモデル確率を更新する。
まず、以下の式（９）、式（１０）にしたがって、各モデルの残差ベクトルｙ k（ｊ）チルダと、残差共分散行列Ｄk（ｊ）を算出する。
まず、以下の式（９）、式（１０）にしたがって、各モデルの残差ベクトルｙ 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-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（ｊ）に応じた混合加重で混合し、混合後の状態ベクトル推定値ｘ m k-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐm k-1|k-1（ｊ）を複数のカルマンフィルタ処理部に出力する混合処理部
（１）予め用意されているモデルのカルマンフィルタ処理を実施して、当該モデルの状態ベクトル推定値ｘ k|k（ｊ）ハットと推定誤差共分散行列Ｐk|k（ｊ）を更新する複数のカルマンフィルタ処理部
（２）複数のカルマンフィルタ処理部により更新された状態ベクトル推定値ｘ k|k（ｊ）ハットと推定誤差共分散行列Ｐk|k（ｊ）から、式（９）〜式（１１）によって、各モデルのモデル尤度Λk（ｊ）を計算するモデル尤度計算部
（３）モデル尤度計算部により計算されたモデル尤度Λk（ｊ）から、式（１２）によって、各モデルのモデル確率μk（ｊ）を計算するモデル確率計算部
（４）複数のカルマンフィルタ処理部により更新された状態ベクトル推定値ｘ k|k（ｊ）ハットと推定誤差共分散行列Ｐk|k（ｊ）を、式（１４）〜式（１５）によって、モデル確率計算部により計算されたモデル確率μk（ｊ）に応じた統合加重で統合する統合処理部
（５）複数のカルマンフィルタ処理部により更新された状態ベクトル推定値ｘ k|k（ｊ）ハットと推定誤差共分散行列Ｐk|k（ｊ）を、式（５）〜式（８）によって、モデル確率計算部により計算されたモデル確率μk（ｊ）に応じた混合加重で混合し、混合後の状態ベクトル推定値ｘ m k-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐm k-1|k-1（ｊ）を複数のカルマンフィルタ処理部に出力する混合処理部
上記のＩＭＭフィルタを搭載している従来の状態推定装置は、複数のモデルに対応するカルマンフィルタを並列動作させることによって、システムの動特性や観測系の性質が変動する場合でも、状態量を推定することができる。
また、事前に設定したモデルの中に、システムや観測系の性質を正確に描写したものが存在しなくても、それに近いモデルのフィルタ出力ほど高いモデル確率で重み付けされて統合処理されることにより、近似的にシステム状態量を計算することができる。
また、事前に設定したモデルの中に、システムや観測系の性質を正確に描写したものが存在しなくても、それに近いモデルのフィルタ出力ほど高いモデル確率で重み付けされて統合処理されることにより、近似的にシステム状態量を計算することができる。
ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎ ｏｎ Ａｕｔｏｍａｔｉｃ Ｃｏｎｔｒｏｌ，Ｖｏｌ． ３３，Ｎｏ．８，ｐｐ．７８０−７８３，Ａｕｇｕｓｔ １９８８
従来の状態推定装置は以上のように構成されているので、推定処理の開始から終了に至るまで、予め固定的に設定されたｓ個のモデルのカルマンフィルタの処理結果が統合され、その統合結果がフィルタ全体の推定結果として得られる。しかし、実際には、推定処理に必要となるモデルの数や種類が時間の経過に伴って変動する場合があり、ある時刻において、無用なモデルのカルマンフィルタを使用すると、観測雑音の影響で、そのモデルのモデル確率が零より大きな値となるため、式（１４）、式（１５）の統合処理に影響を与えて、推定精度の劣化を招くことがある。また、式（５）〜式（８）の混合処理において、無用なモデルのカルマンフィルタの処理結果が、他のモデルのカルマンフィルタの入力に影響を与えるため、結果的にフィルタ全体の推定精度の劣化を引き起こすことがある課題があった。
この発明は上記のような課題を解決するためになされたもので、推定処理の途中で、必要なモデルを判別して、不要なモデルのカルマンフィルタの処理結果がフィルタ全体の推定精度に与える悪影響を抑制することができる状態推定装置及び状態推定方法を得ることを目的とする。
この発明に係る状態推定装置は、複数のモデル尤度計算部により計算されたモデルのモデル尤度を所定の閾値と比較して、そのモデル尤度が閾値より高ければ、そのモデルは必要なモデルであると判別し、そのモデル尤度が閾値より低ければ、そのモデルは不要なモデルであると判別するモデル構成判定部を設け、統合処理部が、複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列のうち、モデル構成判定部により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列を複数のモデル確率計算部により計算されたモデル確率に応じた統合加重で統合するようにしたものである。
この発明によれば、複数のモデル尤度計算部により計算されたモデルのモデル尤度を所定の閾値と比較して、そのモデル尤度が閾値より高ければ、そのモデルは必要なモデルであると判別し、そのモデル尤度が閾値より低ければ、そのモデルは不要なモデルであると判別するモデル構成判定部を設け、統合処理部が、複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列のうち、モデル構成判定部により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列を複数のモデル確率計算部により計算されたモデル確率に応じた統合加重で統合するように構成したので、不要なモデルのカルマンフィルタの処理結果がフィルタ全体の推定精度に与える悪影響を抑制することができる効果がある。
実施の形態１．
図１はこの発明の実施の形態１による状態推定装置を示す構成図である。図１では、モデル数がｓ＝３の場合の例を示している。言うまでもないが、他のモデル数の場合にも同様に構成することができる。
図において、観測装置１は例えばセンサなどの計測手段であり、時刻ｔkの観測ベクトルｙ kを状態推定装置に出力する。
図１はこの発明の実施の形態１による状態推定装置を示す構成図である。図１では、モデル数がｓ＝３の場合の例を示している。言うまでもないが、他のモデル数の場合にも同様に構成することができる。
図において、観測装置１は例えばセンサなどの計測手段であり、時刻ｔ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（ｊ）に応じた混合加重で混合する処理を実施する。ただし、ｊ＝１，２，・・・，ｓである。
なお、混合処理部２は混合処理手段を構成している。
なお、混合処理部２は混合処理手段を構成している。
カルマンフィルタ処理部３−１は混合処理部２により混合されたモデル（１）の状態ベクトル推定値ｘ m k-1|k-1（１）ハット及び推定誤差共分散行列Ｐm k-1|k-1（１）と、観測装置１から出力された観測ベクトルｙ kとを入力して、モデル（１）のカルマンフィルタ処理を実施し、モデル（１）の状態ベクトル推定値の更新値ｘ k|k（１）ハット及び推定誤差共分散行列の更新値Ｐk|k（１）を推定値記憶部４−１及び統合処理部１０に出力する。
カルマンフィルタ処理部３−２は混合処理部２により混合されたモデル（２）の状態ベクトル推定値ｘ m k-1|k-1（２）ハット及び推定誤差共分散行列Ｐm k-1|k-1（２）と、観測装置１から出力された観測ベクトルｙ kとを入力して、モデル（２）のカルマンフィルタ処理を実施し、モデル（２）の状態ベクトル推定値の更新値ｘ k|k（２）ハット及び推定誤差共分散行列の更新値Ｐk|k（２）を推定値記憶部４−２及び統合処理部１０に出力する。
カルマンフィルタ処理部３−２は混合処理部２により混合されたモデル（２）の状態ベクトル推定値ｘ m k-1|k-1（２）ハット及び推定誤差共分散行列Ｐm k-1|k-1（２）と、観測装置１から出力された観測ベクトルｙ kとを入力して、モデル（２）のカルマンフィルタ処理を実施し、モデル（２）の状態ベクトル推定値の更新値ｘ k|k（２）ハット及び推定誤差共分散行列の更新値Ｐk|k（２）を推定値記憶部４−２及び統合処理部１０に出力する。
カルマンフィルタ処理部３−３は混合処理部２により混合されたモデル（３）の状態ベクトル推定値ｘ m k-1|k-1（３）ハット及び推定誤差共分散行列Ｐm k-1|k-1（３）と、観測装置１から出力された観測ベクトルｙ kとを入力して、モデル（３）のカルマンフィルタ処理を実施し、モデル（３）の状態ベクトル推定値の更新値ｘ k|k（３）ハット及び推定誤差共分散行列の更新値Ｐk|k（３）を推定値記憶部４−３及び統合処理部１０に出力する。
なお、カルマンフィルタ処理部３−１〜３−３はカルマンフィルタ処理手段を構成している。
推定値記憶部４−１〜４−３はカルマンフィルタ処理部３−１〜３−３から出力された状態ベクトル推定値の更新値ｘ k|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）を記憶するメモリである。
なお、カルマンフィルタ処理部３−１〜３−３はカルマンフィルタ処理手段を構成している。
推定値記憶部４−１〜４−３はカルマンフィルタ処理部３−１〜３−３から出力された状態ベクトル推定値の更新値ｘ k|k（ｊ）ハット及び推定誤差共分散行列の更新値Ｐk|k（ｊ）を記憶するメモリである。
モデル尤度計算部５はカルマンフィルタ処理部３−１〜３−３から状態ベクトル予測値ｘ k|k-1（ｊ）ハット、予測誤差共分散行列Ｐk|k-1（ｊ）及び観測ベクトルｙ 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（ｊ）から、式（７）〜式（８）によって、混合加重ｍijを計算する処理を実施する。
混合計算部１２−１は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットを混合加重ｍijで混合して、モデル（１）の状態ベクトル推定値ｘ m k-1|k-1（１）ハットをカルマンフィルタ処理部３−１に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合加重ｍijで混合して、モデル（１）の推定誤差共分散行列Ｐm k-1|k-1（１）をカルマンフィルタ処理部３−１に出力する。
混合計算部１２−１は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットを混合加重ｍijで混合して、モデル（１）の状態ベクトル推定値ｘ m k-1|k-1（１）ハットをカルマンフィルタ処理部３−１に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合加重ｍijで混合して、モデル（１）の推定誤差共分散行列Ｐm k-1|k-1（１）をカルマンフィルタ処理部３−１に出力する。
混合計算部１２−２は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットを混合加重ｍijで混合して、モデル（２）の状態ベクトル推定値ｘ m k-1|k-1（２）ハットをカルマンフィルタ処理部３−２に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合加重ｍijで混合して、モデル（２）の推定誤差共分散行列Ｐm k-1|k-1（２）をカルマンフィルタ処理部３−２に出力する。
混合計算部１２−３は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットを混合加重ｍijで混合して、モデル（３）の状態ベクトル推定値ｘ m k-1|k-1（３）ハットをカルマンフィルタ処理部３−３に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合加重ｍijで混合して、モデル（３）の推定誤差共分散行列Ｐm k-1|k-1（３）をカルマンフィルタ処理部３−３に出力する。
混合計算部１２−３は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットを混合加重ｍijで混合して、モデル（３）の状態ベクトル推定値ｘ m k-1|k-1（３）ハットをカルマンフィルタ処理部３−３に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合加重ｍijで混合して、モデル（３）の推定誤差共分散行列Ｐm k-1|k-1（３）をカルマンフィルタ処理部３−３に出力する。
図３はこの発明の実施の形態１による状態推定装置の統合処理部１０を示す構成図であり、図において、統合加重調整部２１はモデル構成記憶部９に記憶されている前時刻ｔk-1のモデル構成判定結果Ｓ’を参照して、現時刻ｔkで使用するモデルを認識し、現時刻ｔkで使用するモデルの統合加重の総和が１になるように（使用しないモデルの統合加重は０とする）、モデル確率計算部６により計算されたモデル確率μk（ｊ）に応じた各モデルの統合加重μ’（ｊ）を計算する処理を実施する。
統合計算部２２はカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘ k|k（ｊ）ハットを統合加重μ’（ｊ）で統合して、統合後の状態ベクトル推定値ｘ k|kを出力するとともに、カルマンフィルタ処理部３−１〜３−３から出力された各モデルの推定誤差共分散行列の更新値Ｐk|k（ｊ）を統合加重μ’（ｊ）で統合して、統合後の推定誤差共分散行列Ｐk|kを出力する。
図４はこの発明の実施の形態１による状態推定方法を示すフローチャートである。
統合計算部２２はカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘ k|k（ｊ）ハットを統合加重μ’（ｊ）で統合して、統合後の状態ベクトル推定値ｘ k|kを出力するとともに、カルマンフィルタ処理部３−１〜３−３から出力された各モデルの推定誤差共分散行列の更新値Ｐk|k（ｊ）を統合加重μ’（ｊ）で統合して、統合後の推定誤差共分散行列Ｐk|kを出力する。
図４はこの発明の実施の形態１による状態推定方法を示すフローチャートである。
次に動作について説明する。
最初に、モデル確率記憶部７にモデル確率の初期値μ0（ｊ）（ｊ＝１，２，・・・，ｓ）が設定され、また、推定値記憶部４−１〜４−３に各モデルの状態ベクトル推定値の初期値ｘ 0|0（ｊ）ハットと、推定誤差共分散行列の初期値Ｐ0|0（ｊ）が設定される。
その後、観測装置１から時刻ｔ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（ｊ）の読み出しを行う。
混合処理部２の混合計算部１２−１〜１２−３は、混合加重計算部１１が混合加重ｍijを計算すると、推定値記憶部４−１〜４−３から前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐk-1|k-1（ｊ）の読み出しを行う。
混合計算部１２−１〜１２−３は、前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットと混合加重ｍijを式（５）に代入して、前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットを混合し、混合後の状態ベクトル推定値ｘ m k-1|k-1（ｊ）ハットをカルマンフィルタ処理部３−１〜３−３に出力する（ステップＳＴ２）。
また、混合計算部１２−１〜１２−３は、前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）と、前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットと、混合後の状態ベクトル推定値ｘ m k-1|k-1（ｊ）ハットと、混合加重ｍijとを式（６）に代入して、前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合し、混合後の推定誤差共分散行列Ｐm k-1|k-1（ｊ）をカルマンフィルタ処理部３−１〜３−３に出力する（ステップＳＴ３）。
また、混合計算部１２−１〜１２−３は、前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）と、前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットと、混合後の状態ベクトル推定値ｘ m k-1|k-1（ｊ）ハットと、混合加重ｍijとを式（６）に代入して、前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を混合し、混合後の推定誤差共分散行列Ｐm k-1|k-1（ｊ）をカルマンフィルタ処理部３−１〜３−３に出力する（ステップＳＴ３）。
カルマンフィルタ処理部３−１は、混合処理部２により混合されたモデル（１）の状態ベクトル推定値ｘ m k-1|k-1（１）ハット及び推定誤差共分散行列Ｐm k-1|k-1（１）と、観測装置１から出力された観測ベクトルｙ kとを入力して、モデル（１）のカルマンフィルタ処理を実施し、モデル（１）の状態ベクトル推定値の更新値ｘ k|k（１）ハット及び推定誤差共分散行列の更新値Ｐk|k（１）を推定値記憶部４−１及び統合処理部１０に出力する（ステップＳＴ４）。
また、カルマンフィルタ処理部３−１は、モデル（１）の状態ベクトル予測値ｘ k|k-1（１）ハット及び予測誤差共分散行列Ｐk|k-1（１）と、観測ベクトルｙ kとをモデル尤度計算部５に出力する。
また、カルマンフィルタ処理部３−１は、モデル（１）の状態ベクトル予測値ｘ k|k-1（１）ハット及び予測誤差共分散行列Ｐk|k-1（１）と、観測ベクトルｙ kとをモデル尤度計算部５に出力する。
カルマンフィルタ処理部３−２は、カルマンフィルタ処理部３−１と並列にモデル（２）のカルマンフィルタ処理を実施する。
即ち、カルマンフィルタ処理部３−２は、混合処理部２により混合されたモデル（２）の状態ベクトル推定値ｘ m k-1|k-1（２）ハット及び推定誤差共分散行列Ｐm k-1|k-1（２）と、観測装置１から出力された観測ベクトルｙ kとを入力して、モデル（２）のカルマンフィルタ処理を実施し、モデル（２）の状態ベクトル推定値の更新値ｘ k|k（２）ハット及び推定誤差共分散行列の更新値Ｐk|k（２）を推定値記憶部４−２及び統合処理部１０に出力する（ステップＳＴ４）。
また、カルマンフィルタ処理部３−２は、モデル（２）の状態ベクトル予測値ｘ k|k-1（２）ハット及び予測誤差共分散行列Ｐk|k-1（２）と、観測ベクトルｙ kとをモデル尤度計算部５に出力する。
即ち、カルマンフィルタ処理部３−２は、混合処理部２により混合されたモデル（２）の状態ベクトル推定値ｘ m k-1|k-1（２）ハット及び推定誤差共分散行列Ｐm k-1|k-1（２）と、観測装置１から出力された観測ベクトルｙ kとを入力して、モデル（２）のカルマンフィルタ処理を実施し、モデル（２）の状態ベクトル推定値の更新値ｘ k|k（２）ハット及び推定誤差共分散行列の更新値Ｐk|k（２）を推定値記憶部４−２及び統合処理部１０に出力する（ステップＳＴ４）。
また、カルマンフィルタ処理部３−２は、モデル（２）の状態ベクトル予測値ｘ k|k-1（２）ハット及び予測誤差共分散行列Ｐk|k-1（２）と、観測ベクトルｙ kとをモデル尤度計算部５に出力する。
カルマンフィルタ処理部３−３は、カルマンフィルタ処理部３−１，３−２と並列にモデル（３）のカルマンフィルタ処理を実施する。
即ち、カルマンフィルタ処理部３−３は、混合処理部２により混合されたモデル（３）の状態ベクトル推定値ｘ m k-1|k-1（３）ハット及び推定誤差共分散行列Ｐm k-1|k-1（３）と、観測装置１から出力された観測ベクトルｙ kとを入力して、モデル（３）のカルマンフィルタ処理を実施し、モデル（３）の状態ベクトル推定値の更新値ｘ k|k（３）ハット及び推定誤差共分散行列の更新値Ｐk|k（３）を推定値記憶部４−３及び統合処理部１０に出力する（ステップＳＴ４）。
また、カルマンフィルタ処理部３−３は、モデル（３）の状態ベクトル予測値ｘ k|k-1（３）ハット及び予測誤差共分散行列Ｐk|k-1（３）と、観測ベクトルｙ kとをモデル尤度計算部５に出力する。
即ち、カルマンフィルタ処理部３−３は、混合処理部２により混合されたモデル（３）の状態ベクトル推定値ｘ m k-1|k-1（３）ハット及び推定誤差共分散行列Ｐm k-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-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（ｊ）と、各モデルの状態ベクトル推定値の更新値ｘ 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|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で混合して、モデル（１）の状態ベクトル推定値ｘ m k-1|k-1（１）ハットをカルマンフィルタ処理部３−１に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を調整後の混合加重ｍ’ijで混合して、モデル（１）の推定誤差共分散行列Ｐm k-1|k-1（１）をカルマンフィルタ処理部３−１に出力する。
混合加重調整部３２はモデル構成記憶部９に記憶されている前時刻ｔk-1のモデル構成判定結果Ｓ’を参照して、現時刻ｔkで使用するモデルを認識し、現時刻ｔkで使用するモデルの混合加重の総和が１になるように（使用しないモデルの統合加重は０とする）、混合加重計算部３１により計算された混合加重ｍijを調整する処理を実施する。
混合計算部３３−１は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットを調整後の混合加重ｍ’ijで混合して、モデル（１）の状態ベクトル推定値ｘ m k-1|k-1（１）ハットをカルマンフィルタ処理部３−１に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を調整後の混合加重ｍ’ijで混合して、モデル（１）の推定誤差共分散行列Ｐm k-1|k-1（１）をカルマンフィルタ処理部３−１に出力する。
混合計算部３３−２は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットを調整後の混合加重ｍ’ijで混合して、モデル（２）の状態ベクトル推定値ｘ m k-1|k-1（２）ハットをカルマンフィルタ処理部３−２に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を調整後の混合加重ｍ’ijで混合して、モデル（２）の推定誤差共分散行列Ｐm k-1|k-1（２）をカルマンフィルタ処理部３−２に出力する。
混合計算部３３−３は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットを調整後の混合加重ｍ’ijで混合して、モデル（３）の状態ベクトル推定値ｘ m k-1|k-1（３）ハットをカルマンフィルタ処理部３−３に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を調整後の混合加重ｍ’ijで混合して、モデル（３）の推定誤差共分散行列Ｐm k-1|k-1（３）をカルマンフィルタ処理部３−３に出力する。
混合計算部３３−３は推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットを調整後の混合加重ｍ’ijで混合して、モデル（３）の状態ベクトル推定値ｘ m k-1|k-1（３）ハットをカルマンフィルタ処理部３−３に出力するとともに、推定値記憶部４−１〜４−３に記憶されている前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）を調整後の混合加重ｍ’ijで混合して、モデル（３）の推定誤差共分散行列Ｐm k-1|k-1（３）をカルマンフィルタ処理部３−３に出力する。
次に動作について説明する。
混合処理部３０以外は、上記実施の形態１と同様であるため、混合処理部３０の処理内容のみを説明する。
混合処理部３０の混合加重計算部３１は、モデル確率記憶部７に記憶されている前時刻ｔk-1のモデル確率μk-1（ｊ）を式（７）〜式（８）に代入して、混合加重ｍijを計算する。
混合処理部３０以外は、上記実施の形態１と同様であるため、混合処理部３０の処理内容のみを説明する。
混合処理部３０の混合加重計算部３１は、モデル確率記憶部７に記憶されている前時刻ｔk-1のモデル確率μk-1（ｊ）を式（７）〜式（８）に代入して、混合加重ｍijを計算する。
混合処理部３０の混合加重調整部３２は、混合加重計算部３１が混合加重ｍijを計算すると、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すべきでないと判別されたモデルの前時刻ｔk-1におけるカルマンフィルタの処理結果が、現時刻ｔkにおけるカルマンフィルタの混合計算に入力されるのを阻止するため、混合加重計算部３１により計算された混合加重ｍijを調整する。
即ち、混合加重調整部３２は、モデル構成記憶部９に記憶されている前時刻ｔk-1のモデル構成判定結果Ｓ’を参照して、現時刻ｔkで使用するモデルを認識する。
混合加重調整部３２は、下記の式（１９）に示すように、現時刻ｔkで使用しないモデルの混合加重を０とし、現時刻ｔkで使用するモデルの混合加重の総和が１になるように、混合加重計算部３１により計算された混合加重ｍijを調整する。
混合加重調整部３２は、調整後の混合加重ｍ’ijを混合計算部３３−１〜３３−３に出力する。ただし、ｉ＝１，２，・・・，ｓである。
混合加重調整部３２は、下記の式（１９）に示すように、現時刻ｔkで使用しないモデルの混合加重を０とし、現時刻ｔkで使用するモデルの混合加重の総和が１になるように、混合加重計算部３１により計算された混合加重ｍijを調整する。
混合加重調整部３２は、調整後の混合加重ｍ’ijを混合計算部３３−１〜３３−３に出力する。ただし、ｉ＝１，２，・・・，ｓである。
混合計算部３３−１〜３３−３は、前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットと、混合加重調整部３２により調整された混合加重ｍ’ijを下記の式（２０）に代入して、前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットを混合し、混合後の状態ベクトル推定値ｘ m k-1|k-1（ｊ）ハットをカルマンフィルタ処理部３−１〜３−３に出力する。
また、混合計算部３３−１〜３３−３は、前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）と、前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットと、混合後の状態ベクトル推定値ｘ m k-1|k-1（ｊ）ハットと、調整後の混合加重ｍ’ijとを下記の式（２１）に代入して、前時刻ｔk-1の推定誤差共分散行列Ｐm k-1|k-1（ｊ）を混合し、混合後の推定誤差共分散行列Ｐm k-1|k-1（ｊ）をカルマンフィルタ処理部３−１〜３−３に出力する。
また、混合計算部３３−１〜３３−３は、前時刻ｔk-1の推定誤差共分散行列Ｐk-1|k-1（ｊ）と、前時刻ｔk-1の状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットと、混合後の状態ベクトル推定値ｘ m k-1|k-1（ｊ）ハットと、調整後の混合加重ｍ’ijとを下記の式（２１）に代入して、前時刻ｔk-1の推定誤差共分散行列Ｐm k-1|k-1（ｊ）を混合し、混合後の推定誤差共分散行列Ｐm k-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ハットを入力し、その状態ベクトル推定値ｘ k|kハットに含まれている状態変数の値を調べて、使用するモデルを判別する。
モデル構成判定部４１の判別処理は、特に状態変数の値に応じて、どのようなモデルのカルマンフィルタが必要であるかを事前知識として有している場合に有効である。
状態量によるモデル構成判定部４１は、統合処理部１０から統合後の状態ベクトル推定値ｘ k|kハットを入力し、その状態ベクトル推定値ｘ k|kハットに含まれている状態変数の値を調べて、使用するモデルを判別する。
モデル構成判定部４１の判別処理は、特に状態変数の値に応じて、どのようなモデルのカルマンフィルタが必要であるかを事前知識として有している場合に有効である。
図８は状態ベクトルｘ kが２つの状態変数ｘ1 k，ｘ2 kで構成されているとき、２つの状態変数ｘ1 k，ｘ2 kの値に応じて、使用するモデルを判別している例を示す説明図である。
図８では、以下に示すようなモデルの選択ルールを表している。ただし、ａ，ｂは閾値である。
ｘ1 k＜ａ ＡＮＤ ｘ2 k＜ｂ → モデル（１）（２）（３）を使用
ｘ1 k＜ａ ＡＮＤ ｘ2 k≧ｂ → モデル（１）（２）を使用
ｘ1 k≧ａ ＡＮＤ ｘ2 k＜ｂ → モデル（２）（３）を使用
ｘ1 k≧ａ ＡＮＤ ｘ2 k≧ｂ → モデル（２）を使用
なお、上記の選択ルールは、一例であり、ＡＮＤ条件の代わりにＯＲ条件を使用したり、あるいは、例えば、状態変数ｘ1 k，ｘ2 kの和や積をとるなど、状態変数ｘ1 k，ｘ2 kに対して、何らかの演算を施したりして、その結果の範囲に対する条件などでルールを作成してもよい。
図８では、以下に示すようなモデルの選択ルールを表している。ただし、ａ，ｂは閾値である。
ｘ1 k＜ａ ＡＮＤ ｘ2 k＜ｂ → モデル（１）（２）（３）を使用
ｘ1 k＜ａ ＡＮＤ ｘ2 k≧ｂ → モデル（１）（２）を使用
ｘ1 k≧ａ ＡＮＤ ｘ2 k＜ｂ → モデル（２）（３）を使用
ｘ1 k≧ａ ＡＮＤ ｘ2 k≧ｂ → モデル（２）を使用
なお、上記の選択ルールは、一例であり、ＡＮＤ条件の代わりにＯＲ条件を使用したり、あるいは、例えば、状態変数ｘ1 k，ｘ2 kの和や積をとるなど、状態変数ｘ1 k，ｘ2 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（ｊ）を変更するように構成したので、モデル構成の変更が生じた場合にも、フィルタ出力に過渡応答が生じて収束が遅れるのを抑制することができるようになり、この結果、上記実施の形態１，２よりも、更に、推定精度を高めることができる効果を奏する。
１ 観測装置、２ 混合処理部（混合処理手段）、３−１〜３−３ カルマンフィルタ処理部（カルマンフィルタ処理手段）、４−１〜４−３ 推定値記憶部、５ モデル尤度計算部（モデル確率算出手段）、６ モデル確率計算部（モデル確率算出手段）、７ モデル確率記憶部、８ モデル構成判定部（モデル判別手段）、９ モデル構成記憶部、１０ 統合処理部（統合処理手段）、１１ 混合加重計算部、１２−１〜１２−３ 混合計算部、２１ 統合加重調整部、２２ 統合計算部、３０ 混合処理部（混合処理手段）、３１ 混合加重計算部、３２ 混合加重調整部、３３−１〜３３−３ 混合計算部、４１ 状態量によるモデル構成判定部、４２ モデル尤度によるモデル構成判定部、４３ モデル構成決定部、４４ モデル確率調整部。
Claims (9)
- 予め用意されている複数のモデルのうち、当該モデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値とを、当該モデルに対応するカルマンフィルタに入力して、上記カルマンフィルタの処理を実施することで、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理部と、
上記カルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列から当該モデルのモデル尤度を計算する複数のモデル尤度計算部と、
上記モデル尤度計算部により計算されたモデル尤度から当該モデルのモデル確率を計算する複数のモデル確率計算部と、
上記複数のモデル尤度計算部により計算されたモデルのモデル尤度を所定の閾値と比較して、上記モデル尤度が上記閾値より高ければ、上記モデルは必要なモデルであると判別し、上記モデル尤度が上記閾値より低ければ、上記モデルは不要なモデルであると判別するモデル構成判定部と、
上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列のうち、上記モデル構成判定部により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算部により計算されたモデル確率に応じた統合加重で統合する統合処理部と
を備えた状態推定装置。 - 予め用意されている複数のモデルのうち、当該モデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値とを、当該モデルに対応するカルマンフィルタに入力して、上記カルマンフィルタの処理を実施することで、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理部と、
上記カルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列から当該モデルのモデル尤度を計算する複数のモデル尤度計算部と、
上記モデル尤度計算部により計算されたモデル尤度から当該モデルのモデル確率を計算する複数のモデル確率計算部と、
上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算部により計算されたモデル確率に応じた統合加重で統合する統合処理部と、
上記統合処理部により統合された状態推定値に含まれている状態変数の値に基づいて、予め用意されている複数のモデルの中から、必要なモデルを判別するモデル構成判定部とを備え、
上記統合処理部が上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列を統合する際、上記モデル構成判定部により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを統合することを特徴とする状態推定装置。 - 予め用意されている複数のモデルのうち、当該モデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値とを、当該モデルに対応するカルマンフィルタに入力して、上記カルマンフィルタの処理を実施することで、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理部と、
上記カルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列から当該モデルのモデル尤度を計算する複数のモデル尤度計算部と、
上記モデル尤度計算部により計算されたモデル尤度から当該モデルのモデル確率を計算する複数のモデル確率計算部と、
上記複数のモデル尤度計算部により計算されたモデルのモデル尤度を所定の閾値と比較して、上記モデル尤度が上記閾値より高ければ、上記モデルは必要なモデルであると判別し、上記モデル尤度が上記閾値より低ければ、上記モデルは不要なモデルであると判別するモデル構成判定部と、
上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算部により計算されたモデル確率に応じた統合加重で統合する統合処理部と、
上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列のうち、上記モデル構成判定部により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを上記複数のモデル確率計算部により計算されたモデル確率に応じた混合加重で混合し、混合後の状態推定値及び推定誤差共分散行列を上記複数のカルマンフィルタ処理部に出力する混合処理部と
を備えた状態推定装置。 - 予め用意されている複数のモデルのうち、当該モデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値とを、当該モデルに対応するカルマンフィルタに入力して、上記カルマンフィルタの処理を実施することで、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理部と、
上記カルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列から当該モデルのモデル尤度を計算する複数のモデル尤度計算部と、
上記モデル尤度計算部により計算されたモデル尤度から当該モデルのモデル確率を計算する複数のモデル確率計算部と、
上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算部により計算されたモデル確率に応じた統合加重で統合する統合処理部と、
上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算部により計算されたモデル確率に応じた混合加重で混合し、混合後の状態推定値及び推定誤差共分散行列を上記複数のカルマンフィルタ処理部に出力する混合処理部と、
上記統合処理部により統合された状態推定値に含まれている状態変数の値に基づいて、予め用意されている複数のモデルの中から、必要なモデルを判別するモデル構成判定部とを備え、
上記混合処理部が上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列を混合する際、上記モデル構成判定部により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを混合することを特徴とする状態推定装置。 - モデル構成判定部は、統合処理部により統合された状態推定値に含まれている状態変数の値に基づいて、予め用意されている複数のモデルの中から、必要なモデルを判別する第１の判別部のほかに、
複数のモデル尤度計算部により計算されたモデルのモデル尤度を所定の閾値と比較して、上記モデル尤度が上記閾値より高ければ、上記モデルは必要なモデルであると判別し、上記モデル尤度が上記閾値より低ければ、上記モデルは不要なモデルであると判別する第２の判別部を備えるとともに、
上記第１の判別部の判別結果と上記第２の判別部の判別結果とを統合して、最終的に必要なモデルを判別する第３の判別部を備えている
ことを特徴とする請求項２または請求項４記載の状態推定装置。 - モデル構成判定部は、複数のモデル尤度計算部により計算されたモデルのモデル尤度を所定の閾値と比較して、上記モデル尤度が上記閾値より高ければ、上記モデルは必要なモデルであると判別し、上記モデル尤度が上記閾値より低ければ、上記モデルは不要なモデルであると判別する第２の判別部のほかに、
統合処理部により統合された状態推定値に含まれている状態変数の値に基づいて、予め用意されている複数のモデルの中から、必要なモデルを判別する第１の判別部を備えるとともに、
上記第１の判別部の判別結果と上記第２の判別部の判別結果とを統合して、最終的に必要なモデルを判別する第３の判別部を備えている
ことを特徴とする請求項１または請求項３記載の状態推定装置。 - モデル構成判定部は、必要なモデルのモデル尤度を正規化し、正規化後のモデル尤度にしたがってモデル確率計算部により計算されたモデル確率を変更することを特徴とする請求項１、請求項３、請求項５または請求項６記載の状態推定装置。
- 複数のカルマンフィルタ処理部が、予め用意されている複数のモデルのうち、当該モデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値とを、当該モデルに対応するカルマンフィルタに入力して、上記カルマンフィルタの処理を実施することで、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理ステップと、
複数のモデル尤度計算部が、上記カルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列から当該モデルのモデル尤度を計算する複数のモデル尤度計算ステップと、
複数のモデル確率計算部が、上記モデル尤度計算ステップで計算されたモデル尤度から当該モデルのモデル確率を計算する複数のモデル確率計算ステップと、
統合処理部が、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算ステップで計算されたモデル確率に応じた統合加重で統合する統合処理ステップと、
モデル構成判定部が、上記統合処理ステップで統合された状態推定値に含まれている状態変数の値に基づいて、予め用意されている複数のモデルの中から、必要なモデルを判別する第１の判別処理と、上記複数のモデル尤度計算ステップで計算されたモデルのモデル尤度を所定の閾値と比較して、上記モデル尤度が上記閾値より高ければ、上記モデルは必要なモデルであると判別し、上記モデル尤度が上記閾値より低ければ、上記モデルは不要なモデルであると判別する第２の判別処理と、上記第１の判別処理の判別結果と上記第２の判別処理の判別結果とを統合して、最終的に必要なモデルを判別する第３の判別処理とを実施するモデル構成判定ステップとを備え、
上記統合処理ステップが上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を統合する際、上記モデル構成判定ステップで必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを統合することを特徴とする状態推定方法。 - 複数のカルマンフィルタ処理部が、予め用意されている複数のモデルのうち、当該モデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値とを、当該モデルに対応するカルマンフィルタに入力して、上記カルマンフィルタの処理を実施することで、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理ステップと、
複数のモデル尤度計算部が、上記カルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列から当該モデルのモデル尤度を計算する複数のモデル尤度計算ステップと、
複数のモデル確率計算部が、上記モデル尤度計算ステップで計算されたモデル尤度から当該モデルのモデル確率を計算する複数のモデル確率計算ステップと、
統合処理部が、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算ステップで計算されたモデル確率に応じた統合加重で統合する統合処理ステップと、
混合処理部が、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算ステップで計算されたモデル確率に応じた混合加重で混合し、混合後の状態推定値及び推定誤差共分散行列を上記カルマンフィルタ処理ステップに出力する混合処理ステップと、
モデル構成判定部が、上記統合処理ステップで統合された状態推定値に含まれている状態変数の値に基づいて、予め用意されている複数のモデルの中から、必要なモデルを判別する第１の判別処理と、上記複数のモデル尤度計算ステップで計算されたモデルのモデル尤度を所定の閾値と比較して、上記モデル尤度が上記閾値より高ければ、上記モデルは必要なモデルであると判別し、上記モデル尤度が上記閾値より低ければ、上記モデルは不要なモデルであると判別する第２の判別処理と、上記第１の判別処理の判別結果と上記第２の判別処理の判別結果とを統合して、最終的に必要なモデルを判別する第３の判別処理とを実施するモデル構成判定ステップとを備え、
上記混合処理ステップが上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を混合する際、上記モデル構成判定ステップで必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを混合することを特徴とする状態推定方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2007194570A JP5089281B2 (ja) | 2007-07-26 | 2007-07-26 | 状態推定装置及び状態推定方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2007194570A JP5089281B2 (ja) | 2007-07-26 | 2007-07-26 | 状態推定装置及び状態推定方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2009031096A JP2009031096A (ja) | 2009-02-12 |
| JP5089281B2 true JP5089281B2 (ja) | 2012-12-05 |
Family
ID=40401763
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2007194570A Expired - Fee Related JP5089281B2 (ja) | 2007-07-26 | 2007-07-26 | 状態推定装置及び状態推定方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP5089281B2 (ja) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103312297A (zh) * | 2013-06-13 | 2013-09-18 | 北京航空航天大学 | 一种迭代扩展增量卡尔曼滤波方法 |
| CN109391195A (zh) * | 2017-08-08 | 2019-02-26 | 西门子股份公司 | 系统状态预测 |
Families Citing this family (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5266846B2 (ja) * | 2008-04-01 | 2013-08-21 | セイコーエプソン株式会社 | 測位方法、プログラム及び測位装置 |
| JP5424662B2 (ja) * | 2009-01-30 | 2014-02-26 | 三菱電機株式会社 | 目標追尾装置 |
| JP5725701B2 (ja) * | 2009-10-20 | 2015-05-27 | 三菱電機株式会社 | 追尾装置 |
| JP5618566B2 (ja) * | 2010-02-18 | 2014-11-05 | 三菱電機株式会社 | 追尾装置 |
| CN101873121B (zh) * | 2010-06-09 | 2012-06-27 | 浙江大学 | 一种基于直方图估计粒子滤波算法的非线性动态系统信号处理方法 |
| JP5818608B2 (ja) * | 2011-09-27 | 2015-11-18 | インターナショナル・ビジネス・マシーンズ・コーポレーションＩｎｔｅｒｎａｔｉｏｎａｌ Ｂｕｓｉｎｅｓｓ Ｍａｃｈｉｎｅｓ Ｃｏｒｐｏｒａｔｉｏｎ | カルマン・フィルタの処理方法、プログラム及びシステム |
| JP6541026B2 (ja) * | 2015-05-13 | 2019-07-10 | 株式会社Ihi | 状態データ更新装置と方法 |
| JP6795453B2 (ja) * | 2017-05-22 | 2020-12-02 | 日本電信電話株式会社 | 測定装置及び測定方法 |
| JP6581276B1 (ja) * | 2018-10-18 | 2019-09-25 | 株式会社ショーワ | 状態量推定装置、制御装置、および状態量推定方法 |
| CN109558594B (zh) * | 2018-12-03 | 2023-03-21 | 深圳大学 | 交互式t-s模糊语义模型估计方法、系统和计算机可读存储介质 |
| CN113474796A (zh) | 2019-03-04 | 2021-10-01 | 三菱电机株式会社 | 信息处理装置、信息处理方法以及信息处理程序 |
| JP7105711B2 (ja) * | 2019-03-08 | 2022-07-25 | 三菱電機株式会社 | 追尾処理装置 |
| CN113566821A (zh) * | 2021-06-28 | 2021-10-29 | 江南造船(集团)有限责任公司 | 基于交互式滤波的无人机航向估计方法、系统及电子设备 |
| CN113919223A (zh) * | 2021-10-09 | 2022-01-11 | 福州大学 | 结合贝叶斯网络和卡尔曼滤波的结构系统噪声误差评估方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4348535B2 (ja) * | 2004-03-24 | 2009-10-21 | 三菱電機株式会社 | 目標追尾装置 |
-
2007
- 2007-07-26 JP JP2007194570A patent/JP5089281B2/ja not_active Expired - Fee Related
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103312297A (zh) * | 2013-06-13 | 2013-09-18 | 北京航空航天大学 | 一种迭代扩展增量卡尔曼滤波方法 |
| CN103312297B (zh) * | 2013-06-13 | 2015-12-09 | 北京航空航天大学 | 一种迭代扩展增量卡尔曼滤波方法 |
| CN109391195A (zh) * | 2017-08-08 | 2019-02-26 | 西门子股份公司 | 系统状态预测 |
| CN109391195B (zh) * | 2017-08-08 | 2022-04-26 | 西门子股份公司 | 系统状态预测 |
| US11347996B2 (en) | 2017-08-08 | 2022-05-31 | Siemens Aktiengesellschaft | System state prediction |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2009031096A (ja) | 2009-02-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP5089281B2 (ja) | 状態推定装置及び状態推定方法 | |
| CN107765347B (zh) | 一种高斯过程回归和粒子滤波的短期风速预测方法 | |
| Gao et al. | Adaptive Kalman filtering with recursive noise estimator for integrated SINS/DVL systems | |
| CN109827579B (zh) | 一种组合定位中滤波模型实时校正的方法和系统 | |
| CN112578419B (zh) | 一种基于gru网络和卡尔曼滤波的gps数据重构方法 | |
| EP2685214A2 (en) | Multiple truth reference system and method | |
| CN115615417A (zh) | 自适应滤波系统构造方法、自适应滤波方法及装置 | |
| Saglam et al. | Estimation error correction in deep reinforcement learning for deterministic actor-critic methods | |
| CN111971628A (zh) | 求得被测变量的时间曲线的方法、预测系统、致动器控制系统、训练致动器控制系统的方法、训练系统、计算机程序和机器可读的存储介质 | |
| Nag et al. | Model based fault diagnosis of low earth orbiting (leo) satellite using spherical unscented kalman filter | |
| CN109655057B (zh) | 一种六推无人机加速器测量值的滤波优化方法及其系统 | |
| CN114741659B (zh) | 一种自适应模型在线重构建鲁棒滤波方法、设备及系统 | |
| US9733341B1 (en) | System and method for covariance fidelity assessment | |
| JP3795865B2 (ja) | 人工衛星の姿勢決定装置 | |
| CN114061592B (zh) | 基于多模型的自适应鲁棒auv导航方法 | |
| WO2021181687A1 (ja) | 予測モデル作成装置、予測モデル作成方法及びプログラム | |
| Kim et al. | A single-pass noise covariance estimation algorithm in multiple-model adaptive kalman filtering for non-stationary systems | |
| Zhu et al. | Tuning-free filtering for stochastic systems with unmodeled measurement dynamics | |
| Dahan et al. | Uncertainty Quantification in Deep Learning Based Kalman Filters | |
| US12002549B2 (en) | Knowledge reuse-based method and system for predicting cell concentration in fermentation process | |
| US20240038324A1 (en) | Knowledge reuse-based method and system for predicting cell concentration in fermentation process | |
| Voiculescu et al. | Bayesian parameter estimation with prior weighting in ALT model | |
| CN116881385B (zh) | 轨迹平滑方法、装置、电子设备及可读存储介质 | |
| Tiano | Comparison of non linear identification methods for underwater vehicles | |
| CN112269200B (zh) | 一种基于可观测度的惯性/卫星系统自适应混合校正方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20100408 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20120228 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20120416 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20120814 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20120911 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20150921 Year of fee payment: 3 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| LAPS | Cancellation because of no payment of annual fees |