(57)【特許請求の範囲】
【請求項１】
対象システムの出力と、前記対象システムをモデル化した非線形モデルとを入力として、拡張カルマンフィルタによって前記対象システムの状態を推定する状態推定装置において、
前記非線形モデルは非線形連続時間状態方程式を含み、
前記非線形モデルの観測方程式に基づき、ある時刻の前記対象システムの状態を推定した状態推定値を求め、前記状態推定値の誤差共分散行列の推定値を求める状態及び誤差推定部と、
前記時刻の前記状態推定値に基づいて、前記非線形連続時間状態方程式を二次以上の積分手法で離散化して離散時間状態方程式を求める状態方程式離散化部と、
前記時刻の前記離散時間状態方程式を用いて、差分法によって前記時刻のヤコビアンの近似値を求める状態方程式線形化部と、
前記ヤコビアンの前記近似値から前記時刻の微小時間後の前記誤差共分散行列を予測し、前記離散時間状態方程式から前記時刻の微小時間後の前記状態を予測する状態及び誤差予測部とを備える状態推定装置。
【請求項２】
差分法に用いる微小スカラー値を入力とし、
前記状態方程式線形化部が、前記離散時間状態方程式から前記微小スカラー値に基づいて、差分法により前記ヤコビアンの前記近似値を求めることを特徴とする請求項１に記載の状態推定装置。
【請求項３】
差分法に用いる微小値ベクトルを入力とし、
前記状態方程式線形化部が、前記離散時間状態方程式から、各状態に応じた状態方程式の偏微分の近似値を、対応する前記微小値ベクトルの各成分に基づいて、差分法により求めることを特徴とする請求項１に記載の状態推定装置。
【請求項４】
前記非線形モデルが非線形の観測方程式を含む場合には、前記非線形の観測方程式のヤコビアンを解析的に求める観測方程式線形化部を前記状態及び誤差推定部の前段に備えることを特徴とする請求項１から３のいずれか一項に記載の状態推定装置。
【請求項５】
前記対象システムの各時刻の入力と前記状態推定値とに基づいて前記非線形連続時間状態方程式を二次以上の積分手法で離散化するモデル離散化部を備えることを特徴とする請求項１から４のいずれか一項に記載の状態推定装置。
【請求項６】
前記対象システムをモデル化した連続時間モデルが未知パラメータを含み、
前記未知パラメータと元の状態とを合わせたベクトルを新しい状態とする拡大系非線形連続時間モデルに変換するモデル変換部を備え、
入力となる前記非線形モデルを前記モデル変換部で変換された前記拡大系非線形連続時間モデルとすることを特徴とする請求項１から５のいずれか一項に記載の状態推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、対象システムの状態又はパラメータを推定する状態推定装置に関する。
【背景技術】
【０００２】
従来の非線形システムの状態推定装置として、例えば、非特許文献１には拡張カルマンフィルタ（ＥＫＦ：Extended  Kalman  Filter）に基づくものが開示されている。拡張カルマンフィルタは、対象システムの出力と、対象システムをモデル化した非線形離散時間システムとに基づいて、システムの内部状態を推定する。また、システムに未知パラメータを含む場合には、未知パラメータを状態として含む拡大系に変換し、拡張カルマンフィルタを適用することで、状態及びパラメータを推定する。このような拡張カルマンフィルタは、一次テイラー近似によりシステムを線形化することで、非線形システムにカルマンフィルタを適用するものであり、状態方程式又は観測方程式のヤコビアンを要する。また、対象システムを非線形離散時間システムとしているため、状態方程式が連続時間で与えられる場合には、予め離散化することを要する。【０００３】
例えば、特許文献１には、対象システムが非線形連続時間システムで与えられる場合に、オイラー法により予め離散化することで拡張カルマンフィルタを適用し、状態及び未知パラメータを推定する技術が開示されている。【０００４】
このように、拡張カルマンフィルタでは、対象システムを非線形離散時間システムとし、状態方程式が連続時間で与えられる場合には、離散化することを要する。拡張カルマンフィルタによる状態推定では、その過程で、状態方程式のヤコビアンを求めることを要する。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００５−２４８９４６号公報
【非特許文献】
【０００６】
【非特許文献１】谷萩隆嗣、「カルマンフィルタと適応信号処理」、コロナ社、２００５年１２月、ｐ．４８−５１
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、上記の従来技術では、連続時間状態方程式の離散化に二次以上の離散化手法を用いた場合に、ヤコビアンを求めることが困難となる。従って、解析的にヤコビアンの導出が可能なオイラー法により離散化することを要するが、オイラー法は一次の近似精度であるため、対象が連続時間状態方程式で与えられる場合には、離散化誤差が大きく、精度の低い推定しかできない、という問題があった。【０００８】
本発明は、上記に鑑みてなされたものであって、対象システムの状態方程式が非線形連続時間関数であっても、拡張カルマンフィルタにより高精度に状態を推定可能な状態推定装置を得ることを目的とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決し、目的を達成するために、本発明は、対象システムの出力と、前記対象システムをモデル化した非線形モデルとを入力として、拡張カルマンフィルタによって前記対象システムの状態を推定する状態推定装置において、前記非線形モデルは非線形連続時間状態方程式を含み、前記非線形モデルの観測方程式に基づき、ある時刻の前記対象システムの状態を推定した状態推定値を求め、前記状態推定値の誤差共分散行列の推定値を求める状態及び誤差推定部と、前記時刻の前記状態推定値に基づいて、前記非線形連続時間状態方程式を二次以上の積分手法で離散化して離散時間状態方程式を求める状態方程式離散化部と、前記時刻の前記離散時間状態方程式を用いて、差分法によって前記時刻のヤコビアンの近似値を求める状態方程式線形化部と、前記ヤコビアンの前記近似値から前記時刻の微小時間後の前記誤差共分散行列を予測し、前記離散時間状態方程式から前記時刻の微小時間後の前記状態を予測する状態及び誤差予測部とを備えることを特徴とする。
【発明の効果】
【００１０】
本発明によれば、対象システムの状態方程式が非線形連続時間関数であっても、拡張カルマンフィルタにより高精度に状態を推定可能な状態推定装置を得ることができるという効果を奏する。
【図面の簡単な説明】
【００１１】
【図１】実施の形態１に係る状態推定装置の全体構成の概略を示すブロック線図
【図２】図１に示す状態及び誤差予測部の詳細を示すブロック線図
【図３】図１に示す状態推定装置のハードウェア構成を示すブロック図
【図４】実施の形態１に係る状態推定装置の動作を示すフローチャート
【図５】実施の形態２に係る状態推定装置の全体構成の概略を示すブロック線図
【図６】実施の形態２に係る状態推定装置の動作を示すフローチャート
【図７】実施の形態３に係る状態推定装置の全体構成の概略を示すブロック線図
【図８】実施の形態４に係る状態推定装置の全体構成の概略を示すブロック線図
【発明を実施するための形態】
【００１２】
以下に、本発明の実施の形態に係る状態推定装置を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。
【００１３】
なお、以下の説明において、各パラメータは、時刻ｔ及びスカラー値εを除いてベクトル量であるが、明細書中においては特に区別せずに表記し、数式中ではベクトル量を太字で表記する。また、時間微分は、明細書中においては（ｄ／ｄｔ）ｘで表記するが、数式中では当該文字の上にドットを付する。また、予測値及び推定値は、明細書中においては（ハット）で表記するが、数式中では当該文字の上にハット記号を付する。また、拡大系モデルの状態量又は方程式は、明細書中においては（チルダ）で表記するが、数式中では当該文字の上にチルダを付する。
【００１４】
実施の形態１．
図１は、本実施の形態に係る状態推定装置の全体構成の概略を示すブロック線図である。図１に示す状態推定装置１０は、状態及び誤差推定部１と、状態方程式離散化部２と、状態方程式線形化部３と、状態及び誤差予測部４とを備える。本実施の形態に係る状態推定装置では、対象システムの非線形連続時間状態方程式（ｄ／ｄｔ）ｘ＝ｆ（ｔ，ｘ）と、線形の観測方程式ｙ＝Ｃｘと、各時刻のシステム出力ｙ（ｔｋ）とを入力とし、システム雑音共分散行列Ｑと、観測雑音共分散行列Ｒとを設計パラメータとする。
【００１５】
状態及び誤差推定部１は、線形の観測方程式の係数Ｃと、現在の時刻ｔｋにおけるシステム出力ｙ（ｔｋ）と、観測雑音共分散行列Ｒと、前の時刻に算出した現在の状態予測値ｘｋ｜ｋ−１（ハット）と、誤差共分散行列予測値Σｋ｜ｋ−１（ハット）とに基づき、現在の状態と誤差共分散行列の推定値ｘｋ｜ｋ（ハット），Σｋ｜ｋ（ハット）とを推定する。
【００１６】
状態方程式離散化部２は、非線形連続時間状態方程式ｆ（ｔ，ｘ）と、現在の状態の推定値ｘｋ｜ｋ（ハット）とに基づいて、四次のルンゲクッタ法により離散化された離散時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット））を出力する。
【００１７】
状態方程式線形化部３は、離散時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット））から状態方程式のヤコビアンの近似値∂ｆｄ（ｔ，ｘ）／∂ｘＴ｜ｔ＝ｔｋ、ｘ＝ｘｋ｜ｋ（ハット）を差分法により算出する。なお、添字Ｔは転置行列を表す。
【００１８】
図２は、図１に示す状態及び誤差予測部４の詳細を示すブロック線図である。図２に示す状態及び誤差予測部４は、状態予測部５と誤差予測部６とを備える。
【００１９】
状態予測部５は、離散時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット））を次の時刻の状態の予測値ｘｋ＋１｜ｋ（ハット）として出力する。誤差予測部６は、状態方程式のヤコビアンＦｋである∂ｆｄ（ｔ，ｘ）／∂ｘＴ｜ｔ＝ｔｋ、ｘ＝ｘｋ｜ｋ（ハット）と、現在の誤差共分散行列の推定値Σｋ｜ｋ（ハット）と、システム雑音共分散行列Ｑとから、次の時刻の誤差共分散行列の予測値Σｋ＋１｜ｋ（ハット）を算出する。
【００２０】
図３は、図１に示す状態推定装置１０のハードウェア構成を示すブロック図である。図３に示す状態推定装置１０は、状態推定を実行するプロセッサ２１と、プログラム及びデータを格納するメモリ２２と、対象システムの出力を読み込むシステムインターフェース２３とを備え、初期値及び設計パラメータを入力する入力装置２４と、推定結果の出力手段の一例である表示装置２５とに接続される。プロセッサ２１は、入力装置２４を介して設定された初期値及び設計パラメータと、システムインターフェース２３を介して取得した対象システムの出力値とに基づいて、メモリ２２に格納されたプログラムを実行することで対象システムの状態推定値を算出する。プロセッサ２１から出力された状態推定値は、表示装置２５に表示される。プロセッサ２１は、例えばＣＰＵ（Central  Processing  Unit）であり、メモリ２２は、例えばＲＡＭ（Random  Access  Memory）、ＲＯＭ（Read  Only  Memory）又はフラッシュメモリである。
【００２１】
図４は、本実施の形態に係る状態推定装置の動作を示すフローチャートである。対象システムの非線形モデルは、非線形の状態方程式及び線形の観測方程式である下記の式（１）で表されるものとする。
【００２２】
【数１】
0006768155.tif 000002
【００２３】
また、時刻ｔｋにおける出力値ｙ（ｔｋ）に基づいて、時刻ｔｋにおける状態を推定した値をｘｋ｜ｋ（ハット）とし、誤差共分散行列を推定した値をΣｋ｜ｋ（ハット）とする。更に、時刻ｔｋにおける出力値ｙ（ｔｋ）に基づいて、次の時刻ｔｋ＋１における状態を予測した値をｘｋ＋１｜ｋ（ハット）とし、誤差共分散行列を予測した値をΣｋ＋１｜ｋ（ハット）とする。
【００２４】
状態及び誤差推定部１は、まず、初期化を行う（Ｓ１）。具体的には、状態及び誤差推定部１は、初期値として、状態推定開始時刻ｔ１での状態予測値ｘ１｜０（ハット）及び誤差共分散行列の予測値Σ１｜０（ハット）を与える。
【００２５】
次に、状態及び誤差推定部１は、カルマンゲインＫｋを算出する（Ｓ２）。具体的には、状態及び誤差推定部１は、時刻ｔｋにおいて、前の時刻ｔｋ−１に算出した現在の誤差共分散行列の予測値Σｋ｜ｋ−１（ハット）と、線形の観測方程式の係数Ｃと、観測雑音共分散行列Ｒとに基づき、下記の式（２）によりカルマンゲインＫｋを算出する。なお、ここで、Σｋ｜ｋ−１（ハット）は、ｎ×ｎ行列であり、Ｒはｑ×ｑ行列であり、Ｋｋはｎ×ｑ行列である。なお、添字−１は逆行列を表す。
【００２６】
【数２】
0006768155.tif 000003
【００２７】
次に、状態及び誤差推定部１は、誤差共分散行列推定値Σｋ｜ｋ（ハット）を算出する（Ｓ３）。状態及び誤差推定部１は、具体的には、現在の誤差共分散行列推定値Σｋ｜ｋ（ハット）を、誤差共分散行列の予測値Σｋ｜ｋ−１（ハット）と、この予測値Σｋ｜ｋ−１（ハット）にカルマンゲインＫｋと、線形の観測方程式の係数Ｃとをかけた行列との差として下記の式（３）により算出する。なお、ここで、Σｋ｜ｋ（ハット）は、ｎ×ｎ行列である。
【００２８】
【数３】
0006768155.tif 000004
【００２９】
更に、状態及び誤差推定部１は、状態推定値ｘｋ｜ｋ（ハット）を算出する（Ｓ４）。状態及び誤差推定部１は、具体的には、現在の状態推定値ｘｋ｜ｋ（ハット）を、現在のシステム出力ｙ（ｔｋ）と、状態予測値ｘｋ｜ｋ−１（ハット）とに基づいて、線形の観測方程式から予測される出力ｙｋ｜ｋ−１（ハット）との差にカルマンゲインＫｋを乗じた値を現在の状態予測値ｘｋ｜ｋ−１（ハット）に加えた値として下記の式（４）により算出する。なお、ここで、ｘｋ｜ｋ（ハット）は、ｎ×１行列である。
【００３０】
【数４】
0006768155.tif 000005
【００３１】
状態方程式離散化部２では、次の時刻の状態ｘ（ｔｋ＋１）が現在の状態ｘ（ｔｋ）と状態方程式の時刻ｔｋからｔｋ＋１における積分の和とにより下記の式（５）となる。
【００３２】
【数５】
0006768155.tif 000006
【００３３】
そのため、状態方程式離散化部２は、二次以上の積分手法により、非線形離散時間状態方程式ｘ（ｔｋ＋１）＝ｆｄ（ｔｋ，ｘ（ｔｋ））の近似値を算出する。ここでは、二次以上の数値積分手法の一例として、四次のルンゲクッタ法による計算手法を以下に示す。
【００３４】
まず、現在の状態ｘ（ｔｋ）よりα＝ｆ（ｔｋ，ｘ（ｔｋ））を計算する。
【００３５】
次に、このαの算出結果と時間幅ΔＴ＝ｔｋ＋１−ｔｋとを用いてβ＝ｘ（ｔｋ）＋（ΔＴ／２）αを計算する。
【００３６】
次に、このβの算出結果を用いてγ＝ｆ（ｔｋ＋（ΔＴ／２），β）を計算する。
【００３７】
次に、このγの算出結果を用いてλ＝ｘ（ｔｋ）＋（ΔＴ／２）γを計算する。
【００３８】
次に、このλの算出結果を用いてζ＝ｆ（ｔｋ＋（ΔＴ／２），λ）を計算する。
【００３９】
次に、このζの算出結果を用いてη＝ｘ（ｔｋ）＋ΔＴζを計算する。
【００４０】
次に、このηの算出結果を用いてξ＝ｆ（ｔｋ＋１，η）を計算する。
【００４１】
次に、離散時間状態方程式ｆｄ（ｔｋ，ｘ（ｔｋ））＝ｘ（ｔｋ）＋（ΔＴ／６）（α＋２（γ＋ζ）＋ξ）を算出する。
【００４２】
そして、状態方程式離散化部２は、ｘ（ｔｋ）にｘｋ｜ｋ（ハット）を代入し（Ｓ５）、二次以上の数値積分法による算出を行う（Ｓ６）。状態方程式離散化部２は、具体的には、現在の状態ｘ（ｔｋ）として現在の状態推定値ｘｋ｜ｋ（ハット）を代入し、上述のルンゲクッタ法により離散時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット））を算出する。
【００４３】
状態予測部５は、状態予測値ｘｋ＋１｜ｋ（ハット）を算出する（Ｓ７）。状態予測部５は、具体的には、下記の式（６）に示すように、離散時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット））の値を次の時刻ｔｋ＋１での状態予測値ｘｋ＋１｜ｋ（ハット）として出力する。
【００４４】
【数６】
0006768155.tif 000007
【００４５】
状態方程式線形化部３は、離散時間状態方程式ｆｄ及び予め定められた微小スカラー値εに基づいて、ヤコビアンの近似値Ｆｋを差分法により求める。なお、ここで、Ｆｋは、ｎ×ｎ行列である。まず、状態方程式線形化部３は、ｘｉ（ハット）を算出する（Ｓ８）。状態方程式線形化部３は、具体的には、下記の式（７）のように、現在の状態推定値ｘｋ｜ｋ（ハット）から状態の第ｉ成分のみ（１＋ε）倍した値をｘｉ（ハット）とする。
【００４６】
【数７】
0006768155.tif 000008
【００４７】
次に、状態方程式線形化部３は、二次以上の数値積分法による算出を行い（Ｓ９）、差分法によりヤコビアン近似値の第ｉ列を算出する（Ｓ１０）。状態方程式線形化部３は、具体的には、ｘｉ（ハット）に基づいて、上述の数値積分手法により離散時間状態方程式の値を求め、現在の状態推定値ｘｋ｜ｋ（ハット）における離散時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット））との差を、現在の状態推定値の第ｉ成分をε倍した値εｘｋ｜ｋ（ｉ）（ハット）により除した値で、ヤコビアンの第ｉ列（ｉ＝１，２，・・・，ｎ）を下記の式（８）で近似する。なお、これをｉ＝１から順にｉ＝ｎまで繰り返す。
【００４８】
【数８】
0006768155.tif 000009
【００４９】
次に、誤差予測部６は、誤差共分散行列予測値Σｋ＋１｜ｋ（ハット）を算出する（Ｓ１１）。誤差予測部６は、具体的には、ヤコビアンの近似値Ｆｋと、現在の誤差共分散行列の推定値Σｋ｜ｋ（ハット）と、システム雑音共分散行列Ｑとに基づいて、下記の式（９）により次の時刻の誤差共分散行列Σｋ＋１｜ｋ（ハット）を算出する。なお、ここで、Ｑは、ｎ×ｎ行列である。
【００５０】
【数９】
0006768155.tif 000010
【００５１】
そして、状態推定装置１０は、時刻をｔｋからｔｋ＋１に進め、上述のステップＳ２からＳ１１を繰り返すことで、各時刻の状態推定値を出力する。上記ステップにおける計算処理は、プロセッサ２１で実行され、各ステップで求めたカルマンゲインＫｋ、推定値ｘｋ｜ｋ（ハット），Σｋ｜ｋ（ハット）、予測値ｘｋ｜ｋ−１（ハット），Σｋ｜ｋ−１（ハット）、離散時間状態方程式ｆｄ及び状態方程式のヤコビアンの近似値Ｆｋ等の算出値はメモリ２２に格納され、各ステップにおいてメモリ２２から読み出されて利用される。
【００５２】
以上説明したように、本実施の形態に係る状態推定装置によれば、対象システムの状態方程式が非線形連続時間関数で与えられる場合にも、各時刻の状態推定値に基づいてヤコビアンの近似値を求めることで、状態方程式の離散化に二次以上の高次の積分手法を適用することができる。これにより、拡張カルマンフィルタによる状態推定の過程における離散化誤差を軽減し、状態推定の精度を高めることができる。
【００５３】
また、非線形システムの状態推定装置として、拡張カルマンフィルタの他に、アンセンテッドカルマンフィルタを用いた状態推定装置が知られている。本発明に係る状態推定装置は、拡張カルマンフィルタに基づくため、アンセンテッドカルマンフィルタに比べて調整が必要な設計パラメータが少なく、計算量も抑えることができる。なお、本実施の形態の状態推定装置では、上記の式（５）の近似に、四次のルンゲクッタ法を用いているが、本発明はこれに限定されるものではなく、ホイン法等の二次以上の精度の積分手法を用いればよい。
【００５４】
また、状態方程式線形化部３が差分法に用いる微小スカラー値εは、対象システムに応じてできる限り小さい値を適用することが好ましく、表示装置２５に表示される推定結果に応じて、入力装置２４を介して外部から調整できるようにしてもよい。微小スカラー値εを外部から調整する場合には、推定結果の発散等の異常が生じない範囲で最小の値にすることで、ヤコビアンの近似による誤差を低減することができる。更に、微小スカラー値εの代わりに、εをベクトルｎ×１の行列とし、下記の式（１０）のように状態毎に一部又は全てが異なる微小値ε（ｉ）を用いて求めてもよい。
【００５５】
【数１０】
0006768155.tif 000011
【００５６】
また、ｘｋ｜ｋ（ｉ）（ハット）≒０となる場合には、一時的又は全時間にわたって微小値ε´により、下記の式（１１）としてもよい。
【００５７】
【数１１】
0006768155.tif 000012
【００５８】
実施の形態２．
図５は、本実施の形態に係る状態推定装置の全体構成の概略を示すブロック線図である。図５に示す状態推定装置１０Ａは、観測方程式線形化部７と、図１に示す状態推定装置１０とを備える。なお、実施の形態１と同様の構成及び動作については説明を省略する。本実施の形態に係る状態推定装置では、対象システムが非線形連続時間状態方程式（ｄ／ｄｔ）ｘ＝ｆ（ｔ，ｘ）及び非線形の観測方程式ｙ＝ｈ（ｔ，ｘ）で表され、これらの方程式と各時刻のシステム出力ｙ（ｔｋ）とを入力とし、システム雑音共分散行列Ｑと、観測雑音共分散行列Ｒとを設計パラメータとする。非線形の観測方程式ｙ＝ｈ（ｔ，ｘ）は、線形の観測方程式ｙ＝Ｃｘに代えて用いられるものである。
【００５９】
観測方程式線形化部７は、観測方程式ｈ（ｔ，ｘ）から解析的にヤコビアンＨを算出する。状態及び誤差推定部１は、観測方程式のヤコビアンＨｋと、システム出力ｙ（ｔｋ）と、現在の状態予測値ｘｋ｜ｋ−１（ハット）とΣｋ｜ｋ−１（ハット）とから、状態と誤差共分散行列の現在の推定値ｘｋ｜ｋ（ハット）とΣｋ｜ｋ（ハット）とを推定する。状態方程式離散化部２、状態方程式線形化部３並びに状態及び誤差予測部４は、実施の形態１と同様に動作する。
【００６０】
図６は、本実施の形態に係る状態推定装置の動作を示すフローチャートである。なお、Ｓ１，Ｓ２からＳ１１については図４を援用する。なお、Ｓ２からＳ１１については、ｉ＝１，ｉ＋＋の処理も含まれる。対象システムの非線形モデルが、下記の式（１２）の非線形連続時間状態方程式及び非線形の観測方程式で表されるとする。
【００６１】
【数１２】
0006768155.tif 000013
【００６２】
図６に示す状態推定装置１０Ａの動作では、図４に示すフローチャートにおけるステップＳ１とステップＳ２との間に、観測方程式線形化部７の動作が追加されている。観測方程式線形化部７は、下記の式（１３）のように観測方程式のヤコビアンＨを解析的に算出する（Ｓ２１）。なお、ここで、Ｈは、ｑ×ｎ行列である。
【００６３】
【数１３】
0006768155.tif 000014
【００６４】
次に、観測方程式線形化部７は、各時刻で現在の時刻ｔｋにおける状態予測値ｘｋ｜ｋ−１（ハット）を下記の式（１４）のように観測方程式のヤコビアンに代入し、観測方程式の現在のヤコビアンＨｋとする（Ｓ２２）。なお、ここで、ｘｋ｜ｋ−１（ハット）は、ｎ×１行列であり、Ｈｋは、ｑ×ｎ行列である。
【００６５】
【数１４】
0006768155.tif 000015
【００６６】
式（１４）により、現在の状態予測値ｘｋ｜ｋ−１（ハット）の周りで、観測方程式を状態にヤコビアンを乗じた値として下記の式（１５）のように近似することができる。
【００６７】
【数１５】
0006768155.tif 000016
【００６８】
従って、実施の形態１における観測方程式の係数ＣをヤコビアンＨｋに置き換え、出力の予測値ｙｋ｜ｋ−１（ハット）の算出には、元の非線形関数ｈ（ｔｋ，ｘｋ｜ｋ−１（ハット））を用いることで状態推定を行う。上記ステップにおける計算処理は、プロセッサ２１で実行され、各ステップで求めたカルマンゲインＫｋ、推定値ｘｋ｜ｋ（ハット），Σｋ｜ｋ（ハット）、予測値ｘｋ｜ｋ−１（ハット），Σｋ｜ｋ−１（ハット）、離散時間状態方程式ｆｄ及び状態方程式のヤコビアンの近似値Ｆｋ等の算出値はメモリ２２に格納され、各ステップにおいてメモリ２２から読み出されて利用される。
【００６９】
このように、本実施の形態に係る状態推定装置によれば、対象システムの状態方程式が非線形連続時間関数であって、且つ観測方程式が非線形関数で与えられる場合にも、離散化の必要のない観測方程式のヤコビアンを解析的に求めることで計算量を抑え、状態方程式のヤコビアンは、各時刻の状態推定値に基づいて近似値を求めることで、状態方程式の離散化に二次以上の高次の積分手法を適用することができる。これにより、拡張カルマンフィルタによる状態推定の過程における離散化誤差を軽減し、状態推定の精度を高めることができる。
【００７０】
実施の形態３．
図７は、本実施の形態に係る状態推定装置の全体構成の概略を示すブロック線図である。図７に示す状態推定装置１０Ｂは、観測方程式線形化部７と、状態及び誤差推定部１と、状態方程式離散化部２と、状態方程式線形化部３と、状態予測部５と、誤差予測部６とを備える。なお、実施の形態１，２と同様の構成及び動作については説明を省略する。ハードウェア構成は図３を援用し、フローチャートは図６を援用する。
【００７１】
本実施の形態に係る状態推定装置では、対象システムの非線形連続時間状態方程式（ｄ／ｄｔ）ｘ＝ｆ（ｔ，ｘ，ｕ）と、観測方程式ｙ＝ｈ（ｔ，ｘ，ｕ）と、各時刻のシステム入出力ｕ（ｔｋ），ｙ（ｔｋ）とを入力とし、システム雑音共分散行列Ｑと、観測雑音共分散行列Ｒと、差分法における微小スカラー値εとを設計パラメータとする。非線形の観測方程式ｙ＝ｈ（ｔ，ｘ，ｕ）は、線形の観測方程式ｙ＝Ｃｘに代えて用いられるものである。
【００７２】
観測方程式線形化部７は、観測方程式ｈ（ｔ，ｘ，ｕ）から解析的にヤコビアンＨを算出する。状態及び誤差推定部１は、観測方程式のヤコビアンＨｋと、システム出力ｙ（ｔｋ）と、現在の状態予測値ｘｋ｜ｋ−１（ハット）と、誤差共分散行列予測値Σｋ｜ｋ−１（ハット）とから、状態と誤差共分散行列の現在の推定値ｘｋ｜ｋ（ハット），Σｋ｜ｋ（ハット）を推定する。
【００７３】
状態方程式離散化部２は、非線形連続時間状態方程式（ｄ／ｄｔ）ｘ＝ｆ（ｔ，ｘ，ｕ）と、現在の状態および誤差共分散行列ｘｋ｜ｋ（ハット），Σｋ｜ｋ（ハット）と、システム入力ｕ（ｔｋ）とに基づいて、四次のルンゲクッタ法により離散化された離散時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット），ｕ（ｔｋ））を出力する。
【００７４】
状態方程式線形化部３は、離散化時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット），ｕ（ｔｋ））と、微小スカラー値εとから、状態方程式のヤコビアン∂ｆｄ（ｔ，ｘ，ｕ）／∂ｘＴ｜ｔ＝ｔｋ，ｘ＝ｘｋ｜ｋ（ハット），ｕ＝ｕ（ｔｋ）を差分法により算出する。状態予測部５は、離散化時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット），ｕ（ｔｋ））を次の時刻の状態の予測値ｘｋ＋１｜ｋ（ハット）として出力する。誤差予測部６は、状態方程式のヤコビアン∂ｆｄ（ｔ，ｘ，ｕ）／∂ｘＴ｜ｔ＝ｔｋ，ｘ＝ｘｋ｜ｋ（ハット），ｕ＝ｕ（ｔｋ）と、現在の誤差共分散行列の推定値Σｋ｜ｋ（ハット）と、システム雑音共分散行列Ｑとから、次の時刻の誤差共分散行列の予測値Σｋ＋１｜ｋ（ハット）を算出する。
【００７５】
次に、本実施の形態に係る状態推定装置について詳述する。まず、対象システムの非線形モデルが、下記の式（１６）の状態方程式及び観測方程式で表されるとする。
【００７６】
【数１６】
0006768155.tif 000017
【００７７】
Ｓ１の初期化は、実施の形態１と同様である。観測方程式線形化部７は、下記の式（１７）のように、観測方程式のヤコビアンＨを解析的に算出する（Ｓ２１）。なお、ここで、Ｈは、ｑ×ｎ行列である。
【００７８】
【数１７】
0006768155.tif 000018
【００７９】
次に、観測方程式線形化部７は、各時刻で現在の時刻ｔｋにおける状態の予測値ｘｋ｜ｋ−１（ハット）及び入力ｕ（ｔｋ）を、観測方程式のヤコビアンに代入し、下記の式（１８）のように、現在のヤコビアンＨｋとする（Ｓ２２）。なお、ここで、ｘｋ｜ｋ−１（ハット）は、ｎ×１行列であり、Ｈｋは、ｑ×ｎ行列である。
【００８０】
【数１８】
0006768155.tif 000019
【００８１】
次に、状態及び誤差推定部１は、現在の誤差共分散行列の予測値Σｋ｜ｋ−１（ハット）と、ヤコビアンＨｋと、観測雑音共分散行列Ｒとにより、下記の式（１９）のようにカルマンゲインＫｋを算出する（Ｓ２）。
【００８２】
【数１９】
0006768155.tif 000020
【００８３】
次に、状態及び誤差推定部１は、現在の誤差共分散行列推定値Σｋ｜ｋ（ハット）を、誤差共分散行列の予測値Σｋ｜ｋ−１（ハット）と、予測値Σｋ｜ｋ−１（ハット）にカルマンゲインＫｋと観測方程式の係数Ｈｋとをかけた行列との差として下記の式（２０）のように算出する（Ｓ３）。なお、ここで、Σｋ｜ｋ（ハット）は、ｎ×ｎ行列である。
【００８４】
【数２０】
0006768155.tif 000021
【００８５】
更に、状態及び誤差推定部１は、現在の状態推定値ｘｋ｜ｋ（ハット）を、現在のシステム出力ｙ（ｔｋ）と、状態予測値ｘｋ｜ｋ−１（ハット）及び入力ｕ（ｔｋ）に基づいて観測方程式から予測される出力ｙｋ｜ｋ−１（ハット）との差に、カルマンゲインを乗じた値を現在の状態予測値ｘｋ｜ｋ−１（ハット）に加えた値として下記の式（２１）のように算出する（Ｓ４）。なお、ここで、ｘｋ｜ｋ（ハット）は、ｎ×１行列である。
【００８６】
【数２１】
0006768155.tif 000022
【００８７】
そして、状態方程式離散化部２では、次の時刻の状態ｘ（ｔｋ＋１）が、現在の状態ｘ（ｔｋ）と状態方程式の時刻ｔｋからｔｋ＋１における積分の和とにより下記の式（２２）となることから、二次以上の積分手法により、非線形離散時間状態方程式ｘ（ｔｋ＋１）＝ｆｄ（ｔｋ，ｘ（ｔｋ），ｕ（ｔｋ））の近似値を算出する。ここでは、二次以上の数値積分手法の一例として、システム入力を考慮した四次のルンゲクッタ法による計算手法を以下に示す。
【００８８】
【数２２】
0006768155.tif 000023
【００８９】
まず、現在の状態ｘ（ｔｋ）及びシステム入力ｕ（ｔｋ）よりα＝ｆ（ｔｋ，ｘ（ｔｋ），ｕ（ｔｋ））を計算する。
【００９０】
次に、このαの算出結果と時間幅ΔＴ＝ｔｋ＋１−ｔｋとを用いてβ＝ｘ（ｔｋ）＋（ΔＴ／２）αを計算する。
【００９１】
次に、このβの算出結果を用いてγ＝ｆ（ｔｋ＋（ΔＴ／２），β，，ｕ（ｔｋ））を計算する。
【００９２】
次に、このγの算出結果を用いてλ＝ｘ（ｔｋ）＋（ΔＴ／２）γを計算する。
【００９３】
次に、このλの算出結果を用いてζ＝ｆ（ｔｋ＋（ΔＴ／２），λ，ｕ（ｔｋ））を計算する。
【００９４】
次に、このζの算出結果を用いてη＝ｘ（ｔｋ）＋ΔＴζを計算する。
【００９５】
次に、このηの算出結果を用いてξ＝ｆ（ｔｋ＋１，η，ｕ（ｔｋ））を計算する。
【００９６】
次に、離散時間状態方程式ｆｄ（ｔｋ，ｘ（ｔｋ），ｕ（ｔｋ））＝ｘ（ｔｋ）＋（ΔＴ／６）（α＋２（γ＋ζ）＋ξ）を算出する。
【００９７】
そして、状態方程式離散化部２は、現在の状態ｘ（ｔｋ）として現在の状態推定値ｘｋ｜ｋ（ハット）を代入し（Ｓ５）、上述のルンゲクッタ法により離散時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット），ｕ（ｔｋ））を算出する（Ｓ６）。
【００９８】
状態予測部５は、下記の式（２３）に示すように、離散時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット），ｕ（ｔｋ））を次の時刻ｔｋ＋１での状態の予測値ｘｋ＋１｜ｋ（ハット）として出力する（Ｓ７）。
【００９９】
【数２３】
0006768155.tif 000024
【０１００】
状態方程式線形化部３は、離散時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット），ｕ（ｔｋ））と予め定められた微小スカラー値εに基づいて、ヤコビアンの近似値Ｆｋを差分法により求める。なお、ここで、Ｆｋは、ｎ×ｎ行列である。まず、状態方程式線形化部３は、上記の式（７）のように、現在の状態推定値ｘｋ｜ｋ（ハット）からｘｉ（ハット）を求める（Ｓ８）。
【０１０１】
次に、状態方程式線形化部３では、ｘｉ（ハット）に基づいて、上述の数値積分手法により離散時間状態方程式の値を求め（Ｓ９）、現在の状態推定値ｘｋ｜ｋ（ハット）における離散時間状態方程式ｆｄ（ｔｋ，ｘｋ｜ｋ（ハット），ｕ（ｔｋ））との差を、現在の状態推定値の第ｉ成分をε倍した値εｘｋ｜ｋ（ｉ）（ハット）で除した値でヤコビアンの第ｉ列（ｉ＝１，２，・・・，ｎ）を下記の式（２４）で近似する（Ｓ１０）。
【０１０２】
【数２４】
0006768155.tif 000025
【０１０３】
次に、誤差予測部６は、ヤコビアンの近似値Ｆｋと、現在の誤差共分散行列の推定値Σｋ｜ｋ（ハット）と、システム雑音共分散行列Ｑとに基づいて、下記の式（２５）により、次の時刻の誤差共分散行列Σｋ＋１｜ｋ（ハット）を算出する（Ｓ１１）。なお、ここで、Ｑは、ｎ×ｎ行列である。
【０１０４】
【数２５】
0006768155.tif 000026
【０１０５】
上記ステップにおける計算処理は、プロセッサ２１で実行され、各ステップで求めたカルマンゲインＫｋ、推定値ｘｋ｜ｋ（ハット），Σｋ｜ｋ（ハット）、予測値ｘｋ｜ｋ−１（ハット），Σｋ｜ｋ−１（ハット）、離散時間状態方程式ｆｄ及び状態方程式のヤコビアンの近似値Ｆｋ等の算出値はメモリ２２に格納され、各ステップにおいてメモリ２２から読み出されて利用される。
【０１０６】
以上説明したように、本実施の形態に係る状態推定装置によれば、対象システムに入力があり、対象システムの状態方程式が非線形連続時間関数で与えられる場合にも、各時刻の状態推定値に基づいてヤコビアンの近似値を求めることで、状態方程式の離散化に二次以上の高次の積分手法を適用することができる。これにより、拡張カルマンフィルタによる状態推定の過程における離散化誤差を軽減し、状態推定の精度を高めることができる。
【０１０７】
なお、本実施の形態に係る状態推定装置では、上記の式（２２）の近似に、四次のルンゲクッタ法を用いているが、本発明はこれに限定されるものではなく、ホイン法等の二次以上の精度の積分手法を用いればよい。
【０１０８】
また、本実施の形態では、対象システムの観測方程式が非線形としているが、本発明は観測方程式が線形関数である場合にも適用可能である。観測方程式が線形関数である場合には、観測方程式線形化部７を省略し、観測方程式における状態の係数行列をＨｋとすればよい。
【０１０９】
また、差分法に用いる微小スカラー値εは、対象システムに応じてできる限り小さい値を適用することが好ましく、表示装置２５に表示される推定結果に応じて、入力装置２４を介して外部から調整できるようにしてもよい。微小スカラー値εを外部から調整する場合には、推定結果の発散等の異常が生じない範囲で最小の値に調整することで、ヤコビアンの近似による誤差を低減することができる。更に、微小スカラー値εの代わりに、εをベクトルｎ×１の行列とし、下記の式（２６）のように状態毎に一部又は全てが異なる微小値ε（ｉ）を用いて求めてもよい。
【０１１０】
【数２６】
0006768155.tif 000027
【０１１１】
また、ｘｋ｜ｋ（ｉ）（ハット）≒０となる場合には、一時的又は全時間にわたって微小値ε´により、下記の式（２７）としてもよい。
【０１１２】
【数２７】
0006768155.tif 000028
【０１１３】
実施の形態４．
図８は、本実施の形態に係る状態推定装置の全体構成の概略を示すブロック線図である。図８に示す状態推定装置１０Ｃは、モデル変換部８と、図７に示す状態推定装置１０Ｂとを備える。なお、実施の形態１から３と同様の構成及び動作については説明を省略する。ハードウェア構成は図３を援用し、フローチャートは図４を援用する。
【０１１４】
本実施の形態に係る状態推定装置は、対象システムの未知パラメータθを含む連続時間モデル（ｄ／ｄｔ）ｘ＝ｆ（ｔ，ｘ，θ），ｙ＝ｈ（ｔ，ｘ，θ）と、各時刻のシステム出力ｙ（ｔｋ）とを入力とし、システム雑音共分散行列Ｑと、観測雑音共分散行列Ｒと、差分法における微小スカラー値εを設計パラメータとし、未知パラメータθ及び状態ｘを同時に推定する。非線形の観測方程式ｙ＝ｈ（ｔ，ｘ，θ）は、線形の観測方程式ｙ＝Ｃｘに代えて用いられるものである。
【０１１５】
モデル変換部８は、元の状態ｘ及び未知パラメータθを新しい状態ベクトルとして、連続時間モデルを非線形連続時間モデルに変換する。
【０１１６】
状態推定装置１０Ｂは、変換された非線形連続時間モデルを入力とし、新しい状態ベクトルの推定値を算出する。状態推定装置１０Ｂにおける観測方程式線形化部７、状態及び誤差推定部１、状態方程式離散化部２、状態方程式線形化部３、状態予測部５及び誤差予測部６は、実施の形態１から３と同じである。
【０１１７】
次に、本実施の形態に係る状態推定装置による、状態及びパラメータ推定装置について詳述する。対象システムのモデルが、次の未知パラメータθを含む連続時間状態方程式及び観測方程式で下記の式（２８）のように表されるものとする。
【０１１８】
【数２８】
0006768155.tif 000029
【０１１９】
このとき、図４又は図６における初期化ステップ（Ｓ１）の前段に、モデル変換部８による動作が追加される。
【０１２０】
モデル変換部８は、元の状態ｘ及び未知パラメータθを新しい状態ベクトルｘ（チルダ）＝［ｘＴ  θＴ］Ｔとして、未知パラメータθを含むシステムの連続時間モデルを下記の式（２９）の新しい非線形連続時間モデルに変換する。ここで、ｘ（チルダ）は（ｎ＋ｐ）×１行列である。
【０１２１】
【数２９】
0006768155.tif 000030
【０１２２】
このとき、未知パラメータθに時間変化がある場合にはモデル化し、ｆ（チルダ）のｎ＋１：ｎ＋ｐ成分とする。未知パラメータθが一定の場合又は時間変化がサンプリング周期に対して１０倍以上遅い場合には、ｆ（チルダ）のｎ＋１：ｎ＋ｐ成分を０とする。それ以後の動作は、図４と同様であり、新しいベクトルｘ（チルダ）の推定値を下記の式（３０）で算出する。
【０１２３】
【数３０】
0006768155.tif 000031
【０１２４】
上記ステップにおける計算処理は、プロセッサ２１で実行され、各ステップで求めたカルマンゲインＫｋ、推定値ｘｋ｜ｋ（チルダ）（ハット），Σｋ｜ｋ（ハット）、予測値ｘｋ｜ｋ−１（チルダ）（ハット），Σｋ｜ｋ−１（ハット）、離散時間状態方程式ｆｄ（チルダ）及び状態方程式のヤコビアンの近似値Ｆｋ（チルダ）等の算出値はメモリ２２に格納され、各ステップにおいてメモリ２２から読み出されて利用される。
【０１２５】
以上説明したように、本実施の形態に係る状態推定装置によれば、対象システムの状態方程式が連続時間関数で、未知パラメータを含む場合にも、元の状態と未知パラメータとを新しい状態とするモデルに変換することで状態の推定が可能であり、離散化誤差を軽減し、高精度に状態推定及びパラメータ推定が可能である。
【０１２６】
なお、本実施の形態の入力とする未知パラメータを含む連続時間モデルは、非線形でも線形でもよく、線形の場合でも、モデル変換部の出力は非線形モデルとなる。また、未知パラメータは、状態方程式及び観測方程式のいずれか一方又は双方に含まれていてもよく、モデル変換部の出力が非線形連続時間状態方程式を含むような連続時間モデルであればよい。また、連続時間モデルは、システム入力を含んでもよいし、含まなくてもよい。
【０１２７】
以上の実施の形態に示した構成は、本発明の内容の一例を示すものであり、別の公知の技術と組み合わせることも可能であるし、本発明の要旨を逸脱しない範囲で、構成の一部を省略、変更することも可能である。
【符号の説明】
【０１２８】
１  状態及び誤差推定部、２  状態方程式離散化部、３  状態方程式線形化部、４  状態及び誤差予測部、５  状態予測部、６  誤差予測部、７  観測方程式線形化部、８  モデル変換部、１０，１０Ａ，１０Ｂ，１０Ｃ  状態推定装置、２１  プロセッサ、２２  メモリ、２３  システムインターフェース、２４  入力装置、２５  表示装置。
【図１】
0006768155.tif 000032
【図２】
0006768155.tif 000033
【図３】
0006768155.tif 000034
【図４】
0006768155.tif 000035
【図５】
0006768155.tif 000036
【図６】
0006768155.tif 000037
【図７】
0006768155.tif 000038
【図８】
0006768155.tif 000039