航跡生成システム、その誤差共分散行列初期値設定装置、航跡生成方法およびその誤差共分散行列初期値設定方法
【課題】誤差共分散行列初期値として適切な値を算出し設定することによって、航跡を生成した直後における航跡諸元の精度および安定性を向上させる。
【解決手段】所定状態および誤差共分散行列の初期値を設定してその後の状態および誤差共分散行列を推定するカルマンフィルタを用いたシステムにおける前記誤差共分散行列の初期値を設定する誤差共分散行列初期値設定装置１０において、前記誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して状態の誤差を適切な値へ収束させる誤差共分散行列初期値収束手段１４を有する。
【解決手段】所定状態および誤差共分散行列の初期値を設定してその後の状態および誤差共分散行列を推定するカルマンフィルタを用いたシステムにおける前記誤差共分散行列の初期値を設定する誤差共分散行列初期値設定装置１０において、前記誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して状態の誤差を適切な値へ収束させる誤差共分散行列初期値収束手段１４を有する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、航跡生成システム、その誤差共分散行列初期値設定装置、航跡生成方法およびその誤差共分散行列初期値設定方法に関し、特に航跡諸元の精度および安定性を改善した跡生成方法およびその誤差共分散行列初期値設定方法に関する。
【背景技術】
【０００２】
一般に、航空機などの目標追尾を行う代表例として、航跡追尾システムがある。この航跡追尾システムは、通常カルマンフィルタを用いた演算により、目標の航跡諸元の推定を行っている。この航跡諸元の推定は、航跡および誤差共分散行列（航空機の追尾結果の位置と速度（航跡位置と速度）の誤差範囲）の初期値を設定してから演算が開始される。
【０００３】
なお、誤差共分散行列とは、航空機の追尾結果の位置と速度（航跡位置と速度）の誤差範囲を表し、航空機の位置と速度がどの程度確からしいかというのが行列として表現されている。そして追尾処理を始めるに当たり、一番初めに、この位置辺りに航空機がいるか、その速度はこの位かと思って、およその航跡を生成する。この時、一番初めに与える誤差共分散が、「誤差共分散行列初期値」である。
【０００４】
ここで前提となる航跡生成装置２０の全体構成を、図６を参照しながら説明する。図６において、航跡生成装置２０は、航跡諸元初期値算出器２１と誤差共分散行列初期値算出器１０ｂとから構成されている。航跡諸元初期値算出器２０は、観測値を入力することによって航跡諸元初期値を算出する。誤差共分散行列初期値算出器１０ｂは、観測値等を入力することによって誤差共分散行列初期値を算出する。
【０００５】
ここでは、カルマンフィルタを用いた目標追尾の代表的な例として、観測値Ｑをセンサ中心の距離ｒおよび方位θで与え、その誤差をそれぞれの標準偏差σｒ，σθで与える系を考える。また、航跡諸元は２次元ｘｙ座標での位置ｘ ，ｙおよび速度ｘドット，ｙドットとし、駆動雑音として加速度に相当する標準偏差αｘ，αｙで与える。観測値および航跡の更新周期をΔＴとし、航跡の誤差共分散行列をＰ、その初期値をＰ０とする。このような系を考えると、航跡諸元初期値Ｘ０、誤差共分散行列初期値Ｐ０、観測値Ｑ０，Ｑ１は次の式（１）〜（３）で表される。なお、Ｐ１１〜Ｐ４４は、航跡の誤差共分散行列初期値の各要素で４＊４の行列を示す。
【０００６】
【数１】
【０００７】
従来の誤差共分散行列初期値算出器の一例を、図７のブロック図に示し、その処理フローを図８に示す。この誤差共分散行列初期値算出器１０ｂは、変換行列算出部１１と、観測誤差分散行列算出部１２と、誤差共分散行列初期値算出部１３と、制御部１５ｂとから構成されている。
制御部１５ｂは、変換行列算出部１１、観測誤差分散行列算出部１２、誤差共分散行列初期値算出部１の全体を制御し、誤差共分散行列初期値算出器１０ｂ内の各機能の作動順序の制御を行う。変換行列算出部１１は、観測値を用いて変換行列を算出する。観測誤差分散行列算出部１２は、変換行列および誤差標準偏差σｒ，σθを用いて観測誤差分散行列を算出する。誤差共分散行列初期値算出部１３は、観測誤差分散行列および更新周期ΔＴを用いて最初の誤差共分散行列初期値を算出する。
【０００８】
このような構成の誤差共分散行列初期値算出器１０ｂは、次のように動作する。まず、変換行列算出部１１は、観測値Ｑを用いて変換行列ΓＣｎを式（４）のように算出する（ステップＳ１）。
【０００９】
【数２】
【００１０】
次に、観測誤差分散行列算出部１２は、変換行列ΓＣｎおよび誤差標準偏差σｒ，σθを用いて観測誤差分散行列Ｗｎを式（５）のように算出する（ステップＳ２）。
【００１１】
【数３】
【００１２】
さらに、誤差共分散行列初期値算出部１３は、観測誤差分散行列Ｗｎおよび更新周期ΔＴを用いて最初の誤差共分散行列初期値Ｐ０，１を式（６）のように算出する（ステップＳ３）。
【００１３】
【数４】
【００１４】
このような従来の誤差共分散行列初期値算出器は、得られた観測値の観測誤差から誤差共分散行列初期値を算出していた。そのため、航跡を生成した後しばらくの間、誤差共分散行列が最適な値ではなく、結果として航跡諸元の精度および安定性が悪かった。また、操作者が航跡諸元を変更した場合において、その後しばらくの間は誤差共分散行列が最適な値ではなく、結果として航跡諸元の精度および安定性が悪かったり、航跡諸元が操作者の意図しない挙動となる事があった。
【００１５】
なお、特許文献１には、カルマンフィルタを用いて目標追尾を行う追尾装置が説明されているが、これは、目標の着弾点の予測の初期値精度を向上させたものである。この装置は、目標の観測位置および距離、仰角、方位角の観測雑音の標準偏差を用いて、平滑ベクトル、平滑誤差共分散行列を算出する構成となっている。この構成によれば、事前に決められた時刻より前の平滑ベクトルおよび平滑誤差共分散行列を再算出することにより、着弾点予測の初期値の精度を向上させ、さらに、着弾点予測を精度良く行うことができる。
【００１６】
さらに、特許文献２にも、カルマンフィルタ平滑効果を制御できる目標追尾装置が示されている。この装置は、観測雑音共分散行列、疑似予測誤差共分散行列、駆動雑音共分散行列及び観測時刻を用いて繰り返し計算を行い疑似平滑誤差共分散行列を求めることが説明されている。
【００１７】
【特許文献１】特開２００３−３２９７７１号
【特許文献２】特開２００２−１７４６８１号
【発明の開示】
【発明が解決しようとする課題】
【００１８】
しかし、従来の誤差共分散行列初期値の算出方法は、得られた観測値の観測誤差から誤差共分散行列初期値を算出していた。そのため、航跡を生成した後しばらくの間、誤差共分散行列が最適な値ではなく、結果として航跡諸元の精度および安定性が悪かった。また、操作者が航跡諸元を変更した場合において、その後しばらくの間誤差共分散行列が最適な値ではなく、結果として航跡諸元の精度および安定性が悪かったり、航跡諸元が操作者の意図しない挙動となる事があった。
【００１９】
なお、この特許文献１における誤差共分散行列は、最初の観測位置および距離、仰角、方位角の観測雑音の標準偏差を用いて平滑誤差共分散行列を計算しているかもしれないが、
その誤差共分散行列は追尾途中の誤差範囲を示しているに過ぎず、また上記従来例の誤差共分散行列初期値算出器と同等の構成にすぎない。
【００２０】
また、特許文献２に説明された誤差共分散行列も、特許文献１と同様に、追尾途中の誤差範囲を示しているに過ぎない。
【００２１】
本発明の主な目的は、差共分散行列初期値として最適な値を算出し設定することによって、航跡を生成した直後における航跡諸元の精度および安定性を向上させた航跡生成装置、その誤差共分散行列初期値演算装置、航跡生成方法およびその誤差共分散行列初期値演算方法を提供することにある。
【課題を解決するための手段】
【００２２】
本発明の構成は、所定状態および誤差共分散行列の初期値を設定してその後の状態および誤差共分散行列を推定するカルマンフィルタを用いたシステムにおける前記誤差共分散行列の初期値を算出し設定する誤差共分散行列初期値設定装置において、前記誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して状態の誤差を適切な値へ収束させる誤差共分散行列初期値収束手段を有することを特徴とする。
【００２３】
本発明において、誤差共分散行列初期値収束手段が、最初の誤差共分散行列初期値から誤差共分散行列の予測値を計算し、この誤差共分散行列の予測値と観測誤差分散行列とを用いてカルマンゲインを計算し、前記誤差共分散行列の予測値と前記カルマンゲインとから誤差共分散行列の推定値を計算し、この誤差共分散行列の推定値が所定範囲にない時、前記誤差共分散行列の推定値を前記最初の誤差共分散行列初期値として次の誤差共分散行列の予測値、推定値を計算し、前記誤差共分散行列の推定値が所定範囲になった時、この演算を終了し、前記誤差共分散行列初期値を収束させることができる。
【００２４】
また、最初の誤差共分散行列初期値Ｐ０，１が、観測誤差標準偏差から求められた観測誤差共分散行列から計算される時、誤差共分散行列初期値収束手段が、前記誤差共分散行列初期値Ｐ０，１と目標の運動モデルΦと駆動雑音用変換行列ΓＤと駆動雑音分散行列Ｕとを用いて誤差共分散行列の予測値Ｐ０，ｋチルダを
【００２５】
【数１２】
【００２６】
により算出し（ただし、Ｐ０，１チルダ＝Ｐ０，１、ｋ−１｛ｋは２以上の整数｝は繰り返し計算回数、また、行列、ベクトルの右上添字「Ｔ」は、行列、ベクトルの転置を表し、行列の右上添字「−１」は、行列の逆行列を表す）、前記誤差共分散行列の予測値Ｐ０，ｋチルダと観測行列Ｈと観測誤差分散行列Ｗｎとを用いてカルマンゲインＫを
【００２７】
【数１３】
【００２８】
により算出し、前記誤差共分散行列の予測値Ｐ０，ｋチルダと前記カルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて誤差共分散行列の推定値Ｐ０，ｋハットを
【００２９】
【数１４】
【００３０】
により算出することができる。さらに、観測誤差標準偏差から観測誤差共分散行列Ｗ１を計算し、この観測誤差共分散行列Ｗ１と更新周期ΔＴとから 最初の誤差共分散行列初期値Ｐ０，１を計算する、または予め定義しておいた値を使用し最初の誤差共分散行列初期値Ｐ０，１を設定することができ、また、観測誤差標準偏差が状態諸元の座標系と異なる座標系で求められた時、観測誤差共分散行列を座標変換行列で変換し、観測誤差共分散行列を求めりことができる。
【００３１】
本発明の他の構成は、移動する目標体を追尾する場合に、前記目標体の航跡の生成または航跡諸元の変更を行う航跡生成システムにおいて、前記航跡の生成または前記諸元の変更を行うする場合の航跡の誤差共分散行列初期値を設定する際に、前述の誤差共分散行列初期値設定装置を用いたことを特徴とする。
【００３２】
本発明のさらに他の構成は、ナビゲーションシステム等における、自己位置の確立または変更を行う自己位置確定システムにおいて、前記自己位置の生成する場合の位置の誤差共分散行列初期値を設定する際に、請求項１乃至５のうちの１項に記載の航誤差共分散行列初期値設定装置を用いたことを特徴とする
本発明のまた他の構成は、気温や傾斜などの状態の観測装置における、状態の確立または変更を行う観測確立システムにおいて、前記状態の確立または変更を行う場合の状態の誤差共分散行列初期値を設定する際に、上述した誤差共分散行列初期値設定装置を用いたことを特徴とする。
【００３３】
本発明の他の構成は、所定状態および誤差共分散行列の初期値を設定してその後の状態および誤差共分散行列を推定するカルマンフィルタを用いたシステムにおける前記誤差共分散行列の初期値を算出し設定する誤差共分散行列初期値設定方法において、前記状態を生成する際に状態の誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して状態の誤差を適切な値へ収束させる誤差共分散行列初期値収束方法を用いることを特徴とする。
【００３４】
本発明のさらに他の構成は、移動する目標体を追尾する場合に、前記目標体の航跡の生成および航跡諸元の変更を行う航跡生成方法において、前記航跡を生成する場合の航跡の誤差共分散行列初期値を設定する際に、上述した誤差共分散行列初期値設定方法を用いることを特徴とする。
【発明の効果】
【００３５】
以上説明したように、この発明の構成によれば、誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して演算する誤差共分散行列初期値収束手段を持つため、誤差共分散行列初期値を適切な値に算出することができ、状態（航跡）を生成した直後においても状態（航跡）諸元の精度および安定性を向上することができるという効果がある。
【発明を実施するための最良の形態】
【００３６】
次に図面により本発明の実施形態を説明する。図１は本発明の一実施形態の誤差共分散行列初期値設定装置の基になる誤差共分散行列初期値算出器のブロック図、図２はその処理を説明するフロー図である。この図１に示すように、本実施形態のは誤差共分散行列初期値設定装置は、従来例と同様の図６に示す航跡生成システムに用いられるが、誤差共分散行列初期値算出器１０の算出結果を基にして、誤差共分散行列初期値を設定するものである。ただし、図６，７の誤差共分散行列初期値算出器１０ｂの代わりに、図１の誤差共分散行列初期値算出器１０を用いる。
【００３７】
本実施形態において、誤差共分散行列初期値設定装置は、誤差共分散行列初期値算出器１０の算出結果を基にして、誤差共分散行列初期値を設定する。誤差共分散行列初期値算出器１０は、従来例の図７に対して、誤差共分散行列初期値収束部１４が付加されており、また、図２に示すように、従来例の図８に対して、本実施形態は、誤差共分散行列初期値を収束させるステップＳ４が付加されている。すなわち、本実施形態では、誤差共分散行列初期値算出器１０が、変換行列算出部１１と、観測誤差分散行列算出部１２と、誤差共分散行列初期値算出部１３と、誤差共分散行列初期値収束部１４と、制御部１５とから構成されている。
【００３８】
本実施形態において、変換行列算出部１１は観測値から変換行列を算出する。観測誤差分散行列算出部１２は変換行列および誤差標準偏差を用いて観測誤差分散行列を算出する。誤差共分散行列初期値算出部１３は観測誤差分散行列および更新周期を用いて最初の誤差共分散行列初期値を算出する。誤差共分散行列初期値収束部１４は観測誤差分散行列、更新周期および駆動雑音を用いて繰返し計算を行い誤差共分散行列初期値を収束させる。得られた収束後の誤差共分散行列初期値を、生成する航跡の誤差共分散行列初期値として使用する。制御部１５は、誤差共分散行列初期値算出器１０が、変換行列算出部１１、観測誤差分散行列算出部１２、誤差共分散行列初期値算出部１３、誤差共分散行列初期値収束部１４を制御し、誤差共分散行列初期値算出器１０内の各機能の作動順序の制御を行う。
【００３９】
本実施形態における誤差共分散行列初期値収束部１４は、「時間が経過するとともにある観測値が入力されるだろう」と想像して航跡の誤差を適切な値へ収束させるよう動作する。
【００４０】
本実施形態の主要動作は次の通りである。まず、変換行列算出部１１は観測値Ｑから変換行列ΓＣｎを算出する（ステップＳ１）。観測誤差分散行列算出部１２は変換行列ΓＣｎおよび誤差標準偏差σを用いて観測誤差分散行列Ｗｎを算出する（ステップＳ２）。誤差共分散行列初期値算出部１３は観測誤差分散行列Ｗｎおよび更新周期ΔＴを用いて最初の誤差共分散行列初期値Ｐ０，１を算出する（ステップＳ３）。誤差共分散行列初期値収束部１４は観測誤差分散行列Ｗｎ、更新周期ΔＴおよび駆動雑音αを用いて繰返し計算を行い誤差共分散行列初期値を収束させる（ステップＳ４）。得られた収束後の誤差共分散行列初期値を、生成する航跡の誤差共分散行列初期値として使用する。また、制御部１５は誤差共分散行列初期値算出器１０内の各機能の作動順序の制御を行う。
【００４１】
このステップＳ４は、図３の詳細フロー図に示される。この図は、誤差共分散行列初期値を収束させるための処理を説明している。ここで用いる計算式は、本来追尾処理において徐々に収束していくものであるが、観測値が入力されるだろうと想定して、航跡生成時に一気に収束させるようにしている。そのために追尾処理に用いられる数式を使用する必要がある。
なお、目標の運動モデルΦを設定し、カルマンフィルタの条件から駆動雑音用変換行列ΓＤと駆動雑音分散行列Ｕを設定する。また、観測行列Ｈも設定でき、観測誤差分散行列Ｗｎも計算できる。なお、図３において、行列およびベクトルの右上添字「Ｔ」は、行列およびベクトルの転置を表すこととし、また、行列の右上添字「−１」は、行列の逆行列を表すこととする。また、ｋ−１（ｋは２以上の整数）は繰り返し計算回数を示す。
【００４２】
図３において、前述のステップＳ３で算出した最初の誤差共分散行列初期値Ｐ０，１と目標の運動モデルΦと駆動雑音用変換行列ΓＤと駆動雑音分散行列Ｕとを用いて誤差共分散行列の予測値Ｐ０，ｋチルダが算出できる（ステップＳ１１）。
【００４３】
【数１２】
【００４４】
次に、ステップＳ１１で算出した誤差共分散行列の予測値Ｐ０，ｋチルダと観測行列Ｈと観測誤差分散行列Ｗｎとを用いてカルマンゲインＫが算出できる（ステップＳ１２）。
【００４５】
【数１３】
【００４６】
次に、ステップＳ１１〜Ｓ１２で算出した誤差共分散行列の予測値Ｐ０，ｋチルダとカルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて誤差共分散行列の推定値Ｐ０，ｋハットが算出できる（ステップＳ１３）。
【００４７】
【数１４】
【００４８】
最後に、ステップＳ１４で、終了するか否かを判定し、終了しない（否）ならステップＳ１３で算出した誤差共分散行列の推定値を用いて再度ステップＳ１１〜Ｓ１３を繰り返す。また、ステップＳ１４を終了するなら、ステップＳ１３で算出した誤差共分散行列の推定値を誤差共分散行列初期値算出器１０の出力としてステップＳ４を終了する。なお、ステップＳ１４の判定方法としては、規定の回数に到達したら終了する方法、または誤差共分散行列の推定値が規定の大きさ以内に到達したら終了する方法等がある。
【００４９】
なお、最初の誤差共分散行列初期値Ｐ０，１は、予め定義しておいた値を使用して最初の誤差共分散行列初期値として設定することもできる。
【００５０】
本実施形態によれば、カルマンフィルタを用いた目標追尾において、追尾する目標の航跡を生成する際に航跡の誤差共分散行列初期値を設定するが、この誤差共分散行列初期値として適切な値を算出し設定することによって、航跡を生成した直後における航跡諸元の精度および安定性を向上する。
【００５１】
また、誤差標準偏差および駆動雑音を任意に設定することができ、任意の誤差共分散行列初期値を設定することができるため、航跡を生成した直後において航跡に任意の挙動特性を持たせることができる。
【００５２】
また、これら効果については、航跡を生成した場合の他、操作者が航跡諸元を変更した場合や、他のシステムから航跡を移管した場合にも適用可能であり、同様の効果を得ることができる。
【実施例１】
【００５３】
本発明の第１の実施例は、図１〜図３で説明した実施形態と同様である。次に、図１、図２を参照して本実施形態の動作について詳細に説明する。
【００５４】
ここでは、従来例と同様に、カルマンフィルタを用いた目標追尾の代表的な例として、観測値Ｑをセンサ中心の距離ｒおよび方位θで与え、その誤差をそれぞれの標準偏差σｒ，σθで与える系を考える。また、航跡諸元は２次元ｘｙ座標での位置ｘ ，ｙおよび速度ｘドット，ｙドットとし、駆動雑音として加速度に相当する標準偏差αｘ，αｙで与える。観測値および航跡の更新周期をΔＴとし、航跡の誤差共分散行列をＰ、その初期値をＰ０とする。このような系を考えると、航跡諸元初期値Ｘ０、誤差共分散行列初期値Ｐ０、観測値Ｑ０，Ｑ１は、前述の式（１）〜（３）で表される。
【００５５】
【数１】
【００５６】
この誤差共分散行列初期値算出器１０は次のように動作する。まず、変換行列算出部１１は、観測値Ｑを用いて変換行列ΓＣｎを式（４）のように算出する（ステップＳ１）。
【００５７】
【数２】
【００５８】
次に、観測誤差分散行列算出部１２は、変換行列ΓＣｎおよび誤差標準偏差σｒ，σθを用いて観測誤差分散行列Ｗｎを式（５）のように算出する（ステップＳ２）。
【００５９】
【数３】
【００６０】
さらに、誤差共分散行列初期値算出部１３は、観測誤差分散行列Ｗｎおよび更新周期ΔＴを用いて最初の誤差共分散行列初期値Ｐ０，１を式（６）のように算出する（ステップＳ３）。
【００６１】
【数４】
【００６２】
そして、誤差共分散行列初期値収束部１４は、観測誤差分散行列Ｗｎ、更新周期ΔＴをおよび駆動雑音を用いて繰返し計算を行い誤差共分散行列初期値を収束させる（ステップＳ４）。
【００６３】
このステップＳ４は、図３のフロー図を用いて詳しく説明する。ここでは、誤差共分散行列初期値を収束させるための処理である。ここで用いる計算式は、本来追尾処理において徐々に収束していくものであるが、観測値が入力されるだろうと想定して、航跡生成時に一気に収束させるようにしている。そのために追尾処理に用いられる数式を使用する必要がある。
【００６４】
ここで、目標の運動モデルΦは、航跡の想定する飛行モデルであり、例えば次の式（７）のように定義できる。また、駆動雑音用変換行列ΓＤは、駆動雑音分散行列Ｕを誤差共分散行列初期値Ｐに加えるときに必要な変換であり、次の式（８）のようになる。また、駆動雑音分散行列Ｕは、誤差共分散行列初期値Ｐへある一定の雑音を付加し、航跡の軌道の不確定さを表すもので、例えば次の式（９）のように定義できる。また、観測行列Ｈは、誤差共分散行列初期値Ｐのもつ情報種類から観測値のもつ情報種類を取出す為の行列であり、次の式（１０）のようになるが、ここでは、Ｘ位置、Ｙ位置のみの情報を取出す働きを持つ。なお、行列およびベクトルの右上添字「Ｔ」は、行列およびベクトルの転置を表すこととし、また、行列の右上添字「−１」は、行列の逆行列を表すこととする。
【００６５】
【数５】
【００６６】
これらの式とステップＳ３で算出した最初の誤差共分散行列初期値Ｐ０，１とを用いると、誤差共分散行列の予測値Ｐ０，ｋチルダが、次の式（１１）のように算出できる。この式（１１）は、収束させるために、擬似的に誤差共分散行列初期値Ｐ０を予測することを意味し、追尾処理中では、推定した航跡の誤差に対し、ある時間経過後の誤差を算出している予測式を意味する。ただし、Ｐ０，１ハット＝Ｐ０，１であり、ｋ−１は繰り返し計算回数を示す。
【００６７】
【数６】
【００６８】
また、観測行列Ｈ、観測誤差分散行列ＷｎおよびステップＳ１１で算出した誤差共分散行列の予測値Ｐ０，ｋチルダを用いると、カルマンゲインＫは次の式（１２）のように算出できる。この式も、収束させるために、擬似的にカルマンゲインＫを求めている。
【００６９】
【数７】
【００７０】
また、誤差共分散行列の予測値Ｐ０，ｋチルダと、カルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いると、誤差共分散行列の推定値Ｐ０，ｋハットを次の式（１３）のように算出できる。この式（１３）も、収束させるために、擬似的にＰ０を推定しており、追尾処理中では、予測した航跡の誤差に対し、ある観測値を得た後の誤差を算出している推定式を意味する。
【００７１】
【数８】
【００７２】
なお、「航跡の予測値」というのは、継続的に行う追尾処理の最中の、航跡諸元（位置と速度）の時間外挿値（未来の値）のことである。この「航跡の予測値」の誤差範囲も物理的には「誤差共分散行列初期値」と同じ内容であるが、追尾最中であるか、一番初めの値であるか、の相違がある。
【００７３】
従って、図３においては、前述のステップＳ３で算出した最初の誤差共分散行列初期値Ｐ０，１と目標の運動モデルΦと駆動雑音用変換行列ΓＤと駆動雑音分散行列Ｕとを用いて誤差共分散行列の予測値Ｐ０，ｋチルダを算出する（ステップＳ１１）。
【００７４】
次に、ステップＳ１１で算出した誤差共分散行列の予測値Ｐ０，ｋチルダと観測行列Ｈと観測誤差分散行列Ｗｎとを用いてカルマンゲインＫを算出する（ステップＳ１２）。
【００７５】
次に、ステップＳ１１〜Ｓ１２で算出した誤差共分散行列の予測値Ｐ０，ｋチルダとカルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて誤差共分散行列の推定値Ｐ０，ｋハットを算出する（ステップＳ１３）。
【００７６】
最後に、ステップＳ１４で、終了するか否かを判定し、終了しない（否）ならステップＳ１３で算出した誤差共分散行列の推定値（式（１３））を用いて再度ステップＳ１１〜Ｓ１３を繰り返す。また、ステップＳ１４を終了するなら、ステップＳ１３で算出した誤差共分散行列の推定値を誤差共分散行列初期値算出器１０の出力としてステップＳ４を終了する。なお、ステップＳ１４の判定方法としては、規定の回数に到達したら終了する方法、または誤差共分散行列の推定値が規定の大きさ以内に到達したら終了する方法等がある。
【００７７】
次に、本実施例の効果について説明する。本実施例では、誤差共分散行列初期値算出器１０の中に誤差共分散行列初期値収束部１４を持つ構成となっているため、誤差共分散行列初期値を適切な値に算出することができ、航跡を生成した直後においても航跡諸元の精度および安定性を向上させることができる。
【００７８】
また、本実施例では、さらに、誤差標準偏差および駆動雑音を任意に設定することができ且つ誤差共分散行列初期値収束部１４を持つ構成であるため、航跡を生成した直後において任意の誤差共分散行列初期値を設定し、航跡に任意の挙動特性を持たせることができる。すなわち、航跡の位置の誤差範囲はセンサ中心の距離および方位による誤差標準偏差σｒ，σθで任意に与えることができる。
【実施例２】
【００７９】
次に、図４は本発明の第２の実施例の誤差共分散行列初期値算出器１０ａの構成を説明するブロック図、図５はその処理フロー図である。図４を参照すると、本実施例の誤差共分散行列初期値算出器１０ａの構成は、第１の実施例から変換行列算出部１１および図２におけるステップＳ１を省いたものとなっている。すなわち、本実施例は、観測値が極座標ｒ，θの代わりに座標ｘｙを用いているので、変換行列の算出（Ｓ１）が不要となり、計算が実施例１よりも簡単化されている。本実施例は、観測誤差分散行列算出部１２ａと、誤差共分散行列初期値算出部１３と、誤差共分散行列初期値収束部１４と、制御部１５ａとから構成されている。ここで前提となる航跡生成装置２０は、従来例の全体構成を図６と同様である。
【００８０】
観測誤差分散行列算出部１２ａは、誤差標準偏差を用いて観測誤差分散行列を算出する。誤差共分散行列初期値算出部１３は観測誤差分散行列および更新周期を用いて最初の誤差共分散行列初期値を算出する。誤差共分散行列初期値収束部１４は観測誤差分散行列、更新周期および駆動雑音を用いて繰返し計算を行い誤差共分散行列初期値を収束させる。制御部１５ａは、誤差共分散行列初期値算出器１０ａ内の各機能の作動順序の制御を行う。
【００８１】
次に、図４、図５を参照して本実施例の動作について詳細に説明する。ここではカルマンフィルタを用いた目標追尾の代表的な例として、観測値は２次元ｘｙ座標で与えられ、その誤差はそれぞれの標準偏差σｘ，σｙで与えられる。また、航跡諸元Ｘも２次元ｘｙ座標での位置および速度とし、駆動雑音として加速度に相当する標準偏差で与える。観測値および航跡の更新周期をΔＴとし、航跡の誤差共分散行列をＰとする。このような系を考えると、航跡諸元初期値Ｘ０、誤差共分散行列初期値Ｐ０は、前述の式（１）（２）と同様で、観測値Ｑ０，Ｑ１は次の式（１４）で与えられる。
【００８２】
【数９】
【００８３】
まず、観測誤差分散行列算出部１２ａは，誤差標準偏差σｘ，σｙを用いて観測誤差分散行列Ｗ１を次の式（１５）で算出する（ステップＳ２ａ）。
【００８４】
【数１０】
【００８５】
次に、誤差共分散行列初期値算出部１３は、観測誤差分散行列Ｗ１および更新周期ΔＴを用いて最初の誤差共分散行列初期値Ｐ０，１を次の式（１６）で算出する（ステップＳ３）。
【００８６】
【数１１】
【００８７】
さらに、誤差共分散行列初期値収束部１４は、観測誤差分散行列Ｗ１、更新周期ΔＴおよび駆動雑音を用いて繰返し計算を行い誤差共分散行列初期値を収束させる（ステップＳ４）。なお、ステップＳ４は第１の実施例と同様のため説明は省略する。
【００８８】
次に、本実施例の効果について説明する。本実施例は、前述のように変換行列の算出（Ｓ１）が不要となり、計算が実施例１よりも簡単化されている。また、本実施例でも、第１の実施例と同様に、誤差共分散行列初期値算出器１０の中に誤差共分散行列初期値収束部１４を持つ構成のため、誤差共分散行列初期値を適切な値に算出することができ、航跡を生成した直後においても航跡諸元の精度および安定性を向上することができる。
【００８９】
また、本実施例では、さらに、誤差標準偏差および駆動雑音を任意に設定することができ且つ誤差共分散行列初期値収束部１４を持つ構成のため、航跡を生成した直後において任意の誤差共分散行列初期値を設定し航跡に任意の挙動特性を持たせることができる。すなわち、航跡の位置の誤差範囲および航跡の速度の誤差範囲はｘｙ座標のｘおよびｙによる誤差標準偏差σｘ，σｙで任意に与えることができる。
【実施例３】
【００９０】
次に、本発明の第３の実施例について説明する。第１の実施例および第２の実施例では２次元上での目標追尾を前提に説明したが、本実施例では、１次元上で同様に実施することである。航跡諸元初期値、誤差共分散行列初期値、観測値、誤差標準偏差、および駆動雑音の要素が変更となり、それに伴い算出式等が変更となるが公知の理論により求めることができるため、ここでは説明を省略する。
【００９１】
さらに、本発明の第４の実施例について説明する。第１の実施例および第２の実施例では２次元上での目標追尾を前提に説明したが、本実施例では、ｎ（ただしｎは３以上）次元上で同様に実施することである。航跡諸元初期値、誤差共分散行列初期値、観測値、誤差標準偏差、および駆動雑音の要素が変更となり、それに伴い算出式等が変更となるが公知の理論により求めることができるため、ここでは説明を省略する。
【産業上の利用可能性】
【００９２】
本発明においては、航空機などの目標体を追尾する場合における、航跡の生成および航跡諸元の変更に適用した場合を説明したが、本発明は、航空機の他に、車両（自動車、戦車）、船舶、ミサイル、人工衛星等移動する物体を追尾する場合における、航跡の生成および航跡諸元の変更にも適用することができる。
【００９３】
また、本発明は、ナビゲーションシステム等における、自己位置の確立および変更や、気温や傾斜などの状態の観測装置における、状態の確立および変更にも適用することができる。
【００９４】
この場合、ナビゲーションシステム等における、自己位置の確立または変更を行う自己位置確定システムとしては、自己位置を生成または変更する場合の位置の誤差共分散行列初期値を設定する際に、前述した誤差共分散行列初期値設定装置を用いることができる。また、気温や傾斜などの状態の観測装置における、状態の確立または変更を行う観測確立システムとしては、状態の確立または変更する場合の状態の誤差共分散行列初期値を設定する際に、前述した誤差共分散行列初期値設定装置を用いることができる。
【図面の簡単な説明】
【００９５】
【図１】本発明の第１の実施形態の誤差共分散行列初期値算出器を説明するブロック図である。
【図２】図１の処理を説明するフロー図である。
【図３】図２のステップＳ４の演算処理を説明するフロー図である。
【図４】本発明の第２の実施例の誤差共分散行列初期値算出器を説明するブロック図である。
【図５】図５の処理を説明するフロー図である。
【図６】一般の誤差共分散行列初期値算出器を用いた航跡生成装置の一例のブロック図である。
【図７】従来例（図７）の誤差共分散行列初期値算出器を説明するブロック図である。
【図８】図７の処理を説明するフロー図である。
【符号の説明】
【００９６】
１０，１０ａ，１０ｂ 誤差共分散行列初期値算出器
１１ 変換行列算出部
１２，１２ａ 観測誤差分散行列算出部
１３ 誤差共分散行列初期値算出部
１４ 誤差共分散行列初期値収束部
１５，１５ａ，１５ｂ 制御部
２０ 航跡生成装置
２１ 航跡諸元初期値算出器
【技術分野】
【０００１】
本発明は、航跡生成システム、その誤差共分散行列初期値設定装置、航跡生成方法およびその誤差共分散行列初期値設定方法に関し、特に航跡諸元の精度および安定性を改善した跡生成方法およびその誤差共分散行列初期値設定方法に関する。
【背景技術】
【０００２】
一般に、航空機などの目標追尾を行う代表例として、航跡追尾システムがある。この航跡追尾システムは、通常カルマンフィルタを用いた演算により、目標の航跡諸元の推定を行っている。この航跡諸元の推定は、航跡および誤差共分散行列（航空機の追尾結果の位置と速度（航跡位置と速度）の誤差範囲）の初期値を設定してから演算が開始される。
【０００３】
なお、誤差共分散行列とは、航空機の追尾結果の位置と速度（航跡位置と速度）の誤差範囲を表し、航空機の位置と速度がどの程度確からしいかというのが行列として表現されている。そして追尾処理を始めるに当たり、一番初めに、この位置辺りに航空機がいるか、その速度はこの位かと思って、およその航跡を生成する。この時、一番初めに与える誤差共分散が、「誤差共分散行列初期値」である。
【０００４】
ここで前提となる航跡生成装置２０の全体構成を、図６を参照しながら説明する。図６において、航跡生成装置２０は、航跡諸元初期値算出器２１と誤差共分散行列初期値算出器１０ｂとから構成されている。航跡諸元初期値算出器２０は、観測値を入力することによって航跡諸元初期値を算出する。誤差共分散行列初期値算出器１０ｂは、観測値等を入力することによって誤差共分散行列初期値を算出する。
【０００５】
ここでは、カルマンフィルタを用いた目標追尾の代表的な例として、観測値Ｑをセンサ中心の距離ｒおよび方位θで与え、その誤差をそれぞれの標準偏差σｒ，σθで与える系を考える。また、航跡諸元は２次元ｘｙ座標での位置ｘ ，ｙおよび速度ｘドット，ｙドットとし、駆動雑音として加速度に相当する標準偏差αｘ，αｙで与える。観測値および航跡の更新周期をΔＴとし、航跡の誤差共分散行列をＰ、その初期値をＰ０とする。このような系を考えると、航跡諸元初期値Ｘ０、誤差共分散行列初期値Ｐ０、観測値Ｑ０，Ｑ１は次の式（１）〜（３）で表される。なお、Ｐ１１〜Ｐ４４は、航跡の誤差共分散行列初期値の各要素で４＊４の行列を示す。
【０００６】
【数１】
【０００７】
従来の誤差共分散行列初期値算出器の一例を、図７のブロック図に示し、その処理フローを図８に示す。この誤差共分散行列初期値算出器１０ｂは、変換行列算出部１１と、観測誤差分散行列算出部１２と、誤差共分散行列初期値算出部１３と、制御部１５ｂとから構成されている。
制御部１５ｂは、変換行列算出部１１、観測誤差分散行列算出部１２、誤差共分散行列初期値算出部１の全体を制御し、誤差共分散行列初期値算出器１０ｂ内の各機能の作動順序の制御を行う。変換行列算出部１１は、観測値を用いて変換行列を算出する。観測誤差分散行列算出部１２は、変換行列および誤差標準偏差σｒ，σθを用いて観測誤差分散行列を算出する。誤差共分散行列初期値算出部１３は、観測誤差分散行列および更新周期ΔＴを用いて最初の誤差共分散行列初期値を算出する。
【０００８】
このような構成の誤差共分散行列初期値算出器１０ｂは、次のように動作する。まず、変換行列算出部１１は、観測値Ｑを用いて変換行列ΓＣｎを式（４）のように算出する（ステップＳ１）。
【０００９】
【数２】
【００１０】
次に、観測誤差分散行列算出部１２は、変換行列ΓＣｎおよび誤差標準偏差σｒ，σθを用いて観測誤差分散行列Ｗｎを式（５）のように算出する（ステップＳ２）。
【００１１】
【数３】
【００１２】
さらに、誤差共分散行列初期値算出部１３は、観測誤差分散行列Ｗｎおよび更新周期ΔＴを用いて最初の誤差共分散行列初期値Ｐ０，１を式（６）のように算出する（ステップＳ３）。
【００１３】
【数４】
【００１４】
このような従来の誤差共分散行列初期値算出器は、得られた観測値の観測誤差から誤差共分散行列初期値を算出していた。そのため、航跡を生成した後しばらくの間、誤差共分散行列が最適な値ではなく、結果として航跡諸元の精度および安定性が悪かった。また、操作者が航跡諸元を変更した場合において、その後しばらくの間は誤差共分散行列が最適な値ではなく、結果として航跡諸元の精度および安定性が悪かったり、航跡諸元が操作者の意図しない挙動となる事があった。
【００１５】
なお、特許文献１には、カルマンフィルタを用いて目標追尾を行う追尾装置が説明されているが、これは、目標の着弾点の予測の初期値精度を向上させたものである。この装置は、目標の観測位置および距離、仰角、方位角の観測雑音の標準偏差を用いて、平滑ベクトル、平滑誤差共分散行列を算出する構成となっている。この構成によれば、事前に決められた時刻より前の平滑ベクトルおよび平滑誤差共分散行列を再算出することにより、着弾点予測の初期値の精度を向上させ、さらに、着弾点予測を精度良く行うことができる。
【００１６】
さらに、特許文献２にも、カルマンフィルタ平滑効果を制御できる目標追尾装置が示されている。この装置は、観測雑音共分散行列、疑似予測誤差共分散行列、駆動雑音共分散行列及び観測時刻を用いて繰り返し計算を行い疑似平滑誤差共分散行列を求めることが説明されている。
【００１７】
【特許文献１】特開２００３−３２９７７１号
【特許文献２】特開２００２−１７４６８１号
【発明の開示】
【発明が解決しようとする課題】
【００１８】
しかし、従来の誤差共分散行列初期値の算出方法は、得られた観測値の観測誤差から誤差共分散行列初期値を算出していた。そのため、航跡を生成した後しばらくの間、誤差共分散行列が最適な値ではなく、結果として航跡諸元の精度および安定性が悪かった。また、操作者が航跡諸元を変更した場合において、その後しばらくの間誤差共分散行列が最適な値ではなく、結果として航跡諸元の精度および安定性が悪かったり、航跡諸元が操作者の意図しない挙動となる事があった。
【００１９】
なお、この特許文献１における誤差共分散行列は、最初の観測位置および距離、仰角、方位角の観測雑音の標準偏差を用いて平滑誤差共分散行列を計算しているかもしれないが、
その誤差共分散行列は追尾途中の誤差範囲を示しているに過ぎず、また上記従来例の誤差共分散行列初期値算出器と同等の構成にすぎない。
【００２０】
また、特許文献２に説明された誤差共分散行列も、特許文献１と同様に、追尾途中の誤差範囲を示しているに過ぎない。
【００２１】
本発明の主な目的は、差共分散行列初期値として最適な値を算出し設定することによって、航跡を生成した直後における航跡諸元の精度および安定性を向上させた航跡生成装置、その誤差共分散行列初期値演算装置、航跡生成方法およびその誤差共分散行列初期値演算方法を提供することにある。
【課題を解決するための手段】
【００２２】
本発明の構成は、所定状態および誤差共分散行列の初期値を設定してその後の状態および誤差共分散行列を推定するカルマンフィルタを用いたシステムにおける前記誤差共分散行列の初期値を算出し設定する誤差共分散行列初期値設定装置において、前記誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して状態の誤差を適切な値へ収束させる誤差共分散行列初期値収束手段を有することを特徴とする。
【００２３】
本発明において、誤差共分散行列初期値収束手段が、最初の誤差共分散行列初期値から誤差共分散行列の予測値を計算し、この誤差共分散行列の予測値と観測誤差分散行列とを用いてカルマンゲインを計算し、前記誤差共分散行列の予測値と前記カルマンゲインとから誤差共分散行列の推定値を計算し、この誤差共分散行列の推定値が所定範囲にない時、前記誤差共分散行列の推定値を前記最初の誤差共分散行列初期値として次の誤差共分散行列の予測値、推定値を計算し、前記誤差共分散行列の推定値が所定範囲になった時、この演算を終了し、前記誤差共分散行列初期値を収束させることができる。
【００２４】
また、最初の誤差共分散行列初期値Ｐ０，１が、観測誤差標準偏差から求められた観測誤差共分散行列から計算される時、誤差共分散行列初期値収束手段が、前記誤差共分散行列初期値Ｐ０，１と目標の運動モデルΦと駆動雑音用変換行列ΓＤと駆動雑音分散行列Ｕとを用いて誤差共分散行列の予測値Ｐ０，ｋチルダを
【００２５】
【数１２】
【００２６】
により算出し（ただし、Ｐ０，１チルダ＝Ｐ０，１、ｋ−１｛ｋは２以上の整数｝は繰り返し計算回数、また、行列、ベクトルの右上添字「Ｔ」は、行列、ベクトルの転置を表し、行列の右上添字「−１」は、行列の逆行列を表す）、前記誤差共分散行列の予測値Ｐ０，ｋチルダと観測行列Ｈと観測誤差分散行列Ｗｎとを用いてカルマンゲインＫを
【００２７】
【数１３】
【００２８】
により算出し、前記誤差共分散行列の予測値Ｐ０，ｋチルダと前記カルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて誤差共分散行列の推定値Ｐ０，ｋハットを
【００２９】
【数１４】
【００３０】
により算出することができる。さらに、観測誤差標準偏差から観測誤差共分散行列Ｗ１を計算し、この観測誤差共分散行列Ｗ１と更新周期ΔＴとから 最初の誤差共分散行列初期値Ｐ０，１を計算する、または予め定義しておいた値を使用し最初の誤差共分散行列初期値Ｐ０，１を設定することができ、また、観測誤差標準偏差が状態諸元の座標系と異なる座標系で求められた時、観測誤差共分散行列を座標変換行列で変換し、観測誤差共分散行列を求めりことができる。
【００３１】
本発明の他の構成は、移動する目標体を追尾する場合に、前記目標体の航跡の生成または航跡諸元の変更を行う航跡生成システムにおいて、前記航跡の生成または前記諸元の変更を行うする場合の航跡の誤差共分散行列初期値を設定する際に、前述の誤差共分散行列初期値設定装置を用いたことを特徴とする。
【００３２】
本発明のさらに他の構成は、ナビゲーションシステム等における、自己位置の確立または変更を行う自己位置確定システムにおいて、前記自己位置の生成する場合の位置の誤差共分散行列初期値を設定する際に、請求項１乃至５のうちの１項に記載の航誤差共分散行列初期値設定装置を用いたことを特徴とする
本発明のまた他の構成は、気温や傾斜などの状態の観測装置における、状態の確立または変更を行う観測確立システムにおいて、前記状態の確立または変更を行う場合の状態の誤差共分散行列初期値を設定する際に、上述した誤差共分散行列初期値設定装置を用いたことを特徴とする。
【００３３】
本発明の他の構成は、所定状態および誤差共分散行列の初期値を設定してその後の状態および誤差共分散行列を推定するカルマンフィルタを用いたシステムにおける前記誤差共分散行列の初期値を算出し設定する誤差共分散行列初期値設定方法において、前記状態を生成する際に状態の誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して状態の誤差を適切な値へ収束させる誤差共分散行列初期値収束方法を用いることを特徴とする。
【００３４】
本発明のさらに他の構成は、移動する目標体を追尾する場合に、前記目標体の航跡の生成および航跡諸元の変更を行う航跡生成方法において、前記航跡を生成する場合の航跡の誤差共分散行列初期値を設定する際に、上述した誤差共分散行列初期値設定方法を用いることを特徴とする。
【発明の効果】
【００３５】
以上説明したように、この発明の構成によれば、誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して演算する誤差共分散行列初期値収束手段を持つため、誤差共分散行列初期値を適切な値に算出することができ、状態（航跡）を生成した直後においても状態（航跡）諸元の精度および安定性を向上することができるという効果がある。
【発明を実施するための最良の形態】
【００３６】
次に図面により本発明の実施形態を説明する。図１は本発明の一実施形態の誤差共分散行列初期値設定装置の基になる誤差共分散行列初期値算出器のブロック図、図２はその処理を説明するフロー図である。この図１に示すように、本実施形態のは誤差共分散行列初期値設定装置は、従来例と同様の図６に示す航跡生成システムに用いられるが、誤差共分散行列初期値算出器１０の算出結果を基にして、誤差共分散行列初期値を設定するものである。ただし、図６，７の誤差共分散行列初期値算出器１０ｂの代わりに、図１の誤差共分散行列初期値算出器１０を用いる。
【００３７】
本実施形態において、誤差共分散行列初期値設定装置は、誤差共分散行列初期値算出器１０の算出結果を基にして、誤差共分散行列初期値を設定する。誤差共分散行列初期値算出器１０は、従来例の図７に対して、誤差共分散行列初期値収束部１４が付加されており、また、図２に示すように、従来例の図８に対して、本実施形態は、誤差共分散行列初期値を収束させるステップＳ４が付加されている。すなわち、本実施形態では、誤差共分散行列初期値算出器１０が、変換行列算出部１１と、観測誤差分散行列算出部１２と、誤差共分散行列初期値算出部１３と、誤差共分散行列初期値収束部１４と、制御部１５とから構成されている。
【００３８】
本実施形態において、変換行列算出部１１は観測値から変換行列を算出する。観測誤差分散行列算出部１２は変換行列および誤差標準偏差を用いて観測誤差分散行列を算出する。誤差共分散行列初期値算出部１３は観測誤差分散行列および更新周期を用いて最初の誤差共分散行列初期値を算出する。誤差共分散行列初期値収束部１４は観測誤差分散行列、更新周期および駆動雑音を用いて繰返し計算を行い誤差共分散行列初期値を収束させる。得られた収束後の誤差共分散行列初期値を、生成する航跡の誤差共分散行列初期値として使用する。制御部１５は、誤差共分散行列初期値算出器１０が、変換行列算出部１１、観測誤差分散行列算出部１２、誤差共分散行列初期値算出部１３、誤差共分散行列初期値収束部１４を制御し、誤差共分散行列初期値算出器１０内の各機能の作動順序の制御を行う。
【００３９】
本実施形態における誤差共分散行列初期値収束部１４は、「時間が経過するとともにある観測値が入力されるだろう」と想像して航跡の誤差を適切な値へ収束させるよう動作する。
【００４０】
本実施形態の主要動作は次の通りである。まず、変換行列算出部１１は観測値Ｑから変換行列ΓＣｎを算出する（ステップＳ１）。観測誤差分散行列算出部１２は変換行列ΓＣｎおよび誤差標準偏差σを用いて観測誤差分散行列Ｗｎを算出する（ステップＳ２）。誤差共分散行列初期値算出部１３は観測誤差分散行列Ｗｎおよび更新周期ΔＴを用いて最初の誤差共分散行列初期値Ｐ０，１を算出する（ステップＳ３）。誤差共分散行列初期値収束部１４は観測誤差分散行列Ｗｎ、更新周期ΔＴおよび駆動雑音αを用いて繰返し計算を行い誤差共分散行列初期値を収束させる（ステップＳ４）。得られた収束後の誤差共分散行列初期値を、生成する航跡の誤差共分散行列初期値として使用する。また、制御部１５は誤差共分散行列初期値算出器１０内の各機能の作動順序の制御を行う。
【００４１】
このステップＳ４は、図３の詳細フロー図に示される。この図は、誤差共分散行列初期値を収束させるための処理を説明している。ここで用いる計算式は、本来追尾処理において徐々に収束していくものであるが、観測値が入力されるだろうと想定して、航跡生成時に一気に収束させるようにしている。そのために追尾処理に用いられる数式を使用する必要がある。
なお、目標の運動モデルΦを設定し、カルマンフィルタの条件から駆動雑音用変換行列ΓＤと駆動雑音分散行列Ｕを設定する。また、観測行列Ｈも設定でき、観測誤差分散行列Ｗｎも計算できる。なお、図３において、行列およびベクトルの右上添字「Ｔ」は、行列およびベクトルの転置を表すこととし、また、行列の右上添字「−１」は、行列の逆行列を表すこととする。また、ｋ−１（ｋは２以上の整数）は繰り返し計算回数を示す。
【００４２】
図３において、前述のステップＳ３で算出した最初の誤差共分散行列初期値Ｐ０，１と目標の運動モデルΦと駆動雑音用変換行列ΓＤと駆動雑音分散行列Ｕとを用いて誤差共分散行列の予測値Ｐ０，ｋチルダが算出できる（ステップＳ１１）。
【００４３】
【数１２】
【００４４】
次に、ステップＳ１１で算出した誤差共分散行列の予測値Ｐ０，ｋチルダと観測行列Ｈと観測誤差分散行列Ｗｎとを用いてカルマンゲインＫが算出できる（ステップＳ１２）。
【００４５】
【数１３】
【００４６】
次に、ステップＳ１１〜Ｓ１２で算出した誤差共分散行列の予測値Ｐ０，ｋチルダとカルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて誤差共分散行列の推定値Ｐ０，ｋハットが算出できる（ステップＳ１３）。
【００４７】
【数１４】
【００４８】
最後に、ステップＳ１４で、終了するか否かを判定し、終了しない（否）ならステップＳ１３で算出した誤差共分散行列の推定値を用いて再度ステップＳ１１〜Ｓ１３を繰り返す。また、ステップＳ１４を終了するなら、ステップＳ１３で算出した誤差共分散行列の推定値を誤差共分散行列初期値算出器１０の出力としてステップＳ４を終了する。なお、ステップＳ１４の判定方法としては、規定の回数に到達したら終了する方法、または誤差共分散行列の推定値が規定の大きさ以内に到達したら終了する方法等がある。
【００４９】
なお、最初の誤差共分散行列初期値Ｐ０，１は、予め定義しておいた値を使用して最初の誤差共分散行列初期値として設定することもできる。
【００５０】
本実施形態によれば、カルマンフィルタを用いた目標追尾において、追尾する目標の航跡を生成する際に航跡の誤差共分散行列初期値を設定するが、この誤差共分散行列初期値として適切な値を算出し設定することによって、航跡を生成した直後における航跡諸元の精度および安定性を向上する。
【００５１】
また、誤差標準偏差および駆動雑音を任意に設定することができ、任意の誤差共分散行列初期値を設定することができるため、航跡を生成した直後において航跡に任意の挙動特性を持たせることができる。
【００５２】
また、これら効果については、航跡を生成した場合の他、操作者が航跡諸元を変更した場合や、他のシステムから航跡を移管した場合にも適用可能であり、同様の効果を得ることができる。
【実施例１】
【００５３】
本発明の第１の実施例は、図１〜図３で説明した実施形態と同様である。次に、図１、図２を参照して本実施形態の動作について詳細に説明する。
【００５４】
ここでは、従来例と同様に、カルマンフィルタを用いた目標追尾の代表的な例として、観測値Ｑをセンサ中心の距離ｒおよび方位θで与え、その誤差をそれぞれの標準偏差σｒ，σθで与える系を考える。また、航跡諸元は２次元ｘｙ座標での位置ｘ ，ｙおよび速度ｘドット，ｙドットとし、駆動雑音として加速度に相当する標準偏差αｘ，αｙで与える。観測値および航跡の更新周期をΔＴとし、航跡の誤差共分散行列をＰ、その初期値をＰ０とする。このような系を考えると、航跡諸元初期値Ｘ０、誤差共分散行列初期値Ｐ０、観測値Ｑ０，Ｑ１は、前述の式（１）〜（３）で表される。
【００５５】
【数１】
【００５６】
この誤差共分散行列初期値算出器１０は次のように動作する。まず、変換行列算出部１１は、観測値Ｑを用いて変換行列ΓＣｎを式（４）のように算出する（ステップＳ１）。
【００５７】
【数２】
【００５８】
次に、観測誤差分散行列算出部１２は、変換行列ΓＣｎおよび誤差標準偏差σｒ，σθを用いて観測誤差分散行列Ｗｎを式（５）のように算出する（ステップＳ２）。
【００５９】
【数３】
【００６０】
さらに、誤差共分散行列初期値算出部１３は、観測誤差分散行列Ｗｎおよび更新周期ΔＴを用いて最初の誤差共分散行列初期値Ｐ０，１を式（６）のように算出する（ステップＳ３）。
【００６１】
【数４】
【００６２】
そして、誤差共分散行列初期値収束部１４は、観測誤差分散行列Ｗｎ、更新周期ΔＴをおよび駆動雑音を用いて繰返し計算を行い誤差共分散行列初期値を収束させる（ステップＳ４）。
【００６３】
このステップＳ４は、図３のフロー図を用いて詳しく説明する。ここでは、誤差共分散行列初期値を収束させるための処理である。ここで用いる計算式は、本来追尾処理において徐々に収束していくものであるが、観測値が入力されるだろうと想定して、航跡生成時に一気に収束させるようにしている。そのために追尾処理に用いられる数式を使用する必要がある。
【００６４】
ここで、目標の運動モデルΦは、航跡の想定する飛行モデルであり、例えば次の式（７）のように定義できる。また、駆動雑音用変換行列ΓＤは、駆動雑音分散行列Ｕを誤差共分散行列初期値Ｐに加えるときに必要な変換であり、次の式（８）のようになる。また、駆動雑音分散行列Ｕは、誤差共分散行列初期値Ｐへある一定の雑音を付加し、航跡の軌道の不確定さを表すもので、例えば次の式（９）のように定義できる。また、観測行列Ｈは、誤差共分散行列初期値Ｐのもつ情報種類から観測値のもつ情報種類を取出す為の行列であり、次の式（１０）のようになるが、ここでは、Ｘ位置、Ｙ位置のみの情報を取出す働きを持つ。なお、行列およびベクトルの右上添字「Ｔ」は、行列およびベクトルの転置を表すこととし、また、行列の右上添字「−１」は、行列の逆行列を表すこととする。
【００６５】
【数５】
【００６６】
これらの式とステップＳ３で算出した最初の誤差共分散行列初期値Ｐ０，１とを用いると、誤差共分散行列の予測値Ｐ０，ｋチルダが、次の式（１１）のように算出できる。この式（１１）は、収束させるために、擬似的に誤差共分散行列初期値Ｐ０を予測することを意味し、追尾処理中では、推定した航跡の誤差に対し、ある時間経過後の誤差を算出している予測式を意味する。ただし、Ｐ０，１ハット＝Ｐ０，１であり、ｋ−１は繰り返し計算回数を示す。
【００６７】
【数６】
【００６８】
また、観測行列Ｈ、観測誤差分散行列ＷｎおよびステップＳ１１で算出した誤差共分散行列の予測値Ｐ０，ｋチルダを用いると、カルマンゲインＫは次の式（１２）のように算出できる。この式も、収束させるために、擬似的にカルマンゲインＫを求めている。
【００６９】
【数７】
【００７０】
また、誤差共分散行列の予測値Ｐ０，ｋチルダと、カルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いると、誤差共分散行列の推定値Ｐ０，ｋハットを次の式（１３）のように算出できる。この式（１３）も、収束させるために、擬似的にＰ０を推定しており、追尾処理中では、予測した航跡の誤差に対し、ある観測値を得た後の誤差を算出している推定式を意味する。
【００７１】
【数８】
【００７２】
なお、「航跡の予測値」というのは、継続的に行う追尾処理の最中の、航跡諸元（位置と速度）の時間外挿値（未来の値）のことである。この「航跡の予測値」の誤差範囲も物理的には「誤差共分散行列初期値」と同じ内容であるが、追尾最中であるか、一番初めの値であるか、の相違がある。
【００７３】
従って、図３においては、前述のステップＳ３で算出した最初の誤差共分散行列初期値Ｐ０，１と目標の運動モデルΦと駆動雑音用変換行列ΓＤと駆動雑音分散行列Ｕとを用いて誤差共分散行列の予測値Ｐ０，ｋチルダを算出する（ステップＳ１１）。
【００７４】
次に、ステップＳ１１で算出した誤差共分散行列の予測値Ｐ０，ｋチルダと観測行列Ｈと観測誤差分散行列Ｗｎとを用いてカルマンゲインＫを算出する（ステップＳ１２）。
【００７５】
次に、ステップＳ１１〜Ｓ１２で算出した誤差共分散行列の予測値Ｐ０，ｋチルダとカルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて誤差共分散行列の推定値Ｐ０，ｋハットを算出する（ステップＳ１３）。
【００７６】
最後に、ステップＳ１４で、終了するか否かを判定し、終了しない（否）ならステップＳ１３で算出した誤差共分散行列の推定値（式（１３））を用いて再度ステップＳ１１〜Ｓ１３を繰り返す。また、ステップＳ１４を終了するなら、ステップＳ１３で算出した誤差共分散行列の推定値を誤差共分散行列初期値算出器１０の出力としてステップＳ４を終了する。なお、ステップＳ１４の判定方法としては、規定の回数に到達したら終了する方法、または誤差共分散行列の推定値が規定の大きさ以内に到達したら終了する方法等がある。
【００７７】
次に、本実施例の効果について説明する。本実施例では、誤差共分散行列初期値算出器１０の中に誤差共分散行列初期値収束部１４を持つ構成となっているため、誤差共分散行列初期値を適切な値に算出することができ、航跡を生成した直後においても航跡諸元の精度および安定性を向上させることができる。
【００７８】
また、本実施例では、さらに、誤差標準偏差および駆動雑音を任意に設定することができ且つ誤差共分散行列初期値収束部１４を持つ構成であるため、航跡を生成した直後において任意の誤差共分散行列初期値を設定し、航跡に任意の挙動特性を持たせることができる。すなわち、航跡の位置の誤差範囲はセンサ中心の距離および方位による誤差標準偏差σｒ，σθで任意に与えることができる。
【実施例２】
【００７９】
次に、図４は本発明の第２の実施例の誤差共分散行列初期値算出器１０ａの構成を説明するブロック図、図５はその処理フロー図である。図４を参照すると、本実施例の誤差共分散行列初期値算出器１０ａの構成は、第１の実施例から変換行列算出部１１および図２におけるステップＳ１を省いたものとなっている。すなわち、本実施例は、観測値が極座標ｒ，θの代わりに座標ｘｙを用いているので、変換行列の算出（Ｓ１）が不要となり、計算が実施例１よりも簡単化されている。本実施例は、観測誤差分散行列算出部１２ａと、誤差共分散行列初期値算出部１３と、誤差共分散行列初期値収束部１４と、制御部１５ａとから構成されている。ここで前提となる航跡生成装置２０は、従来例の全体構成を図６と同様である。
【００８０】
観測誤差分散行列算出部１２ａは、誤差標準偏差を用いて観測誤差分散行列を算出する。誤差共分散行列初期値算出部１３は観測誤差分散行列および更新周期を用いて最初の誤差共分散行列初期値を算出する。誤差共分散行列初期値収束部１４は観測誤差分散行列、更新周期および駆動雑音を用いて繰返し計算を行い誤差共分散行列初期値を収束させる。制御部１５ａは、誤差共分散行列初期値算出器１０ａ内の各機能の作動順序の制御を行う。
【００８１】
次に、図４、図５を参照して本実施例の動作について詳細に説明する。ここではカルマンフィルタを用いた目標追尾の代表的な例として、観測値は２次元ｘｙ座標で与えられ、その誤差はそれぞれの標準偏差σｘ，σｙで与えられる。また、航跡諸元Ｘも２次元ｘｙ座標での位置および速度とし、駆動雑音として加速度に相当する標準偏差で与える。観測値および航跡の更新周期をΔＴとし、航跡の誤差共分散行列をＰとする。このような系を考えると、航跡諸元初期値Ｘ０、誤差共分散行列初期値Ｐ０は、前述の式（１）（２）と同様で、観測値Ｑ０，Ｑ１は次の式（１４）で与えられる。
【００８２】
【数９】
【００８３】
まず、観測誤差分散行列算出部１２ａは，誤差標準偏差σｘ，σｙを用いて観測誤差分散行列Ｗ１を次の式（１５）で算出する（ステップＳ２ａ）。
【００８４】
【数１０】
【００８５】
次に、誤差共分散行列初期値算出部１３は、観測誤差分散行列Ｗ１および更新周期ΔＴを用いて最初の誤差共分散行列初期値Ｐ０，１を次の式（１６）で算出する（ステップＳ３）。
【００８６】
【数１１】
【００８７】
さらに、誤差共分散行列初期値収束部１４は、観測誤差分散行列Ｗ１、更新周期ΔＴおよび駆動雑音を用いて繰返し計算を行い誤差共分散行列初期値を収束させる（ステップＳ４）。なお、ステップＳ４は第１の実施例と同様のため説明は省略する。
【００８８】
次に、本実施例の効果について説明する。本実施例は、前述のように変換行列の算出（Ｓ１）が不要となり、計算が実施例１よりも簡単化されている。また、本実施例でも、第１の実施例と同様に、誤差共分散行列初期値算出器１０の中に誤差共分散行列初期値収束部１４を持つ構成のため、誤差共分散行列初期値を適切な値に算出することができ、航跡を生成した直後においても航跡諸元の精度および安定性を向上することができる。
【００８９】
また、本実施例では、さらに、誤差標準偏差および駆動雑音を任意に設定することができ且つ誤差共分散行列初期値収束部１４を持つ構成のため、航跡を生成した直後において任意の誤差共分散行列初期値を設定し航跡に任意の挙動特性を持たせることができる。すなわち、航跡の位置の誤差範囲および航跡の速度の誤差範囲はｘｙ座標のｘおよびｙによる誤差標準偏差σｘ，σｙで任意に与えることができる。
【実施例３】
【００９０】
次に、本発明の第３の実施例について説明する。第１の実施例および第２の実施例では２次元上での目標追尾を前提に説明したが、本実施例では、１次元上で同様に実施することである。航跡諸元初期値、誤差共分散行列初期値、観測値、誤差標準偏差、および駆動雑音の要素が変更となり、それに伴い算出式等が変更となるが公知の理論により求めることができるため、ここでは説明を省略する。
【００９１】
さらに、本発明の第４の実施例について説明する。第１の実施例および第２の実施例では２次元上での目標追尾を前提に説明したが、本実施例では、ｎ（ただしｎは３以上）次元上で同様に実施することである。航跡諸元初期値、誤差共分散行列初期値、観測値、誤差標準偏差、および駆動雑音の要素が変更となり、それに伴い算出式等が変更となるが公知の理論により求めることができるため、ここでは説明を省略する。
【産業上の利用可能性】
【００９２】
本発明においては、航空機などの目標体を追尾する場合における、航跡の生成および航跡諸元の変更に適用した場合を説明したが、本発明は、航空機の他に、車両（自動車、戦車）、船舶、ミサイル、人工衛星等移動する物体を追尾する場合における、航跡の生成および航跡諸元の変更にも適用することができる。
【００９３】
また、本発明は、ナビゲーションシステム等における、自己位置の確立および変更や、気温や傾斜などの状態の観測装置における、状態の確立および変更にも適用することができる。
【００９４】
この場合、ナビゲーションシステム等における、自己位置の確立または変更を行う自己位置確定システムとしては、自己位置を生成または変更する場合の位置の誤差共分散行列初期値を設定する際に、前述した誤差共分散行列初期値設定装置を用いることができる。また、気温や傾斜などの状態の観測装置における、状態の確立または変更を行う観測確立システムとしては、状態の確立または変更する場合の状態の誤差共分散行列初期値を設定する際に、前述した誤差共分散行列初期値設定装置を用いることができる。
【図面の簡単な説明】
【００９５】
【図１】本発明の第１の実施形態の誤差共分散行列初期値算出器を説明するブロック図である。
【図２】図１の処理を説明するフロー図である。
【図３】図２のステップＳ４の演算処理を説明するフロー図である。
【図４】本発明の第２の実施例の誤差共分散行列初期値算出器を説明するブロック図である。
【図５】図５の処理を説明するフロー図である。
【図６】一般の誤差共分散行列初期値算出器を用いた航跡生成装置の一例のブロック図である。
【図７】従来例（図７）の誤差共分散行列初期値算出器を説明するブロック図である。
【図８】図７の処理を説明するフロー図である。
【符号の説明】
【００９６】
１０，１０ａ，１０ｂ 誤差共分散行列初期値算出器
１１ 変換行列算出部
１２，１２ａ 観測誤差分散行列算出部
１３ 誤差共分散行列初期値算出部
１４ 誤差共分散行列初期値収束部
１５，１５ａ，１５ｂ 制御部
２０ 航跡生成装置
２１ 航跡諸元初期値算出器
【特許請求の範囲】
【請求項１】
所定状態および誤差共分散行列の初期値を設定してその後の状態および誤差共分散行列を推定するカルマンフィルタを用いたシステムにおける前記誤差共分散行列の初期値を算出し設定する誤差共分散行列初期値設定装置において、前記誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して状態の誤差を適切な値へ収束させる誤差共分散行列初期値収束手段を有することを特徴とする誤差共分散行列初期値設定装置。
【請求項２】
誤差共分散行列初期値収束手段が、最初の誤差共分散行列初期値から誤差共分散行列の予測値を計算し、この誤差共分散行列の予測値と観測誤差分散行列とを用いてカルマンゲインを計算し、前記誤差共分散行列の予測値と前記カルマンゲインとから誤差共分散行列の推定値を計算し、この誤差共分散行列の推定値が所定範囲にない時、前記誤差共分散行列の推定値を前記最初の誤差共分散行列初期値として次の誤差共分散行列の予測値、推定値を計算し、前記誤差共分散行列の推定値が所定範囲になった時、この演算を終了し、前記誤差共分散行列初期値を収束させる請求項１記載の誤差共分散行列初期値設定装置。
【請求項３】
最初の誤差共分散行列初期値Ｐ０，１が、観測誤差標準偏差から求められた観測誤差共分散行列から計算される時、誤差共分散行列初期値収束手段が、前記誤差共分散行列初期値Ｐ０，１と目標の運動モデルΦと駆動雑音用変換行列ΓＤと駆動雑音分散行列Ｕとを用いて誤差共分散行列の予測値Ｐ０，ｋチルダを
【数１２】
により算出し（ただし、Ｐ０，１ハット＝Ｐ０，１、ｋ−１｛ｋは２以上の整数｝は繰り返し計算回数、また、行列、ベクトルの右上添字「Ｔ」は、行列、ベクトルの転置を表し、行列の右上添字「−１」は、行列の逆行列を表す）、前記誤差共分散行列の予測値Ｐ０，ｋチルダと観測行列Ｈと観測誤差分散行列Ｗｎとを用いてカルマンゲインＫを
【数１３】
により算出し、前記誤差共分散行列の予測値Ｐ０，ｋチルダと前記カルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて誤差共分散行列の推定値Ｐ０，ｋハットを
【数１４】
により算出する請求項２記載の誤差共分散行列初期値設定装置。
【請求項４】
観測誤差標準偏差から観測誤差共分散行列Ｗ１を計算し、この観測誤差共分散行列Ｗ１と更新周期ΔＴとから最初の誤差共分散行列初期値Ｐ０，１を計算する、または予め定義しておいた値を使用し最初の誤差共分散行列初期値Ｐ０，１を設定する請求項３記載の誤差共分散行列初期値設定装置。
【請求項５】
観測誤差標準偏差が状態諸元の座標系と異なる座標系で求められた時、観測誤差共分散行列を座標変換行列で変換し、観測誤差共分散行列を求める請求項４記載の航誤差共分散行列初期値装設定置。
【請求項６】
移動する目標体を追尾する場合に、前記目標体の航跡の生成または航跡諸元の変更を行う航跡生成システムにおいて、前記航跡の生成または前記航跡諸元の変更を行う場合の航跡の誤差共分散行列初期値を設定する際に、請求項１乃至５のうちの１項に記載の誤差共分散行列初期値設定装置を用いたことを特徴とする航跡生成システム。
【請求項７】
ナビゲーションシステム等における、自己位置の確立または変更を行う自己位置確定システムにおいて、前記自己位置を生成または変更する場合の位置の誤差共分散行列初期値を設定する際に、請求項１乃至５のうちの１項に記載の誤差共分散行列初期値設定装置を用いたことを特徴とする自己位置確定システム。
【請求項８】
気温や傾斜などの状態の観測装置における、状態の確立または変更を行う観測確立システムにおいて、前記状態の確立または変更する場合の状態の誤差共分散行列初期値を設定する際に、請求項１乃至５うちの１項に記載の誤差共分散行列初期値設定装置を用いたことを特徴とする観測確立システム。
【請求項９】
所定状態および誤差共分散行列の初期値を設定してその後の状態および誤差共分散行列を推定するカルマンフィルタを用いたシステムにおける前記誤差共分散行列の初期値を算出し設定する誤差共分散行列初期値設定方法において、前記状態を生成する際に状態の誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して状態の誤差を適切な値へ収束させる誤差共分散行列初期値収束方法を用いることを特徴とする誤差共分散行列初期値設定方法。
【請求項１０】
誤差共分散行列初期値収束方法が、最初の誤差共分散行列初期値から誤差共分散行列の予測値を計算し、この誤差共分散行列の予測値と観測誤差分散行列とを用いてカルマンゲインを計算し、前記誤差共分散行列の予測値と前記カルマンゲインとから誤差共分散行列の推定値を計算し、この誤差共分散行列の推定値が所定範囲にない時、前記誤差共分散行列の推定値を前記最初の誤差共分散行列初期値として次の誤差共分散行列の予測値、推定値を計算し、前記誤差共分散行列の推定値が所定範囲になった時、この演算を終了し、前記誤差共分散行列初期値を収束させる請求項９記載の誤差共分散行列初期値設定方法。
【請求項１１】
最初の誤差共分散行列初期値Ｐ０，１が、観測誤差標準偏差から求められた観測誤差共分散行列から計算される時、誤差共分散行列初期値収束方法が、前記誤差共分散行列初期値Ｐ０，１と目標の運動モデルΦと駆動雑音用変換行列ΓＤと駆動雑音分散行列Ｕとを用いて誤差共分散行列の予測値Ｐ０，ｋチルダを
【数６】
により算出し（ただし、Ｐ０，１チルダ＝Ｐ０，１、ｋ−１は繰り返し計算回数、また、行列、ベクトルの右上添字「Ｔ」は、行列、ベクトルの転置を表し、行列の右上添字「−１」は、行列の逆行列を表す）、前記誤差共分散行列の予測値Ｐ０，ｋチルダと観測行列Ｈと観測誤差分散行列Ｗｎとを用いてカルマンゲインＫを
【数７】
により算出し、前記誤差共分散行列の予測値Ｐ０，ｋチルダと前記カルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて誤差共分散行列の推定値Ｐ０，ｋハットを
【数８】
により算出する請求項２記載の誤差共分散行列初期値設定方法。
【請求項１２】
観測誤差標準偏差から観測誤差共分散行列Ｗ１を計算し、この観測誤差共分散行列Ｗ１と更新周期ΔＴとから最初の誤差共分散行列初期値Ｐ０，１を計算する、または予め定義しておいた値を使用し最初の誤差共分散行列初期値Ｐ０，１を設定する請求項１１記載の誤差共分散行列初期値設定方法。
【請求項１３】
観測誤差標準偏差が状態諸元の座標系と異なる座標系で求められた時、観測誤差共分散行列を座標変換行列で変換し、観測誤差共分散行列を求める請求項１２記載の航誤差共分散行列初期値装設定方法。
【請求項１４】
移動する目標体を追尾する場合に、前記目標体の航跡の生成または航跡諸元の変更を行う航跡生成方法において、前記航跡の生成または前記航跡諸元の変更を行う場合の航跡の誤差共分散行列初期値を設定する際に、請求項９乃至１３のうちの１項に記載の誤差共分散行列初期値設定方法を用いたことを特徴とする航跡生成方法。
【請求項１５】
ナビゲーションシステム等における、自己位置の確立または変更を行う自己位置確定方法において、前記自己位置を確立または変更する場合の位置の誤差共分散行列初期値を設定する際に、請求項９乃至１３のうちの１項に記載の誤差共分散行列初期値設定方法を用いたことを特徴とする自己位置確定方法。
【請求項１６】
気温や傾斜などの状態の観測装置における、状態の確立または変更を行う観測確立方法において、前記状態の確立または変更をする場合の状態の誤差共分散行列初期値を設定する際に、請求項９乃至１３のうちの１項に記載の誤差共分散行列初期値設定方法を用いたことを特徴とする観測確立方法。
【請求項１】
所定状態および誤差共分散行列の初期値を設定してその後の状態および誤差共分散行列を推定するカルマンフィルタを用いたシステムにおける前記誤差共分散行列の初期値を算出し設定する誤差共分散行列初期値設定装置において、前記誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して状態の誤差を適切な値へ収束させる誤差共分散行列初期値収束手段を有することを特徴とする誤差共分散行列初期値設定装置。
【請求項２】
誤差共分散行列初期値収束手段が、最初の誤差共分散行列初期値から誤差共分散行列の予測値を計算し、この誤差共分散行列の予測値と観測誤差分散行列とを用いてカルマンゲインを計算し、前記誤差共分散行列の予測値と前記カルマンゲインとから誤差共分散行列の推定値を計算し、この誤差共分散行列の推定値が所定範囲にない時、前記誤差共分散行列の推定値を前記最初の誤差共分散行列初期値として次の誤差共分散行列の予測値、推定値を計算し、前記誤差共分散行列の推定値が所定範囲になった時、この演算を終了し、前記誤差共分散行列初期値を収束させる請求項１記載の誤差共分散行列初期値設定装置。
【請求項３】
最初の誤差共分散行列初期値Ｐ０，１が、観測誤差標準偏差から求められた観測誤差共分散行列から計算される時、誤差共分散行列初期値収束手段が、前記誤差共分散行列初期値Ｐ０，１と目標の運動モデルΦと駆動雑音用変換行列ΓＤと駆動雑音分散行列Ｕとを用いて誤差共分散行列の予測値Ｐ０，ｋチルダを
【数１２】
により算出し（ただし、Ｐ０，１ハット＝Ｐ０，１、ｋ−１｛ｋは２以上の整数｝は繰り返し計算回数、また、行列、ベクトルの右上添字「Ｔ」は、行列、ベクトルの転置を表し、行列の右上添字「−１」は、行列の逆行列を表す）、前記誤差共分散行列の予測値Ｐ０，ｋチルダと観測行列Ｈと観測誤差分散行列Ｗｎとを用いてカルマンゲインＫを
【数１３】
により算出し、前記誤差共分散行列の予測値Ｐ０，ｋチルダと前記カルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて誤差共分散行列の推定値Ｐ０，ｋハットを
【数１４】
により算出する請求項２記載の誤差共分散行列初期値設定装置。
【請求項４】
観測誤差標準偏差から観測誤差共分散行列Ｗ１を計算し、この観測誤差共分散行列Ｗ１と更新周期ΔＴとから最初の誤差共分散行列初期値Ｐ０，１を計算する、または予め定義しておいた値を使用し最初の誤差共分散行列初期値Ｐ０，１を設定する請求項３記載の誤差共分散行列初期値設定装置。
【請求項５】
観測誤差標準偏差が状態諸元の座標系と異なる座標系で求められた時、観測誤差共分散行列を座標変換行列で変換し、観測誤差共分散行列を求める請求項４記載の航誤差共分散行列初期値装設定置。
【請求項６】
移動する目標体を追尾する場合に、前記目標体の航跡の生成または航跡諸元の変更を行う航跡生成システムにおいて、前記航跡の生成または前記航跡諸元の変更を行う場合の航跡の誤差共分散行列初期値を設定する際に、請求項１乃至５のうちの１項に記載の誤差共分散行列初期値設定装置を用いたことを特徴とする航跡生成システム。
【請求項７】
ナビゲーションシステム等における、自己位置の確立または変更を行う自己位置確定システムにおいて、前記自己位置を生成または変更する場合の位置の誤差共分散行列初期値を設定する際に、請求項１乃至５のうちの１項に記載の誤差共分散行列初期値設定装置を用いたことを特徴とする自己位置確定システム。
【請求項８】
気温や傾斜などの状態の観測装置における、状態の確立または変更を行う観測確立システムにおいて、前記状態の確立または変更する場合の状態の誤差共分散行列初期値を設定する際に、請求項１乃至５うちの１項に記載の誤差共分散行列初期値設定装置を用いたことを特徴とする観測確立システム。
【請求項９】
所定状態および誤差共分散行列の初期値を設定してその後の状態および誤差共分散行列を推定するカルマンフィルタを用いたシステムにおける前記誤差共分散行列の初期値を算出し設定する誤差共分散行列初期値設定方法において、前記状態を生成する際に状態の誤差共分散行列初期値を、時間が経過するとともにある観測値が入力されるだろうと想定して状態の誤差を適切な値へ収束させる誤差共分散行列初期値収束方法を用いることを特徴とする誤差共分散行列初期値設定方法。
【請求項１０】
誤差共分散行列初期値収束方法が、最初の誤差共分散行列初期値から誤差共分散行列の予測値を計算し、この誤差共分散行列の予測値と観測誤差分散行列とを用いてカルマンゲインを計算し、前記誤差共分散行列の予測値と前記カルマンゲインとから誤差共分散行列の推定値を計算し、この誤差共分散行列の推定値が所定範囲にない時、前記誤差共分散行列の推定値を前記最初の誤差共分散行列初期値として次の誤差共分散行列の予測値、推定値を計算し、前記誤差共分散行列の推定値が所定範囲になった時、この演算を終了し、前記誤差共分散行列初期値を収束させる請求項９記載の誤差共分散行列初期値設定方法。
【請求項１１】
最初の誤差共分散行列初期値Ｐ０，１が、観測誤差標準偏差から求められた観測誤差共分散行列から計算される時、誤差共分散行列初期値収束方法が、前記誤差共分散行列初期値Ｐ０，１と目標の運動モデルΦと駆動雑音用変換行列ΓＤと駆動雑音分散行列Ｕとを用いて誤差共分散行列の予測値Ｐ０，ｋチルダを
【数６】
により算出し（ただし、Ｐ０，１チルダ＝Ｐ０，１、ｋ−１は繰り返し計算回数、また、行列、ベクトルの右上添字「Ｔ」は、行列、ベクトルの転置を表し、行列の右上添字「−１」は、行列の逆行列を表す）、前記誤差共分散行列の予測値Ｐ０，ｋチルダと観測行列Ｈと観測誤差分散行列Ｗｎとを用いてカルマンゲインＫを
【数７】
により算出し、前記誤差共分散行列の予測値Ｐ０，ｋチルダと前記カルマンゲインＫと、単位行列Ｉと観測行列Ｈとを用いて誤差共分散行列の推定値Ｐ０，ｋハットを
【数８】
により算出する請求項２記載の誤差共分散行列初期値設定方法。
【請求項１２】
観測誤差標準偏差から観測誤差共分散行列Ｗ１を計算し、この観測誤差共分散行列Ｗ１と更新周期ΔＴとから最初の誤差共分散行列初期値Ｐ０，１を計算する、または予め定義しておいた値を使用し最初の誤差共分散行列初期値Ｐ０，１を設定する請求項１１記載の誤差共分散行列初期値設定方法。
【請求項１３】
観測誤差標準偏差が状態諸元の座標系と異なる座標系で求められた時、観測誤差共分散行列を座標変換行列で変換し、観測誤差共分散行列を求める請求項１２記載の航誤差共分散行列初期値装設定方法。
【請求項１４】
移動する目標体を追尾する場合に、前記目標体の航跡の生成または航跡諸元の変更を行う航跡生成方法において、前記航跡の生成または前記航跡諸元の変更を行う場合の航跡の誤差共分散行列初期値を設定する際に、請求項９乃至１３のうちの１項に記載の誤差共分散行列初期値設定方法を用いたことを特徴とする航跡生成方法。
【請求項１５】
ナビゲーションシステム等における、自己位置の確立または変更を行う自己位置確定方法において、前記自己位置を確立または変更する場合の位置の誤差共分散行列初期値を設定する際に、請求項９乃至１３のうちの１項に記載の誤差共分散行列初期値設定方法を用いたことを特徴とする自己位置確定方法。
【請求項１６】
気温や傾斜などの状態の観測装置における、状態の確立または変更を行う観測確立方法において、前記状態の確立または変更をする場合の状態の誤差共分散行列初期値を設定する際に、請求項９乃至１３のうちの１項に記載の誤差共分散行列初期値設定方法を用いたことを特徴とする観測確立方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２００７−７１６００（Ｐ２００７−７１６００Ａ）
【公開日】平成１９年３月２２日（２００７．３．２２）
【国際特許分類】
【出願番号】特願２００５−２５６８３９（Ｐ２００５−２５６８３９）
【出願日】平成１７年９月５日（２００５．９．５）
【出願人】（０００００４２３７）日本電気株式会社 (19,353)
【Ｆターム（参考）】
【公開日】平成１９年３月２２日（２００７．３．２２）
【国際特許分類】
【出願日】平成１７年９月５日（２００５．９．５）
【出願人】（０００００４２３７）日本電気株式会社 (19,353)
【Ｆターム（参考）】
[ Back to top ]