JP5229126B2 - 目標追尾処理器及びそれに用いる誤差共分散行列の補正方法 - Google Patents

Description

本発明は目標追尾処理器及びそれに用いる誤差共分散行列の補正方法に関し、特にカルマンフィルタを用いて目標追尾を行う目標追尾処理器に関する。

まず、本発明に関連する技術分野及び前提を説明する。ある物体が移動していてその位置、速度及び針路の真値（本当の値）は分からない。ここにセンサを設置し、その物体の位置を観測することができるとする。但し、センサの観測値には誤差が含まれている。ここでは、位置をｘｙ座標の２次元平面とし、高度は考えないこととする。

この観測（時刻情報及び位置情報を得る）からその物体の位置、速度及び針路を推定することが目標追尾である。目標追尾分野では、カルマンフィルタを利用した目標追尾が現在主流である。

このカルマンフィルタを用いた目標追尾（状態変数の算出）においては、観測値を入力することで一連の処理を行って状態変数を算出する。この状態変数が、推定した目標諸元である。また、観測値を入力することで、一連の処理を行い、誤差共分散を算出する。この誤差共分散は、カルマンゲインの算出を経て推定状態変数の算出に使用される。

さらに、誤差共分散は、状態変数の分散を表し、状態変数の確からしさを測る手段として利用したり、得られた観測値がその目標のものであるか否かを判断するゲート検査に利用することができる。

ここで、誤差共分散について補足説明する。誤差共分散は、状態変数の分散を表し（つまり状態変数が持つ値の分布の大きさの情報を持ち）、状態変数の品質であるトラッククオリティ算出に利用したり、得られた観測値がその目標のものであるか否かを判断するゲート検査に利用することができる。

続いて、前提となる記号や数式について説明する。目標の位置ｘ，ｙ及び速度ｘドット，ｙドットを状態変数Ｘとしてカルマンフィルタに適用する。状態変数Ｘは、

・・・（１）
という式で示される行列である。因みに、カルマンフィルタは行列の計算式で表現される。

位置ｘは、東向きの横軸、位置ｙは、北向きの縦軸とする。センサは、ｘｙ平面上に設置し、センサが観測したその目標までの距離をｒ、センサが観測したその目標までの北を基準とした時計回りの方位をθとする。距離ｒ及び方位θを座標変換し、ｘｙ座標で表現したものを観測値Ｚとする。例えば、観測値Ｚは、

・・・（２）
という式で表される。

カルマンフィルタでは予測と推定とを行うが、この予測したものは記号の上部に「〜」を付加し、〜はチルダと呼ぶ。また、推定したものは記号の上部に「＾」を付加し、＾はハットと呼ぶ。

行列の右肩にＴが付加されているものがあるが、Ｔは行列の転置を表す。また、添え字のｋは逐次処理の時点を表し、１回目の計算ならｋ＝１であり、回数を重ねる毎に１加算される。ｋは整数である。ｋ−１ならば、１回前の値という意味である。テキスト文中では添え字にできないので括弧に入れて表現する。例えば、状態変数Ｘのｋ回目は、Ｘ（ｋ）とテキスト文で表現する。

本発明に関連する目標追尾処理器としては、その構成例の一例を図３に示す。図３において、本発明に関連する目標追尾処理器は、観測時刻入力部１１と、観測値入力部１２と、時刻差分算出部３０と、状態変数及び誤差共分散時刻算出部３１と、状態遷移算出部３２と、駆動雑音算出部３３と、観測雑音算出部４１と、カルマンゲイン算出部４２と、予測状態変数算出部５１と、推定状態変数算出部５２と、予測誤差共分散算出部６１と、推定誤差共分散算出部６２とから構成される。

観測時刻入力部１１は、図示せぬセンサから観測時刻ｔｍを得て、その観測時刻ｔｍを時刻差分算出部３０へ送出する。時刻差分算出部３０は、今回の観測時刻ｔｍ（ｋ）と前回の観測時刻ｔｍ（ｋ−１）とから時刻差分ΔＴ（ｋ）を、

・・・（３）
という式を用いて算出する。時刻差分算出部３０は、算出した時刻差分ΔＴ（ｋ）を状態変数及び誤差共分散時刻算出部３１と、状態遷移算出部３２と、駆動雑音算出部３３とにそれぞれ送出する。

状態遷移算出部３２は、状態遷移モデルと時刻差分ΔＴとから状態遷移Φを算出し、算出した状態遷移Φを予測状態変数算出部５１と予測誤差共分散算出部６１とに送出する。

ここで、状態遷移モデルとは、状態変数Ｘが時間と共にどのように変化するかを仮定するモデルであり、例えば目標が等速直進すると仮定するならば状態遷移Φは、

・・・（４）
という式を用いて算出される。

駆動雑音算出部３３は、時刻差分ΔＴと駆動雑音パラメータＡとから駆動雑音Ｕを算出し、算出した駆動雑音Ｕを予測誤差共分散算出部６１に送出する。例えば、駆動雑音をｘ方向の加速度に相当する雑音とｙ方向の加速度に相当する雑音だと仮定すると、駆動雑音Ｕは、

・・・（５）

・・・（６）

・・・（７）
という式を用いて算出される。

次に、状態変数及び誤差共分散時刻算出部３１は、時刻差分ΔＴ（ｋ）と、前回の状態変数及び誤差共分散時刻ｔ（ｋ−１）とから今回の状態変数及び誤差共分散時刻ｔ（ｋ）を、

・・・（８）
という式を用いて算出する。

予測状態変数算出部５１は、状態遷移Φと前回の推定状態変数Ｘ（ｋ−１）ハットとから今回の予測状態変数Ｘ（ｋ）チルダを、

・・・（９）
という式を用いて算出し、算出した今回の予測状態変数Ｘ（ｋ）チルダを推定状態変数算出部５２に送出する。

予測誤差共分散算出部６１は、状態遷移Φと駆動雑音Ｕと前回の推定誤差共分散Ｐ（ｋ−１）ハットとから今回の予測誤差共分散Ｐ（ｋ）チルダを、

・・・（１０）
という式を用いて算出し、算出した今回の予測誤差共分散Ｐ（ｋ）チルダをカルマンゲイン算出部４２及び推定誤差共分散算出部６２に送出する。

観測値入力部１２は、センサからの観測値Ｚ［（２）式参照］を得て、観測雑音算出部４１及び推定状態変数算出部５２に送出する。観測雑音算出部４１は、観測値Ｚと観測雑音パラメータＢとから観測雑音Ｗを算出し、算出した観測雑音Ｗをカルマンゲイン算出部４２へ送る。

例えば、観測雑音Ｗをセンサの距離誤差標準偏差σｒ及び方位誤差標準偏差σθで与えるとすると、観測雑音Ｗは、

・・・（１１）

・・・（１２）

・・・（１３）
という式を用いて算出される。但し、センサから見た目標までの距離をｒ、センサから見た目標の方位をθとする（方位は、北を基準とした時計回りで表す）。

カルマンゲイン算出部４２は、観測雑音Ｗと予測誤差共分散ＰチルダとからカルマンゲインＫを、

・・・（１４）

・・・（１５）
という式を用いて算出し、算出したカルマンゲインＫを推定状態変数算出部５２及び推定誤差共分散算出部６２に送出する。

最後に、推定状態変数算出部５２は、予測状態変数Ｘチルダと観測値ＺとカルマンゲインＫとから推定状態変数Ｘハットを、

・・・（１６）
という式と（１４）式とを用いて算出する。

推定誤差共分散算出部６２は、予測誤差共分散ＰチルダとカルマンゲインＫとから推定誤差共分散Ｐハットを、

・・・（１７）

・・・（１８）
という式と（１４）式とを用いて算出する。

ここで、カルマンフィルタを用いた目標追尾装置としては、下記の特許文献１〜３に記載の技術がある。尚、これら特許文献１〜３に記載の技術では、カルマンフィルタに関して説明している。

特開２００５−０７７１６７号公報 特開２００５−２７４３００号公報 特開２００８−２９８７３８号公報

本発明に関連する目標追尾処理では、観測値が定期的またはある頻度で入力されるのが通常の状態であるが、何らかの理由で観測値の入力頻度が低下した場合、または観測値の入力が無くなった場合、誤差共分散が過度に大きくなったり、異常な値となったりする。

そのため、本発明に関連する目標追尾処理では、
（１）算出した状態変数が安定しない。例えば、再び観測値が入力された際に推定状態変数を算出するが、その推定状態変数が前回の値と比べて極端に大きく変動する。
（２）誤差共分散を利用した処理にて期待しない結果を生じる。例えば、ゲート検査にて、過度に大きくなった誤差共分散を使用したため、その目標から遠く離れた位置に観測した別の目標の観測値を、その目標のものであると誤った判定をしてしまう。また、その結果その目標が多数の観測値と関係付けられ計算負荷が増大する。
（３）処理の計算式にてエラーを発生し、処理の実行が不可能となる。
というような悪影響が生じることがある。

そこで、本発明の目的は上記の問題点を解消し、観測値の入力頻度が低下した場合、または観測値の入力が無くなった場合においても、誤差共分散が過度に大きくなったり、異常な値となったりすることを防止することができる目標追尾処理器及びそれに用いる誤差共分散行列の補正方法を提供することにある。

本発明による目標追尾処理器は、目標の位置を観測するセンサの観測値からその目標の位置、速度及び針路を状態変数として推定するカルマンフィルタを用いた目標追尾を行う目標追尾処理器であって、
前記状態変数の分散を表す誤差共分散を補正する手段を備え、
前記誤差共分散を補正する手段は、ダミーの観測値を作成する作成手段を含み、前記作成手段で作成したダミーの観測値を用いて誤差共分散を補正し、
前記作成手段で作成したダミーの観測値から観測雑音及びカルマンゲインの算出を経て、推定誤差共分散を算出している。

本発明による誤差共分散行列の補正方法は、目標の位置を観測するセンサの観測値からその目標の位置、速度及び針路を状態変数として推定するカルマンフィルタを用いた目標追尾を行う目標追尾処理器に用いる誤差共分散行列の補正方法であって、
前記目標追尾処理器が、前記状態変数の分散を表す誤差共分散を補正する処理を実行し、
前記誤差共分散を補正する処理に、ダミーの観測値を作成する作成処理を含み、
前記目標追尾処理器が、前記作成処理で作成したダミーの観測値を用いて誤差共分散を補正し、
前記目標追尾処理器が、前記作成処理で作成したダミーの観測値から観測雑音及びカルマンゲインの算出を経て、推定誤差共分散を算出している。

本発明は、上記のような構成及び動作とすることで、観測値の入力頻度が低下した場合、または観測値の入力が無くなった場合においても、誤差共分散が過度に大きくなったり、異常な値となったりすることを防止することができるという効果が得られる。

本発明の第１の実施の形態による目標追尾処理器の構成例を示す図である。 本発明の第１の実施の形態による目標追尾処理器の動作例を示すフローチャートである。 本発明に関連する目標追尾処理器の構成例を示す図である。

次に、本発明の実施の形態について図面を参照して説明する。はじめに、本発明の技術分野や前提について説明する。

ある物体が移動していてその位置、速度及び針路の真値（本当の値）は分からない。ここにセンサを設置し、その物体の位置を観測することができるとする。但し、センサの観測値には誤差が含まれている。ここでは位置をｘｙ座標の２次元平面とし、高度は考えないこととする。

この観測（時刻情報と位置情報を得る）からその物体の位置、速度及び針路を推定することが目標追尾である。目標追尾分野では、カルマンフィルタを利用した目標追尾が現在主流である。本発明は、このカルマンフィルタを利用した目標追尾に改良を加えたものである。

本発明では、カルマンフィルタを用いた目標追尾（状態変数の算出）に関して、誤差共分散を補正する仕組み（ダミーの観測値を作成することで実現する）を用意することで、観測値が頻繁に入力されなくなった場合、または入力されなくなった場合でも、誤差共分散が過度に大きくなることを防止し、状態変数の安定性を向上させている。

ここで、誤差共分散について補足説明する。誤差共分散は、状態変数の分散を表し（つまり状態変数が持つ値の分布の大きさの情報を持ち）、状態変数の品質であるトラッククオリティ算出に利用したり、得られた観測値がその目標のものであるか否かを判断するゲート検査に利用することができる。

続いて、前提となる記号や数式について説明する。目標の位置ｘ，ｙ及び速度ｘドット，ｙドットを状態変数Ｘとしてカルマンフィルタに適用する。状態変数Ｘは、上記の（１）式で示される行列である。因みに、カルマンフィルタは行列の計算式で表現される。

位置ｘは東向きの横軸、位置ｙは北向きの縦軸とする。センサはｘｙ平面上に設置し、センサが観測したその目標までの距離をｒ、センサが観測したその目標までの北を基準とした時計回りの方位をθとする。距離ｒ及び方位θを座標変換し、ｘｙ座標で表現したものを観測値Ｚとする［上記の（２）式参照］。

カルマンフィルタでは、予測と推定とを行うが、この予測したものは記号の上部に「〜」を付加し、〜はチルダと呼ぶ。また、推定したものは記号の上部に「＾」を付加し、＾はハットと呼ぶ。行列の右肩にＴが付加されているものがあるが、Ｔは行列の転置を表す。

また、添え字のｋは逐次処理の時点を表し、１回目の計算ならｋ＝１であり、回数を重ねる毎に１加算される。ｋは整数である。ｋ−１なら１回前の値という意味である。テキスト文中では、添え字にできないので括弧に入れて表現する。例えば、状態変数Ｘのｋ回目は、Ｘ（ｋ）とテキスト文で表現する。

本発明による目標追尾処理器は、上述した本発明に関連する目標追尾処理器に、ダミー作成起動部、ダミー観測時刻作成部及びダミー観測値作成部を追加し、観測値を入力する代わりにダミー観測値を作成して入力することができる構成としている。

ダミー作成起動部は、ある期間、観測値の入力がないことを検出し、ダミー作成を起動させる。ダミー観測時刻作成部は、ダミー観測時刻を作成し、時刻差分算出部へ送る。ダミー観測値作成部は、ダミー観測値を作成し、観測雑音算出部及び推定状態変数算出部へ送る。

本発明では、このような構成のため、観測値の入力頻度が低下した場合、または観測値の入力が無くなった場合においても、ダミーの観測値を作成することで、観測雑音Ｗ及びカルマンゲインＫの算出を経て、推定誤差共分散Ｐハットをある頻度で算出することができる。そのため、本発明では、誤差共分散が過度に大きくなったり、異常な値となったりすることを防止することができる。

図１は本発明の第１の実施の形態による目標追尾処理器の構成例を示すブロック図である。図１において、本発明の第１の実施の形態による目標追尾処理器は、ダミー作成起動部２０、ダミー観測時刻作成部２１及びダミー観測値作成部２２を追加し、観測値を入力する代わりにダミー観測値を作成し入力することができる構成とした以外は図３に示す本発明に関連する目標追尾処理器と同様の構成となっており、同一構成要素には同一符号を付してある。尚、図１に示す目標追尾処理器には、トラッククオリティ算出もゲート検査も含まれていないが、これらについては関連技術として一般的に知られているので、その説明は省略する。

図１において、本発明の第１の実施の形態による目標追尾処理器は、観測時刻入力部１１と、観測値入力部１２と、ダミー作成起動部２０と、ダミー観測時刻作成部２１と、ダミー観測値作成部２２と、時刻差分算出部３０と、状態変数及び誤差共分散時刻算出部３１と、状態遷移算出部３２と、駆動雑音算出部３３と、観測雑音算出部４１と、カルマンゲイン算出部４２と、予測状態変数算出部５１と、推定状態変数算出部５２と、予測誤差共分散算出部６１と、推定誤差共分散算出部６２とから構成されている。

ダミー作成起動部２０は、ある期間観測値の入力がないことを検出した場合にダミー作成を起動させるため、ダミー観測時刻作成部２１及びダミー観測値作成部２２へトリガを送出する。

ダミー観測時刻作成部２１は、ダミー作成起動部２０からのトリガに応答してダミー観測時刻を作成し、そのダミー観測時刻を時刻差分算出部３０へ送出する。ダミー観測値作成部２２は、ダミー作成起動部２０からのトリガに応答してダミー観測値を作成し、そのダミー観測値を観測雑音算出部４１及び推定状態変数算出部５２へ送出する。

図２は本発明の第１の実施の形態による目標追尾処理器の処理動作を示すフローチャートである。これら図１及び図２を参照して本発明の第１の実施の形態による目標追尾処理器の処理動作について説明する。

まず、ダミーではなく、センサからの入力がある場合について説明する。この場合（図２ステップＡ１）、観測時刻入力部１１は、センサから観測時刻ｔｍを得ると、その観測時刻ｔｍを時刻差分算出部３０へ送出する（図２ステップＡ２）。

時刻差分算出部３０は、今回の観測時刻ｔｍ（ｋ）と前回の観測時刻ｔｍ（ｋ−１）とから時刻差分ΔＴ（ｋ）を上記の（３）式を用いて算出し、算出した時刻差分ΔＴ（ｋ）を状態変数及び誤差共分散時刻算出部３１と、状態遷移算出部３２と、駆動雑音算出部３３とに送出する。

状態遷移算出部３２は、状態遷移モデルと時刻差分ΔＴとから状態遷移Φを算出し、算出した状態遷移Φを予測状態変数算出部５１及び予測誤差共分散算出部６１に送出する。ここで、状態遷移モデルとは、状態変数Ｘが時間と共にどのように変化するかを仮定するモデルであり、例えば目標が等速直進すると仮定するならば、状態遷移Φは上記の（４）式を用いて算出される。

駆動雑音算出部３３は、時刻差分ΔＴと駆動雑音パラメータＡとから駆動雑音Ｕを算出し、算出した駆動雑音Ｕを予測誤差共分散算出部６１に送出する。例えば、駆動雑音をｘ方向の加速度に相当する雑音とｙ方向の加速度に相当する雑音だと仮定すると、駆動雑音Ｕは上記の（５）式〜（７）式を用いて算出される（図２ステップＡ４）。

次に、状態変数及び誤差共分散時刻算出部３１は、時刻差分ΔＴ（ｋ）と前回の状態変数及び誤差共分散時刻ｔ（ｋ−１）とから今回の状態変数及び誤差共分散時刻ｔ（ｋ）を、上記の（８）式を用いて算出する。

予測状態変数算出部５１は、状態遷移Φと前回の推定状態変数Ｘ（ｋ−１）ハットとから今回の予測状態変数Ｘ（ｋ）チルダを上記の（９）式を用いて算出し、算出した今回の予測状態変数Ｘ（ｋ）チルダを推定状態変数算出部５２へ送出する。

予測誤差共分散算出部６１は、状態遷移Φと駆動雑音Ｕと前回の推定誤差共分散Ｐ（ｋ−１）ハットとから今回の予測誤差共分散Ｐ（ｋ）チルダを上記の（１０）式を用いて算出し、算出した今回の予測誤差共分散Ｐ（ｋ）チルダをカルマンゲイン算出部４２及び推定誤差共分散算出部６２に送出する（図２ステップＡ５）。

続いて、ダミーではなく、センサからの入力がある場合（図２ステップＡ６）、観測値入力部１２は、センサからの観測値Ｚ［上記の（２）式参照］を得て、その観測値Ｚを観測雑音算出部４１及び推定状態変数算出部５２へ送出する（図２ステップＡ７）。

観測雑音算出部４１は、観測値Ｚと観測雑音パラメータＢとから観測雑音Ｗを算出し、算出した観測雑音Ｗをカルマンゲイン算出部４２へ送出する。例えば、観測雑音Ｗをセンサの距離誤差標準偏差σｒと方位誤差標準偏差σθとで与えるとすると、観測雑音Ｗは、上記の（１１）式〜（１３）式を用いて算出される。但し、センサから見た目標までの距離をｒ、センサから見た目標の方位をθとする（方位は、北を基準とした時計回りで表す）。

カルマンゲイン算出部４２は、観測雑音Ｗと予測誤差共分散ＰチルダとからカルマンゲインＫを上記の（１４）式及び（１５）式を用いて算出し、算出したカルマンゲインＫを推定状態変数算出部５２及び推定誤差共分散算出部６２へ送出する（図２ステップＡ９）。

最後に、推定状態変数算出部５２は、予測状態変数Ｘチルダと観測値ＺとカルマンゲインＫとから推定状態変数Ｘハットを上記の（１６）式及び（１４）式を用いて算出する。

推定誤差共分散算出部６２は、予測誤差共分散ＰチルダとカルマンゲインＫとから推定誤差共分散Ｐハットを上記の（１７）式と（１８）式と（１４）式とを用いて算出する（図２ステップＡ１０）。

次に、ダミー入力の場合について説明する。ダミー作成起動部２０は、センサからの入力を監視して、ある期間、センサからの観測値の入力がないことを検出した場合、ダミー作成の指示をダミー観測時刻作成部２１及びダミー観測値作成部２２へ送る（図２ステップＡ１）

ダミー観測時刻作成部２１は、ダミー作成の指示を受けると、ダミーの観測時刻ｔｍを作成し、そのダミーの観測時刻ｔｍを時刻差分算出部３０へ送出する（図２ステップＡ３）。ダミーの観測時刻ｔｍには、ダミー作成指示の時刻を設定する。

時刻差分算出部３０は、今回の観測時刻ｔｍ（ｋ）（ダミーの観測時刻ｔｍ）と前回の観測時刻ｔｍ（ｋ−１）とから時刻差分ΔＴ（ｋ）を上記の（３）式を用いて算出し、算出した時刻差分ΔＴ（ｋ）を状態変数及び誤差共分散時刻算出部３１と、状態遷移算出部３２と、駆動雑音算出部３３とに送出する。

状態遷移算出部３２は、状態遷移モデルと時刻差分ΔＴとから状態遷移Φを算出し、算出した状態遷移Φを予測状態変数算出部５１及び予測誤差共分散算出部６１に送出する。ここで、状態遷移モデルとは、状態変数Ｘが時間と共にどのように変化するかを仮定するモデルであり、例えば目標が等速直進すると仮定するならば、状態遷移Φは上記の（４）式を用いて算出される。

駆動雑音算出部３３は、時刻差分ΔＴと駆動雑音パラメータＡとから駆動雑音Ｕを算出し、算出した駆動雑音Ｕを予測誤差共分散算出部６１に送出する。例えば、駆動雑音をｘ方向の加速度に相当する雑音とｙ方向の加速度に相当する雑音だと仮定すると、駆動雑音Ｕは上記の（５）式〜（７）式を用いて算出される（図２ステップＡ４）。

次に、状態変数及び誤差共分散時刻算出部３１は、時刻差分ΔＴ（ｋ）と前回の状態変数及び誤差共分散時刻ｔ（ｋ−１）とから今回の状態変数及び誤差共分散時刻ｔ（ｋ）を、上記の（８）式を用いて算出する。

予測状態変数算出部５１は、状態遷移Φと前回の推定状態変数Ｘ（ｋ−１）ハットとから今回の予測状態変数Ｘ（ｋ）チルダを上記の（９）式を用いて算出し、算出した今回の予測状態変数Ｘ（ｋ）チルダを推定状態変数算出部５２へ送出する。

予測誤差共分散算出部６１は、状態遷移Φと駆動雑音Ｕと前回の推定誤差共分散Ｐ（ｋ−１）ハットとから今回の予測誤差共分散Ｐ（ｋ）チルダを上記の（１０）式を用いて算出し、算出した今回の予測誤差共分散Ｐ（ｋ）チルダをカルマンゲイン算出部４２及び推定誤差共分散算出部６２に送出する（図２ステップＡ５）。

続いて、ダミー観測値作成部２２は、ダミー作成の指示を受けている場合（図２ステップＡ６）、ダミーの観測値Ｚを作成し、作成したダミーの観測値Ｚを観測雑音算出部４１及び推定状態変数算出部５２に送出する（図２ステップＡ８）。ダミーの観測値Ｚは、予測状態変数Ｘチルダを利用し、

・・・（１９）
という式を用いて作成する。

観測雑音算出部４１は、観測値Ｚと観測雑音パラメータＢとから観測雑音Ｗを算出し、算出した観測雑音Ｗをカルマンゲイン算出部４２へ送出する。例えば、観測雑音をセンサの距離誤差標準偏差σｒと方位誤差標準偏差σθとで与えるとすると、観測雑音は、上記の（１１）式〜（１３）式を用いて算出される。但し、センサから見た目標までの距離をｒ、センサから見た目標の方位をθとする（方位は、北を基準とした時計回りで表す）。

カルマンゲイン算出部４２は、観測雑音Ｗと予測誤差共分散ＰチルダとからカルマンゲインＫを上記の（１４）式及び（１５）式を用いて算出し、算出したカルマンゲインＫを推定状態変数算出部５２及び推定誤差共分散算出部６２へ送出する（図２ステップＡ９）。

最後に、推定状態変数算出部５２は、予測状態変数Ｘチルダと観測値ＺとカルマンゲインＫとから推定状態変数Ｘハットを上記の（１６）式及び（１４）式を用いて算出する。

推定誤差共分散算出部６２は、予測誤差共分散ＰチルダとカルマンゲインＫとから推定誤差共分散Ｐハットを上記の（１７）式と（１８）式と（１４）式とを用いて算出する（図２ステップＡ１０）。

このように、本実施の形態では、観測値の入力頻度が低下した場合、または観測値の入力が無くなった場合においても、誤差共分散が過度に大きくなったり、異常な値となったりすることを防止することができる。

そのため、本実施の形態では、
（１）算出した状態変数が安定しない。例えば、再び観測値が入力された際に推定状態変数を算出するが、その推定状態変数が前回の値と比べて極端に大きく変動する。
（２）誤差共分散を利用した処理にて期待しない結果を生じる。例えばゲート検査にて、過度に大きくなった誤差共分散を使用したため、その目標から遠く離れた位置に観測した別の目標の観測値を、その目標のものであると誤った判定をしてしまう。また、その結果その目標が多数の観測値と関係付けられ計算負荷が増大する。
（３）処理の計算式にてエラーを発生し、処理の実行が不可能となる。
というような悪影響を回避することができる。

これは、本実施の形態において、ダミーの観測値を作成することで、観測雑音Ｗ及びカルマンゲインＫの算出を経て、推定誤差共分散Ｐハットをある頻度で算出するためである。

次に、本発明の第２の実施の形態について説明する。上述した本発明の第１の実施の形態では、２次元上での目標追尾を前提に説明しているが、本発明の第２の実施の形態では、１次元上で本発明の第１の実施の形態と同様に実施している。

本発明の第２の実施の形態では、状態変数Ｘ、誤差共分散Ｐ、観測値Ｚ、観測雑音Ｗ、駆動雑音Ｕ、カルマンゲインＫ、状態遷移Φ、駆動雑音パラメータＡ及び駆動雑音パラメータＢの要素が変更となり、それに伴い、算出式等が変更となるが、これらは公知の理論により求めることができるため、ここではその説明を省略する。

続いて、本発明の第３の実施の形態について説明する。上述した本発明の第１の実施の形態では、２次元上での目標追尾を前提に説明しているが、本発明の第３の実施の形態では、ｎ次元（ｎは３以上の整数）上で本発明の第１の実施の形態と同様に実施している。

本発明の第３の実施の形態では、状態変数Ｘ、誤差共分散Ｐ、観測値Ｚ、観測雑音Ｗ、駆動雑音Ｕ、カルマンゲインＫ、状態遷移Φ、駆動雑音パラメータＡ及び駆動雑音パラメータＢの要素が変更となり、それに伴い、算出式等が変更となるが、これらは公知の理論により求めることができるため、ここではその説明を省略する。

さらに、本発明の第４の実施の形態について説明する。上述した本発明の第１の実施の形態では、ダミー作成起動部２０がセンサからの入力を監視し、ある期間、センサからの観測値の入力がないことを検出した場合、ダミー作成の指示をダミー観測時刻作成部２１及びダミー観測値作成部２２へ送っている（図２ステップＡ１）。

本発明の第４の実施の形態では、ダミー作成起動部２０がセンサからの入力を監視し、ある期間、センサからの観測値の入力がないことを検出した場合に加え、誤差共分散Ｐを監視してある条件を満たした場合（例えば、対角成分があるしきい値以上となった場合等）に、ダミー作成の指示をダミー観測時刻作成部２１及びダミー観測値作成部２２へ送るようにしている。

さらにまた、本発明の第５の実施の形態について説明する。上述した本発明の第１の実施の形態では、駆動雑音Ｕの算出において駆動雑音パラメータＡを使用している。また、本発明の第１の実施の形態では、観測雑音Ｗの算出において観測雑音パラメータＢを使用している。

本発明の第５の実施の形態では、ダミーの観測値による一連の処理において、この駆動雑音パラメータＡの値と観測雑音パラメータＢの値を、センサから入力した観測値による一連の処理における値から変更するようにしている。つまり、本実施の形態では、ダミーの観測値による処理に用いるパラメータ（観測雑音パラメータＡの値）を、センサから入力した観測値による通常の処理に用いるパラメータ（駆動雑音パラメータＡの値）から変更するようにしている。また同様に、ダミーの観測値による処理に用いるパラメータ（観測雑音パラメータＢの値）を、センサから入力した観測値による通常の処理に用いるパラメータ（駆動雑音パラメータＢの値）から変更するようにしている。

本発明は、（１）航空機を追尾する目標追尾システム、（２）車両、船舶、ミサイル等の移動する物体を追尾する目標追尾システム、（３）気温や距離等を観測する観測装置というような用途に適用することができる。

１１ 観測時刻入力部
１２ 観測値入力部
２０ ダミー作成起動部
２１ ダミー観測時刻作成部
２２ ダミー観測値作成部
３０ 時刻差分算出部
３１ 状態変数及び誤差共分散時刻算出部
３２ 状態遷移算出部
３３ 駆動雑音算出部
４１ 観測雑音算出部
４２ カルマンゲイン算出部
５１ 予測状態変数算出部
５２ 推定状態変数算出部
６１ 予測誤差共分散算出部
６２ 推定誤差共分散算出部

Claims (8)

1. 目標の位置を観測するセンサの観測値からその目標の位置、速度及び針路を状態変数として推定するカルマンフィルタを用いた目標追尾を行う目標追尾処理器であって、
   前記状態変数の分散を表す誤差共分散を補正する手段を有し、
   前記誤差共分散を補正する手段は、ダミーの観測値を作成する作成手段を含み、前記作成手段で作成したダミーの観測値を用いて誤差共分散を補正し、
   前記作成手段で作成したダミーの観測値から観測雑音及びカルマンゲインの算出を経て、推定誤差共分散を算出することを特徴とする目標追尾処理器。
2. 前記センサの観測値の入力頻度が低下した場合及び前記センサの観測値の入力がなくなった場合のいずれかの場合に前記作成手段を起動する起動手段を含むことを特徴とする請求項１記載の目標追尾処理器。
3. 前記起動手段は、誤差共分散を監視して少なくとも対角成分がしきい値以上となった場合に前記作成手段を起動することを特徴とする請求項２記載の目標追尾処理器。
4. 前記ダミーの観測値による処理に用いるパラメータを、通常の処理に用いるパラメータから変更することを特徴とする請求項１から請求項３のいずれか記載の目標追尾処理器。
5. 目標の位置を観測するセンサの観測値からその目標の位置、速度及び針路を状態変数として推定するカルマンフィルタを用いた目標追尾を行う目標追尾処理器に用いる誤差共分散行列の補正方法であって、
   前記目標追尾処理器が、前記状態変数の分散を表す誤差共分散を補正する処理を実行し、
   前記誤差共分散を補正する処理に、ダミーの観測値を作成する作成処理を含み、
   前記目標追尾処理器が、前記作成処理で作成したダミーの観測値を用いて誤差共分散を補正し、
   前記目標追尾処理器が、前記作成処理で作成したダミーの観測値から観測雑音及びカルマンゲインの算出を経て、推定誤差共分散を算出することを特徴とする誤差共分散行列の補正方法。
6. 前記目標追尾処理器が、前記センサの観測値の入力頻度が低下した場合及び前記センサの観測値の入力がなくなった場合のいずれかの場合に前記作成処理を起動する起動処理を実行することを特徴とする請求項５記載の誤差共分散行列の補正方法。
7. 前記目標追尾処理器が、前記起動処理において、誤差共分散を監視して少なくとも対角成分がしきい値以上となった場合に前記作成処理を起動することを特徴とする請求項６記載の誤差共分散行列の補正方法。
8. 前記ダミーの観測値による処理に用いるパラメータを、通常の処理に用いるパラメータから変更することを特徴とする請求項５から請求項７のいずれか記載の誤差共分散行列の補正方法。

Images