JP4072017B2 - 状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置 - Google Patents
状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置 Download PDFInfo
- Publication number
- JP4072017B2 JP4072017B2 JP2002225476A JP2002225476A JP4072017B2 JP 4072017 B2 JP4072017 B2 JP 4072017B2 JP 2002225476 A JP2002225476 A JP 2002225476A JP 2002225476 A JP2002225476 A JP 2002225476A JP 4072017 B2 JP4072017 B2 JP 4072017B2
- Authority
- JP
- Japan
- Prior art keywords
- state
- estimation
- observation
- internal state
- covariance
- 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
Description
【０００１】
【発明の属する技術分野】
本発明は、時系列信号処理技術に関し、より詳細には、時系列信号の濾波又は予測により、システムの内部状態の推定を行う状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置に関する。
【０００２】
【従来の技術】
従来、時系列に観測された観測信号に基づいて、現在の状態を推定する「濾波」あるいは未来の状態を推定する「予測」は、カルマンフィルタ（Ｋａｌｍａｎｆｉｌｔｅｒ）による逐次推定処理や、反復収集演算（Ｂａｔｃｈ Ｉｔｅｒａｔｉｏｎ）による最小自乗推定等を用いて推定されていた。
【０００３】
このカルマンフィルタは、離散時間の線形確率システムを対象とした場合、時系列に入力される観測信号に基づいて、システムの状態（内部状態）を逐次出力するものである。このとき、カルマンフィルタは、内部状態と観測信号との関係を表す観測方程式と、内部状態が時系列でどのように遷移するかを表す状態遷移方程式とをモデル化し、濾波及び予測を行う２種類の漸化式（観測方程式及び状態遷移方程式に基づくフィルタ方程式）を、時系列に観測信号が入力されるたびに適用することを特徴としている。例えば、観測方程式は下記の（１）式で、状態遷移方程式は下記の（２）式で表すことができる。
【０００４】
【数１】
【０００５】
【数２】
【０００６】
ここで、ｘkは時刻ｔkにおけるシステムの状態量、ｙkは時刻ｔkにおいて入力される観測量、ｖkは観測雑音、ｗkはシステム雑音（白色雑音）、Ｆkは状態遷移行列、Ｇkは駆動行列、Ｈkは観測行列を示している。なお、Ｆk、Ｇk及びＨkは、物理法則や測定等によって予め確定した行列である。また、内部状態は状態量（ｘk）とシステム雑音（ｗk）の共分散とを指しており、観測信号は観測量（ｙk）と観測雑音（ｖk）の共分散とを指している。
【０００７】
このカルマンフィルタにおける濾波は、現在の内部状態から、観測方程式（（１）式）により導出される観測推定量（観測量）と実際に観測された観測信号との差が小さくなるように内部状態を更新することで、現在の内部状態を推定するものであり、予測は、現在の内部状態を状態遷移方程式（（２）式）に代入することで、１単位時間未来の内部状態を予測するものである。
【０００８】
また、反復収集演算による最小自乗推定は、内部状態の時系列から観測方程式により導出される観測量の時系列と、実際に観測される観測信号の時系列との、ある時間範囲内における誤差和が最小となるように内部状態を求める手法であり、反復演算によって誤差和が小さくなるように内部状態を収束させるものである。
【０００９】
【発明が解決しようとする課題】
しかし、前記従来の技術において、カルマンフィルタによる濾波では、ある時点におけるあらゆる種類の観測信号を１つの観測ベクトルにまとめて、一度に更新を行うため、多くの種類の観測信号に基づいて濾波を行う場合、その濾波を行うための演算に用いるベクトルの数や、行列の行数及び列数が大きくなり、処理を行うハードウェア資源を多く消費し、演算時間が長くなってしまうという問題があった。
【００１０】
さらに、カルマンフィルタによる濾波では、観測ベクトルを構成する観測値の一部分を変更する場合であっても、内部状態の更新に使用する漸化式が変化してしまうため、一度フィルタを作成すると他の目的に再利用することが困難であった。また、観測値の種類や数を時刻によって変化させる場合には、内部状態の更新に用いる漸化式に含まれる観測方程式も変化させなければならず、ハードウェアやソフトウェアによる実装が困難であるという問題があった。
【００１１】
また、カルマンフィルタによる予測では、状態遷移方程式に基づいて、現在の内部状態を１単位時間後の内部状態に遷移させるため、その１単位時間における遷移が、複数連続して発生する場合、その連続する個々の部分的な遷移を１つにまとめなければならなかった。このため、カルマンフィルタによる予測は、部分的な遷移が時間的に変化する場合、その部分的な遷移を１つにまとめる処理を行わなければならず、その処理を行うためのオーバヘッドが大きくなるという問題があった。
【００１２】
また、反復収束演算による最小自乗推定では、前記したカルマンフィルタの問題点に加えて、時間方向においてもまとめて処理を行う必要があるため、さらに、処理を行うハードウェア資源を多く消費し、演算時間が長くなってしまうという問題があった。
【００１３】
本発明は、以上のような問題点に鑑みてなされたものであり、ある時点における１つ以上の観測信号を個々に分離して、段階的に内部状態を更新することが可能な状態推定装置、その方法及びそのプログラムを提供することを目的とする。また、ある時点における１つ以上の観測信号を個々に分離して、段階的に現在の内部状態を推定する現在状態推定装置を提供することを目的とする。さらに、観測信号によらず、内部状態の更新を段階的に行い未来の内部状態を推定する未来状態推定装置を提供することを目的とする。
【００１４】
【課題を解決するための手段】
本発明は、前記目的を達成するために創案されたものであり、まず、請求項１に記載の状態推定装置は、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定する状態推定装置であって、前記観測信号と先に推定した前記内部状態と予め設定された推定規則とに基づいて、現在の内部状態を推定する濾波推定手段を、前記観測信号の種類に対応して多段接続し、前記内部状態と予め設定されたその内部状態を遷移させる状態遷移規則とに基づいて、未来の内部状態を推定する予測推定手段を、前記内部状態の種類に対応して多段接続して構成することとした。
【００１５】
かかる構成によれば、状態推定装置は、濾波推定手段によって、時系列に入力される観測信号と先に推定した内部状態と予め設定された推定規則とに基づいて、現在の内部状態を推定し、予測推定手段によって、内部状態と予め設定された状態遷移規則とに基づいて、未来の内部状態を推定する。そして、この濾波推定手段又は予測推定手段の少なくとも一方を複数備え、多段に接続することで、入力された観測信号に基づいて、順次現在及び未来の内部状態を予測する。つまり、状態推定装置は、濾波推定手段又は予測推定手段を自由な組合せ及び順序で接続することが可能となり、観測信号や予測の目的を変える場合であっても推定処理を柔軟に変化させることが可能になる。
【００１６】
ここでシステムの内部状態とは、時刻に伴って変化する状態量であって、現時刻において現在の状態量を推定することを濾波といい、現時刻において未来の状態量を推定することを予測という。例えば、この状態推定装置は、車の速度に基づいてその車の位置を推定する場合、その車の位置を内部状態として、観測される速度に基づいて、現在又は未来の位置（内部状態）を推定する。
【００１７】
また、この濾波推定手段は、カルマンフィルタにおける濾波と同様に、内部状態と観測信号との関係を表す観測方程式により現在の内部状態を推定するものとすることができる。また、予測推定手段は、カルマンフィルタにおける予測と同様に、内部状態が時系列でどのように遷移するかを表す状態遷移方程式に基づいて、１単位時間未来の内部状態を予測するものとすることができる。すなわち、状態遷移規則は、このカルマンフィルタの状態遷移方程式で使用される状態遷移行列に対応するものである。
【００１８】
また、請求項２に記載の状態推定装置は、請求項１に記載の状態推定装置において、観測信号が、観測により得られる観測量と観測時に混入する観測雑音の共分散である観測雑音共分散とからなり、前記内部状態が、内部に保持している状態量とその状態量の共分散である状態共分散とからなっていることを特徴とする。
【００１９】
かかる構成によれば、状態推定装置は、濾波推定手段によって、現在の内部状態を推定する際に、観測量と観測時に混入する観測雑音の共分散である観測雑音共分散とに基づいて推定を行うことで推定の幅を狭めることができるため、推定の精度を高めることができる。また、状態推定装置は、予測推定手段によって、状態量とその状態量の共分散である状態共分散とに基づいて推定を行うことで推定の幅を狭めることができるため、さらに推定の精度を高めることができる。
【００２０】
また、請求項３に記載の状態推定装置は、請求項２に記載の状態推定装置において、濾波推定手段が、予め設定された推定規則に基づいて、前記状態量から前記観測量の推定量である観測推定量を推定する観測量推定手段と、この観測量推定手段で推定した前記観測推定量と前記観測量との差分により、観測推定誤差を算出する観測推定誤差算出手段と、前記観測雑音共分散と前記状態共分散とに基づいて、濾波推定の感度を算出する感度算出手段と、この感度算出手段で算出した前記感度と、前記観測推定誤差算出手段で算出した前記観測推定誤差とに基づいて、前記状態量及び前記状態共分散を更新する現在状態更新手段と、を備えていることを特徴とする。
【００２１】
かかる構成によれば、状態推定装置の濾波推定手段は、観測量推定手段によって、予め設定された推定規則に基づいて、状態量から観測量の推定量である観測推定量を推定する。なお、この推定規則は、状態量と観測量との関係を定義したもので、例えば、観測量が車の速度で、状態量が車の位置である場合、その車の速度と位置を関係付けたものである。この推定規則は数式によって定義することが可能で、例えば、カルマンフィルタにおいて関数（観測関数）あるいは行列（観測行列）で表されるものである。
【００２２】
そして、濾波推定手段は、観測推定誤差算出手段によって、観測量推定手段で推定した観測推定量と観測量との差分により、観測推定誤差を算出して、感度算出手段によって、観測雑音共分散と状態共分散とに基づいて、濾波推定の感度を算出する。ここで感度とは、濾波推定手段における利得（ゲイン）を表すもので、カルマンフィルタにおけるゲイン（カルマンゲイン）に相当するものである。
【００２３】
そして、現在状態更新手段によって、感度算出手段で算出した感度と、観測推定誤差算出手段で算出した観測推定誤差とに基づいて、状態量及び状態共分散を更新する。
【００２４】
また、請求項４に記載の状態推定装置は、請求項２又は請求項３に記載の状態推定装置において、予測推定手段が、予め設定された内部状態を遷移させる状態遷移規則に基づいて、前記状態量を未来の状態量に更新する未来状態量更新手段と、前記状態遷移規則とシステム内部で発生するシステム雑音の共分散であるシステム雑音共分散とに基づいて、前記状態共分散を未来の状態共分散に更新する未来状態共分散更新手段と、を備えていることを特徴とする。
【００２５】
かかる構成によれば、状態推定装置の予測推定手段は、未来状態量更新手段によって、予め設定された状態遷移規則に基づいて、状態量を未来の状態量に更新する。なお、この状態推定規則は、１単位時間毎に変化する内部状態の遷移を定義したもので、例えば、カルマンフィルタにおける状態遷移方程式で使用される状態遷移行列に対応するものである。
【００２６】
そして、未来状態共分散更新手段によって、状態遷移規則とシステム内部で発生するシステム雑音の共分散であるシステム雑音共分散とに基づいて、状態共分散を未来の状態共分散に更新する。ここで、システム雑音とは、所謂、白色雑音（ｗｈｉｔｅ ｎｏｉｓｅ）を指している。
【００２７】
さらに、請求項５に記載の状態推定方法は、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定する状態推定方法であって、前記観測信号と先に推定した前記内部状態と予め設定された推定規則とに基づいて、前記観測信号の種類に対応して多段接続した濾波推定手段により、現在の内部状態を推定する濾波推定ステップと、前記内部状態と予め設定されたその内部状態を遷移させる状態遷移規則とに基づいて、前記内部状態の種類に対応して多段接続した予測推定手段により、未来の内部状態を推定する予測推定ステップと、を含むことを特徴とする。
【００２８】
この方法によれば、状態推定方法は、濾波推定ステップで、時系列に入力される観測信号と先に推定した内部状態と予め設定された推定規則とに基づいて、現在の内部状態を推定し、予測推定ステップで、内部状態と予め設定された状態遷移規則とに基づいて、未来の内部状態を推定する。そして、この濾波推定ステップ又は予測推定ステップの少なくとも一方を内部状態の種類に応じて複数動作させることで、入力された観測信号に基づいて、順次現在及び未来の内部状態を予測する。
なお、この濾波推定ステップ及び予測推定ステップは、動作順序を問わず、各ステップの出力を他のステップの入力となるように順次動作させるだけでよい。
【００２９】
また、請求項６に記載の状態推定プログラムは、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定するために、コンピュータを、以下の手段によって機能させる構成とした。
すなわち、前記観測信号と先に推定した前記内部状態と予め設定された推定規則とに基づいて、前記観測信号の種類ごとに現在の内部状態を推定する多段接続された濾波推定手段、前記内部状態と予め設定されたその内部状態を遷移させる状態遷移規則とに基づいて、前記内部状態の種類ごとに未来の内部状態を推定する多段接続された予測推定手段、として機能させることを特徴とする。
【００３０】
かかる構成によれば、状態推定プログラムは、濾波推定手段によって、時系列に入力される観測信号と先に推定した内部状態と予め設定された推定規則とに基づいて、現在の内部状態を推定し、予測推定手段によって、内部状態と予め設定された状態遷移規則とに基づいて、未来の内部状態を推定する。そして、この濾波推定手段又は予測推定手段の少なくとも一方を内部状態の種類に応じて複数備え、多段に接続し、動作させることで、入力された観測信号に基づいて、順次現在及び未来の内部状態を予測する。
【００３１】
さらに、請求項７に記載の現在状態推定装置は、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの現在の内部状態を推定する現在状態推定装置であって、前記観測信号と先に推定した前記内部状態と予め設定された推定規則とに基づいて、前記現在の内部状態を推定する濾波推定手段を、前記観測信号の種類に対応して多段接続して構成することを特徴とする。
【００３２】
かかる構成によれば、現在状態推定装置は、複数の濾波推定手段によって、観測により時系列に入力される観測信号と先に推定した前記観測を行ったシステムの内部状態と予め設定された推定規則とに基づいて、現在の内部状態を推定する。この濾波推定手段は、カルマンフィルタにおける濾波と同様に、内部状態と観測信号との関係を表す観測方程式により現在の内部状態を推定するものとすることができる。
【００３３】
また、請求項８に記載の未来状態推定装置は、内部状態が時系列に推移するシステムの未来の内部状態を推定する未来状態推定装置であって、先に推定した内部状態と予め設定されたその内部状態を遷移させる状態遷移規則とに基づいて、前記未来の内部状態を推定する予測推定手段を、前記内部状態の種類に対応して多段接続して構成することを特徴とする。
【００３４】
かかる構成によれば、未来状態推定装置は、複数の予測推定手段によって、システムの内部状態と予め設定された前記内部状態の状態遷移規則とに基づいて、未来の内部状態を推定する。この予測推定手段は、カルマンフィルタにおける予測と同様に、内部状態が時系列でどのように遷移するかを表す状態遷移方程式に基づいて、１単位時間未来の内部状態を予測するものとすることができる。
【００３５】
【発明の実施の形態】
以下、本発明の実施の形態について図面を参照して説明する。
（状態推定装置の構成）
図１は、本発明における状態推定装置１の構成を示したブロック図である。図１に示すように状態推定装置１は、観測によって時系列に入力される観測信号に基づいて、その観測を行ったシステムの現在の内部状態、並びに、未来の内部状態を推定するものである。例えば、航空機等の軌道追跡を行うシステムに状態推定装置１を利用した場合、内部状態として航空機等の位置、速度、姿勢等の諸量をとることができる。また、観測信号としては、レーダによるエコーの遅延時間、ドップラーシフト、方位角、画像観測による機影の画像座標等が挙げられる。この状態推定装置１は、複数の濾波推定手段１０（１０1，１０2，…，１０m）と、複数の予測推定手段２０（２０1，２０2，…，２０n）と、遅延手段３０と、を備える構成とした。
【００３６】
濾波推定手段１０は、観測により時系列に入力される観測信号と、先に推定された内部状態とに基づいて、現在の内部状態を推定（濾波推定値）して出力するものである。この濾波推定手段１０は、カルマンフィルタにおける濾波と同様に、内部状態と観測信号との関係を表す観測方程式により現在の内部状態を推定する。なお、濾波推定手段１０は、観測信号の種類に対応して複数の濾波推定手段１０1，１０2，…，１０mを多段接続した。また、この濾波推定手段１０は、様々な観測信号に応じて、追加及び削除を行えるものとした。
【００３７】
予測推定手段２０は、入力される内部状態と予め設定された状態遷移規則とに基づいて、未来の内部状態を推定（予測推定値）して出力するものである。この予測推定手段２０は、カルマンフィルタにおける予測と同様に、内部状態が時系列でどのように遷移するかを表す状態遷移方程式に基づいて、１単位時間未来の内部状態を予測するものである。なお、予測推定手段２０は、内部状態の種類に対応して複数の予測推定手段２０1，２０2，…，２０nを多段接続した。また、この予測推定手段２０は、予測の目的に応じて、追加及び削除を行えるものとした。
【００３８】
遅延手段３０は、入力される内部状態を１単位時間遅延させて出力するものである。ここでは、遅延手段３０は、予測推定手段２０nからの入力を１単位時間遅延させて、濾波推定手段１０1へ出力する。この遅延手段３０によって、内部状態を折り返すことで、濾波推定手段１０及び予測推定手段２０は、内部状態をその単位時間毎に順次更新した現在及び未来の内部状態として予測することが可能になる。
【００３９】
ここで、図２及び図３（適宜図１）を参照して、濾波推定手段１０及び予測推定手段２０について、さらに詳細に説明を行う。図２は、濾波推定手段１０の内部構成を示したブロック図である。図３は、予測推定手段２０の内部構成を示したブロック図である。
【００４０】
図２に示すように、濾波推定手段１０は、観測量推定手段１１と、観測推定誤差算出手段１２と、感度算出手段１３と、現在状態更新手段１４とを備えている。なお、この濾波推定手段１０で入出力される内部状態は、内部状態の状態量ｘとシステム雑音の共分散である状態共分散Ｐとを含んでいるものとする。また、入力される観測信号は、実際に観測される観測量ｙと観測時に混入する観測雑音の共分散である観測雑音共分散Ｒとを含んでいるものとする。
【００４１】
観測量推定手段１１は、状態量ｘと観測量ｙとの関係に基づいて、入力された状態量ｘから観測量を推定し、観測推定量ｙ´として出力するものである。ここで推定された観測推定量ｙ´は、観測推定誤差算出手段１２へ出力される。この観測量推定手段１１は、状態量ｘと観測量ｙとの関係が線形である場合、観測推定量ｙ´を下記の（３）式により推定する。ここで行例Ｈは、状態量ｘと観測量ｙとの関係を表す観測行列で、濾波推定手段１０毎に確定している行列である。
【００４２】
【数３】
【００４３】
なお、状態量ｘと観測量ｙとの関係が非線形である場合、観測量推定手段１１は、観測推定量ｙ´を下記の（４）式により推定する。ここでベクトル関数ｈは、状態量ｘと観測量ｙとの関係を表す観測関数で、濾波推定手段１０毎に確定している関数である。
【００４４】
【数４】
【００４５】
観測推定誤差算出手段１２は、実際に観測された観測量ｙと、観測量推定手段１１で推定した観測推定量ｙ´との差分をとり、観測推定誤差Δｙを算出するものである。ここで算出された観測推定誤差Δｙは現在状態更新手段１４へ出力される。この観測推定誤差算出手段１２は、入力された観測量ｙと観測推定量ｙ´とに基づいて、観測推定誤差Δｙを下記（５）式により算出する。
【００４６】
【数５】
【００４７】
感度算出手段１３は、入力された観測雑音共分散Ｒと、状態共分散Ｐとに基づいて、濾波推定手段１０の感度（フィルタ感度：カルマンゲイン）を算出するものである。ここで算出された感度は現在状態更新手段１４へ出力される。この感度算出手段１３は、観測雑音共分散Ｒと状態共分散Ｐとに基づいて、フィルタ感度Ｋを下記（６）式により算出する。ここで行例Ｈは、状態量ｘと観測量ｙとの関係を表す観測行列である。また、Ｔは転置を表している
【００４８】
【数６】
【００４９】
なお、状態量ｘと観測量ｙとの関係が非線形の場合は、観測関数ｈと状態量ｘとに基づいて、観測行列Ｈを下記（７）式によって算出し、算出された観測行列Ｈを使用し（６）式によりフィルタ感度Ｋを算出する。
【００５０】
【数７】
【００５１】
現在状態更新手段１４は、現在状態量更新部１４ａと現在状態共分散更新部１４ｂとを備え、内部状態として入力される状態量ｘ及び状態共分散Ｐを、観測推定誤差算出手段１２から入力される観測推定誤差Δｙと感度算出手段１３から入力されるフィルタ感度Ｋとに基づいて、現在の内部状態に更新するものである。現在状態量更新部１４ａは、下記（８）式により、観測推定誤差Δｙとフィルタ感度Ｋとに基づいて、状態量ｘを現在の状態量ｘ´に更新する。
【００５２】
【数８】
【００５３】
現在状態共分散更新部１４ｂは、下記（９）式により、フィルタ感度Ｋと観測行列Ｈとに基づいて、状態共分散Ｐを現在の状態共分散Ｐ´に更新する。
【００５４】
【数９】
【００５５】
このように、濾波推定手段１０は、状態量ｘから観測量を推定（観測推定量ｙ´）し、観測雑音共分散Ｒ及び状態共分散Ｐから算出されるフィルタ感度Ｋに基づいて、実際に観測された観測量ｙとの差分（観測推定誤差Δｙ）が小さくなるように、現在の状態量ｘ´及び状態共分散Ｐ´を推定する。
【００５６】
また、図３に示すように、予測推定手段２０は、未来状態量更新手段２１と、未来状態共分散更新手段２２とを備えている。なお、この予測推定手段２０で入出力される内部状態は、内部状態の状態量ｘとシステム雑音の共分散である状態共分散Ｐとを含んでいるものとする。この予測推定手段２０は、内部状態として入力される状態量ｘ及び状態共分散Ｐから、未来のある時点における状態量ｘ´及び状態共分散Ｐ´を予測するものである。
【００５７】
未来状態量更新手段２１は、入力された状態量ｘから未来のある時点における状態量ｘ´を予測するものである。この未来状態量更新手段２１は、下記（１０）式により、予め設定された状態遷移規則（状態遷移行列Ｆ）に基づいて、入力された状態量ｘから未来のある時点における状態量ｘ´を算出する。
【００５８】
【数１０】
【００５９】
未来状態共分散更新手段２２は、入力された状態共分散Ｐから未来のある時点における状態共分散Ｐ´を予測するものである。この未来状態共分散更新手段２２は、下記（１１）式により、予め設定された状態遷移規則（状態遷移行列Ｆ）及びシステム雑音の共分散（システム雑音共分散Ｑ）に基づいて、入力された状態共分散Ｐから未来のある時点における状態共分散Ｐ´を算出する。
【００６０】
【数１１】
【００６１】
このように、予測推定手段２０は、入力された状態量ｘ及び状態量共分散Ｐを、状態遷移行列Ｆ及びシステム雑音の共分散（システム雑音共分散Ｑ）によって変換を行うことで、未来のある時点における状態量ｘ´及び状態共分散Ｐ´を予測する。
【００６２】
以上、一実施形態に基づいて、状態推定装置１の構成について説明したが、本発明はこれに限定されるものではない。例えば、濾波推定手段１０と予測推定手段２０とを互いに混在させて接続することも可能である。また、濾波推定手段１０と予測推定手段２０との間には優先順序を設ける必要ない。
【００６３】
さらに、個々の濾波推定手段１０1，１０2，…，１０mは、互いに同質の観測信号を扱うものであっても、異質の観測信号を扱うものであっても構わない。例えば、濾波推定手段１０1における観測信号が、カメラから入力される物体の座標位置である場合、別のカメラから入力される物体の座標位置を濾波推定手段１０2の観測信号としてもよい。また、物体の座標位置とは異質の観測信号である物体までの距離を濾波推定手段１０3（図示せず）の観測信号としてもよい。
【００６４】
また、個々の予測推定手段２０1，２０2，…，２０nは、互いに同質の状態遷移を行うものであっても、異質の状態遷移を行うものであっても構わない。例えば、物体が水平面上では等速運動を行い、鉛直方向では等加速度運動を行っている場合、このような同質の運動モデルをを予測推定手段２０1及び２０2にそれぞれ割り当てて予測を行い、物体の温度変化状態を行う状態遷移を予測推定手段２０3（図示せず）で予測することとしてもよい。
【００６５】
さらに、状態推定装置１は、状態推定装置１から予測推定手段２０を削除した構成として、複数の濾波推定手段１０（１０1，１０2，…，１０m）によって、現在の内部状態のみを推定するもの（現在状態推定装置）としてもよい。
【００６６】
また、状態推定装置１は、状態推定装置１から濾波推定手段１０を削除した構成として、複数の予測推定手段２０（２０1，２０2，…，２０n）によって、未来の内部状態のみを推定するもの（未来状態推定装置）としてもよい。この未来状態推定装置の構成においては、予め初期値の内部状態を設定することで、時々刻々と未来の内部状態を推定することになる。
【００６７】
なお、状態推定装置１は、コンピュータにおいて各手段を機能プログラムとして実現することも可能であり、各機能プログラムを結合して状態推定プログラムとして動作させることも可能である。
【００６８】
このとき、複数の濾波推定手段１０及び予測推定手段２０を個々にモジュール化し、様々な観測信号や予測の目的に対して、自由な組合せや順序により機能させることができる。また、状態推定の動作中に観測信号や予測の目的を変える場合であっても、各モジュールの組み込みや切り離しを自由に行うことができる。
【００６９】
（システム構成例１：状態推定装置を用いた物体位置推定システム）
次に、図４を参照して、状態推定装置を用いた物体位置推定システム３の構成例について説明する。図４は、カメラと距離計とに基づいて、物体の重心座標及び物体との距離を観測し、その観測信号に基づいて、物体の３次元位置、速度及び加速度を推定する物体位置推定システム３の構成を示したものである。この物体位置推定システム３は、距離計Ｍと、２台のカメラＡ及びカメラＢとを備えたセンサ系Ｓ１と、状態推定装置１Ｂとを備える構成とした。なお、観測を行う物体Ｔ１は、水平面内では等速度運動を行い、鉛直方向では等加速度運動を行うものとする。
【００７０】
距離計Ｍは、物体Ｔ１との距離を測定するものである。例えば、超音波等を発信し物体Ｔ１に反射して返ってくる信号の遅延時間によって距離計Ｍと物体Ｔ１との距離を測定する。ここでは、物体Ｔ１が３次元座標上に存在しているとしたときの原点座標（０，０，０）を、距離を測定するための原点とし、距離計Ｍはその原点に配置されているものとする。
【００７１】
カメラＡ及びカメラＢは、物体Ｔ１を撮影し、その撮影した撮影画像内における物体Ｔ１の重心座標を測定するものである。例えば、動いている物体Ｔ１の動きベクトルを検出することで、物体Ｔ１の領域を認識し、その領域内で重心座標を測定する。ここで、カメラＡは、第一光学主点位置が座標（ｄ1，０，０）になるように配置され、レンズの焦点距離はｆ1、光軸はｚ軸正方向に向いているものとする。また、カメラＢは、第一光学主点位置が座標（−ｄ2，０，０）になるように配置され、レンズの焦点距離はｆ2、光軸はｚ軸正方向に向いているものとする。
【００７２】
状態推定装置１Ｂは、濾波推定手段１０（１０1，１０2，１０3）と、予測推定手段２０（２０1，２０2）と、遅延手段３０と、を備える構成とした。なお、この状態推定装置１Ｂは、図１で説明した状態推定装置１において、濾波推定手段１０及び予測推定手段２０の数を限定したもので、構成そのものを変更したものではない。
【００７３】
濾波推定手段１０1は、カメラＡで測定される物体Ｔ１の重心座標を観測信号として、現在の内部状態を推定して出力するものである。また、濾波推定手段１０2は、カメラＢで測定される物体Ｔ１の重心座標を観測信号として、現在の内部状態を推定して出力するものである。さらに、濾波推定手段１０3は、距離計Ｍで測定される物体Ｔ１との距離を観測信号として、現在の内部状態（濾波推定値）を推定して出力するものである。状態推定装置１Ｂは、この濾波推定手段１０1、濾波推定手段１０2、及び濾波推定手段１０3によって、物体Ｔ１の現在の３次元位置座標、速度及び加速度の値を推定する。
【００７４】
予測推定手段２０1は、物体Ｔ１が水平面内において等速度運動を行うことを前提として、未来の内部状態を推定して出力するものである。また、予測推定手段２０2は、物体Ｔ１が鉛直方向において等加速度運動を行うことを前提として、未来の内部状態（予測推定値）を推定して出力するものである。状態推定装置１Ｂは、この予測推定手段２０1及び予測推定手段２０2によって、物体Ｔ１の１単位時間後の未来の３次元位置座標、速度及び加速度の値を推定する。
【００７５】
遅延手段３０は、入力される内部状態を１単位時間遅延させて出力するものである。ここでは、遅延手段３０は、予測推定手段２０2からの入力を１単位時間遅延させて、濾波推定手段１０1へ出力する。
なお、濾波推定手段１０及び予測推定手段２０の内部構成は、図２及び図３で説明したものと同一であるので説明は省略する。
【００７６】
（物体位置推定システムにおける状態推定装置の動作）
次に、図４乃至図６を参照して、物体位置推定システム３における状態推定装置１Ｂの動作を説明する。図５及び図６は、状態推定装置１Ｂの動作を示すフローチャートである。なお、状態推定装置１Ｂの内部構成については、適宜図２及び図３を参照するものとする。
【００７７】
この状態推定装置１Ｂにおいては、推定すべき物体Ｔ１の３次元の位置座標（ｐx，ｐy，ｐz）、速度（ｖx，ｖy，ｖz）及び加速度（αx，αy，αz）を、下記（１２）式に示すように１つのベクトルにまとめて状態量ｘとする。また、状態量ｘの誤差の推定量として状態共分散Ｐを用いることとする。
【００７８】
【数１２】
【００７９】
［濾波推定ステップ１（カメラＡによる濾波）］
まず、状態推定装置１Ｂは、濾波推定手段１０1において以下の各ステップで、カメラＡの観測信号から現在の状態を推定する。
この濾波推定手段１０1では、濾波推定手段１０1内の観測量推定手段１１が、遅延手段３０から入力される１単位時間前の状態量ｘから、前記した（４）式に基づいて、物体Ｔ１のカメラＡで撮影した撮像画像における重心座標のあるべき位置（観測推定量ｙ´）を推定する（ステップＳ１０）。
【００８０】
なお、カメラＡは、第一光学主点位置が座標（ｄ1，０，０）、レンズの焦点距離がｆ1、光軸がｚ軸正方向であるため、カメラＡによって撮影された物体Ｔ１の重心座標ｙ＝（ａx，ａy）は、下記（１３）式を満たす。そこで、前記（４）式の観測関数ｈ（ｘ）には、この（１３）式のｈ（ｘ）を用いる。
【００８１】
【数１３】
【００８２】
そして、観測推定誤差算出手段１２が、実際にカメラＡで観測された物体Ｔ１の重心座標ｙと、ステップＳ１０で推定した観測推定量ｙ´との差分（観測推定誤差）Δｙを算出する（ステップＳ１１）。
【００８３】
また、感度算出手段１３が、カメラＡで撮影した撮像画像の観測雑音の共分散（観測雑音共分散Ｒ）と、遅延手段３０から入力される１単位時間前の状態共分散Ｐとに基づいて、フィルタ感度Ｋを前記した（６）式により算出する（ステップＳ１２）。
【００８４】
なお、前記（６）式における観測雑音共分散Ｒは、例えば、カメラＡが画像座標において、水平方向ｒ1x、垂直方向ｒ1yの精度で物体Ｔ１の重心座標を観測できる場合は、下記（１４）式で表される共分散（観測雑音共分散Ｒ）を用いる。
【００８５】
【数１４】
【００８６】
また、前記（６）式における観測行列Ｈは、観測関数ｈ（ｘ）を偏微分した下記（１５）式で算出される行列を使用する。
【００８７】
【数１５】
【００８８】
そして、現在状態更新手段１４の現在状態量更新部１４ａが、観測推定誤差Δｙとフィルタ感度Ｋとに基づいて、前記（８）式により状態量ｘを現在の状態量ｘ´に更新し（ステップＳ１３）、現在状態共分散更新部１４ｂが、フィルタ感度Ｋと前記（１５）式の観測行列Ｈとに基づいて、前記（９）式により状態共分散Ｐを現在の状態共分散Ｐ´に更新する（ステップＳ１４）。ここで更新された状態量ｘ´及び状態共分散Ｐ´は濾波推定手段１０2に出力される。なお、この状態量ｘ´及び状態共分散Ｐ´は、状態量ｘ及び状態共分散Ｐとして、濾波推定手段１０2に入力される。
【００８９】
［濾波推定ステップ２（カメラＢによる濾波）］
次に、状態推定装置１Ｂは、濾波推定手段１０2において以下の各ステップで、カメラＢの観測信号から現在の状態を推定する。
この濾波推定手段１０2では、濾波推定手段１０2内の観測量推定手段１１が、濾波推定手段１０1から入力される状態量ｘから、前記（４）式に基づいて、物体Ｔ１のカメラＢで撮影した撮像画像における重心座標のあるべき位置（観測推定量ｙ´）を推定する（ステップＳ２０）。
【００９０】
なお、カメラＢは、第一光学主点位置が座標（−ｄ2，０，０）、レンズの焦点距離がｆ2、光軸がｚ軸正方向であるため、カメラＢによって撮影された物体Ｔ１の重心座標ｙ＝（ｂx，ｂy）は、下記（１６）式を満たす。そこで、（４）式の観測関数ｈ（ｘ）には、この（１６）式のｈ（ｘ）を用いる。
【００９１】
【数１６】
【００９２】
そして、観測推定誤差算出手段１２が、実際にカメラＢで観測された物体Ｔ１の重心座標ｙと、ステップＳ２０で推定した観測推定量ｙ´との差分（観測推定誤差）Δｙを算出する（ステップＳ２１）。
【００９３】
また、感度算出手段１３が、カメラＢで撮影した撮像画像の観測雑音の共分散（観測雑音共分散Ｒ）と、濾波推定手段１０1から入力される状態共分散Ｐとに基づいて、フィルタ感度Ｋを（６）式により算出する（ステップＳ２２）。
【００９４】
なお、前記（６）式における観測雑音共分散Ｒは、例えば、カメラＢが画像座標において、水平方向ｒ1x、垂直方向ｒ1yの精度で物体Ｔ１の重心座標を観測できる場合は、前記（１４）式で表される共分散（観測雑音共分散Ｒ）を用いる。また、前記（６）式における観測行列Ｈは、観測関数ｈ（ｘ）を偏微分した下記（１７）式で算出される行列を使用する。
【００９５】
【数１７】
【００９６】
そして、現在状態更新手段１４の現在状態量更新部１４ａが、観測推定誤差Δｙとフィルタ感度Ｋとに基づいて、前記（８）式により状態量ｘを現在の状態量ｘ´に更新し（ステップＳ２３）、現在状態共分散更新部１４ｂが、フィルタ感度Ｋと前記（１７）式の観測行列Ｈとに基づいて、前記（９）式により状態共分散Ｐを現在の状態共分散Ｐ´に更新する（ステップＳ２４）。ここで更新された状態量ｘ´及び状態共分散Ｐ´は濾波推定手段１０3へ出力される。なお、この状態量ｘ´及び状態共分散Ｐ´は、状態量ｘ及び状態共分散Ｐとして、濾波推定手段１０3に入力される。
【００９７】
［濾波推定ステップ３（距離計Ｍによる濾波）］
次に、状態推定装置１Ｂは、濾波推定手段１０3において以下の各ステップで、距離計Ｍの観測信号から現在の状態を推定する。
この濾波推定手段１０3では、濾波推定手段１０3内の観測量推定手段１１が、濾波推定手段１０2から入力される状態量ｘから、前記した（４）式に基づいて距離計Ｍから物体Ｔ１への距離（観測推定量ｙ´）を推定する（ステップＳ３０）。
【００９８】
なお、距離計Ｍは、原点座標（０，０，０）に設置されているため、距離計Ｍから物体Ｔ１（位置座標（ｐx，ｐy，ｐz））までの距離ｄは、下記（１８）式を満たす。そこで、前記（４）式の観測関数ｈ（ｘ）には、この（１８）式のｈ（ｘ）を用いる。
【００９９】
【数１８】
【０１００】
そして、観測推定誤差算出手段１２が、実際に距離計Ｍで観測された物体Ｔ１の距離ｙ（＝ｄ）と、ステップＳ３０で推定した観測推定量ｙ´との差分（観測推定誤差）Δｙを算出する（ステップＳ３１）。
また、感度算出手段１３が、距離計Ｍで測定した距離の観測雑音の共分散（観測雑音共分散Ｒ）と、濾波推定手段１０2から入力される状態共分散Ｐとに基づいて、フィルタ感度Ｋを前記（６）式により算出する（ステップＳ３２）。
【０１０１】
なお、前記（６）式における観測雑音共分散Ｒは、例えば、距離計Ｍが距離ｒ3の精度で物体Ｔ１の距離を観測できる場合は、下記（１９）式で表される共分散（観測雑音共分散Ｒ）を用いる。
【０１０２】
【数１９】
【０１０３】
また、前記（６）式における観測行列Ｈは、観測関数ｈ（ｘ）を偏微分した下記（２０）式で算出される行列を使用する。
【０１０４】
【数２０】
【０１０５】
そして、現在状態更新手段１４の現在状態量更新部１４ａが、観測推定誤差Δｙとフィルタ感度Ｋとに基づいて、前記（８）式により状態量ｘを現在の状態量ｘ´に更新し（ステップＳ３３）、現在状態共分散更新部１４ｂが、フィルタ感度Ｋと前記（２０）式の観測行列Ｈとに基づいて、前記（９）式により状態共分散Ｐを現在の状態共分散Ｐ´に更新する（ステップＳ３４）。ここで更新された状態量ｘ´及び状態共分散Ｐ´は、予測推定手段２０1に出力される。なお、この状態量ｘ´及び状態共分散Ｐ´は、状態量ｘ及び状態共分散Ｐとして、予測推定手段２０1に入力される。
また、この段階で生成された状態量ｘ及び状態共分散Ｐを現在の内部状態として出力する（ステップＳ４０）。
【０１０６】
［予測推定ステップ１（水平面内運動の予測）］
次に、状態推定装置１Ｂは、予測推定手段２０1において以下の各ステップで、水平面内の等速度運動による物体Ｔ１の未来における状態を推定する。
この予測推定手段２０1では、未来状態量更新手段２１が、濾波推定手段１０3から入力される状態量ｘから、前記（１０）式に基づいて、未来の状態量ｘ´である物体Ｔ１の１単位時間後の水平面内運動による位置座標（ｐx，ｐy，ｐz）、速度（ｖx，ｖy，ｖz）及び加速度（αx，αy，αz）を推定する（ステップＳ５０）。
【０１０７】
なお、物体Ｔ１は水平面内において等速度運動を行うことを仮定しているので、前記（１０）式における状態遷移行列Ｆは、１単位時間をΔｔとしたとき、下記（２１）式で与えられる行列を用いる。
【０１０８】
【数２１】
【０１０９】
この（２１）式の状態遷移行列Ｆを用いることで、物体Ｔ１の水平面内の加速度αx及びαzは０、水平面内の速度ｖx及びｖzは一定と予測される。また、水平面内のｘ座標位置ｐxは、水平面内の速度ｖxとΔｔとの積を加算することで新しい位置が予測され、水平面内のｚ座標位置ｐzは、水平面内の速度ｖzとΔｔとの積を加算することで新しい位置が予測される。なお、鉛直方向の成分であるｐy、ｖy及びαyには影響を与えない。
【０１１０】
そして、未来状態共分散更新手段２２が、濾波推定手段１０3から入力される状態共分散Ｐから、前記（１１）式に基づいて１単位時間後の状態共分散Ｐ´を算出する（ステップＳ５１）。
【０１１１】
なお、前記（１１）式における状態遷移行列Ｆには、前記（２１）式で与えられた行列を用いる。また、前記（１１）式におけるシステム雑音共分散Ｑは、等速度運動のモデル化誤差や、システムの外部から加えられるノイズ、環境温度、外部振動等による外乱を考慮して設定する行列である。例えば、水平面内の速度ｖx及びｖzに対して独立に誤差が生じる場合、システム雑音共分散Ｑは、下記（２２）式で与えられる行列を用いることができる。なお、ｑx、ｑzは非負の値である。
【０１１２】
【数２２】
【０１１３】
そして、ステップＳ５０で更新され推定された状態量ｘ´及びステップＳ５１で更新された状態共分散Ｐ´は、予測推定手段２０2へ出力される。なお、この状態量ｘ´及び状態共分散Ｐ´は、状態量ｘ及び状態共分散Ｐとして、予測推定手段２０2に入力される。
【０１１４】
［予測推定ステップ２（鉛直方向運動の予測）］
次に、状態推定装置１Ｂは、予測推定手段２０2において以下の各ステップで、鉛直方向の等加速度運動による物体Ｔ１の未来における状態を推定する。
この予測推定手段２０2では、未来状態量更新手段２１が、予測推定手段２０1から入力される状態量ｘから、前記（１０）式に基づいて、未来の状態量ｘ´である物体Ｔ１の１単位時間後の鉛直方向運動による位置座標（ｐx，ｐy，ｐz）、速度（ｖx，ｖy，ｖz）及び加速度（αx，αy，αz）を推定する（ステップＳ６０）。
【０１１５】
なお、物体Ｔ１は鉛直方向において等加速度運動を行うことを仮定しているので、前記（１０）式における状態遷移行列Ｆは、１単位時間をΔｔとしたとき、下記（２３）式で与えられる行列を用いる。
【０１１６】
【数２３】
【０１１７】
この（２３）式の状態遷移行列Ｆを用いることで、物体Ｔ１のｙ座標位置ｐy及び鉛直方向の速度ｖyのみが更新され、新しい鉛直方向の位置及び速度が予測される。なお、それ以外の成分は更新されない。
そして、未来状態共分散更新手段２２が、予測推定手段２０1から入力される状態共分散Ｐから、前記（１１）式に基づいて１単位時間後の状態共分散Ｐ´を算出する（ステップＳ６１）。
【０１１８】
なお、前記（１１）式における状態遷移行列Ｆには、前記（２３）式で与えられた行列を用いる。また、前記（１１）式におけるシステム雑音共分散Ｑは、例えば、鉛直方向の速度ｖyに誤差が生じる場合、下記（２４）式で与えられる行列を用いる。
【０１１９】
【数２４】
【０１２０】
ここで更新された状態量ｘ´及び状態共分散Ｐ´は、遅延手段３０を介して濾波推定手段１０1へ出力される。なお、この状態量ｘ´及び状態共分散Ｐ´は、状態量ｘ及び状態共分散Ｐとして、濾波推定手段１０1に入力される。
また、この段階で生成された状態量ｘ及び状態共分散Ｐを未来の内部状態として出力する（ステップＳ７０）。
【０１２１】
このように動作することで、状態推定装置１Ｂは、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を逐次更新することが可能になる。
【０１２２】
以上、状態推定装置１Ｂを用いた物体位置推定システム３の構成及び動作について説明したように、この状態推定装置１Ｂは、濾波推定手段１０を多段接続することで、種々の状態を含んだ現在の内部状態を逐次更新することを可能にした。このように、濾波推定手段１０を多段接続することで、例えば、カメラＡが故障した場合、濾波推定手段１０1を除去し、濾波推定手段１０2の入力を遅延手段３０の出力と短絡させることで、濾波推定手段１０を２段構成とする変更も容易に行うことができる。さらに、第３のカメラＣ（図示せず）を用いた新たな濾波推定手段１０4（図示せず）を追加することも容易である。
【０１２３】
また、状態推定装置１Ｂは、予測推定手段２０を多段接続することで、種々の状態を含んだ未来の内部状態を逐次更新することを可能にした。このように、予測推定手段２０を多段接続することで、例えば、物体Ｔ１の鉛直方向の動きを等速度運動と仮定したい場合には、予測推定手段２０2を除去し、代わりに、下記（２５）式及び（２６）式（ｑyは非負の値）に示す状態遷移行列Ｆ及びシステム雑音共分散Ｑを用いた予測推定手段２０3（図示せず）を挿入することで、容易に実現することができる。
【０１２４】
【数２５】
【０１２５】
【数２６】
【０１２６】
なお、物体位置推定システム３においては、２台のカメラＡ、カメラＢ及び距離計Ｍを直線上に配置したが、この配置及び機器の台数はこれに限ったものではない。例えば、距離計Ｍを３次元座標上で座標（ｓx，ｓy，ｓz）に配置した場合には、濾波推定手段１０3において、観測関数ｈ（ｘ）には前記（１８）式の代わりに下記（２７）式を用い、観測行列Ｈには前記（２０）式の代わりに下記（２８）式を用いることとすればよい。
【０１２７】
【数２７】
【０１２８】
【数２８】
【０１２９】
また、状態推定装置１Ｂから、予測推定手段２０（２０1，２０2）を取り除き、濾波推定手段１０3の出力と遅延手段３０の入力とを短絡させた構成とし、状態量ｘを前記（１２）式の代わりに、推定すべき物体Ｔ１の３次元の位置座標（ｐx，ｐy，ｐz）のみを示す下記（２９）式とし、濾波推定手段１０（１０1，１０2，１０3）の観測行列Ｈを前記（１５）式、（１７）式及び（２０）式の代わりに下記（３０）式、（３１）式及び（３２）式を用いることで、現在の物体Ｔ１の位置座標のみを推定する現在状態推定装置として構成することも可能である。
【０１３０】
【数２９】
【０１３１】
【数３０】
【０１３２】
【数３１】
【０１３３】
【数３２】
【０１３４】
（システム構成例２：状態推定装置を用いた人工衛星軌道推定システム）
次に、図７を参照して、状態推定装置を用いた人工衛星軌道推定システム５の構成例について説明する。図７は、人工衛星の電波遅延時間とドップラーシフトとを観測し、その観測信号に基づいて、人工衛星の現在の軌道を推定する人工衛星軌道推定システム５の構成を示したものである。この人工衛星軌道推定システム５は、遅延時間測定機Ｅと、周波数計Ｆとを備えたセンサ系Ｓ２と、状態推定装置１Ｃとを備える構成とした。
【０１３５】
遅延時間測定機Ｅは、地球上において人工衛星Ｔ２による電波の遅延時間を測定するものである。すなわち、地球上から電波を発信し、その電波が人工衛星Ｔ２に反射して返ってくるまでの時間を計測するものである。
【０１３６】
周波数計Ｆは、人工衛星Ｔ２に電磁波を照射して、その散乱波の周波数、いわゆるドップラーシフトを測定するものである。この周波数は人工衛星Ｔ２の視線速度に比例して変化するものである。
【０１３７】
状態推定装置１Ｃは、濾波推定手段１０（１０1，１０2）と、予測推定手段２０（２０1）と、遅延手段３０と、を備える構成とした。なお、この状態推定装置１Ｃは、図１で説明した状態推定装置１において、濾波推定手段１０及び予測推定手段２０の数を限定したもので、構成そのものを変更したものではない。
【０１３８】
この濾波推定手段１０1は、遅延時間測定機Ｅで測定される電波遅延時間を観測信号として、現在の内部状態を推定して出力するものである。また、濾波推定手段１０2は、周波数計Ｆで測定されるドップラーシフトを観測信号として、現在の内部状態を推定して出力するものである。
【０１３９】
この状態推定装置１Ｃは、人工衛星Ｔ２のケプラーの軌道６要素を内部状態とする。なお、この軌道６要素とは、軌道長半径、離心率、軌道傾斜角、昇交点赤経、近地点引数及び平均近点離角である。ここで、元期及び現在時刻、人工衛星軌道推定システム５の観測点の緯度及び経度、地球の自転角速度、並びに、人工衛星からの電波の周波数が既知であるとすると、観測すべき電波遅延時間及びドップラーシフトは、軌道６要素から求めることができる（数式は省略）。
【０１４０】
そこで、濾波推定手段１０1では、軌道６要素から電波遅延時間への変換を表す関数を前記（４）式の観測関数ｈ（ｘ）として用い、この観測関数ｈ（ｘ）を軌道６要素により偏微分した結果の行列を前記（６）式の観測行列Ｈとして用いる。また、濾波推定手段１０2では、軌道６要素からドップラーシフトへの変換を表す関数を前記（４）式の観測関数ｈ（ｘ）として用い、この観測関数ｈ（ｘ）を軌道６要素により偏微分した結果の行列を前記（６）式の観測行列Ｈとして用いる。
【０１４１】
なお、予測推定手段２０1では、人工衛星軌道推定システム５における推定すべき状態ベクトルの次元（軌道６要素の６次元）が、観測の総次元数（電波遅延時間とドップラーシフトの２次元）よりも高いため、時間経過に伴う状態の変動（ダイナミクス）を仮定する。
これによって、状態推定装置１Ｃは、人工衛星Ｔ２の現在の軌道を逐次推定することができる。
【０１４２】
なお、ここでは、状態推定装置を用いたシステムとして、物体位置推定システム３（図４）及び人工衛星軌道推定システム（図７）を例に説明したが、これ以外にも種々のシステムにおいて本発明における状態推定装置を利用することが可能である。
【０１４３】
例えば、橋桁に作用する空気力等を測定することで長大橋梁のフラッターを推定したり（土木工学）、超音波画像から臓器の変位を推定したり（医学）、株価データにより株価を推定したり（経済学）、天体表面上のクレータ等の画像上における位置観測により天体の自転速度やニューテーション運動を推定したり（惑星物理学）、ＧＰＳ衛星からの電波、タイヤの回転速度及びジャイロセンサ情報に基づいて車両やロボットの位置や速度を推定したり（ロボット工学、計測工学）、距離計測及び画像に基づいて航空機や宇宙機の位置を推定したり（航空工学、宇宙工学）することも可能である。
【０１４４】
あるいは、レーダによる電波の遅延時間やドップラーシフト、電波到来方向の方向角に基づいてロケットやミサイルの軌道あるいは弾道を推定したり（宇宙工学）、気温、気圧、湿度といった気象データからの未来の気温、気圧、湿度等を予測したり（気象学）、河川流域における降雨量や流速の観測により河川の未来の水位を予測したり（土木工学、気象学）することも可能である。
このように、本発明における状態推定装置は、カルマンフィルタを用いて推定可能な種々の分野におけるシステムにおいて応用が可能である。
【０１４５】
（補足；濾波推定手段の定式化について）
以上、説明したように濾波推定手段１０は、あるゆる観測系に関して、定式化を行うことが可能である。この濾波推定手段１０の定式化は、カルマンフィルタ又は拡張カルマンフィルタの観測方程式の設計法と同様に行うことができる。このカルマンフィルタ又は拡張カルマンフィルタでは、全ての種類の観測量を１つの観測方程式で表現する必要があったが、濾波推定手段１０では、必ずしも１つの観測方程式で表現する必要はない。
【０１４６】
例えば、図４に示した距離計Ｍと、２台のカメラＡ及びカメラＢとにより物体Ｔ１の観測を行う場合、カルマンフィルタ又は拡張カルマンフィルタでは、観測関数ｈ（ｘ）を下記（３３）式で設定しなければならない。
【０１４７】
【数３３】
【０１４８】
しかし、濾波推定手段１０では、前記した（１３）式、（１６）式及び（１８）式に示したように、センサ毎に観測関数ｈ（ｘ）を分割して定式化することができる。
【０１４９】
このように、他の観測系においても、カルマンフィルタ又は拡張カルマンフィルタと同様の定式化を行うか、又はこれを分割して定式化を行えばよい。なお、この分割は、例えば、センサ毎に分割することも可能であるが、複数のセンサを組にして、その組毎に分割を行ってもよい。
【０１５０】
例えば、図４に示した距離計Ｍと、２台のカメラＡ及びカメラＢとにより物体Ｔ１の観測を行う場合、２台のカメラＡ及びカメラＢからの観測信号を１つの濾波推定手段１０1へ入力し、距離計Ｍからの観測信号を他の濾波推定手段１０2へ入力することができる。この場合、濾波推定手段１０1の観測関数ｈ（ｘ）は、下記（３４）式で示した関数を用いる。
【０１５１】
【数３４】
【０１５２】
一方、濾波推定手段１０2の観測関数ｈ（ｘ）は、前記（１８）式で示した関数を用いればよい。
【０１５３】
（補足；予測推定手段の定式化について）
また、予測推定手段２０については、線形の状態遷移の場合について例示して説明したが、拡張カルマンフィルタで用いられる状態遷移方程式の場合と同様に、非線形の場合にも適用することが可能である。この場合、前記（１０）式の代わりに、下記（３５）式の定式化を行えばよい。
【０１５４】
【数３５】
【０１５５】
ここで、関数φは現時点（ｔ）の状態量ｘから次時点（ｔ＋Δｔ）の状態量ｘ´への遷移を表す状態遷移関数である。例えば、ｔを連続量の時刻として、状態量ｘが下記（３６）式の微分方程式を満たす場合、この（３６）式をルンゲクッタ（Ｒｕｎｇｅ−Ｋｕｔｔａ）法等の数値積分法によって解くことで、現時点（ｔ）の状態量ｘから次時点（ｔ＋Δｔ）の状態量ｘ´を求めることができる。この数値積分による状態量ｘから状態量ｘ´への変換が、前記（３５）式における関数φである。
【０１５６】
【数３６】
【０１５７】
この関数φを実現する積分回路を、未来状態量更新手段２１（図３参照）とすることで、非線形の予測推定手段２０を構成することが可能になる。
ここで、ケプラー運動を例にして、非線形の状態遷移について説明する。例えば、中心天体の周りで軌道運動を行うある天体の３次元位置（ｒx，ｒy，ｒz）及び３次元速度（ｖx，ｖy，ｖz）により、その天体の状態推定を行う場合、状態量ｘを下記（３７）式で定義すると、状態量ｘは下記（３８）式を満たす。
【０１５８】
【数３７】
【０１５９】
【数３８】
【０１６０】
ここで、Ｇは万有引力定数、Ｍは中心天体の質量である。この（３８）式に基づいて、ルンゲクッタ法等の数値積分法によって、現時点（ｔ）の状態量ｘから次時点（ｔ＋Δｔ）の状態量ｘ´を求めることができる。この数値積分による状態量ｘから状態量ｘ´への変換が、前記（３５）式における関数φである。
一方、前記（１１）式における状態遷移行列Ｆは、下記（３９）式の微分方程式をルンゲクッタ法等の数値積分法により解くことで求めることができる。
【０１６１】
【数３９】
【０１６２】
このように、予測推定手段２０は、非線形の状態遷移においても、拡張カルマンフィルタにおける状態遷移方程式の場合と同様に設計し、定式化することができる。
【０１６３】
【発明の効果】
以上説明したとおり、本発明に係る状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置では、以下に示す優れた効果を奏する。
【０１６４】
請求項１、請求交５又は至請求項６に記載の発明によれば、濾波推定と予測推定とを自由な組合せ及び順序で動作させることができるため、入力される観測信号の種類や、推定すべき内部状態の性質に応じて、適当な濾波推定手段（濾波推定ステップ）又は予測推定手段（予測推定ステップ）だけを選択して処理することができる。これにより、必要な処理だけを実行することになり、全体処理を高速化し、ハードウェア資源の節約を行うことができる。
【０１６５】
また、本発明は、濾波推定と予測推定とを自由な組合せ及び順序によって、異なる推定を行う処理を容易に再構成することができるため、様々な観測信号や予測の目的に対して、汎用性及び再利用性が高いという効果がある。
【０１６６】
さらに、従来のカルマンフィルタによる内部状態の濾波推定において、全ての観測信号をまとめて処理しなければならなかったところを、本発明は、複数の濾波推定手段（濾波推定ステップ）に分割して処理を行うことができるため、処理を行う演算量を抑えることができ、メモリの消費を抑えかつ高速で処理を行うことができる。
【０１６７】
また、従来のカルマンフィルタによる内部状態の予測推定において、１単位時間未来の予測を一度の処理で行っていたところを、本発明は、複数の予測推定手段（予測推定ステップ）に分割して処理を行うことができるため、複雑な状態遷移モデルであっても、動作が簡易な状態推定モデルに分割することができる。
【０１６８】
さらにまた、本発明は、濾波推定手段（濾波推定ステップ）又は予測推定手段（予測推定ステップ）を自由に追加したり、順序を変えたり、あるいは、削除することができるため、時々刻々と動作している状態であっても、容易に構成を変更することができ、柔軟なシステムを構成することが可能になる。
【０１６９】
請求項２乃至請求項４に記載の発明によれば、従来のカルマンフィルタによる状態推定に用いられる観測方程式や、状態遷移方程式を利用することが可能になり、濾波推定手段及び予測推定手段を容易に定式化することができる。
【０１７０】
請求項７に記載の発明によれば、従来のカルマンフィルタによる内部状態の濾波推定において、全ての観測信号をまとめて処理しなければならなかったところを、本発明は、複数の濾波推定手段に分割して処理を行うことができるため、処理を行う演算量を抑えることができ、メモリの消費を抑えかつ高速で処理を行うことができる。
【０１７１】
請求項８に記載の発明によれば、従来のカルマンフィルタによる内部状態の予測推定において、１単位時間未来の予測を一度の処理で行っていたところを、本発明は、複数の予測推定手段に分割して処理を行うことができるため、複雑な状態遷移モデルであっても、動作が簡易な状態推定モデルに分割することができる。
【図面の簡単な説明】
【図１】本発明の実施の形態に係る状態推定装置の全体構成を示すブロック図である。
【図２】本発明の実施の形態に係る状態推定装置の濾波推定手段の内部構成を示すブロック図である。
【図３】本発明の実施の形態に係る状態推定装置の予測推定手段の内部構成を示すブロック図である。
【図４】本発明の実施の形態に係る状態推定装置を用いた物体位置推定システムの構成例を示すブロック図である。
【図５】物体位置推定システムにおける状態推定装置の動作を示すフローチャート（１／２）である。
【図６】物体位置推定システムにおける状態推定装置の動作を示すフローチャート（２／２）である。
【図７】本発明の実施の形態に係る状態推定装置を用いた人工衛星軌道推定システムの構成例を示すブロック図である。
【符号の説明】
１、１Ｂ、１Ｃ……状態推定装置（現在状態推定装置、未来状態推定装置）
１０……濾波推定手段
１１……観測量推定手段
１２……観測推定誤差算出手段
１３……感度算出手段
１４……現在状態更新手段
１４ａ……現在状態量更新部
１４ｂ……現在状態共分散更新部
２０……予測推定手段
２１……未来状態量更新手段
２２……未来状態共分散更新手段
【発明の属する技術分野】
本発明は、時系列信号処理技術に関し、より詳細には、時系列信号の濾波又は予測により、システムの内部状態の推定を行う状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置に関する。
【０００２】
【従来の技術】
従来、時系列に観測された観測信号に基づいて、現在の状態を推定する「濾波」あるいは未来の状態を推定する「予測」は、カルマンフィルタ（Ｋａｌｍａｎｆｉｌｔｅｒ）による逐次推定処理や、反復収集演算（Ｂａｔｃｈ Ｉｔｅｒａｔｉｏｎ）による最小自乗推定等を用いて推定されていた。
【０００３】
このカルマンフィルタは、離散時間の線形確率システムを対象とした場合、時系列に入力される観測信号に基づいて、システムの状態（内部状態）を逐次出力するものである。このとき、カルマンフィルタは、内部状態と観測信号との関係を表す観測方程式と、内部状態が時系列でどのように遷移するかを表す状態遷移方程式とをモデル化し、濾波及び予測を行う２種類の漸化式（観測方程式及び状態遷移方程式に基づくフィルタ方程式）を、時系列に観測信号が入力されるたびに適用することを特徴としている。例えば、観測方程式は下記の（１）式で、状態遷移方程式は下記の（２）式で表すことができる。
【０００４】
【数１】
【数２】
ここで、ｘkは時刻ｔkにおけるシステムの状態量、ｙkは時刻ｔkにおいて入力される観測量、ｖkは観測雑音、ｗkはシステム雑音（白色雑音）、Ｆkは状態遷移行列、Ｇkは駆動行列、Ｈkは観測行列を示している。なお、Ｆk、Ｇk及びＨkは、物理法則や測定等によって予め確定した行列である。また、内部状態は状態量（ｘk）とシステム雑音（ｗk）の共分散とを指しており、観測信号は観測量（ｙk）と観測雑音（ｖk）の共分散とを指している。
【０００７】
このカルマンフィルタにおける濾波は、現在の内部状態から、観測方程式（（１）式）により導出される観測推定量（観測量）と実際に観測された観測信号との差が小さくなるように内部状態を更新することで、現在の内部状態を推定するものであり、予測は、現在の内部状態を状態遷移方程式（（２）式）に代入することで、１単位時間未来の内部状態を予測するものである。
【０００８】
また、反復収集演算による最小自乗推定は、内部状態の時系列から観測方程式により導出される観測量の時系列と、実際に観測される観測信号の時系列との、ある時間範囲内における誤差和が最小となるように内部状態を求める手法であり、反復演算によって誤差和が小さくなるように内部状態を収束させるものである。
【０００９】
【発明が解決しようとする課題】
しかし、前記従来の技術において、カルマンフィルタによる濾波では、ある時点におけるあらゆる種類の観測信号を１つの観測ベクトルにまとめて、一度に更新を行うため、多くの種類の観測信号に基づいて濾波を行う場合、その濾波を行うための演算に用いるベクトルの数や、行列の行数及び列数が大きくなり、処理を行うハードウェア資源を多く消費し、演算時間が長くなってしまうという問題があった。
【００１０】
さらに、カルマンフィルタによる濾波では、観測ベクトルを構成する観測値の一部分を変更する場合であっても、内部状態の更新に使用する漸化式が変化してしまうため、一度フィルタを作成すると他の目的に再利用することが困難であった。また、観測値の種類や数を時刻によって変化させる場合には、内部状態の更新に用いる漸化式に含まれる観測方程式も変化させなければならず、ハードウェアやソフトウェアによる実装が困難であるという問題があった。
【００１１】
また、カルマンフィルタによる予測では、状態遷移方程式に基づいて、現在の内部状態を１単位時間後の内部状態に遷移させるため、その１単位時間における遷移が、複数連続して発生する場合、その連続する個々の部分的な遷移を１つにまとめなければならなかった。このため、カルマンフィルタによる予測は、部分的な遷移が時間的に変化する場合、その部分的な遷移を１つにまとめる処理を行わなければならず、その処理を行うためのオーバヘッドが大きくなるという問題があった。
【００１２】
また、反復収束演算による最小自乗推定では、前記したカルマンフィルタの問題点に加えて、時間方向においてもまとめて処理を行う必要があるため、さらに、処理を行うハードウェア資源を多く消費し、演算時間が長くなってしまうという問題があった。
【００１３】
本発明は、以上のような問題点に鑑みてなされたものであり、ある時点における１つ以上の観測信号を個々に分離して、段階的に内部状態を更新することが可能な状態推定装置、その方法及びそのプログラムを提供することを目的とする。また、ある時点における１つ以上の観測信号を個々に分離して、段階的に現在の内部状態を推定する現在状態推定装置を提供することを目的とする。さらに、観測信号によらず、内部状態の更新を段階的に行い未来の内部状態を推定する未来状態推定装置を提供することを目的とする。
【００１４】
【課題を解決するための手段】
本発明は、前記目的を達成するために創案されたものであり、まず、請求項１に記載の状態推定装置は、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定する状態推定装置であって、前記観測信号と先に推定した前記内部状態と予め設定された推定規則とに基づいて、現在の内部状態を推定する濾波推定手段を、前記観測信号の種類に対応して多段接続し、前記内部状態と予め設定されたその内部状態を遷移させる状態遷移規則とに基づいて、未来の内部状態を推定する予測推定手段を、前記内部状態の種類に対応して多段接続して構成することとした。
【００１５】
かかる構成によれば、状態推定装置は、濾波推定手段によって、時系列に入力される観測信号と先に推定した内部状態と予め設定された推定規則とに基づいて、現在の内部状態を推定し、予測推定手段によって、内部状態と予め設定された状態遷移規則とに基づいて、未来の内部状態を推定する。そして、この濾波推定手段又は予測推定手段の少なくとも一方を複数備え、多段に接続することで、入力された観測信号に基づいて、順次現在及び未来の内部状態を予測する。つまり、状態推定装置は、濾波推定手段又は予測推定手段を自由な組合せ及び順序で接続することが可能となり、観測信号や予測の目的を変える場合であっても推定処理を柔軟に変化させることが可能になる。
【００１６】
ここでシステムの内部状態とは、時刻に伴って変化する状態量であって、現時刻において現在の状態量を推定することを濾波といい、現時刻において未来の状態量を推定することを予測という。例えば、この状態推定装置は、車の速度に基づいてその車の位置を推定する場合、その車の位置を内部状態として、観測される速度に基づいて、現在又は未来の位置（内部状態）を推定する。
【００１７】
また、この濾波推定手段は、カルマンフィルタにおける濾波と同様に、内部状態と観測信号との関係を表す観測方程式により現在の内部状態を推定するものとすることができる。また、予測推定手段は、カルマンフィルタにおける予測と同様に、内部状態が時系列でどのように遷移するかを表す状態遷移方程式に基づいて、１単位時間未来の内部状態を予測するものとすることができる。すなわち、状態遷移規則は、このカルマンフィルタの状態遷移方程式で使用される状態遷移行列に対応するものである。
【００１８】
また、請求項２に記載の状態推定装置は、請求項１に記載の状態推定装置において、観測信号が、観測により得られる観測量と観測時に混入する観測雑音の共分散である観測雑音共分散とからなり、前記内部状態が、内部に保持している状態量とその状態量の共分散である状態共分散とからなっていることを特徴とする。
【００１９】
かかる構成によれば、状態推定装置は、濾波推定手段によって、現在の内部状態を推定する際に、観測量と観測時に混入する観測雑音の共分散である観測雑音共分散とに基づいて推定を行うことで推定の幅を狭めることができるため、推定の精度を高めることができる。また、状態推定装置は、予測推定手段によって、状態量とその状態量の共分散である状態共分散とに基づいて推定を行うことで推定の幅を狭めることができるため、さらに推定の精度を高めることができる。
【００２０】
また、請求項３に記載の状態推定装置は、請求項２に記載の状態推定装置において、濾波推定手段が、予め設定された推定規則に基づいて、前記状態量から前記観測量の推定量である観測推定量を推定する観測量推定手段と、この観測量推定手段で推定した前記観測推定量と前記観測量との差分により、観測推定誤差を算出する観測推定誤差算出手段と、前記観測雑音共分散と前記状態共分散とに基づいて、濾波推定の感度を算出する感度算出手段と、この感度算出手段で算出した前記感度と、前記観測推定誤差算出手段で算出した前記観測推定誤差とに基づいて、前記状態量及び前記状態共分散を更新する現在状態更新手段と、を備えていることを特徴とする。
【００２１】
かかる構成によれば、状態推定装置の濾波推定手段は、観測量推定手段によって、予め設定された推定規則に基づいて、状態量から観測量の推定量である観測推定量を推定する。なお、この推定規則は、状態量と観測量との関係を定義したもので、例えば、観測量が車の速度で、状態量が車の位置である場合、その車の速度と位置を関係付けたものである。この推定規則は数式によって定義することが可能で、例えば、カルマンフィルタにおいて関数（観測関数）あるいは行列（観測行列）で表されるものである。
【００２２】
そして、濾波推定手段は、観測推定誤差算出手段によって、観測量推定手段で推定した観測推定量と観測量との差分により、観測推定誤差を算出して、感度算出手段によって、観測雑音共分散と状態共分散とに基づいて、濾波推定の感度を算出する。ここで感度とは、濾波推定手段における利得（ゲイン）を表すもので、カルマンフィルタにおけるゲイン（カルマンゲイン）に相当するものである。
【００２３】
そして、現在状態更新手段によって、感度算出手段で算出した感度と、観測推定誤差算出手段で算出した観測推定誤差とに基づいて、状態量及び状態共分散を更新する。
【００２４】
また、請求項４に記載の状態推定装置は、請求項２又は請求項３に記載の状態推定装置において、予測推定手段が、予め設定された内部状態を遷移させる状態遷移規則に基づいて、前記状態量を未来の状態量に更新する未来状態量更新手段と、前記状態遷移規則とシステム内部で発生するシステム雑音の共分散であるシステム雑音共分散とに基づいて、前記状態共分散を未来の状態共分散に更新する未来状態共分散更新手段と、を備えていることを特徴とする。
【００２５】
かかる構成によれば、状態推定装置の予測推定手段は、未来状態量更新手段によって、予め設定された状態遷移規則に基づいて、状態量を未来の状態量に更新する。なお、この状態推定規則は、１単位時間毎に変化する内部状態の遷移を定義したもので、例えば、カルマンフィルタにおける状態遷移方程式で使用される状態遷移行列に対応するものである。
【００２６】
そして、未来状態共分散更新手段によって、状態遷移規則とシステム内部で発生するシステム雑音の共分散であるシステム雑音共分散とに基づいて、状態共分散を未来の状態共分散に更新する。ここで、システム雑音とは、所謂、白色雑音（ｗｈｉｔｅ ｎｏｉｓｅ）を指している。
【００２７】
さらに、請求項５に記載の状態推定方法は、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定する状態推定方法であって、前記観測信号と先に推定した前記内部状態と予め設定された推定規則とに基づいて、前記観測信号の種類に対応して多段接続した濾波推定手段により、現在の内部状態を推定する濾波推定ステップと、前記内部状態と予め設定されたその内部状態を遷移させる状態遷移規則とに基づいて、前記内部状態の種類に対応して多段接続した予測推定手段により、未来の内部状態を推定する予測推定ステップと、を含むことを特徴とする。
【００２８】
この方法によれば、状態推定方法は、濾波推定ステップで、時系列に入力される観測信号と先に推定した内部状態と予め設定された推定規則とに基づいて、現在の内部状態を推定し、予測推定ステップで、内部状態と予め設定された状態遷移規則とに基づいて、未来の内部状態を推定する。そして、この濾波推定ステップ又は予測推定ステップの少なくとも一方を内部状態の種類に応じて複数動作させることで、入力された観測信号に基づいて、順次現在及び未来の内部状態を予測する。
なお、この濾波推定ステップ及び予測推定ステップは、動作順序を問わず、各ステップの出力を他のステップの入力となるように順次動作させるだけでよい。
【００２９】
また、請求項６に記載の状態推定プログラムは、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定するために、コンピュータを、以下の手段によって機能させる構成とした。
すなわち、前記観測信号と先に推定した前記内部状態と予め設定された推定規則とに基づいて、前記観測信号の種類ごとに現在の内部状態を推定する多段接続された濾波推定手段、前記内部状態と予め設定されたその内部状態を遷移させる状態遷移規則とに基づいて、前記内部状態の種類ごとに未来の内部状態を推定する多段接続された予測推定手段、として機能させることを特徴とする。
【００３０】
かかる構成によれば、状態推定プログラムは、濾波推定手段によって、時系列に入力される観測信号と先に推定した内部状態と予め設定された推定規則とに基づいて、現在の内部状態を推定し、予測推定手段によって、内部状態と予め設定された状態遷移規則とに基づいて、未来の内部状態を推定する。そして、この濾波推定手段又は予測推定手段の少なくとも一方を内部状態の種類に応じて複数備え、多段に接続し、動作させることで、入力された観測信号に基づいて、順次現在及び未来の内部状態を予測する。
【００３１】
さらに、請求項７に記載の現在状態推定装置は、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの現在の内部状態を推定する現在状態推定装置であって、前記観測信号と先に推定した前記内部状態と予め設定された推定規則とに基づいて、前記現在の内部状態を推定する濾波推定手段を、前記観測信号の種類に対応して多段接続して構成することを特徴とする。
【００３２】
かかる構成によれば、現在状態推定装置は、複数の濾波推定手段によって、観測により時系列に入力される観測信号と先に推定した前記観測を行ったシステムの内部状態と予め設定された推定規則とに基づいて、現在の内部状態を推定する。この濾波推定手段は、カルマンフィルタにおける濾波と同様に、内部状態と観測信号との関係を表す観測方程式により現在の内部状態を推定するものとすることができる。
【００３３】
また、請求項８に記載の未来状態推定装置は、内部状態が時系列に推移するシステムの未来の内部状態を推定する未来状態推定装置であって、先に推定した内部状態と予め設定されたその内部状態を遷移させる状態遷移規則とに基づいて、前記未来の内部状態を推定する予測推定手段を、前記内部状態の種類に対応して多段接続して構成することを特徴とする。
【００３４】
かかる構成によれば、未来状態推定装置は、複数の予測推定手段によって、システムの内部状態と予め設定された前記内部状態の状態遷移規則とに基づいて、未来の内部状態を推定する。この予測推定手段は、カルマンフィルタにおける予測と同様に、内部状態が時系列でどのように遷移するかを表す状態遷移方程式に基づいて、１単位時間未来の内部状態を予測するものとすることができる。
【００３５】
【発明の実施の形態】
以下、本発明の実施の形態について図面を参照して説明する。
（状態推定装置の構成）
図１は、本発明における状態推定装置１の構成を示したブロック図である。図１に示すように状態推定装置１は、観測によって時系列に入力される観測信号に基づいて、その観測を行ったシステムの現在の内部状態、並びに、未来の内部状態を推定するものである。例えば、航空機等の軌道追跡を行うシステムに状態推定装置１を利用した場合、内部状態として航空機等の位置、速度、姿勢等の諸量をとることができる。また、観測信号としては、レーダによるエコーの遅延時間、ドップラーシフト、方位角、画像観測による機影の画像座標等が挙げられる。この状態推定装置１は、複数の濾波推定手段１０（１０1，１０2，…，１０m）と、複数の予測推定手段２０（２０1，２０2，…，２０n）と、遅延手段３０と、を備える構成とした。
【００３６】
濾波推定手段１０は、観測により時系列に入力される観測信号と、先に推定された内部状態とに基づいて、現在の内部状態を推定（濾波推定値）して出力するものである。この濾波推定手段１０は、カルマンフィルタにおける濾波と同様に、内部状態と観測信号との関係を表す観測方程式により現在の内部状態を推定する。なお、濾波推定手段１０は、観測信号の種類に対応して複数の濾波推定手段１０1，１０2，…，１０mを多段接続した。また、この濾波推定手段１０は、様々な観測信号に応じて、追加及び削除を行えるものとした。
【００３７】
予測推定手段２０は、入力される内部状態と予め設定された状態遷移規則とに基づいて、未来の内部状態を推定（予測推定値）して出力するものである。この予測推定手段２０は、カルマンフィルタにおける予測と同様に、内部状態が時系列でどのように遷移するかを表す状態遷移方程式に基づいて、１単位時間未来の内部状態を予測するものである。なお、予測推定手段２０は、内部状態の種類に対応して複数の予測推定手段２０1，２０2，…，２０nを多段接続した。また、この予測推定手段２０は、予測の目的に応じて、追加及び削除を行えるものとした。
【００３８】
遅延手段３０は、入力される内部状態を１単位時間遅延させて出力するものである。ここでは、遅延手段３０は、予測推定手段２０nからの入力を１単位時間遅延させて、濾波推定手段１０1へ出力する。この遅延手段３０によって、内部状態を折り返すことで、濾波推定手段１０及び予測推定手段２０は、内部状態をその単位時間毎に順次更新した現在及び未来の内部状態として予測することが可能になる。
【００３９】
ここで、図２及び図３（適宜図１）を参照して、濾波推定手段１０及び予測推定手段２０について、さらに詳細に説明を行う。図２は、濾波推定手段１０の内部構成を示したブロック図である。図３は、予測推定手段２０の内部構成を示したブロック図である。
【００４０】
図２に示すように、濾波推定手段１０は、観測量推定手段１１と、観測推定誤差算出手段１２と、感度算出手段１３と、現在状態更新手段１４とを備えている。なお、この濾波推定手段１０で入出力される内部状態は、内部状態の状態量ｘとシステム雑音の共分散である状態共分散Ｐとを含んでいるものとする。また、入力される観測信号は、実際に観測される観測量ｙと観測時に混入する観測雑音の共分散である観測雑音共分散Ｒとを含んでいるものとする。
【００４１】
観測量推定手段１１は、状態量ｘと観測量ｙとの関係に基づいて、入力された状態量ｘから観測量を推定し、観測推定量ｙ´として出力するものである。ここで推定された観測推定量ｙ´は、観測推定誤差算出手段１２へ出力される。この観測量推定手段１１は、状態量ｘと観測量ｙとの関係が線形である場合、観測推定量ｙ´を下記の（３）式により推定する。ここで行例Ｈは、状態量ｘと観測量ｙとの関係を表す観測行列で、濾波推定手段１０毎に確定している行列である。
【００４２】
【数３】
なお、状態量ｘと観測量ｙとの関係が非線形である場合、観測量推定手段１１は、観測推定量ｙ´を下記の（４）式により推定する。ここでベクトル関数ｈは、状態量ｘと観測量ｙとの関係を表す観測関数で、濾波推定手段１０毎に確定している関数である。
【００４４】
【数４】
観測推定誤差算出手段１２は、実際に観測された観測量ｙと、観測量推定手段１１で推定した観測推定量ｙ´との差分をとり、観測推定誤差Δｙを算出するものである。ここで算出された観測推定誤差Δｙは現在状態更新手段１４へ出力される。この観測推定誤差算出手段１２は、入力された観測量ｙと観測推定量ｙ´とに基づいて、観測推定誤差Δｙを下記（５）式により算出する。
【００４６】
【数５】
感度算出手段１３は、入力された観測雑音共分散Ｒと、状態共分散Ｐとに基づいて、濾波推定手段１０の感度（フィルタ感度：カルマンゲイン）を算出するものである。ここで算出された感度は現在状態更新手段１４へ出力される。この感度算出手段１３は、観測雑音共分散Ｒと状態共分散Ｐとに基づいて、フィルタ感度Ｋを下記（６）式により算出する。ここで行例Ｈは、状態量ｘと観測量ｙとの関係を表す観測行列である。また、Ｔは転置を表している
【００４８】
【数６】
なお、状態量ｘと観測量ｙとの関係が非線形の場合は、観測関数ｈと状態量ｘとに基づいて、観測行列Ｈを下記（７）式によって算出し、算出された観測行列Ｈを使用し（６）式によりフィルタ感度Ｋを算出する。
【００５０】
【数７】
現在状態更新手段１４は、現在状態量更新部１４ａと現在状態共分散更新部１４ｂとを備え、内部状態として入力される状態量ｘ及び状態共分散Ｐを、観測推定誤差算出手段１２から入力される観測推定誤差Δｙと感度算出手段１３から入力されるフィルタ感度Ｋとに基づいて、現在の内部状態に更新するものである。現在状態量更新部１４ａは、下記（８）式により、観測推定誤差Δｙとフィルタ感度Ｋとに基づいて、状態量ｘを現在の状態量ｘ´に更新する。
【００５２】
【数８】
現在状態共分散更新部１４ｂは、下記（９）式により、フィルタ感度Ｋと観測行列Ｈとに基づいて、状態共分散Ｐを現在の状態共分散Ｐ´に更新する。
【００５４】
【数９】
このように、濾波推定手段１０は、状態量ｘから観測量を推定（観測推定量ｙ´）し、観測雑音共分散Ｒ及び状態共分散Ｐから算出されるフィルタ感度Ｋに基づいて、実際に観測された観測量ｙとの差分（観測推定誤差Δｙ）が小さくなるように、現在の状態量ｘ´及び状態共分散Ｐ´を推定する。
【００５６】
また、図３に示すように、予測推定手段２０は、未来状態量更新手段２１と、未来状態共分散更新手段２２とを備えている。なお、この予測推定手段２０で入出力される内部状態は、内部状態の状態量ｘとシステム雑音の共分散である状態共分散Ｐとを含んでいるものとする。この予測推定手段２０は、内部状態として入力される状態量ｘ及び状態共分散Ｐから、未来のある時点における状態量ｘ´及び状態共分散Ｐ´を予測するものである。
【００５７】
未来状態量更新手段２１は、入力された状態量ｘから未来のある時点における状態量ｘ´を予測するものである。この未来状態量更新手段２１は、下記（１０）式により、予め設定された状態遷移規則（状態遷移行列Ｆ）に基づいて、入力された状態量ｘから未来のある時点における状態量ｘ´を算出する。
【００５８】
【数１０】
未来状態共分散更新手段２２は、入力された状態共分散Ｐから未来のある時点における状態共分散Ｐ´を予測するものである。この未来状態共分散更新手段２２は、下記（１１）式により、予め設定された状態遷移規則（状態遷移行列Ｆ）及びシステム雑音の共分散（システム雑音共分散Ｑ）に基づいて、入力された状態共分散Ｐから未来のある時点における状態共分散Ｐ´を算出する。
【００６０】
【数１１】
このように、予測推定手段２０は、入力された状態量ｘ及び状態量共分散Ｐを、状態遷移行列Ｆ及びシステム雑音の共分散（システム雑音共分散Ｑ）によって変換を行うことで、未来のある時点における状態量ｘ´及び状態共分散Ｐ´を予測する。
【００６２】
以上、一実施形態に基づいて、状態推定装置１の構成について説明したが、本発明はこれに限定されるものではない。例えば、濾波推定手段１０と予測推定手段２０とを互いに混在させて接続することも可能である。また、濾波推定手段１０と予測推定手段２０との間には優先順序を設ける必要ない。
【００６３】
さらに、個々の濾波推定手段１０1，１０2，…，１０mは、互いに同質の観測信号を扱うものであっても、異質の観測信号を扱うものであっても構わない。例えば、濾波推定手段１０1における観測信号が、カメラから入力される物体の座標位置である場合、別のカメラから入力される物体の座標位置を濾波推定手段１０2の観測信号としてもよい。また、物体の座標位置とは異質の観測信号である物体までの距離を濾波推定手段１０3（図示せず）の観測信号としてもよい。
【００６４】
また、個々の予測推定手段２０1，２０2，…，２０nは、互いに同質の状態遷移を行うものであっても、異質の状態遷移を行うものであっても構わない。例えば、物体が水平面上では等速運動を行い、鉛直方向では等加速度運動を行っている場合、このような同質の運動モデルをを予測推定手段２０1及び２０2にそれぞれ割り当てて予測を行い、物体の温度変化状態を行う状態遷移を予測推定手段２０3（図示せず）で予測することとしてもよい。
【００６５】
さらに、状態推定装置１は、状態推定装置１から予測推定手段２０を削除した構成として、複数の濾波推定手段１０（１０1，１０2，…，１０m）によって、現在の内部状態のみを推定するもの（現在状態推定装置）としてもよい。
【００６６】
また、状態推定装置１は、状態推定装置１から濾波推定手段１０を削除した構成として、複数の予測推定手段２０（２０1，２０2，…，２０n）によって、未来の内部状態のみを推定するもの（未来状態推定装置）としてもよい。この未来状態推定装置の構成においては、予め初期値の内部状態を設定することで、時々刻々と未来の内部状態を推定することになる。
【００６７】
なお、状態推定装置１は、コンピュータにおいて各手段を機能プログラムとして実現することも可能であり、各機能プログラムを結合して状態推定プログラムとして動作させることも可能である。
【００６８】
このとき、複数の濾波推定手段１０及び予測推定手段２０を個々にモジュール化し、様々な観測信号や予測の目的に対して、自由な組合せや順序により機能させることができる。また、状態推定の動作中に観測信号や予測の目的を変える場合であっても、各モジュールの組み込みや切り離しを自由に行うことができる。
【００６９】
（システム構成例１：状態推定装置を用いた物体位置推定システム）
次に、図４を参照して、状態推定装置を用いた物体位置推定システム３の構成例について説明する。図４は、カメラと距離計とに基づいて、物体の重心座標及び物体との距離を観測し、その観測信号に基づいて、物体の３次元位置、速度及び加速度を推定する物体位置推定システム３の構成を示したものである。この物体位置推定システム３は、距離計Ｍと、２台のカメラＡ及びカメラＢとを備えたセンサ系Ｓ１と、状態推定装置１Ｂとを備える構成とした。なお、観測を行う物体Ｔ１は、水平面内では等速度運動を行い、鉛直方向では等加速度運動を行うものとする。
【００７０】
距離計Ｍは、物体Ｔ１との距離を測定するものである。例えば、超音波等を発信し物体Ｔ１に反射して返ってくる信号の遅延時間によって距離計Ｍと物体Ｔ１との距離を測定する。ここでは、物体Ｔ１が３次元座標上に存在しているとしたときの原点座標（０，０，０）を、距離を測定するための原点とし、距離計Ｍはその原点に配置されているものとする。
【００７１】
カメラＡ及びカメラＢは、物体Ｔ１を撮影し、その撮影した撮影画像内における物体Ｔ１の重心座標を測定するものである。例えば、動いている物体Ｔ１の動きベクトルを検出することで、物体Ｔ１の領域を認識し、その領域内で重心座標を測定する。ここで、カメラＡは、第一光学主点位置が座標（ｄ1，０，０）になるように配置され、レンズの焦点距離はｆ1、光軸はｚ軸正方向に向いているものとする。また、カメラＢは、第一光学主点位置が座標（−ｄ2，０，０）になるように配置され、レンズの焦点距離はｆ2、光軸はｚ軸正方向に向いているものとする。
【００７２】
状態推定装置１Ｂは、濾波推定手段１０（１０1，１０2，１０3）と、予測推定手段２０（２０1，２０2）と、遅延手段３０と、を備える構成とした。なお、この状態推定装置１Ｂは、図１で説明した状態推定装置１において、濾波推定手段１０及び予測推定手段２０の数を限定したもので、構成そのものを変更したものではない。
【００７３】
濾波推定手段１０1は、カメラＡで測定される物体Ｔ１の重心座標を観測信号として、現在の内部状態を推定して出力するものである。また、濾波推定手段１０2は、カメラＢで測定される物体Ｔ１の重心座標を観測信号として、現在の内部状態を推定して出力するものである。さらに、濾波推定手段１０3は、距離計Ｍで測定される物体Ｔ１との距離を観測信号として、現在の内部状態（濾波推定値）を推定して出力するものである。状態推定装置１Ｂは、この濾波推定手段１０1、濾波推定手段１０2、及び濾波推定手段１０3によって、物体Ｔ１の現在の３次元位置座標、速度及び加速度の値を推定する。
【００７４】
予測推定手段２０1は、物体Ｔ１が水平面内において等速度運動を行うことを前提として、未来の内部状態を推定して出力するものである。また、予測推定手段２０2は、物体Ｔ１が鉛直方向において等加速度運動を行うことを前提として、未来の内部状態（予測推定値）を推定して出力するものである。状態推定装置１Ｂは、この予測推定手段２０1及び予測推定手段２０2によって、物体Ｔ１の１単位時間後の未来の３次元位置座標、速度及び加速度の値を推定する。
【００７５】
遅延手段３０は、入力される内部状態を１単位時間遅延させて出力するものである。ここでは、遅延手段３０は、予測推定手段２０2からの入力を１単位時間遅延させて、濾波推定手段１０1へ出力する。
なお、濾波推定手段１０及び予測推定手段２０の内部構成は、図２及び図３で説明したものと同一であるので説明は省略する。
【００７６】
（物体位置推定システムにおける状態推定装置の動作）
次に、図４乃至図６を参照して、物体位置推定システム３における状態推定装置１Ｂの動作を説明する。図５及び図６は、状態推定装置１Ｂの動作を示すフローチャートである。なお、状態推定装置１Ｂの内部構成については、適宜図２及び図３を参照するものとする。
【００７７】
この状態推定装置１Ｂにおいては、推定すべき物体Ｔ１の３次元の位置座標（ｐx，ｐy，ｐz）、速度（ｖx，ｖy，ｖz）及び加速度（αx，αy，αz）を、下記（１２）式に示すように１つのベクトルにまとめて状態量ｘとする。また、状態量ｘの誤差の推定量として状態共分散Ｐを用いることとする。
【００７８】
【数１２】
［濾波推定ステップ１（カメラＡによる濾波）］
まず、状態推定装置１Ｂは、濾波推定手段１０1において以下の各ステップで、カメラＡの観測信号から現在の状態を推定する。
この濾波推定手段１０1では、濾波推定手段１０1内の観測量推定手段１１が、遅延手段３０から入力される１単位時間前の状態量ｘから、前記した（４）式に基づいて、物体Ｔ１のカメラＡで撮影した撮像画像における重心座標のあるべき位置（観測推定量ｙ´）を推定する（ステップＳ１０）。
【００８０】
なお、カメラＡは、第一光学主点位置が座標（ｄ1，０，０）、レンズの焦点距離がｆ1、光軸がｚ軸正方向であるため、カメラＡによって撮影された物体Ｔ１の重心座標ｙ＝（ａx，ａy）は、下記（１３）式を満たす。そこで、前記（４）式の観測関数ｈ（ｘ）には、この（１３）式のｈ（ｘ）を用いる。
【００８１】
【数１３】
そして、観測推定誤差算出手段１２が、実際にカメラＡで観測された物体Ｔ１の重心座標ｙと、ステップＳ１０で推定した観測推定量ｙ´との差分（観測推定誤差）Δｙを算出する（ステップＳ１１）。
【００８３】
また、感度算出手段１３が、カメラＡで撮影した撮像画像の観測雑音の共分散（観測雑音共分散Ｒ）と、遅延手段３０から入力される１単位時間前の状態共分散Ｐとに基づいて、フィルタ感度Ｋを前記した（６）式により算出する（ステップＳ１２）。
【００８４】
なお、前記（６）式における観測雑音共分散Ｒは、例えば、カメラＡが画像座標において、水平方向ｒ1x、垂直方向ｒ1yの精度で物体Ｔ１の重心座標を観測できる場合は、下記（１４）式で表される共分散（観測雑音共分散Ｒ）を用いる。
【００８５】
【数１４】
また、前記（６）式における観測行列Ｈは、観測関数ｈ（ｘ）を偏微分した下記（１５）式で算出される行列を使用する。
【００８７】
【数１５】
そして、現在状態更新手段１４の現在状態量更新部１４ａが、観測推定誤差Δｙとフィルタ感度Ｋとに基づいて、前記（８）式により状態量ｘを現在の状態量ｘ´に更新し（ステップＳ１３）、現在状態共分散更新部１４ｂが、フィルタ感度Ｋと前記（１５）式の観測行列Ｈとに基づいて、前記（９）式により状態共分散Ｐを現在の状態共分散Ｐ´に更新する（ステップＳ１４）。ここで更新された状態量ｘ´及び状態共分散Ｐ´は濾波推定手段１０2に出力される。なお、この状態量ｘ´及び状態共分散Ｐ´は、状態量ｘ及び状態共分散Ｐとして、濾波推定手段１０2に入力される。
【００８９】
［濾波推定ステップ２（カメラＢによる濾波）］
次に、状態推定装置１Ｂは、濾波推定手段１０2において以下の各ステップで、カメラＢの観測信号から現在の状態を推定する。
この濾波推定手段１０2では、濾波推定手段１０2内の観測量推定手段１１が、濾波推定手段１０1から入力される状態量ｘから、前記（４）式に基づいて、物体Ｔ１のカメラＢで撮影した撮像画像における重心座標のあるべき位置（観測推定量ｙ´）を推定する（ステップＳ２０）。
【００９０】
なお、カメラＢは、第一光学主点位置が座標（−ｄ2，０，０）、レンズの焦点距離がｆ2、光軸がｚ軸正方向であるため、カメラＢによって撮影された物体Ｔ１の重心座標ｙ＝（ｂx，ｂy）は、下記（１６）式を満たす。そこで、（４）式の観測関数ｈ（ｘ）には、この（１６）式のｈ（ｘ）を用いる。
【００９１】
【数１６】
そして、観測推定誤差算出手段１２が、実際にカメラＢで観測された物体Ｔ１の重心座標ｙと、ステップＳ２０で推定した観測推定量ｙ´との差分（観測推定誤差）Δｙを算出する（ステップＳ２１）。
【００９３】
また、感度算出手段１３が、カメラＢで撮影した撮像画像の観測雑音の共分散（観測雑音共分散Ｒ）と、濾波推定手段１０1から入力される状態共分散Ｐとに基づいて、フィルタ感度Ｋを（６）式により算出する（ステップＳ２２）。
【００９４】
なお、前記（６）式における観測雑音共分散Ｒは、例えば、カメラＢが画像座標において、水平方向ｒ1x、垂直方向ｒ1yの精度で物体Ｔ１の重心座標を観測できる場合は、前記（１４）式で表される共分散（観測雑音共分散Ｒ）を用いる。また、前記（６）式における観測行列Ｈは、観測関数ｈ（ｘ）を偏微分した下記（１７）式で算出される行列を使用する。
【００９５】
【数１７】
そして、現在状態更新手段１４の現在状態量更新部１４ａが、観測推定誤差Δｙとフィルタ感度Ｋとに基づいて、前記（８）式により状態量ｘを現在の状態量ｘ´に更新し（ステップＳ２３）、現在状態共分散更新部１４ｂが、フィルタ感度Ｋと前記（１７）式の観測行列Ｈとに基づいて、前記（９）式により状態共分散Ｐを現在の状態共分散Ｐ´に更新する（ステップＳ２４）。ここで更新された状態量ｘ´及び状態共分散Ｐ´は濾波推定手段１０3へ出力される。なお、この状態量ｘ´及び状態共分散Ｐ´は、状態量ｘ及び状態共分散Ｐとして、濾波推定手段１０3に入力される。
【００９７】
［濾波推定ステップ３（距離計Ｍによる濾波）］
次に、状態推定装置１Ｂは、濾波推定手段１０3において以下の各ステップで、距離計Ｍの観測信号から現在の状態を推定する。
この濾波推定手段１０3では、濾波推定手段１０3内の観測量推定手段１１が、濾波推定手段１０2から入力される状態量ｘから、前記した（４）式に基づいて距離計Ｍから物体Ｔ１への距離（観測推定量ｙ´）を推定する（ステップＳ３０）。
【００９８】
なお、距離計Ｍは、原点座標（０，０，０）に設置されているため、距離計Ｍから物体Ｔ１（位置座標（ｐx，ｐy，ｐz））までの距離ｄは、下記（１８）式を満たす。そこで、前記（４）式の観測関数ｈ（ｘ）には、この（１８）式のｈ（ｘ）を用いる。
【００９９】
【数１８】
そして、観測推定誤差算出手段１２が、実際に距離計Ｍで観測された物体Ｔ１の距離ｙ（＝ｄ）と、ステップＳ３０で推定した観測推定量ｙ´との差分（観測推定誤差）Δｙを算出する（ステップＳ３１）。
また、感度算出手段１３が、距離計Ｍで測定した距離の観測雑音の共分散（観測雑音共分散Ｒ）と、濾波推定手段１０2から入力される状態共分散Ｐとに基づいて、フィルタ感度Ｋを前記（６）式により算出する（ステップＳ３２）。
【０１０１】
なお、前記（６）式における観測雑音共分散Ｒは、例えば、距離計Ｍが距離ｒ3の精度で物体Ｔ１の距離を観測できる場合は、下記（１９）式で表される共分散（観測雑音共分散Ｒ）を用いる。
【０１０２】
【数１９】
また、前記（６）式における観測行列Ｈは、観測関数ｈ（ｘ）を偏微分した下記（２０）式で算出される行列を使用する。
【０１０４】
【数２０】
そして、現在状態更新手段１４の現在状態量更新部１４ａが、観測推定誤差Δｙとフィルタ感度Ｋとに基づいて、前記（８）式により状態量ｘを現在の状態量ｘ´に更新し（ステップＳ３３）、現在状態共分散更新部１４ｂが、フィルタ感度Ｋと前記（２０）式の観測行列Ｈとに基づいて、前記（９）式により状態共分散Ｐを現在の状態共分散Ｐ´に更新する（ステップＳ３４）。ここで更新された状態量ｘ´及び状態共分散Ｐ´は、予測推定手段２０1に出力される。なお、この状態量ｘ´及び状態共分散Ｐ´は、状態量ｘ及び状態共分散Ｐとして、予測推定手段２０1に入力される。
また、この段階で生成された状態量ｘ及び状態共分散Ｐを現在の内部状態として出力する（ステップＳ４０）。
【０１０６】
［予測推定ステップ１（水平面内運動の予測）］
次に、状態推定装置１Ｂは、予測推定手段２０1において以下の各ステップで、水平面内の等速度運動による物体Ｔ１の未来における状態を推定する。
この予測推定手段２０1では、未来状態量更新手段２１が、濾波推定手段１０3から入力される状態量ｘから、前記（１０）式に基づいて、未来の状態量ｘ´である物体Ｔ１の１単位時間後の水平面内運動による位置座標（ｐx，ｐy，ｐz）、速度（ｖx，ｖy，ｖz）及び加速度（αx，αy，αz）を推定する（ステップＳ５０）。
【０１０７】
なお、物体Ｔ１は水平面内において等速度運動を行うことを仮定しているので、前記（１０）式における状態遷移行列Ｆは、１単位時間をΔｔとしたとき、下記（２１）式で与えられる行列を用いる。
【０１０８】
【数２１】
この（２１）式の状態遷移行列Ｆを用いることで、物体Ｔ１の水平面内の加速度αx及びαzは０、水平面内の速度ｖx及びｖzは一定と予測される。また、水平面内のｘ座標位置ｐxは、水平面内の速度ｖxとΔｔとの積を加算することで新しい位置が予測され、水平面内のｚ座標位置ｐzは、水平面内の速度ｖzとΔｔとの積を加算することで新しい位置が予測される。なお、鉛直方向の成分であるｐy、ｖy及びαyには影響を与えない。
【０１１０】
そして、未来状態共分散更新手段２２が、濾波推定手段１０3から入力される状態共分散Ｐから、前記（１１）式に基づいて１単位時間後の状態共分散Ｐ´を算出する（ステップＳ５１）。
【０１１１】
なお、前記（１１）式における状態遷移行列Ｆには、前記（２１）式で与えられた行列を用いる。また、前記（１１）式におけるシステム雑音共分散Ｑは、等速度運動のモデル化誤差や、システムの外部から加えられるノイズ、環境温度、外部振動等による外乱を考慮して設定する行列である。例えば、水平面内の速度ｖx及びｖzに対して独立に誤差が生じる場合、システム雑音共分散Ｑは、下記（２２）式で与えられる行列を用いることができる。なお、ｑx、ｑzは非負の値である。
【０１１２】
【数２２】
そして、ステップＳ５０で更新され推定された状態量ｘ´及びステップＳ５１で更新された状態共分散Ｐ´は、予測推定手段２０2へ出力される。なお、この状態量ｘ´及び状態共分散Ｐ´は、状態量ｘ及び状態共分散Ｐとして、予測推定手段２０2に入力される。
【０１１４】
［予測推定ステップ２（鉛直方向運動の予測）］
次に、状態推定装置１Ｂは、予測推定手段２０2において以下の各ステップで、鉛直方向の等加速度運動による物体Ｔ１の未来における状態を推定する。
この予測推定手段２０2では、未来状態量更新手段２１が、予測推定手段２０1から入力される状態量ｘから、前記（１０）式に基づいて、未来の状態量ｘ´である物体Ｔ１の１単位時間後の鉛直方向運動による位置座標（ｐx，ｐy，ｐz）、速度（ｖx，ｖy，ｖz）及び加速度（αx，αy，αz）を推定する（ステップＳ６０）。
【０１１５】
なお、物体Ｔ１は鉛直方向において等加速度運動を行うことを仮定しているので、前記（１０）式における状態遷移行列Ｆは、１単位時間をΔｔとしたとき、下記（２３）式で与えられる行列を用いる。
【０１１６】
【数２３】
この（２３）式の状態遷移行列Ｆを用いることで、物体Ｔ１のｙ座標位置ｐy及び鉛直方向の速度ｖyのみが更新され、新しい鉛直方向の位置及び速度が予測される。なお、それ以外の成分は更新されない。
そして、未来状態共分散更新手段２２が、予測推定手段２０1から入力される状態共分散Ｐから、前記（１１）式に基づいて１単位時間後の状態共分散Ｐ´を算出する（ステップＳ６１）。
【０１１８】
なお、前記（１１）式における状態遷移行列Ｆには、前記（２３）式で与えられた行列を用いる。また、前記（１１）式におけるシステム雑音共分散Ｑは、例えば、鉛直方向の速度ｖyに誤差が生じる場合、下記（２４）式で与えられる行列を用いる。
【０１１９】
【数２４】
ここで更新された状態量ｘ´及び状態共分散Ｐ´は、遅延手段３０を介して濾波推定手段１０1へ出力される。なお、この状態量ｘ´及び状態共分散Ｐ´は、状態量ｘ及び状態共分散Ｐとして、濾波推定手段１０1に入力される。
また、この段階で生成された状態量ｘ及び状態共分散Ｐを未来の内部状態として出力する（ステップＳ７０）。
【０１２１】
このように動作することで、状態推定装置１Ｂは、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を逐次更新することが可能になる。
【０１２２】
以上、状態推定装置１Ｂを用いた物体位置推定システム３の構成及び動作について説明したように、この状態推定装置１Ｂは、濾波推定手段１０を多段接続することで、種々の状態を含んだ現在の内部状態を逐次更新することを可能にした。このように、濾波推定手段１０を多段接続することで、例えば、カメラＡが故障した場合、濾波推定手段１０1を除去し、濾波推定手段１０2の入力を遅延手段３０の出力と短絡させることで、濾波推定手段１０を２段構成とする変更も容易に行うことができる。さらに、第３のカメラＣ（図示せず）を用いた新たな濾波推定手段１０4（図示せず）を追加することも容易である。
【０１２３】
また、状態推定装置１Ｂは、予測推定手段２０を多段接続することで、種々の状態を含んだ未来の内部状態を逐次更新することを可能にした。このように、予測推定手段２０を多段接続することで、例えば、物体Ｔ１の鉛直方向の動きを等速度運動と仮定したい場合には、予測推定手段２０2を除去し、代わりに、下記（２５）式及び（２６）式（ｑyは非負の値）に示す状態遷移行列Ｆ及びシステム雑音共分散Ｑを用いた予測推定手段２０3（図示せず）を挿入することで、容易に実現することができる。
【０１２４】
【数２５】
【数２６】
なお、物体位置推定システム３においては、２台のカメラＡ、カメラＢ及び距離計Ｍを直線上に配置したが、この配置及び機器の台数はこれに限ったものではない。例えば、距離計Ｍを３次元座標上で座標（ｓx，ｓy，ｓz）に配置した場合には、濾波推定手段１０3において、観測関数ｈ（ｘ）には前記（１８）式の代わりに下記（２７）式を用い、観測行列Ｈには前記（２０）式の代わりに下記（２８）式を用いることとすればよい。
【０１２７】
【数２７】
【数２８】
また、状態推定装置１Ｂから、予測推定手段２０（２０1，２０2）を取り除き、濾波推定手段１０3の出力と遅延手段３０の入力とを短絡させた構成とし、状態量ｘを前記（１２）式の代わりに、推定すべき物体Ｔ１の３次元の位置座標（ｐx，ｐy，ｐz）のみを示す下記（２９）式とし、濾波推定手段１０（１０1，１０2，１０3）の観測行列Ｈを前記（１５）式、（１７）式及び（２０）式の代わりに下記（３０）式、（３１）式及び（３２）式を用いることで、現在の物体Ｔ１の位置座標のみを推定する現在状態推定装置として構成することも可能である。
【０１３０】
【数２９】
【数３０】
【数３１】
【数３２】
（システム構成例２：状態推定装置を用いた人工衛星軌道推定システム）
次に、図７を参照して、状態推定装置を用いた人工衛星軌道推定システム５の構成例について説明する。図７は、人工衛星の電波遅延時間とドップラーシフトとを観測し、その観測信号に基づいて、人工衛星の現在の軌道を推定する人工衛星軌道推定システム５の構成を示したものである。この人工衛星軌道推定システム５は、遅延時間測定機Ｅと、周波数計Ｆとを備えたセンサ系Ｓ２と、状態推定装置１Ｃとを備える構成とした。
【０１３５】
遅延時間測定機Ｅは、地球上において人工衛星Ｔ２による電波の遅延時間を測定するものである。すなわち、地球上から電波を発信し、その電波が人工衛星Ｔ２に反射して返ってくるまでの時間を計測するものである。
【０１３６】
周波数計Ｆは、人工衛星Ｔ２に電磁波を照射して、その散乱波の周波数、いわゆるドップラーシフトを測定するものである。この周波数は人工衛星Ｔ２の視線速度に比例して変化するものである。
【０１３７】
状態推定装置１Ｃは、濾波推定手段１０（１０1，１０2）と、予測推定手段２０（２０1）と、遅延手段３０と、を備える構成とした。なお、この状態推定装置１Ｃは、図１で説明した状態推定装置１において、濾波推定手段１０及び予測推定手段２０の数を限定したもので、構成そのものを変更したものではない。
【０１３８】
この濾波推定手段１０1は、遅延時間測定機Ｅで測定される電波遅延時間を観測信号として、現在の内部状態を推定して出力するものである。また、濾波推定手段１０2は、周波数計Ｆで測定されるドップラーシフトを観測信号として、現在の内部状態を推定して出力するものである。
【０１３９】
この状態推定装置１Ｃは、人工衛星Ｔ２のケプラーの軌道６要素を内部状態とする。なお、この軌道６要素とは、軌道長半径、離心率、軌道傾斜角、昇交点赤経、近地点引数及び平均近点離角である。ここで、元期及び現在時刻、人工衛星軌道推定システム５の観測点の緯度及び経度、地球の自転角速度、並びに、人工衛星からの電波の周波数が既知であるとすると、観測すべき電波遅延時間及びドップラーシフトは、軌道６要素から求めることができる（数式は省略）。
【０１４０】
そこで、濾波推定手段１０1では、軌道６要素から電波遅延時間への変換を表す関数を前記（４）式の観測関数ｈ（ｘ）として用い、この観測関数ｈ（ｘ）を軌道６要素により偏微分した結果の行列を前記（６）式の観測行列Ｈとして用いる。また、濾波推定手段１０2では、軌道６要素からドップラーシフトへの変換を表す関数を前記（４）式の観測関数ｈ（ｘ）として用い、この観測関数ｈ（ｘ）を軌道６要素により偏微分した結果の行列を前記（６）式の観測行列Ｈとして用いる。
【０１４１】
なお、予測推定手段２０1では、人工衛星軌道推定システム５における推定すべき状態ベクトルの次元（軌道６要素の６次元）が、観測の総次元数（電波遅延時間とドップラーシフトの２次元）よりも高いため、時間経過に伴う状態の変動（ダイナミクス）を仮定する。
これによって、状態推定装置１Ｃは、人工衛星Ｔ２の現在の軌道を逐次推定することができる。
【０１４２】
なお、ここでは、状態推定装置を用いたシステムとして、物体位置推定システム３（図４）及び人工衛星軌道推定システム（図７）を例に説明したが、これ以外にも種々のシステムにおいて本発明における状態推定装置を利用することが可能である。
【０１４３】
例えば、橋桁に作用する空気力等を測定することで長大橋梁のフラッターを推定したり（土木工学）、超音波画像から臓器の変位を推定したり（医学）、株価データにより株価を推定したり（経済学）、天体表面上のクレータ等の画像上における位置観測により天体の自転速度やニューテーション運動を推定したり（惑星物理学）、ＧＰＳ衛星からの電波、タイヤの回転速度及びジャイロセンサ情報に基づいて車両やロボットの位置や速度を推定したり（ロボット工学、計測工学）、距離計測及び画像に基づいて航空機や宇宙機の位置を推定したり（航空工学、宇宙工学）することも可能である。
【０１４４】
あるいは、レーダによる電波の遅延時間やドップラーシフト、電波到来方向の方向角に基づいてロケットやミサイルの軌道あるいは弾道を推定したり（宇宙工学）、気温、気圧、湿度といった気象データからの未来の気温、気圧、湿度等を予測したり（気象学）、河川流域における降雨量や流速の観測により河川の未来の水位を予測したり（土木工学、気象学）することも可能である。
このように、本発明における状態推定装置は、カルマンフィルタを用いて推定可能な種々の分野におけるシステムにおいて応用が可能である。
【０１４５】
（補足；濾波推定手段の定式化について）
以上、説明したように濾波推定手段１０は、あるゆる観測系に関して、定式化を行うことが可能である。この濾波推定手段１０の定式化は、カルマンフィルタ又は拡張カルマンフィルタの観測方程式の設計法と同様に行うことができる。このカルマンフィルタ又は拡張カルマンフィルタでは、全ての種類の観測量を１つの観測方程式で表現する必要があったが、濾波推定手段１０では、必ずしも１つの観測方程式で表現する必要はない。
【０１４６】
例えば、図４に示した距離計Ｍと、２台のカメラＡ及びカメラＢとにより物体Ｔ１の観測を行う場合、カルマンフィルタ又は拡張カルマンフィルタでは、観測関数ｈ（ｘ）を下記（３３）式で設定しなければならない。
【０１４７】
【数３３】
しかし、濾波推定手段１０では、前記した（１３）式、（１６）式及び（１８）式に示したように、センサ毎に観測関数ｈ（ｘ）を分割して定式化することができる。
【０１４９】
このように、他の観測系においても、カルマンフィルタ又は拡張カルマンフィルタと同様の定式化を行うか、又はこれを分割して定式化を行えばよい。なお、この分割は、例えば、センサ毎に分割することも可能であるが、複数のセンサを組にして、その組毎に分割を行ってもよい。
【０１５０】
例えば、図４に示した距離計Ｍと、２台のカメラＡ及びカメラＢとにより物体Ｔ１の観測を行う場合、２台のカメラＡ及びカメラＢからの観測信号を１つの濾波推定手段１０1へ入力し、距離計Ｍからの観測信号を他の濾波推定手段１０2へ入力することができる。この場合、濾波推定手段１０1の観測関数ｈ（ｘ）は、下記（３４）式で示した関数を用いる。
【０１５１】
【数３４】
一方、濾波推定手段１０2の観測関数ｈ（ｘ）は、前記（１８）式で示した関数を用いればよい。
【０１５３】
（補足；予測推定手段の定式化について）
また、予測推定手段２０については、線形の状態遷移の場合について例示して説明したが、拡張カルマンフィルタで用いられる状態遷移方程式の場合と同様に、非線形の場合にも適用することが可能である。この場合、前記（１０）式の代わりに、下記（３５）式の定式化を行えばよい。
【０１５４】
【数３５】
ここで、関数φは現時点（ｔ）の状態量ｘから次時点（ｔ＋Δｔ）の状態量ｘ´への遷移を表す状態遷移関数である。例えば、ｔを連続量の時刻として、状態量ｘが下記（３６）式の微分方程式を満たす場合、この（３６）式をルンゲクッタ（Ｒｕｎｇｅ−Ｋｕｔｔａ）法等の数値積分法によって解くことで、現時点（ｔ）の状態量ｘから次時点（ｔ＋Δｔ）の状態量ｘ´を求めることができる。この数値積分による状態量ｘから状態量ｘ´への変換が、前記（３５）式における関数φである。
【０１５６】
【数３６】
この関数φを実現する積分回路を、未来状態量更新手段２１（図３参照）とすることで、非線形の予測推定手段２０を構成することが可能になる。
ここで、ケプラー運動を例にして、非線形の状態遷移について説明する。例えば、中心天体の周りで軌道運動を行うある天体の３次元位置（ｒx，ｒy，ｒz）及び３次元速度（ｖx，ｖy，ｖz）により、その天体の状態推定を行う場合、状態量ｘを下記（３７）式で定義すると、状態量ｘは下記（３８）式を満たす。
【０１５８】
【数３７】
【数３８】
ここで、Ｇは万有引力定数、Ｍは中心天体の質量である。この（３８）式に基づいて、ルンゲクッタ法等の数値積分法によって、現時点（ｔ）の状態量ｘから次時点（ｔ＋Δｔ）の状態量ｘ´を求めることができる。この数値積分による状態量ｘから状態量ｘ´への変換が、前記（３５）式における関数φである。
一方、前記（１１）式における状態遷移行列Ｆは、下記（３９）式の微分方程式をルンゲクッタ法等の数値積分法により解くことで求めることができる。
【０１６１】
【数３９】
このように、予測推定手段２０は、非線形の状態遷移においても、拡張カルマンフィルタにおける状態遷移方程式の場合と同様に設計し、定式化することができる。
【０１６３】
【発明の効果】
以上説明したとおり、本発明に係る状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置では、以下に示す優れた効果を奏する。
【０１６４】
請求項１、請求交５又は至請求項６に記載の発明によれば、濾波推定と予測推定とを自由な組合せ及び順序で動作させることができるため、入力される観測信号の種類や、推定すべき内部状態の性質に応じて、適当な濾波推定手段（濾波推定ステップ）又は予測推定手段（予測推定ステップ）だけを選択して処理することができる。これにより、必要な処理だけを実行することになり、全体処理を高速化し、ハードウェア資源の節約を行うことができる。
【０１６５】
また、本発明は、濾波推定と予測推定とを自由な組合せ及び順序によって、異なる推定を行う処理を容易に再構成することができるため、様々な観測信号や予測の目的に対して、汎用性及び再利用性が高いという効果がある。
【０１６６】
さらに、従来のカルマンフィルタによる内部状態の濾波推定において、全ての観測信号をまとめて処理しなければならなかったところを、本発明は、複数の濾波推定手段（濾波推定ステップ）に分割して処理を行うことができるため、処理を行う演算量を抑えることができ、メモリの消費を抑えかつ高速で処理を行うことができる。
【０１６７】
また、従来のカルマンフィルタによる内部状態の予測推定において、１単位時間未来の予測を一度の処理で行っていたところを、本発明は、複数の予測推定手段（予測推定ステップ）に分割して処理を行うことができるため、複雑な状態遷移モデルであっても、動作が簡易な状態推定モデルに分割することができる。
【０１６８】
さらにまた、本発明は、濾波推定手段（濾波推定ステップ）又は予測推定手段（予測推定ステップ）を自由に追加したり、順序を変えたり、あるいは、削除することができるため、時々刻々と動作している状態であっても、容易に構成を変更することができ、柔軟なシステムを構成することが可能になる。
【０１６９】
請求項２乃至請求項４に記載の発明によれば、従来のカルマンフィルタによる状態推定に用いられる観測方程式や、状態遷移方程式を利用することが可能になり、濾波推定手段及び予測推定手段を容易に定式化することができる。
【０１７０】
請求項７に記載の発明によれば、従来のカルマンフィルタによる内部状態の濾波推定において、全ての観測信号をまとめて処理しなければならなかったところを、本発明は、複数の濾波推定手段に分割して処理を行うことができるため、処理を行う演算量を抑えることができ、メモリの消費を抑えかつ高速で処理を行うことができる。
【０１７１】
請求項８に記載の発明によれば、従来のカルマンフィルタによる内部状態の予測推定において、１単位時間未来の予測を一度の処理で行っていたところを、本発明は、複数の予測推定手段に分割して処理を行うことができるため、複雑な状態遷移モデルであっても、動作が簡易な状態推定モデルに分割することができる。
【図面の簡単な説明】
【図１】本発明の実施の形態に係る状態推定装置の全体構成を示すブロック図である。
【図２】本発明の実施の形態に係る状態推定装置の濾波推定手段の内部構成を示すブロック図である。
【図３】本発明の実施の形態に係る状態推定装置の予測推定手段の内部構成を示すブロック図である。
【図４】本発明の実施の形態に係る状態推定装置を用いた物体位置推定システムの構成例を示すブロック図である。
【図５】物体位置推定システムにおける状態推定装置の動作を示すフローチャート（１／２）である。
【図６】物体位置推定システムにおける状態推定装置の動作を示すフローチャート（２／２）である。
【図７】本発明の実施の形態に係る状態推定装置を用いた人工衛星軌道推定システムの構成例を示すブロック図である。
【符号の説明】
１、１Ｂ、１Ｃ……状態推定装置（現在状態推定装置、未来状態推定装置）
１０……濾波推定手段
１１……観測量推定手段
１２……観測推定誤差算出手段
１３……感度算出手段
１４……現在状態更新手段
１４ａ……現在状態量更新部
１４ｂ……現在状態共分散更新部
２０……予測推定手段
２１……未来状態量更新手段
２２……未来状態共分散更新手段
Claims (8)
- 観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定する状態推定装置であって、
前記観測信号と先に推定した前記内部状態と予め設定された推定規則とに基づいて、現在の内部状態を推定する濾波推定手段を、前記観測信号の種類に対応して多段接続し、
前記内部状態と予め設定されたその内部状態を遷移させる状態遷移規則とに基づいて、未来の内部状態を推定する予測推定手段を、前記内部状態の種類に対応して多段接続して構成することを特徴とする状態推定装置。 - 前記観測信号が、観測により得られる観測量と観測時に混入する観測雑音の共分散である観測雑音共分散とからなり、前記内部状態が、内部に保持している状態量とその状態量の共分散である状態共分散とからなっていることを特徴とする請求項１に記載の状態推定装置。
- 前記濾波推定手段が、
予め設定された推定規則に基づいて、前記状態量から前記観測量の推定量である観測推定量を推定する観測量推定手段と、
この観測量推定手段で推定した前記観測推定量と前記観測量との差分により、観測推定誤差を算出する観測推定誤差算出手段と、
前記観測雑音共分散と前記状態共分散とに基づいて、濾波推定の感度を算出する感度算出手段と、
この感度算出手段で算出した前記感度と、前記観測推定誤差算出手段で算出した前記観測推定誤差とに基づいて、前記状態量及び前記状態共分散を更新する現在状態更新手段と、
を備えていることを特徴とする請求項２に記載の状態推定装置。 - 前記予測推定手段が、
予め設定された内部状態を遷移させる状態遷移規則に基づいて、前記状態量を未来の状態量に更新する未来状態量更新手段と、
前記状態遷移規則とシステム内部で発生するシステム雑音の共分散であるシステム雑音共分散とに基づいて、前記状態共分散を未来の状態共分散に更新する未来状態共分散更新手段と、
を備えていることを特徴とする請求項２又は請求項３に記載の状態推定装置。 - 観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定する状態推定方法であって、
前記観測信号と先に推定した前記内部状態と予め設定された推定規則とに基づいて、前記観測信号の種類に対応して多段接続した濾波推定手段により、現在の内部状態を推定する濾波推定ステップと、
前記内部状態と予め設定されたその内部状態を遷移させる状態遷移規則とに基づいて、前記内部状態の種類に対応して多段接続した予測推定手段により、未来の内部状態を推定する予測推定ステップと、
を含むことを特徴とする状態推定方法。 - 観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定するために、コンピュータを、
前記観測信号と先に推定した前記内部状態と予め設定された推定規則とに基づいて、前記観測信号の種類に対応して現在の内部状態を推定する多段接続された濾波推定手段、
前記内部状態と予め設定されたその内部状態を遷移させる状態遷移規則とに基づいて、前記内部状態の種類に対応して未来の内部状態を推定する多段接続された予測推定手段、
として機能させることを特徴とする状態推定プログラム。 - 観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの現在の内部状態を推定する現在状態推定装置であって、
前記観測信号と先に推定した前記内部状態と予め設定された推定規則とに基づいて、前記現在の内部状態を推定する濾波推定手段を、前記観測信号の種類に対応して多段接続し て構成することを特徴とする現在状態推定装置。 - 内部状態が時系列に推移するシステムの未来の内部状態を推定する未来状態推定装置であって、
先に推定した内部状態と予め設定されたその内部状態を遷移させる状態遷移規則とに基づいて、前記未来の内部状態を推定する予測推定手段を、前記内部状態の種類に対応して多段接続して構成することを特徴とする未来状態推定装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002225476A JP4072017B2 (ja) | 2002-08-02 | 2002-08-02 | 状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002225476A JP4072017B2 (ja) | 2002-08-02 | 2002-08-02 | 状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2004070452A JP2004070452A (ja) | 2004-03-04 |
| JP4072017B2 true JP4072017B2 (ja) | 2008-04-02 |
Family
ID=32013091
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2002225476A Expired - Fee Related JP4072017B2 (ja) | 2002-08-02 | 2002-08-02 | 状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP4072017B2 (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103517789A (zh) * | 2011-05-12 | 2014-01-15 | 株式会社Ihi | 运动预测控制装置和方法 |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4551245B2 (ja) * | 2005-03-03 | 2010-09-22 | 日本放送協会 | オンライン座標値修正装置およびオンライン座標値修正プログラム |
| JP4820132B2 (ja) * | 2005-09-06 | 2011-11-24 | 日本放送協会 | 状態推定装置及びそのプログラム、並びに、統合状態推定システム及び統合状態推定装置 |
| JP5560154B2 (ja) * | 2010-09-30 | 2014-07-23 | 日本放送協会 | モデルパラメータ推定装置およびそのプログラム |
| JP2014041565A (ja) * | 2012-08-23 | 2014-03-06 | Nippon Telegr & Teleph Corp <Ntt> | 時系列データ解析装置、方法、及びプログラム |
| JP2014041547A (ja) * | 2012-08-23 | 2014-03-06 | Nippon Telegr & Teleph Corp <Ntt> | 時系列データ解析装置、方法、及びプログラム |
-
2002
- 2002-08-02 JP JP2002225476A patent/JP4072017B2/ja not_active Expired - Fee Related
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103517789A (zh) * | 2011-05-12 | 2014-01-15 | 株式会社Ihi | 运动预测控制装置和方法 |
| US9108321B2 (en) | 2011-05-12 | 2015-08-18 | Ihi Corporation | Motion prediction control device and method |
| CN103517789B (zh) * | 2011-05-12 | 2015-11-25 | 株式会社Ihi | 运动预测控制装置和方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2004070452A (ja) | 2004-03-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106767837B (zh) | 基于容积四元数估计的航天器姿态估计方法 | |
| Ahn et al. | Fast alignment using rotation vector and adaptive Kalman filter | |
| CN112146655B (zh) | 一种BeiDou/SINS紧组合导航系统弹性模型设计方法 | |
| Kaniewski et al. | Estimation of UAV position with use of smoothing algorithms | |
| CN111965685B (zh) | 一种基于多普勒信息的低轨卫星/惯性组合导航定位方法 | |
| CN108919283B (zh) | 一种星上自主的非合作目标相对导航方法和系统 | |
| JP4072017B2 (ja) | 状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置 | |
| Jingsen et al. | Integrating extreme learning machine with Kalman filter to bridge GPS outages | |
| CN115356754A (zh) | 一种基于gnss和低轨卫星的组合导航定位方法 | |
| CN114510076A (zh) | 基于无迹变换的目标协同探测与制导一体化方法及系统 | |
| JP5219547B2 (ja) | 車載ナビゲーションシステム及びナビゲーション方法 | |
| CN112525188B (zh) | 一种基于联邦滤波的组合导航方法 | |
| CN110736459B (zh) | 惯性量匹配对准的角形变测量误差评估方法 | |
| Taghizadeh et al. | A low-cost integrated navigation system based on factor graph nonlinear optimization for autonomous flight | |
| CN117053782A (zh) | 一种水陆两栖机器人组合导航方法 | |
| CN114897942B (zh) | 点云地图的生成方法、设备及相关存储介质 | |
| CN116222541A (zh) | 利用因子图的智能多源组合导航方法及装置 | |
| Proletarsky et al. | Method for improving accuracy of INS using scalar parametric identification | |
| CN114705223A (zh) | 多移动智能体在目标跟踪中的惯导误差补偿方法及系统 | |
| CN115113646A (zh) | 基于卡尔曼滤波的卫星编队平根状态连续估计方法及系统 | |
| Bhusal et al. | Generalized Polynomial Chaos-based Ensemble Kalman Filtering for Orbit Estimation | |
| JPH01500714A (ja) | 分散型カルマンフィルタ | |
| Gamagedara et al. | Unscented Kalman Filter for INS/GNSS Data Fusion with Time Delay | |
| Stepanyan et al. | Adaptive multi-sensor information fusion for autonomous urban air mobility operations | |
| Wang et al. | Application of a Sigma-point Kalman filter for alignment of MEMS-IMU |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20050207 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20070123 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20070425 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070620 |
|
| 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: 20080115 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20080118 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110125 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120125 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130125 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140125 Year of fee payment: 6 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| LAPS | Cancellation because of no payment of annual fees |