JP5315585B2 - 航跡生成システム、その誤差共分散行列初期値設定装置、航跡生成方法およびその誤差共分散行列初期値設定方法 - Google Patents

Description

本発明は、航跡生成システム、その誤差共分散行列初期値設定装置、航跡生成方法およびその誤差共分散行列初期値設定方法に関し、特に航跡諸元の精度および安定性を改善した跡生成方法およびその誤差共分散行列初期値設定方法に関する。

一般に、航空機などの目標追尾を行う代表例として、航跡追尾システムがある。この航跡追尾システムは、通常カルマンフィルタを用いた演算により、目標の航跡諸元の推定を行っている。この航跡諸元の推定は、航跡および誤差共分散行列（航空機の追尾結果の位置と速度（航跡位置と速度）の誤差範囲）の初期値を設定してから演算が開始される。

なお、誤差共分散行列とは、航空機の追尾結果の位置と速度（航跡位置と速度）の誤差範囲を表し、航空機の位置と速度がどの程度確からしいかというのが行列として表現されている。そして追尾処理を始めるに当たり、一番初めに、この位置辺りに航空機がいるか、その速度はこの位かと思って、およその航跡を生成する。この時、一番初めに与える誤差共分散が、「誤差共分散行列初期値」である。

ここで前提となる航跡生成装置２０の全体構成を、図６を参照しながら説明する。図６において、航跡生成装置２０は、航跡諸元初期値算出器２１と誤差共分散行列初期値算出器１０ｂとから構成されている。航跡諸元初期値算出器２１は、観測値を入力することによって航跡諸元初期値を算出する。誤差共分散行列初期値算出器１０ｂは、観測値等を入力することによって誤差共分散行列初期値を算出する。

ここでは、カルマンフィルタを用いた目標追尾の代表的な例として、観測値Ｑをセンサ中心の距離ｒおよび方位θで与え、その誤差をそれぞれの標準偏差σ_ｒ，σθで与える系を考える。また、航跡諸元は２次元ｘｙ座標での位置ｘ ，ｙおよび速度ｘドット，ｙドットとし、駆動雑音として加速度に相当する標準偏差α_ｘ，α_ｙで与える。観測値および航跡の更新周期をΔＴとし、航跡の誤差共分散行列をＰ、その初期値をＰ_０とする。このような系を考えると、航跡諸元初期値Ｘ_０、誤差共分散行列初期値Ｐ_０、観測値Ｑ_０，Ｑ_１は次の式（１）〜（３）で表される。なお、Ｐ_１１〜Ｐ_４４は、航跡の誤差共分散行列初期値の各要素で４＊４の行列を示す。

従来の誤差共分散行列初期値算出器の一例を、図７のブロック図に示し、その処理フローを図８に示す。この誤差共分散行列初期値算出器１０ｂは、変換行列算出手段１１と、観測誤差分散行列算出手段１２と、誤差共分散行列初期値算出手段１３と、制御手段１５ｂとから構成されている。制御手段１５ｂは、変換行列算出手段１１、観測誤差分散行列算出手段１２、誤差共分散行列初期値算出手段１３の全体を制御し、誤差共分散行列初期値算出器１０ｂ内の各機能の作動順序の制御を行う。変換行列算出手段１１は、観測値を用いて変換行列を算出する。観測誤差分散行列算出手段１２は、変換行列及び誤差標準偏差σｒ，σθを用いて観測誤差分散行列を算出する。誤差共分散行列初期値算出手段１３は、観測誤差分散行列及び更新周期ΔＴを用いて最初の誤差共分散行列初期値を算出する。

このような構成の誤差共分散行列初期値算出器１０ｂは、次のように動作する。まず、変換行列算出部１１は、観測値Ｑを用いて変換行列Γ_Ｃｎを式（４）のように算出する（ステップＳ１）。

次に、観測誤差分散行列算出部１２は、変換行列Γ_Ｃｎおよび誤差標準偏差σ_ｒ，σθを用いて観測誤差分散行列Ｗ_ｎを式（５）のように算出する（ステップＳ２）。

さらに、誤差共分散行列初期値算出部１３は、観測誤差分散行列Ｗ_ｎおよび更新周期ΔＴを用いて最初の誤差共分散行列初期値Ｐ_０，_１を式（６）のように算出する（ステップＳ３）。

このような従来の誤差共分散行列初期値算出器は、得られた観測値の観測誤差から誤差共分散行列初期値を算出していた。そのため、航跡を生成した後しばらくの間、誤差共分散行列が最適な値ではなく、結果として航跡諸元の精度および安定性が悪かった。また、操作者が航跡諸元を変更した場合において、その後しばらくの間は誤差共分散行列が最適な値ではなく、結果として航跡諸元の精度および安定性が悪かったり、航跡諸元が操作者の意図しない挙動となる事があった。

なお、特許文献１には、カルマンフィルタを用いて目標追尾を行う追尾装置が説明されているが、これは、目標の着弾点の予測の初期値精度を向上させたものである。この装置は、目標の観測位置および距離、仰角、方位角の観測雑音の標準偏差を用いて、平滑ベクトル、平滑誤差共分散行列を算出する構成となっている。この構成によれば、事前に決められた時刻より前の平滑ベクトルおよび平滑誤差共分散行列を再算出することにより、着弾点予測の初期値の精度を向上させ、さらに、着弾点予測を精度良く行うことができる。

さらに、特許文献２にも、カルマンフィルタ平滑効果を制御できる目標追尾装置が示されている。この装置は、観測雑音共分散行列、疑似予測誤差共分散行列、駆動雑音共分散行列及び観測時刻を用いて繰り返し計算を行い疑似平滑誤差共分散行列を求めることが説明されている。

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

しかし、従来の誤差共分散行列初期値の算出方法は、得られた観測値の観測誤差から誤差共分散行列初期値を算出していた。そのため、航跡を生成した後しばらくの間、誤差共分散行列が最適な値ではなく、結果として航跡諸元の精度および安定性が悪かった。また、操作者が航跡諸元を変更した場合において、その後しばらくの間誤差共分散行列が最適な値ではなく、結果として航跡諸元の精度および安定性が悪かったり、航跡諸元が操作者の意図しない挙動となる事があった。

なお、この特許文献１における誤差共分散行列は、最初の観測位置および距離、仰角、方位角の観測雑音の標準偏差を用いて平滑誤差共分散行列を計算しているかもしれないが、
その誤差共分散行列は追尾途中の誤差範囲を示しているに過ぎず、また上記従来例の誤差共分散行列初期値算出器と同等の構成にすぎない。

また、特許文献２に説明された誤差共分散行列も、特許文献１と同様に、追尾途中の誤差範囲を示しているに過ぎない。

本発明の主な目的は、差共分散行列初期値として最適な値を算出し設定することによって、航跡を生成した直後における航跡諸元の精度および安定性を向上させた航跡生成装置、その誤差共分散行列初期値演算装置、航跡生成方法およびその誤差共分散行列初期値演算方法を提供することにある。

本発明の構成は、観測値から変換行列を算出する変換行列算出手段と、前記変換行列及び誤差標準偏差を用いて観測誤差分散行列を算出する観測誤差分散行列算出手段と、更新周期及び前記観測誤差分散行列を用いて誤差共分散行列初期値を算出する誤差共分散行列初期値算出手段と、前記観測誤差分散行列初期値、前記更新周期及び駆動雑音を用いて前記誤差共分散行列初期値の収束値を算出する誤差共分散行列初期値収束手段と、を有することを特徴とする。

本発明において、前記誤差共分散行列初期値収束手段は、前記誤差共分散行列初期値から誤差共分散行列の予測値を計算する予測値算出処理と、前記誤差共分散行列の予測値と前記観測誤差分散行列とを用いてカルマンゲインを計算するカルマンゲイン算出処理と、前記誤差共分散行列の予測値と前記カルマンゲインとから前記誤差共分散行列の推定値を計算する推定値算出処理と、前記誤差共分散行列の推定値が所定範囲内か否かを判断して、所定範囲外の場合には、前記推定値算出処理で算出した前記誤差共分散行列の推定値を前記誤差共分散行列初期値とみなして繰り返し前記予測値算出処理、カルマンゲイン算出処理、推定値算出処理を順次行わせ、また所定範囲内の場合には、前記推定値算出処理で算出された前記誤差共分散行列の推定値を前記誤差共分散行列初期値の前記収束値として出力する収束処理と、を行うことを特徴とする。

また、前記誤差共分散行列初期値収束手段は、観測誤差標準偏差に基づき計算された観測誤差分散行列を用いて前記観測誤差分散行列算出手段が算出した前記誤差共分散行列初期値Ｐ_０，１と、目標の運動モデルΦと、駆動雑音用変換行列ΓＤと、駆動雑音分散行列Ｕと、を用いて前記誤差共分散行列の予測値Ｐ_０，ｋチルダを

により算出し（ただし、Ｐ_０，１ハット＝Ｐ_０，１、ｋ−１｛ｋは２以上の整数｝は繰り返し計算回数、また、行列、ベクトルの右上添字「Ｔ」は、行列、ベクトルの転置を表し、行列の右上添字「−１」は、行列の逆行列を表す）、前記誤差共分散行列の予測値Ｐ_０，ｋチルダと、観測行列Ｈと、観測誤差分散行列Ｗｎと、を用いてカルマンゲインＫを

により算出し、前記誤差共分散行列の予測値Ｐ_０，ｋチルダと、前記カルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて前記誤差共分散行列の推定値Ｐ_０，ｋハットを

により算出することを特徴とする。

さらに、前記変換行列算出手段は、前記観測誤差標準偏差が状態諸元の座標系と異なる座標系で求められた時、前記観測誤差分散行列を座標変換行列により変換することを特徴とする。

本発明の他の構成は、移動する目標体を追尾する場合に、前記目標体の航跡の生成または航跡諸元の変更を行う航跡生成システムにおいて、前記航跡の生成または前記航跡諸元の変更を行う場合の航跡の前記誤差共分散行列初期値を設定する際に、前述の誤差共分散行列初期値設定装置により得られた前記誤差共分散行列初期値の前記収束値を用いたことを特徴とする。

本発明のさらに他の構成は、ナビゲーションシステム等における、自己位置の確立または変更を行う自己位置確定システムにおいて、前記自己位置を生成または変更する場合の位置の前記誤差共分散行列初期値を設定する際に、請求項１乃至４のうちの１項に記載の誤差共分散行列初期値設定装置により得られた前記誤差共分散行列初期値の前記収束値を用いたことを特徴とする。
また、本発明の他の構成は、気温や傾斜などの状態の観測装置における、状態の確立または変更を行う観測確立システムにおいて、前記状態の確立または変更する場合の状態の前記誤差共分散行列初期値を設定する際に、上述した誤差共分散行列初期値設定装置により得られた前記誤差共分散行列初期値の前記収束値を用いたことを特徴とする。

本発明の他の構成は、観測値から変換行列を算出する変換行列算出手順と、前記変換行列及び誤差標準偏差を用いて観測誤差分散行列を算出する観測誤差分散行列算出手順と、更新周期及び前記観測誤差分散行列を用いて誤差共分散行列初期値を算出する誤差共分散行列初期値算出手順と、前記観測誤差分散行列初期値、前記更新周期及び駆動雑音を用いて前記誤差共分散行列初期値の収束値を算出する誤差共分散行列初期値収束手順と、を含むことを特徴とする。

本発明のさらに他の構成は、移動する目標体を追尾する場合に、前記目標体の航跡の生成または航跡諸元の変更を行う航跡生成方法において、前記航跡の生成または前記航跡諸元の変更を行う場合の航跡の前記誤差共分散行列初期値を設定する際に、上述した誤差共分散行列初期値設定方法により得られた前記誤差共分散行列初期値の前記収束値を用いたことを特徴とする。

以上説明したように、この発明の構成によれば、誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して演算する誤差共分散行列初期値収束手段を持つため、誤差共分散行列初期値を適切な値に算出することができ、状態（航跡）を生成した直後においても状態（航跡）諸元の精度および安定性を向上することができるという効果がある。

次に図面により本発明の実施形態を説明する。図１は本発明の一実施形態の誤差共分散行列初期値設定装置の基になる誤差共分散行列初期値算出器のブロック図、図２はその処理を説明するフロー図である。この図１に示すように、本実施形態の誤差共分散行列初期値設定装置は、従来例と同様の図６に示す航跡生成システムに用いられるが、誤差共分散行列初期値算出器１０の算出結果を基にして、誤差共分散行列初期値を設定するものである。ただし、図６，７の誤差共分散行列初期値算出器１０ｂの代わりに、図１の誤差共分散行列初期値算出器１０を用いる。

本実施形態において、誤差共分散行列初期値設定装置は、誤差共分散行列初期値算出器１０の算出結果を基にして、誤差共分散行列初期値を設定する。誤差共分散行列初期値算出器１０は、従来例の図７に対して、誤差共分散行列初期値収束部１４が付加されており、また、図２に示すように、従来例の図８に対して、本実施形態は、誤差共分散行列初期値を収束させるステップＳ４が付加されている。すなわち、本実施形態では、誤差共分散行列初期値算出器１０が、変換行列算出部１１と、観測誤差分散行列算出部１２と、誤差共分散行列初期値算出部１３と、誤差共分散行列初期値収束部１４と、制御部１５とから構成されている。

本実施形態において、変換行列算出部１１は観測値から変換行列を算出する。観測誤差分散行列算出部１２は変換行列および誤差標準偏差を用いて観測誤差分散行列を算出する。誤差共分散行列初期値算出部１３は観測誤差分散行列および更新周期を用いて最初の誤差共分散行列初期値を算出する。誤差共分散行列初期値収束部１４は観測誤差分散行列、更新周期および駆動雑音を用いて繰返し計算を行い誤差共分散行列初期値を収束させる。得られた収束後の誤差共分散行列初期値を、生成する航跡の誤差共分散行列初期値として使用する。制御部１５は、誤差共分散行列初期値算出器１０が、変換行列算出部１１、観測誤差分散行列算出部１２、誤差共分散行列初期値算出部１３、誤差共分散行列初期値収束部１４を制御し、誤差共分散行列初期値算出器１０内の各機能の作動順序の制御を行う。

本実施形態における誤差共分散行列初期値収束部１４は、「時間が経過するとともにある観測値が入力されるだろう」と想像して航跡の誤差を適切な値へ収束させるよう動作する。

本実施形態の主要動作は次の通りである。まず、変換行列算出部１１は観測値Ｑから変換行列Γ_Ｃｎを算出する（ステップＳ１）。観測誤差分散行列算出部１２は変換行列Γ_Ｃｎおよび誤差標準偏差σを用いて観測誤差分散行列Ｗ_ｎを算出する（ステップＳ２）。誤差共分散行列初期値算出部１３は観測誤差分散行列Ｗ_ｎおよび更新周期ΔＴを用いて最初の誤差共分散行列初期値Ｐ_０，_１を算出する（ステップＳ３）。誤差共分散行列初期値収束部１４は観測誤差分散行列Ｗ_ｎ、更新周期ΔＴおよび駆動雑音αを用いて繰返し計算を行い誤差共分散行列初期値を収束させる（ステップＳ４）。得られた収束後の誤差共分散行列初期値を、生成する航跡の誤差共分散行列初期値として使用する。また、制御部１５は誤差共分散行列初期値算出器１０内の各機能の作動順序の制御を行う。

このステップＳ４は、図３の詳細フロー図に示される。この図は、誤差共分散行列初期値を収束させるための処理を説明している。ここで用いる計算式は、本来追尾処理において徐々に収束していくものであるが、観測値が入力されるだろうと想定して、航跡生成時に一気に収束させるようにしている。そのために追尾処理に用いられる数式を使用する必要がある。
なお、目標の運動モデルΦを設定し、カルマンフィルタの条件から駆動雑音用変換行列Γ_Ｄと駆動雑音分散行列Ｕを設定する。また、観測行列Ｈも設定でき、観測誤差分散行列Ｗ_ｎも計算できる。なお、図３において、行列およびベクトルの右上添字「Ｔ」は、行列およびベクトルの転置を表すこととし、また、行列の右上添字「−１」は、行列の逆行列を表すこととする。また、ｋ−１（ｋは２以上の整数）は繰り返し計算回数を示す。

図３において、前述のステップＳ３で算出した最初の誤差共分散行列初期値Ｐ_０，_１と目標の運動モデルΦと駆動雑音用変換行列Γ_Ｄと駆動雑音分散行列Ｕとを用いて誤差共分散行列の予測値Ｐ_０，_ｋチルダが算出できる（ステップＳ１１）。

次に、ステップＳ１１で算出した誤差共分散行列の予測値Ｐ_０，_ｋチルダと観測行列Ｈと観測誤差分散行列Ｗ_ｎとを用いてカルマンゲインＫが算出できる（ステップＳ１２）。

次に、ステップＳ１１〜Ｓ１２で算出した誤差共分散行列の予測値Ｐ_０，_ｋチルダとカルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて誤差共分散行列の推定値Ｐ_０，_ｋハットが算出できる（ステップＳ１３）。

最後に、ステップＳ１４で、終了するか否かを判定し、終了しない（否）ならステップＳ１３で算出した誤差共分散行列の推定値を用いて再度ステップＳ１１〜Ｓ１３を繰り返す。また、ステップＳ１４を終了するなら、ステップＳ１３で算出した誤差共分散行列の推定値を誤差共分散行列初期値算出器１０の出力としてステップＳ４を終了する。なお、ステップＳ１４の判定方法としては、規定の回数に到達したら終了する方法、または誤差共分散行列の推定値が規定の大きさ以内に到達したら終了する方法等がある。

なお、最初の誤差共分散行列初期値Ｐ_０，_１は、予め定義しておいた値を使用して最初の誤差共分散行列初期値として設定することもできる。

本実施形態によれば、カルマンフィルタを用いた目標追尾において、追尾する目標の航跡を生成する際に航跡の誤差共分散行列初期値を設定するが、この誤差共分散行列初期値として適切な値を算出し設定することによって、航跡を生成した直後における航跡諸元の精度および安定性を向上する。

また、誤差標準偏差および駆動雑音を任意に設定することができ、任意の誤差共分散行列初期値を設定することができるため、航跡を生成した直後において航跡に任意の挙動特性を持たせることができる。

また、これら効果については、航跡を生成した場合の他、操作者が航跡諸元を変更した場合や、他のシステムから航跡を移管した場合にも適用可能であり、同様の効果を得ることができる。

本発明の第１の実施例は、図１〜図３で説明した実施形態と同様である。次に、図１、図２を参照して本実施形態の動作について詳細に説明する。

ここでは、従来例と同様に、カルマンフィルタを用いた目標追尾の代表的な例として、観測値Ｑをセンサ中心の距離ｒおよび方位θで与え、その誤差をそれぞれの標準偏差σ_ｒ，σθで与える系を考える。また、航跡諸元は２次元ｘｙ座標での位置ｘ ，ｙおよび速度ｘドット，ｙドットとし、駆動雑音として加速度に相当する標準偏差α_ｘ，α_ｙで与える。観測値および航跡の更新周期をΔＴとし、航跡の誤差共分散行列をＰ、その初期値をＰ_０とする。このような系を考えると、航跡諸元初期値Ｘ_０、誤差共分散行列初期値Ｐ_０、観測値Ｑ_０，Ｑ_１は、前述の式（１）〜（３）で表される。

この誤差共分散行列初期値算出器１０は次のように動作する。まず、変換行列算出部１１は、観測値Ｑを用いて変換行列Γ_Ｃｎを式（４）のように算出する（ステップＳ１）。

次に、観測誤差分散行列算出部１２は、変換行列Γ_Ｃｎおよび誤差標準偏差σ_ｒ，σθを用いて観測誤差分散行列Ｗ_ｎを式（５）のように算出する（ステップＳ２）。

さらに、誤差共分散行列初期値算出部１３は、観測誤差分散行列Ｗ_ｎおよび更新周期ΔＴを用いて最初の誤差共分散行列初期値Ｐ_０，_１を式（６）のように算出する（ステップＳ３）。

そして、誤差共分散行列初期値収束部１４は、観測誤差分散行列Ｗ_ｎ、更新周期ΔＴをおよび駆動雑音を用いて繰返し計算を行い誤差共分散行列初期値を収束させる（ステップＳ４）。

このステップＳ４は、図３のフロー図を用いて詳しく説明する。ここでは、誤差共分散行列初期値を収束させるための処理である。ここで用いる計算式は、本来追尾処理において徐々に収束していくものであるが、観測値が入力されるだろうと想定して、航跡生成時に一気に収束させるようにしている。そのために追尾処理に用いられる数式を使用する必要がある。

ここで、目標の運動モデルΦは、航跡の想定する飛行モデルであり、例えば次の式（７）のように定義できる。また、駆動雑音用変換行列Γ_Ｄは、駆動雑音分散行列Ｕを誤差共分散行列初期値Ｐに加えるときに必要な変換であり、次の式（８）のようになる。また、駆動雑音分散行列Ｕは、誤差共分散行列初期値Ｐへある一定の雑音を付加し、航跡の軌道の不確定さを表すもので、例えば次の式（９）のように定義できる。また、観測行列Ｈは、誤差共分散行列初期値Ｐのもつ情報種類から観測値のもつ情報種類を取出す為の行列であり、次の式（１０）のようになるが、ここでは、Ｘ位置、Ｙ位置のみの情報を取出す働きを持つ。なお、行列およびベクトルの右上添字「Ｔ」は、行列およびベクトルの転置を表すこととし、また、行列の右上添字「−１」は、行列の逆行列を表すこととする。

これらの式とステップＳ３で算出した最初の誤差共分散行列初期値Ｐ_０，_１とを用いると、誤差共分散行列の予測値Ｐ_０，_ｋチルダが、次の式（１１）のように算出できる。この式（１１）は、収束させるために、擬似的に誤差共分散行列初期値Ｐ_０を予測することを意味し、追尾処理中では、推定した航跡の誤差に対し、ある時間経過後の誤差を算出している予測式を意味する。ただし、Ｐ_０，_１ハット＝Ｐ_０，_１であり、ｋ−１は繰り返し計算回数を示す。

また、観測行列Ｈ、観測誤差分散行列Ｗ_ｎおよびステップＳ１１で算出した誤差共分散行列の予測値Ｐ_０，_ｋチルダを用いると、カルマンゲインＫは次の式（１２）のように算出できる。この式も、収束させるために、擬似的にカルマンゲインＫを求めている。

また、誤差共分散行列の予測値Ｐ_０，_ｋチルダと、カルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いると、誤差共分散行列の推定値Ｐ_０，_ｋハットを次の式（１３）のように算出できる。この式（１３）も、収束させるために、擬似的にＰ_０を推定しており、追尾処理中では、予測した航跡の誤差に対し、ある観測値を得た後の誤差を算出している推定式を意味する。

なお、「航跡の予測値」というのは、継続的に行う追尾処理の最中の、航跡諸元（位置と速度）の時間外挿値（未来の値）のことである。この「航跡の予測値」の誤差範囲も物理的には「誤差共分散行列初期値」と同じ内容であるが、追尾最中であるか、一番初めの値であるか、の相違がある。

従って、図３においては、前述のステップＳ３で算出した最初の誤差共分散行列初期値Ｐ_０，_１と目標の運動モデルΦと駆動雑音用変換行列Γ_Ｄと駆動雑音分散行列Ｕとを用いて誤差共分散行列の予測値Ｐ_０，_ｋチルダを算出する（ステップＳ１１）。

次に、ステップＳ１１で算出した誤差共分散行列の予測値Ｐ_０，_ｋチルダと観測行列Ｈと観測誤差分散行列Ｗ_ｎとを用いてカルマンゲインＫを算出する（ステップＳ１２）。

次に、ステップＳ１１〜Ｓ１２で算出した誤差共分散行列の予測値Ｐ_０，_ｋチルダとカルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて誤差共分散行列の推定値Ｐ_０，_ｋハットを算出する（ステップＳ１３）。

最後に、ステップＳ１４で、終了するか否かを判定し、終了しない（否）ならステップＳ１３で算出した誤差共分散行列の推定値（式（１３））を用いて再度ステップＳ１１〜Ｓ１３を繰り返す。また、ステップＳ１４を終了するなら、ステップＳ１３で算出した誤差共分散行列の推定値を誤差共分散行列初期値算出器１０の出力としてステップＳ４を終了する。なお、ステップＳ１４の判定方法としては、規定の回数に到達したら終了する方法、または誤差共分散行列の推定値が規定の大きさ以内に到達したら終了する方法等がある。

次に、本実施例の効果について説明する。本実施例では、誤差共分散行列初期値算出器１０の中に誤差共分散行列初期値収束部１４を持つ構成となっているため、誤差共分散行列初期値を適切な値に算出することができ、航跡を生成した直後においても航跡諸元の精度および安定性を向上させることができる。

また、本実施例では、さらに、誤差標準偏差および駆動雑音を任意に設定することができ且つ誤差共分散行列初期値収束部１４を持つ構成であるため、航跡を生成した直後において任意の誤差共分散行列初期値を設定し、航跡に任意の挙動特性を持たせることができる。すなわち、航跡の位置の誤差範囲はセンサ中心の距離および方位による誤差標準偏差σ_ｒ，σθで任意に与えることができる。

次に、図４は本発明の第２の実施例の誤差共分散行列初期値算出器１０ａの構成を説明するブロック図、図５はその処理フロー図である。図４を参照すると、本実施例の誤差共分散行列初期値算出器１０ａの構成は、第１の実施例から変換行列算出部１１および図２におけるステップＳ１を省いたものとなっている。すなわち、本実施例は、観測値が極座標ｒ，θの代わりに座標ｘｙを用いているので、変換行列の算出（Ｓ１）が不要となり、計算が実施例１よりも簡単化されている。本実施例は、観測誤差分散行列算出部１２ａと、誤差共分散行列初期値算出部１３と、誤差共分散行列初期値収束部１４と、制御部１５ａとから構成されている。ここで前提となる航跡生成装置２０は、従来例の全体構成を図６と同様である。

観測誤差分散行列算出部１２ａは、誤差標準偏差を用いて観測誤差分散行列を算出する。誤差共分散行列初期値算出部１３は観測誤差分散行列および更新周期を用いて最初の誤差共分散行列初期値を算出する。誤差共分散行列初期値収束部１４は観測誤差分散行列、更新周期および駆動雑音を用いて繰返し計算を行い誤差共分散行列初期値を収束させる。制御部１５ａは、誤差共分散行列初期値算出器１０ａ内の各機能の作動順序の制御を行う。

次に、図４、図５を参照して本実施例の動作について詳細に説明する。ここではカルマンフィルタを用いた目標追尾の代表的な例として、観測値は２次元ｘｙ座標で与えられ、その誤差はそれぞれの標準偏差σ_ｘ，σ_ｙで与えられる。また、航跡諸元Ｘも２次元ｘｙ座標での位置および速度とし、駆動雑音として加速度に相当する標準偏差で与える。観測値および航跡の更新周期をΔＴとし、航跡の誤差共分散行列をＰとする。このような系を考えると、航跡諸元初期値Ｘ_０、誤差共分散行列初期値Ｐ_０は、前述の式（１）（２）と同様で、観測値Ｑ_０，Ｑ_１は次の式（１４）で与えられる。

まず、観測誤差分散行列算出部１２ａは，誤差標準偏差σ_ｘ，σ_ｙを用いて観測誤差分散行列Ｗ_１を次の式（１５）で算出する（ステップＳ２ａ）。

次に、誤差共分散行列初期値算出部１３は、観測誤差分散行列Ｗ_１および更新周期ΔＴを用いて最初の誤差共分散行列初期値Ｐ_０，_１を次の式（１６）で算出する（ステップＳ３）。

さらに、誤差共分散行列初期値収束部１４は、観測誤差分散行列Ｗ_１、更新周期ΔＴおよび駆動雑音を用いて繰返し計算を行い誤差共分散行列初期値を収束させる（ステップＳ４）。なお、ステップＳ４は第１の実施例と同様のため説明は省略する。

次に、本実施例の効果について説明する。本実施例は、前述のように変換行列の算出（Ｓ１）が不要となり、計算が実施例１よりも簡単化されている。また、本実施例でも、第１の実施例と同様に、誤差共分散行列初期値算出器１０の中に誤差共分散行列初期値収束部１４を持つ構成のため、誤差共分散行列初期値を適切な値に算出することができ、航跡を生成した直後においても航跡諸元の精度および安定性を向上することができる。

また、本実施例では、さらに、誤差標準偏差および駆動雑音を任意に設定することができ且つ誤差共分散行列初期値収束部１４を持つ構成のため、航跡を生成した直後において任意の誤差共分散行列初期値を設定し航跡に任意の挙動特性を持たせることができる。すなわち、航跡の位置の誤差範囲および航跡の速度の誤差範囲はｘｙ座標のｘおよびｙによる誤差標準偏差σ_ｘ，σ_ｙで任意に与えることができる。

次に、本発明の第３の実施例について説明する。第１の実施例および第２の実施例では２次元上での目標追尾を前提に説明したが、本実施例では、１次元上で同様に実施することである。航跡諸元初期値、誤差共分散行列初期値、観測値、誤差標準偏差、および駆動雑音の要素が変更となり、それに伴い算出式等が変更となるが公知の理論により求めることができるため、ここでは説明を省略する。

さらに、本発明の第４の実施例について説明する。第１の実施例および第２の実施例では２次元上での目標追尾を前提に説明したが、本実施例では、ｎ（ただしｎは３以上）次元上で同様に実施することである。航跡諸元初期値、誤差共分散行列初期値、観測値、誤差標準偏差、および駆動雑音の要素が変更となり、それに伴い算出式等が変更となるが公知の理論により求めることができるため、ここでは説明を省略する。

本発明においては、航空機などの目標体を追尾する場合における、航跡の生成および航跡諸元の変更に適用した場合を説明したが、本発明は、航空機の他に、車両（自動車、戦車）、船舶、ミサイル、人工衛星等移動する物体を追尾する場合における、航跡の生成および航跡諸元の変更にも適用することができる。

また、本発明は、ナビゲーションシステム等における、自己位置の確立および変更や、気温や傾斜などの状態の観測装置における、状態の確立および変更にも適用することができる。

この場合、ナビゲーションシステム等における、自己位置の確立または変更を行う自己位置確定システムとしては、自己位置を生成または変更する場合の位置の誤差共分散行列初期値を設定する際に、前述した誤差共分散行列初期値設定装置を用いることができる。また、気温や傾斜などの状態の観測装置における、状態の確立または変更を行う観測確立システムとしては、状態の確立または変更する場合の状態の誤差共分散行列初期値を設定する際に、前述した誤差共分散行列初期値設定装置を用いることができる。

本発明の第１の実施形態の誤差共分散行列初期値算出器を説明するブロック図である。 図１の処理を説明するフロー図である。 図２のステップＳ４の演算処理を説明するフロー図である。 本発明の第２の実施例の誤差共分散行列初期値算出器を説明するブロック図である。 図５の処理を説明するフロー図である。 一般の誤差共分散行列初期値算出器を用いた航跡生成装置の一例のブロック図である。 従来例（図７）の誤差共分散行列初期値算出器を説明するブロック図である。 図７の処理を説明するフロー図である。

符号の説明

１０，１０ａ，１０ｂ 誤差共分散行列初期値算出器
１１ 変換行列算出部
１２，１２ａ 観測誤差分散行列算出部
１３ 誤差共分散行列初期値算出部
１４ 誤差共分散行列初期値収束部
１５，１５ａ，１５ｂ 制御部
２０ 航跡生成装置
２１ 航跡諸元初期値算出器

Claims (5)

1. 目標体を観測するセンサの中心位置から当該目標までの距離ｒ及び前記目標の方位θを観測し、この観測値を用いて、
   
   

   

   で表される変換行列Γ _cn を算出する変換行列算出手段と、
   前記変換行列Γ _cn 及び前記観測値の誤差標準偏差σ _ｒ 、σ _θ を用いて、
   
   

   

   で表される観測誤差分散行列Ｗ _ｎ を算出する観測誤差分散行列算出手段と、
   繰返し航跡を求める際の周期をなす更新周期をΔＴとして、当該更新周期ΔＴと前記観測誤差分散行列Ｗ _ｎ とを用いて、
   
   

   

   で表される最初の誤差共分散行列初期値Ｐ _０，１ を算出する誤差共分散行列初期値算出手段と、
   前記目標の加速度に相当する標準偏差α _ｘ ，α _ｙ を駆動雑音とし又は任意に設定した値を当該駆動雑音として、該駆動雑音、前記観測誤差分散行列Ｗ _ｎ 、更新周期ΔＴを用いて前記誤差共分散行列初期値Ｐ _０，１ を繰返し算出し、当該差共分散行列初期値の収束値を算出する誤差共分散行列初期値収束手段と、
   を有することを特徴とする誤差共分散行列初期値設定装置。
2. 前記誤差共分散行列初期値収束手段は、
   前記誤差共分散行列初期値Ｐ _０，１ から
   
   

   

   （ただし、Ｐ _０，１ ハット＝Ｐ _０，１ 、ｋ−１｛ｋは２以上の整数｝は繰り返し計算回数、また、行列、ベクトルの右上添字「Ｔ」は、行列、ベクトルの転置を表し、行列の右上添字「−１」は、行列の逆行列を表す）
   で表される前記誤差共分散行列の予測値Ｐ _0,k チルダを計算する予測値算出処理と、
   前記誤差共分散行列の予測値Ｐ _0,k チルダと、観測行列Ｈと、前記観測誤差分散行列Ｗ _ｎ とを用いて、
   
   

   

   で表されるカルマンゲインＫを計算するカルマンゲイン算出処理と、
   前記誤差共分散行列の予測値Ｐ _０，ｋ チルダと、前記カルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて、
   
   

   

   で表される前記誤差共分散行列の推定値Ｐ _０，ｋ ハットを計算する推定値算出処理と、
   前記誤差共分散行列の推定値Ｐ _０，ｋ ハットが所定範囲内か否かを判断して、所定範囲外の場合には、前記推定値算出処理で算出した前記誤差共分散行列の推定値Ｐ _０，ｋ ハットを前記誤差共分散行列初期値Ｐ _０，１ とみなして、前記誤差共分散行列の推定値Ｐ _０，ｋ ハットが当該所定範囲に収るまで、前記予測値算出処理、カルマンゲイン算出処理、推定値算出処理を順次繰返し行うことにより前記誤差共分散行列初期値の収束値を算出する収束処理と、
   を行うことを特徴とする請求項１記載の誤差共分散行列初期値設定装置。
3. 移動する目標体を追尾する際に、当該目標体の航跡位置や速度の誤差範囲を示す誤差共分散行列の初期値を観測値に基づき設定して前記目標体の航跡諸元を推定し、該推測した前記航跡諸元に従い当該目標を追尾する航跡生成システムにおいて、
   前記観測値に基づき設定した前記誤差共分散行列の初期値を用いて、請求項１又は２に記載の誤差共分散行列初期値設定装置により前記誤差共分散行列初期値の収束値を算出し、当該算出された誤差共分散行列初期値の収束値を用いて前記目標体の航跡諸元を推定し、該推測した前記航跡諸元に従い当該目標を追尾することを特徴とする航跡生成システム。
4. 目標体を観測するセンサの中心位置から当該目標までの距離ｒ及び前記目標の方位θを観測し、この観測値を用いて、
   
   

   

   で表される変換行列Γ _cn を算出する変換行列算出手順と、
   前記変換行列Γ _cn 及び前記観測値の誤差標準偏差σ _ｒ 、σ _θ を用いて、
   
   

   

   で表される観測誤差分散行列Ｗ _ｎ を算出する観測誤差分散行列算出手順と、
   繰返し航跡を求める際の周期をなす更新周期をΔＴとして、当該更新周期ΔＴと前記観測誤差分散行列Ｗ _ｎ とを用いて、
   
   

   

   で表される最初の誤差共分散行列初期値Ｐ _０，１ を算出する誤差共分散行列初期値算出手順と、
   前記目標の加速度に相当する標準偏差α _ｘ ，α _ｙ を駆動雑音とし又は任意に設定した値を当該駆動雑音として、該駆動雑音、前記観測誤差分散行列Ｗ _ｎ 、更新周期ΔＴを用いて前記誤差共分散行列初期値Ｐ _０，１ を繰返し算出し、当該差共分散行列初期値の収束値を算出する誤差共分散行列初期値収束手順と、
   を含むことを特徴とする誤差共分散行列初期値設定方法。
5. 前記誤差共分散行列初期値収束手順は、
   前記誤差共分散行列初期値Ｐ _０，１ から
   
   

   

   （ただし、Ｐ _０，１ ハット＝Ｐ _０，１ 、ｋ−１｛ｋは２以上の整数｝は繰り返し計算回数、また、行列、ベクトルの右上添字「Ｔ」は、行列、ベクトルの転置を表し、行列の右上添字「−１」は、行列の逆行列を表す）
   で表される前記誤差共分散行列の予測値Ｐ _0,k チルダを計算する予測値算出手順と、
   前記誤差共分散行列の予測値Ｐ _0,k チルダと、観測行列Ｈと、前記観測誤差分散行列Ｗ _ｎ とを用いて、
   
   

   

   で表されるカルマンゲインＫを計算するするカルマンゲイン算出手順と、
   前記誤差共分散行列の予測値Ｐ _０，ｋ チルダと、前記カルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて、
   
   

   

   で表される前記誤差共分散行列の推定値Ｐ _０，ｋ ハットを計算する推定値算出手順と、
   前記誤差共分散行列の推定値Ｐ _０，ｋ ハットが所定範囲内か否かを判断して、所定範囲外の場合には、前記推定値算出処理で算出した前記誤差共分散行列の推定値Ｐ _０，ｋ ハットを前記誤差共分散行列初期値Ｐ _０，１ とみなして、前記誤差共分散行列の推定値Ｐ _０，ｋ ハットが当該所定範囲に収るまで、前記予測値算出処理、カルマンゲイン算出処理、推定値算出処理を順次繰返し行うことにより前記誤差共分散行列初期値の収束値を算出する収束手順と、を含むことを特徴とする請求項４記載の誤差共分散行列初期値設定方法。

Images