JP5089281B2 - 状態推定装置及び状態推定方法 - Google Patents

状態推定装置及び状態推定方法 Download PDF

Info

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
Application number
JP2007194570A
Other languages
English (en)
Other versions
JP2009031096A (ja
Inventor
正義 系
洋志 亀田
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Mitsubishi Electric Corp
Original Assignee
Mitsubishi Electric Corp
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Mitsubishi Electric Corp filed Critical Mitsubishi Electric Corp
Priority to JP2007194570A priority Critical patent/JP5089281B2/ja
Publication of JP2009031096A publication Critical patent/JP2009031096A/ja
Application granted granted Critical
Publication of JP5089281B2 publication Critical patent/JP5089281B2/ja
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Radar Systems Or Details Thereof (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)
  • Filters That Use Time-Delay Elements (AREA)

Description

この発明は、観測装置により観測された観測値に基づいて、動特性を有するシステムの状態値を推定する状態推定装置及び状態推定方法に関し、特に、化学、鉄鋼などのプラント、自動車、各種モータなどの制御機器、画像、音声、レーダなどの信号処理機器、医療分野における生体計測機器、衛星や飛翔体などの移動体追跡システム、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）や慣性航法装置を応用した測位システム、通信システム、経済予測システムなどの広範な分野に応用される状態推定装置及び状態推定方法に関するものである。
システムの状態量の動特性を表す運動方程式と、状態量と観測量の関係を表す観測方程式（以下、上記の運動方程式と観測方程式を併せて、「状態方程式」と称する）とに基づいて、観測量の時系列データからシステムの状態量を推定する最適フィルタ方式として、カルマンフィルタがよく知られている。
上記の状態方程式は、システムの動特性と観測系の性質を数学的に表現した数式モデルに相当する。
ただし、カルマンフィルタは、この数式モデルが時間的に変動せず一定であるか、もしくは、その変動が確定的で事前に既知であることを前提としている。
実際の応用場面では、モデルが時間とともに変動し、また、その変動の程度やタイミングが未知である場合も多い。
したがって、このような場面では使用することができないカルマンフィルタの弱点を克服するフィルタ方式として、事前に想定される複数のモデルに対応するカルマンフィルタを並列に動作させる多重モデル方式のフィルタが各種提案されている。
その中でも、推定精度と演算負荷のバランスから最も有効とされるのが、ＩＭＭ（Ｉｎｔｅｒａｃｔｉｎｇ Ｍｕｌｔｉｐｌｅ Ｍｏｄｅｌ）フィルタである（例えば、非特許文献１を参照）。
ＩＭＭフィルタでは、例えば、以下に示すような状態方程式を使用する。アンダーラインを付している変数はベクトルであることを示している。
Figure 0005089281
式（１）はシステムの運動方程式であり、式（２）は観測方程式である。
また、ｘ 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を予め規定する。ただし、定義により、下記の式（４）が成立する。
Figure 0005089281
ＩＭＭフィルタは、上記の状態方程式の記述に基づいて、ｓ個のカルマンフィルタからなるフィルタバンクを構成し、以下のように処理を実行する。
まず、初期時刻ｔ0に対するｓ個のモデルｊ（ｊ＝１，２，・・・，ｓ）のモデル確率の初期値μ0（ｊ）と、各モデルに対応するカルマンフィルタの状態ベクトル推定値の初期値ｘ 0|0（ｊ）ハットと、推定誤差共分散行列の初期値Ｐ0|0（ｊ）とを設定して、フィルタ処理を開始する。
サンプリング時刻毎の処理ループに入ると、最初に、以下の混合処理を行う。ただし、以下では、時刻ｔkにおける処理を示すものとする。
混合処理では、各モデルのカルマンフィルタが前時刻ｔk-1で算出した状態ベクトル推定値ｘ k-1|k-1（ｊ）ハットと推定誤差共分散行列Ｐk-1|k-1（ｊ）を互いに交換して、次式のような重み付け統合を行う。式（７）のｍijは混合加重である。また、Ｔは行列の転置を表している。
Figure 0005089281
次に、式（５）のｘ 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（ｊ）を算出する。
Figure 0005089281
式（１０）の右辺第２項のＲkは観測誤差共分散行列である。
残差ベクトルｙ k（ｊ）チルダは、状態ベクトル予測値から求められた観測ベクトルの予測値と、実際に取得された観測ベクトルとの差を表すベクトルである。
この残差ベクトルｙ k（ｊ）チルダは、平均零ベクトルと残差共分散行列Ｄk（ｊ）の多変量ガウス分布にしたがって実現すると仮定すれば、式（１１）により算出される確率密度Λk（ｊ）はモデルｊの尤度を表すものとなる。
なお、モデルｊの尤度Λk（ｊ）は現時刻ｔkの観測ベクトルｙ kを受けた結果として、各モデルのフィルタがどの程度適合しているかを測る指標値となる。
この尤度Λk（ｊ）を使用する式（１２）にしたがって、現時刻ｔkに対応するモデル確率μk（ｊ）が算出される。
式（１２）と式（８）により逐次的に計算されるモデル確率μk（ｊ）は、前時刻ｔk-1のモデル確率μk-1（ｊ）及びモデル間の推移ｐijと、現時刻ｔkの尤度Λk（ｊ）から推論された「モデルｊが正しい確率」であり、下記の式（１３）が成立する。
Figure 0005089281
最後に、ＩＭＭフィルタは、各モデルのカルマンフィルタの出力である状態ベクトル推定値ｘ k|k（ｊ）ハットと、推定誤差共分散行列Ｐk|k（ｊ）とを上記のモデル確率μk（ｊ）で重み付け統合した値を時刻ｔkの処理結果として外部に出力し、時刻ｔkの処理を終了する。
Figure 0005089281
上記のＩＭＭフィルタを搭載している従来の状態推定装置は、以下のように構成されている。
（１）予め用意されているモデルのカルマンフィルタ処理を実施して、当該モデルの状態ベクトル推定値ｘ 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-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（３）を推定値記憶部４−３及び統合処理部１０に出力する。
なお、カルマンフィルタ処理部３−１〜３−３はカルマンフィルタ処理手段を構成している。
推定値記憶部４−１〜４−３はカルマンフィルタ処理部３−１〜３−３から出力された状態ベクトル推定値の更新値ｘ 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で混合して、モデル（１）の状態ベクトル推定値ｘ 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を出力する。
図４はこの発明の実施の形態１による状態推定方法を示すフローチャートである。
次に動作について説明する。
最初に、モデル確率記憶部７にモデル確率の初期値μ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（ｊ）ハットを混合し、混合後の状態ベクトル推定値ｘ 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とをモデル尤度計算部５に出力する。
カルマンフィルタ処理部３−２は、カルマンフィルタ処理部３−１と並列にモデル（２）のカルマンフィルタ処理を実施する。
即ち、カルマンフィルタ処理部３−２は、混合処理部２により混合されたモデル（２）の状態ベクトル推定値ｘ 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（ｊ）を式（１２）に代入して、各モデルのモデル確率μ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（ｊ）に応じた各モデルの統合加重μ’（ｊ）を計算する（ステップＳＴ８）。
Figure 0005089281
統合処理部１０の統合計算部２２は、統合加重調整部２１が各モデルの統合加重μ’（ｊ）を計算すると、その統合加重μ’（ｊ）とカルマンフィルタ処理部３−１〜３−３から出力された各モデルの状態ベクトル推定値の更新値ｘ k|k（ｊ）ハットを下記の式（１７）に代入して、各モデルの状態ベクトル推定値の更新値ｘ k|k（ｊ）ハットを統合し、統合後の状態ベクトル推定値ｘ k|kを出力する（ステップＳＴ９）。
また、統合計算部２２は、各モデルの統合加重μ’（ｊ）と、各モデルの推定誤差共分散行列の更新値Ｐk|k（ｊ）と、各モデルの状態ベクトル推定値の更新値ｘ k|k（ｊ）ハットと、統合後の状態ベクトル推定値ｘ k|kとを下記の式（１８）に代入して、各モデルの推定誤差共分散行列の更新値Ｐk|k（ｊ）を統合し、統合後の推定誤差共分散行列Ｐk|kを出力する（ステップＳＴ１０）。
Figure 0005089281
なお、この実施の形態１では、現時刻ｔ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で混合して、モデル（１）の状態ベクトル推定値ｘ 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を計算する。
混合処理部３０の混合加重調整部３２は、混合加重計算部３１が混合加重ｍijを計算すると、前時刻ｔk-1のモデル構成判定部８の処理により現時刻ｔkで使用すべきでないと判別されたモデルの前時刻ｔk-1におけるカルマンフィルタの処理結果が、現時刻ｔkにおけるカルマンフィルタの混合計算に入力されるのを阻止するため、混合加重計算部３１により計算された混合加重ｍijを調整する。
即ち、混合加重調整部３２は、モデル構成記憶部９に記憶されている前時刻ｔk-1のモデル構成判定結果Ｓ’を参照して、現時刻ｔkで使用するモデルを認識する。
混合加重調整部３２は、下記の式（１９）に示すように、現時刻ｔkで使用しないモデルの混合加重を０とし、現時刻ｔkで使用するモデルの混合加重の総和が１になるように、混合加重計算部３１により計算された混合加重ｍijを調整する。
混合加重調整部３２は、調整後の混合加重ｍ’ijを混合計算部３３−１〜３３−３に出力する。ただし、ｉ＝１，２，・・・，ｓである。
Figure 0005089281
混合計算部３３−１〜３３−３は、前時刻ｔ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（ｊ）をカルマンフィルタ処理部３−１〜３−３に出力する。
Figure 0005089281
なお、この実施の形態２では、現時刻ｔ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が２つの状態変数ｘ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（ｊ）の移動平均を求めたり、あるいは、式（２３）のように、一次の低域通過フィルタを通すことによって実現することができる。ただし、これらの平滑化処理は、当然ながら、モデル番号毎に独立に実行する。式（２３）の右辺のαは、０＜α＜１を満たすように選択された平滑化ゲインである。
Figure 0005089281
モデル構成決定部４３は、モデル構成判定部４１，４２の判別結果を入力し、モデル番号毎に、そのモデルのカルマンフィルタを次時刻において使用するか否かの最終判定を行う。
判定基準としては、状態量によるモデル構成判定部４１と、モデル尤度によるモデル構成判定部４２の両方で使用すると判別されたモデルを、最終的に使用するモデルとするＡＮＤ条件や、いずれか一方で使用すると判別されたモデルを、最終的に使用するモデルとするＯＲ条件を用いることが考えられる。
また、状態量によるモデル構成判定部４１の判別結果と、モデル尤度によるモデル構成判定部４２の判別結果に優先度を設定し、優先度の高い方の判別結果が使用するモデルとされていれば、そのモデルを使用するようにしてもよい。
さらに、上記優先度が時刻や運用条件によって変動せず、予め固定的に設定できる場合は、当初から状態量によるモデル構成判定部４１、モデル尤度によるモデル構成判定部４２のいずれか必要な方のみを動作させ、モデル構成決定部４３の処理を省略することにより装置を簡素化できることは言うまでもない。
なお、この判定基準は、各モデルに共通でもよいし、また、各モデル毎に独立に設定してもよい。
さらに、あるモデルについての判別結果が、時刻毎に頻繁に入れ替わるのを防ぐ目的で、連続して何回か同じ判別結果が得られた場合に限り、「使用／不使用」の判別結果を反転させることができるとする条件を加味することも考えられる。
以上で明らかなように、この実施の形態３によれば、モデル尤度計算部５により計算されたモデル尤度Λk（ｊ）と統合処理部１０より統合された状態ベクトル推定値ｘ k|kハットから、使用するモデルを判別するように構成したので、的確なモデルを使用することができるようになり、その結果、状態推定精度を高めることができる効果を奏する。
実施の形態４．
図９はこの発明の実施の形態４による状態推定装置のモデル構成判定部８を示す構成図であり、図において、図７と同一符号は同一または相当部分を示すので説明を省略する。
モデル確率調整部４４はモデル構成決定部４３により最終的に使用すると判別されたモデルのモデル尤度Λk（ｊ）（または、時間平滑化後のモデル尤度Λk（ｊ）バー）を正規化し、正規化後のモデル尤度Λk（ｊ）にしたがってモデル確率記憶部７に記憶されているモデル確率μk（ｊ）を変更する処理を実施する。
次に動作について説明する。
モデル確率調整部４４は、モデル構成決定部４３により決定された現時刻のモデル構成判定結果が前時刻のモデル構成判定結果と異なり、あるモデルの使用が中止されたり、逆にあるモデルの使用が新たに開始された場合に、モデル確率記憶部７に記憶されているモデル確率μk（ｊ）を変更するものである。
ここで、式（１２）と式（８）によって逐次的に計算されるモデル確率μk（ｊ）は、過去から現時刻に至るまでのモデル尤度Λk（ｊ）の情報を含んでいる。このため、モデル構成が変更された場合には、過去の情報を極力排除し、最近のモデル尤度の情報のみでモデル確率を再設定することが、フィルタ全体の収束性を高めるために有効である。
ただし、各時刻のモデル尤度は、観測雑音の影響を受けて変動する可能性があるため、式（２２）や式（２３）によって時間平滑化されたモデル尤度Λk（ｊ）バーを用いることが望ましい。
そこで、モデル確率調整部４４では、モデル尤度によるモデル構成判定部４２から時間平滑化されたモデル尤度Λk（ｊ）バーを入力し、下記の式（２４）に示すように、モデル構成決定部４３により最終的に使用すると決定されたモデル群に対して、モデル尤度Λk（ｊ）バーを正規化することよってモデル確率μk（ｊ）を計算し、そのモデル確率μk（ｊ）をモデル確率記憶部７に格納する。
Figure 0005089281
以上で明らかなように、この実施の形態４によれば、モデル構成決定部４３により最終的に使用すると判別されたモデルのモデル尤度Λk（ｊ）を正規化し、正規化後のモデル尤度Λk（ｊ）にしたがってモデル確率記憶部７に記憶されているモデル確率μk（ｊ）を変更するように構成したので、モデル構成の変更が生じた場合にも、フィルタ出力に過渡応答が生じて収束が遅れるのを抑制することができるようになり、この結果、上記実施の形態１，２よりも、更に、推定精度を高めることができる効果を奏する。
この発明の実施の形態１による状態推定装置を示す構成図である。 この発明の実施の形態１による状態推定装置の混合処理部２を示す構成図である。 この発明の実施の形態１による状態推定装置の統合処理部１０を示す構成図である。 この発明の実施の形態１による状態推定方法を示すフローチャートである。 この発明の実施の形態２による状態推定装置を示す構成図である。 この発明の実施の形態２による状態推定装置の混合処理部３０を示す構成図である。 この発明の実施の形態３による状態推定装置のモデル構成判定部８を示す構成図である。 状態ベクトルｘ kが２つの状態変数ｘ1 k，ｘ2 kで構成されているとき、２つの状態変数ｘ1 k，ｘ2 kの値に応じて、使用するモデルを判別している例を示す説明図である。 この発明の実施の形態４による状態推定装置のモデル構成判定部８を示す構成図である。
符号の説明
１ 観測装置、２ 混合処理部（混合処理手段）、３−１〜３−３ カルマンフィルタ処理部（カルマンフィルタ処理手段）、４−１〜４−３ 推定値記憶部、５ モデル尤度計算部（モデル確率算出手段）、６ モデル確率計算部（モデル確率算出手段）、７ モデル確率記憶部、８ モデル構成判定部（モデル判別手段）、９ モデル構成記憶部、１０ 統合処理部（統合処理手段）、１１ 混合加重計算部、１２−１〜１２−３ 混合計算部、２１ 統合加重調整部、２２ 統合計算部、３０ 混合処理部（混合処理手段）、３１ 混合加重計算部、３２ 混合加重調整部、３３−１〜３３−３ 混合計算部、４１ 状態量によるモデル構成判定部、４２ モデル尤度によるモデル構成判定部、４３ モデル構成決定部、４４ モデル確率調整部。

Claims (9)

  1. 予め用意されている複数のモデルのうち、当該モデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値とを、当該モデルに対応するカルマンフィルタに入力して、上記カルマンフィルタの処理を実施することで、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理部と、
    上記カルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列から当該モデルのモデル尤度を計算する複数のモデル尤度計算部と、
    上記モデル尤度計算部により計算されたモデル尤度から当該モデルのモデル確率を計算する複数のモデル確率計算部と、
    上記複数のモデル尤度計算部により計算されたモデルのモデル尤度を所定の閾値と比較して、上記モデル尤度が上記閾値より高ければ、上記モデルは必要なモデルであると判別し、上記モデル尤度が上記閾値より低ければ、上記モデルは不要なモデルであると判別するモデル構成判定部と、
    上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列のうち、上記モデル構成判定部により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算部により計算されたモデル確率に応じた統合加重で統合する統合処理部と
    を備えた状態推定装置。
  2. 予め用意されている複数のモデルのうち、当該モデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値とを、当該モデルに対応するカルマンフィルタに入力して、上記カルマンフィルタの処理を実施することで、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理部と、
    上記カルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列から当該モデルのモデル尤度を計算する複数のモデル尤度計算部と、
    上記モデル尤度計算部により計算されたモデル尤度から当該モデルのモデル確率を計算する複数のモデル確率計算部と、
    上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算部により計算されたモデル確率に応じた統合加重で統合する統合処理部と、
    上記統合処理部により統合された状態推定値に含まれている状態変数の値に基づいて、予め用意されている複数のモデルの中から、必要なモデルを判別するモデル構成判定部とを備え、
    上記統合処理部が上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列を統合する際、上記モデル構成判定部により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを統合することを特徴とする状態推定装置。
  3. 予め用意されている複数のモデルのうち、当該モデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値とを、当該モデルに対応するカルマンフィルタに入力して、上記カルマンフィルタの処理を実施することで、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理部と、
    上記カルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列から当該モデルのモデル尤度を計算する複数のモデル尤度計算部と、
    上記モデル尤度計算部により計算されたモデル尤度から当該モデルのモデル確率を計算する複数のモデル確率計算部と、
    上記複数のモデル尤度計算部により計算されたモデルのモデル尤度を所定の閾値と比較して、上記モデル尤度が上記閾値より高ければ、上記モデルは必要なモデルであると判別し、上記モデル尤度が上記閾値より低ければ、上記モデルは不要なモデルであると判別するモデル構成判定部と、
    上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算部により計算されたモデル確率に応じた統合加重で統合する統合処理部と、
    上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列のうち、上記モデル構成判定部により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを上記複数のモデル確率計算部により計算されたモデル確率に応じた混合加重で混合し、混合後の状態推定値及び推定誤差共分散行列を上記複数のカルマンフィルタ処理部に出力する混合処理部と
    を備えた状態推定装置。
  4. 予め用意されている複数のモデルのうち、当該モデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値とを、当該モデルに対応するカルマンフィルタに入力して、上記カルマンフィルタの処理を実施することで、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理部と、
    上記カルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列から当該モデルのモデル尤度を計算する複数のモデル尤度計算部と、
    上記モデル尤度計算部により計算されたモデル尤度から当該モデルのモデル確率を計算する複数のモデル確率計算部と、
    上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算部により計算されたモデル確率に応じた統合加重で統合する統合処理部と、
    上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算部により計算されたモデル確率に応じた混合加重で混合し、混合後の状態推定値及び推定誤差共分散行列を上記複数のカルマンフィルタ処理部に出力する混合処理部と、
    上記統合処理部により統合された状態推定値に含まれている状態変数の値に基づいて、予め用意されている複数のモデルの中から、必要なモデルを判別するモデル構成判定部とを備え、
    上記混合処理部が上記複数のカルマンフィルタ処理部により更新された状態推定値及び推定誤差共分散行列を混合する際、上記モデル構成判定部により必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを混合することを特徴とする状態推定装置。
  5. モデル構成判定部は、統合処理部により統合された状態推定値に含まれている状態変数の値に基づいて、予め用意されている複数のモデルの中から、必要なモデルを判別する第１の判別部のほかに、
    複数のモデル尤度計算部により計算されたモデルのモデル尤度を所定の閾値と比較して、上記モデル尤度が上記閾値より高ければ、上記モデルは必要なモデルであると判別し、上記モデル尤度が上記閾値より低ければ、上記モデルは不要なモデルであると判別する第２の判別部を備えるとともに、
    上記第１の判別部の判別結果と上記第２の判別部の判別結果とを統合して、最終的に必要なモデルを判別する第３の判別部を備えている
    ことを特徴とする請求項２または請求項４記載の状態推定装置。
  6. モデル構成判定部は、複数のモデル尤度計算部により計算されたモデルのモデル尤度を所定の閾値と比較して、上記モデル尤度が上記閾値より高ければ、上記モデルは必要なモデルであると判別し、上記モデル尤度が上記閾値より低ければ、上記モデルは不要なモデルであると判別する第２の判別部のほかに、
    統合処理部により統合された状態推定値に含まれている状態変数の値に基づいて、予め用意されている複数のモデルの中から、必要なモデルを判別する第１の判別部を備えるとともに、
    上記第１の判別部の判別結果と上記第２の判別部の判別結果とを統合して、最終的に必要なモデルを判別する第３の判別部を備えている
    ことを特徴とする請求項１または請求項３記載の状態推定装置。
  7. モデル構成判定部は、必要なモデルのモデル尤度を正規化し、正規化後のモデル尤度にしたがってモデル確率計算部により計算されたモデル確率を変更することを特徴とする請求項１、請求項３、請求項５または請求項６記載の状態推定装置。
  8. 複数のカルマンフィルタ処理部が、予め用意されている複数のモデルのうち、当該モデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値とを、当該モデルに対応するカルマンフィルタに入力して、上記カルマンフィルタの処理を実施することで、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理ステップと、
    複数のモデル尤度計算部が、上記カルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列から当該モデルのモデル尤度を計算する複数のモデル尤度計算ステップと、
    複数のモデル確率計算部が、上記モデル尤度計算ステップで計算されたモデル尤度から当該モデルのモデル確率を計算する複数のモデル確率計算ステップと、
    統合処理部が、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算ステップで計算されたモデル確率に応じた統合加重で統合する統合処理ステップと、
    モデル構成判定部が、上記統合処理ステップで統合された状態推定値に含まれている状態変数の値に基づいて、予め用意されている複数のモデルの中から、必要なモデルを判別する第１の判別処理と、上記複数のモデル尤度計算ステップで計算されたモデルのモデル尤度を所定の閾値と比較して、上記モデル尤度が上記閾値より高ければ、上記モデルは必要なモデルであると判別し、上記モデル尤度が上記閾値より低ければ、上記モデルは不要なモデルであると判別する第２の判別処理と、上記第１の判別処理の判別結果と上記第２の判別処理の判別結果とを統合して、最終的に必要なモデルを判別する第３の判別処理とを実施するモデル構成判定ステップとを備え、
    上記統合処理ステップが上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を統合する際、上記モデル構成判定ステップで必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを統合することを特徴とする状態推定方法。
  9. 複数のカルマンフィルタ処理部が、予め用意されている複数のモデルのうち、当該モデルの状態推定値及び推定誤差共分散行列と観測装置により観測された観測値とを、当該モデルに対応するカルマンフィルタに入力して、上記カルマンフィルタの処理を実施することで、当該モデルの状態推定値及び推定誤差共分散行列を更新する複数のカルマンフィルタ処理ステップと、
    複数のモデル尤度計算部が、上記カルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列から当該モデルのモデル尤度を計算する複数のモデル尤度計算ステップと、
    複数のモデル確率計算部が、上記モデル尤度計算ステップで計算されたモデル尤度から当該モデルのモデル確率を計算する複数のモデル確率計算ステップと、
    統合処理部が、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算ステップで計算されたモデル確率に応じた統合加重で統合する統合処理ステップと、
    混合処理部が、上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を上記複数のモデル確率計算ステップで計算されたモデル確率に応じた混合加重で混合し、混合後の状態推定値及び推定誤差共分散行列を上記カルマンフィルタ処理ステップに出力する混合処理ステップと、
    モデル構成判定部が、上記統合処理ステップで統合された状態推定値に含まれている状態変数の値に基づいて、予め用意されている複数のモデルの中から、必要なモデルを判別する第１の判別処理と、上記複数のモデル尤度計算ステップで計算されたモデルのモデル尤度を所定の閾値と比較して、上記モデル尤度が上記閾値より高ければ、上記モデルは必要なモデルであると判別し、上記モデル尤度が上記閾値より低ければ、上記モデルは不要なモデルであると判別する第２の判別処理と、上記第１の判別処理の判別結果と上記第２の判別処理の判別結果とを統合して、最終的に必要なモデルを判別する第３の判別処理とを実施するモデル構成判定ステップとを備え、
    上記混合処理ステップが上記複数のカルマンフィルタ処理ステップで更新された状態推定値及び推定誤差共分散行列を混合する際、上記モデル構成判定ステップで必要であると判別されたモデルの状態推定値及び推定誤差共分散行列だけを混合することを特徴とする状態推定方法。
JP2007194570A 2007-07-26 2007-07-26 状態推定装置及び状態推定方法 Expired - Fee Related JP5089281B2 (ja)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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模糊语义模型估计方法、系统和计算机可读存储介质
WO2020178938A1 (ja) * 2019-03-04 2020-09-10 三菱電機株式会社 情報処理装置、情報処理方法及び情報処理プログラム
JP7105711B2 (ja) * 2019-03-08 2022-07-25 三菱電機株式会社 追尾処理装置
CN113566821A (zh) * 2021-06-28 2021-10-29 江南造船(集团)有限责任公司 基于交互式滤波的无人机航向估计方法、系统及电子设备
CN113919223B (zh) * 2021-10-09 2024-09-06 福州大学 结合贝叶斯网络和卡尔曼滤波的结构系统噪声误差评估方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4348535B2 (ja) * 2004-03-24 2009-10-21 三菱電機株式会社 目標追尾装置

Cited By (5)

* Cited by examiner, † Cited by third party
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) 状態推定装置及び状態推定方法
Gao et al. Adaptive Kalman filtering with recursive noise estimator for integrated SINS/DVL systems
CN109827579B (zh) 一种组合定位中滤波模型实时校正的方法和系统
Oussalah et al. Adaptive Kalman filter for noise identification
CN110375731A (zh) 一种混合交互式多模型滤波方法
CN103776449B (zh) 一种提高鲁棒性的动基座初始对准方法
CN111913175A (zh) 一种传感器短暂失效下带补偿机制的水面目标跟踪方法
US12002549B2 (en) Knowledge reuse-based method and system for predicting cell concentration in fermentation process
CN112578419B (zh) 一种基于gru网络和卡尔曼滤波的gps数据重构方法
CN107292410A (zh) 隧道形变预测方法和装置
CN111971628A (zh) 求得被测变量的时间曲线的方法、预测系统、致动器控制系统、训练致动器控制系统的方法、训练系统、计算机程序和机器可读的存储介质
CN115615417A (zh) 自适应滤波系统构造方法、自适应滤波方法及装置
CN116881385B (zh) 轨迹平滑方法、装置、电子设备及可读存储介质
US20200001886A1 (en) Methods for monitoring the output performance of state estimators in navigation systems
Nag et al. Model based fault diagnosis of low earth orbiting (leo) satellite using spherical unscented kalman filter
Dahan et al. Uncertainty quantification in deep learning based kalman filters
CN116834977A (zh) 一种卫星轨道数据的范围控制方法
US9733341B1 (en) System and method for covariance fidelity assessment
Yang et al. Variational Bayesian and generalized maximum-likelihood based adaptive robust nonlinear filtering framework
JP3795865B2 (ja) 人工衛星の姿勢決定装置
Zhu et al. Tuning-free filtering for stochastic systems with unmodeled measurement dynamics
CN114741659A (zh) 一种自适应模型在线重构建鲁棒滤波方法、设备及系统
WO2021181687A1 (ja) 予測モデル作成装置、予測モデル作成方法及びプログラム
CN110514209B (zh) 一种交互式多模型组合导航方法
Kim et al. A single-pass noise covariance estimation algorithm in multiple-model adaptive kalman filtering for non-stationary systems

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 Request for written amendment filed

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