(57)【要約】【課題】バイアスベクトル推定値を高精度に算出可能なセンサバイアス推定装置を得る。【解決手段】それぞれのセンサが、観測値と予測値とに基づいて相関観測値を算出するとともに、相関観測値に基づいてセンサ航跡を算出し、算出結果を出力する装置に適用され、複数のセンサ１０、２０のバイアスを推定するセンサバイアス推定装置であって、センサ航跡に基づいて、時刻同期および航跡相関を実行し、同一航跡と判定されたセンサ航跡の組を出力する同期処理部３０と、同一航跡と判定されたセンサ航跡の組に対してバイアス格子点探索処理を実行し、バイアス推定値を算出するバイアス格子点探索処理部４０と、相関観測値に基づいて、バイアス推定フィルタ処理を実行し、バイアスベクトル推定値を算出するバイアス推定フィルタ処理部６０とを備え、バイアス推定フィルタ処理部６０は、バイアス推定値をバイアスベクトル推定値の初期値として設定するものである。【選択図】図１
2011064484.tif 000002
【特許請求の範囲】
【請求項１】
複数のセンサを有する装置であり、それぞれのセンサが、観測値と予測値とに基づいて相関処理を実行して相関観測値を算出するとともに、前記相関観測値に基づいてセンサ航跡を算出し、前記相関観測値および前記センサ航跡を出力する装置に適用され、前記複数のセンサのバイアスを推定するセンサバイアス推定装置であって、
前記センサ航跡に基づいて、時刻同期および航跡相関を実行し、同一航跡と判定されたセンサ航跡の組を出力する同期処理手段と、
前記同一航跡と判定されたセンサ航跡の組に対してバイアス格子点探索処理を実行し、バイアス推定値を算出するバイアス格子点探索処理手段と、
前記相関観測値に基づいて、バイアス推定フィルタ処理を実行し、バイアスベクトル推定値を算出するバイアス推定フィルタ処理手段と、を備え、
前記バイアス推定フィルタ処理手段は、前記バイアス推定値を前記バイアスベクトル推定値の初期値として設定する
ことを特徴とするセンサバイアス推定装置。
【請求項２】
前記複数のセンサと前記バイアス推定フィルタ処理手段との間に設けられ、前記センサ航跡に対応する前記相関観測値が、所定の中心に対する所定のゲート範囲内にある場合に、前記相関観測値を前記バイアス推定フィルタ処理手段に出力する統合相関処理手段をさらに備えたことを特徴とする請求項１に記載のセンサバイアス推定装置。
【請求項３】
前記バイアス推定フィルタ処理手段は、前記バイアスベクトル推定値およびその推定誤差共分散行列と、状態ベクトル推定値およびその推定誤差共分散行列とを前記バイアス格子点探索処理手段に出力し、
前記バイアス格子点探索処理手段は、前記バイアスベクトル推定値およびその推定誤差共分散行列と、前記状態ベクトル推定値およびその推定誤差共分散行列とに基づいて、前記バイアス格子点探索処理の探索範囲を設定する
ことを特徴とする請求項１または請求項２に記載のセンサバイアス推定装置。
【請求項４】
前記バイアス格子点探索処理におけるバイアス格子間隔を、探索の開始時には粗く設定し、探索が進むにつれて細かく設定する制御信号を前記バイアス格子点探索処理手段に出力するとともに、前記バイアス格子点探索処理が終了しておらず、かつ前記バイアス格子点探索処理手段から得られる探索開始時刻と現時刻との差が、事前に決めた探索終了判定時刻しきい値よりも大きい場合に、現時点において最良のバイアス候補を出力させる制御信号を前記バイアス格子点探索処理手段に出力するバイアス格子間隔制御処理手段をさらに備えたことを特徴とする請求項１から請求項３までの何れか１項に記載のセンサバイアス推定装置。
【請求項５】
前記バイアス格子点探索処理が終了しておらず、かつ前記バイアス格子点探索処理手段から得られる探索開始時刻と現時刻との差が、事前に決めた格子間隔再設定時刻しきい値よりも大きい場合に、次時刻のバイアス格子間隔を現時刻のバイアス格子間隔よりも粗く設定する制御信号を前記バイアス格子点探索処理手段に出力するバイアス格子間隔再設定処理手段をさらに備えたことを特徴とする請求項１から請求項４までの何れか１項に記載に記載のセンサバイアス推定装置。
【請求項６】
前記バイアス格子点探索処理が終了しておらず、かつ前記バイアス格子点探索処理手段から得られる探索開始時刻と現時刻との差が、事前に決めた探索範囲再設定時刻しきい値よりも大きい場合に、前記バイアス格子点探索処理の探索範囲を狭くする制御信号を前記バイアス格子点探索処理手段に出力する第１のバイアス格子点探索範囲再設定処理手段をさらに備えたことを特徴とする請求項３から請求項５までの何れか１項に記載に記載のセンサバイアス推定装置。
【請求項７】
前記バイアス格子点探索処理が終了し、かつ前記バイアス格子点探索処理手段から得られる探索開始時刻と現時刻との差が、事前に決めた探索範囲再設定時刻しきい値よりも大きい場合に、前記バイアス格子点探索処理の探索範囲を広くする制御信号を前記バイアス格子点探索処理手段に出力する第２のバイアス格子点探索範囲再設定処理手段をさらに備えたことを特徴とする請求項３から請求項５までの何れか１項に記載に記載のセンサバイアス推定装置。
【請求項８】
前記バイアス推定値の推定順序として、ある成分についてバイアス推定値を算出した後に、残りの成分についてバイアス推定値を算出させる制御信号を、前記バイアス格子点探索処理手段および前記バイアス推定フィルタ処理手段に出力するバイアス成分推定順序設定処理手段をさらに備えたことを特徴とする請求項１から請求項７までの何れか１項に記載に記載のセンサバイアス推定装置。
【請求項９】
前記複数のセンサから得られる観測ベクトルの次数が互いに異なる場合に、推定すべきバイアス成分の次数を、観測される成分の次数に合わせるような制御信号を、前記同期処理手段、前記バイアス格子点探索処理手段および前記バイアス推定フィルタ処理手段に出力するバイアス成分次数設定処理手段をさらに備えたことを特徴とする請求項１から請求項８までの何れか１項に記載のセンサバイアス推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、例えば複数のセンサを有する追尾装置に適用され、それぞれのセンサのバイアス（バイアス誤差）を推定するセンサバイアス推定装置に関する。
【背景技術】
【０００２】
従来のセンサのバイアス誤差推定装置は、複数の目標に対して、それぞれ第１および第２のセンサからのデータ入力を観測する第１および第２の観測器の時間毎の出力に基づいて、観測行列を生成する観測行列生成器と、観測行列出力と観測雑音の共分散とからバイアス誤差推定値の共分散行列を算出する推定値評価器と、観測行列と推定値評価器出力とから仮のバイアス誤差値を算出する推定値算出器と、観測行列生成器と推定値評価器との間に設けられ、観測行列とその転置行列との積の最小固有値が所定の定数よりも大きい場合にのみ、観測行列を出力する階数判定器とを備え、累積演算結果からカルマンフィルタによりセンサのバイアス誤差を推定している（例えば、特許文献１参照）。
【０００３】
次に、図１４を参照しながら、バイアス（バイアス誤差）の補正を実行する目的について説明する。なお、本明細書において、バイアスとバイアス誤差とは同義とする。
図１４は、２つのセンサのバイアスをそれぞれ補正する様子を、ＰＰＩ（Ｐｌａｎ  Ｐｏｓｉｔｉｏｎ  Ｉｎｄｉｃａｔｏｒ）表示で例示する説明図である。
図１４において、２つのセンサを構成するセンサＡおよびセンサＢは、それぞれ方位角方向にバイアスを有しており、センサＡおよびセンサＢともに、目標１、２、３を同時に観測している（図中（ａ）の状態）。
【０００４】
このとき、センサＡおよびＢは、それぞれ方位角方向にバイアスを有しているので、センサＡおよびＢで同一の目標を観測していたとしても、センサＡおよびＢの観測結果を重ね合わせた場合、目標同士が重なり合わず、だぶって見えることとなる（図中（ｂ）の状態）。そこで、このような状況を改善するために、センサＡおよびＢのバイアスを推定して、バイアスを補正することにより、センサＡおよびＢの観測結果を重ね合わせた場合に、目標同士を１つに重ね合わせることができる（図中（ｃ）の状態）。
すなわち、センサＡおよびＢで観測した目標の位置、速度、加速度等からなる目標の状態量を一致させることが、バイアスの補正を実行する目的である。
【０００５】
続いて、特許文献１に示したようなカルマンフィルタにより、距離、仰角、方位角の各成分からなるバイアスベクトルと、目標の位置、速度の各成分からなる状態ベクトルとを推定することを考える。
【０００６】
ここで、バイアスベクトルは、距離、方位角の各成分で構成されてもよいし、仰角、方位角の各成分で構成されてもよいし、距離、仰角の各成分で構成されてもよいし、距離成分のみで構成されてもよいし、仰角成分のみで構成されてもよいし、方位角成分のみで構成されてもよい。すなわち、バイアスベクトルの次数およびその成分は、事前に任意に設定してカルマンフィルタを構成することができる。
また、バイアスベクトルは、Ｘ，Ｙ，Ｚの各成分で構成されてもよいし、バイアス推定を実行する構成は、上記距離、仰角、方位角の各成分の組み合わせと同様に、様々な組み合わせによって実現することができる。
【０００７】
同様に、状態ベクトルは、それぞれ３次元のＸ，Ｙ，Ｚ成分からなる位置、速度の６次元の状態ベクトルとして構成されてもよいし、それぞれ３次元のＸ，Ｙ，Ｚ成分からなる位置、速度、加速度の９次元の状態ベクトルとして構成されてもよいし、それぞれ２次元のＸ，Ｙ成分からなる位置、速度の４次元の状態ベクトルとして構成されてもよいし、それぞれ２次元のＸ，Ｙ成分からなる位置、速度、加速度の６次元の状態ベクトルとして構成されてもよい。すなわち、状態ベクトルの次数およびその成分は、事前に任意に設定してカルマンフィルタを構成することができる。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特許第３３０２８７０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００９】
しかしながら、従来技術には、以下のような課題がある。
カルマンフィルタおよび最小２乗フィルタによりバイアス推定を実行する場合には、バイアスベクトル推定値の初期値が必要になる。ここで、従来のセンサのバイアス誤差推定装置では、バイアスベクトル推定値の初期値の設定方法が明らかにされていない。
そのため、バイアスベクトル推定値の初期値が、バイアスベクトルの真値から大きく外れていた場合には、バイアスベクトル推定値の精度が低下したり、バイアスベクトル推定値の収束が遅くなったりするという問題がある。
【００１０】
この発明は、上記のような課題を解決するためになされたものであり、バイアスベクトル推定値を高精度に算出することができるセンサバイアス推定装置を得ることを目的とする。
【課題を解決するための手段】
【００１１】
この発明に係るセンサバイアス推定装置は、複数のセンサを有する装置であり、それぞれのセンサが、観測値と予測値とに基づいて相関処理を実行して相関観測値を算出するとともに、相関観測値に基づいてセンサ航跡を算出し、相関観測値およびセンサ航跡を出力する装置に適用され、複数のセンサのバイアスを推定するセンサバイアス推定装置であって、センサ航跡に基づいて、時刻同期および航跡相関を実行し、同一航跡と判定されたセンサ航跡の組を出力する同期処理手段と、同一航跡と判定されたセンサ航跡の組に対してバイアス格子点探索処理を実行し、バイアス推定値を算出するバイアス格子点探索処理手段と、相関観測値に基づいて、バイアス推定フィルタ処理を実行し、バイアスベクトル推定値を算出するバイアス推定フィルタ処理手段と、を備え、バイアス推定フィルタ処理手段は、バイアス推定値をバイアスベクトル推定値の初期値として設定するものである。
【発明の効果】
【００１２】
この発明に係るセンサバイアス推定装置によれば、センサからの相関観測値に基づいてバイアス推定フィルタ処理を実行し、バイアスベクトル推定値を算出するバイアス推定フィルタ処理手段は、バイアス格子点探索処理手段によるバイアス格子点探索処理によって算出されるバイアス推定値をバイアスベクトル推定値の初期値として設定する。そのため、バイアスベクトル推定値の初期値を適切に設定することができ、バイアスベクトル推定値を高精度に算出することができるセンサバイアス推定装置を得ることができる。
【図面の簡単な説明】
【００１３】
【図１】この発明の実施の形態１に係るセンサバイアス推定装置を示すブロック構成図である。
【図２】この発明の実施の形態１に係る同期処理部による同期処理を説明するためのタイミングチャートである。
【図３】この発明の実施の形態１に係る同期処理部による航跡相関を示す説明図である。
【図４】この発明の実施の形態１に係るバイアス格子点探索処理部による、距離、仰角、方位角バイアスを持つ場合のバイアス格子点探索処理を示す説明図である。
【図５】この発明の実施の形態１に係るバイアス格子点探索処理部による、Ｘ，Ｙ，Ｚバイアスを持つ場合のバイアス格子点探索処理を示す説明図である。
【図６】この発明の実施の形態１に係るバイアス推定フィルタ処理部からのバイアスベクトル推定値およびその推定誤差共分散行列、または状態ベクトル推定値およびその推定誤差共分散行列に基づいて、バイアス格子点の探索範囲を絞り込む処理を示す説明図である。
【図７】この発明の実施の形態１に係るバイアス格子点探索処理部によるバイアス格子点探索処理の手順を示すフローチャートである。
【図８】この発明の実施の形態２に係るセンサバイアス推定装置を示すブロック構成図である。
【図９】この発明の実施の形態３に係るセンサバイアス推定装置を示すブロック構成図である。
【図１０】この発明の実施の形態４に係るセンサバイアス推定装置を示すブロック構成図である。
【図１１】この発明の実施の形態５に係るセンサバイアス推定装置を示すブロック構成図である。
【図１２】この発明の実施の形態６に係るセンサバイアス推定装置を示すブロック構成図である。
【図１３】この発明の実施の形態７に係るセンサバイアス推定装置を示すブロック構成図である。
【図１４】２つのセンサのバイアスをそれぞれ補正する様子を、ＰＰＩ表示で例示する説明図である。
【発明を実施するための形態】
【００１４】
以下、この発明のセンサバイアス推定装置の好適な実施の形態につき図面を用いて説明するが、各図において同一、または相当する部分については、同一符号を付して説明する。
【００１５】
実施の形態１．
図１は、この発明の実施の形態１に係るセンサバイアス推定装置を示すブロック構成図である。
図１において、このセンサバイアス推定装置は、第１センサ１０、第２センサ２０、同期処理部３０、バイアス格子点探索処理部４０、統合相関処理部５０、バイアス推定フィルタ処理部６０およびバイアスデータベース（バイアスＤＢ）７０とを備えている。
【００１６】
第１センサ１０は、第１信号処理部１１、第１相関処理部１２、第１データ更新処理部１３、第１予測処理部１４および第１遅延処理部１５を有している。また、第２センサ２０は、第２信号処理部２１、第２相関処理部２２、第２データ更新処理部２３、第２予測処理部２４および第２遅延処理部２５を有している。なお、第１センサ１０における第１信号処理部１１から第１遅延処理部１５までの処理と、第２センサ２０における第２信号処理部２１から第２遅延処理部２５までの処理とは、互いに同一の処理である。
【００１７】
第１センサ１０および第２センサ２０は、絶対座標に対してバイアスを有している。また、第１センサ１０からバイアスＤＢ７０までの処理により、第１センサ１０および第２センサ２０の両方のバイアスを推定することもできるし、第１センサ１０に対する第２センサ２０のバイアスを推定することもできる。
【００１８】
ここで、絶対座標に対する第１センサ１０および第２センサ２０のバイアスを絶対バイアスと呼び、絶対座標に対する第１センサ１０の絶対バイアスを０とした場合の第２センサ２０の絶対バイアスを相対バイアスと呼ぶ。また、絶対座標を第１センサ１０とすれば、第１センサ１０の絶対バイアスが０でない場合でも、第１センサ１０に対する第２センサ２０のバイアスを相対バイアスと呼ぶことができる。
【００１９】
また、第１センサ１０および第２センサ２０は、距離、仰角、方位角を観測するレーダのような３次元のセンサであってもよいし、仰角、方位角を観測するＩＲ（Ｉｎｆｒａ  Ｒｅｄ）センサ、またはＥＳＭ（Ｅｌｅｃｔｒｉｃ  Ｓｕｐｐｏｒｔ  Ｍｅａｓｕｒｅｓ）センサのような２次元のセンサであってもよい。また、第１センサ１０および第２センサ２０は、距離、方位角を観測する２次元のセンサであってもよいし、距離のみを観測する１次元のセンサであってもよいし、方位角のみを観測する１次元のセンサであってもよいし、仰角のみを観測する１次元のセンサであってもよい。
【００２０】
以下、このセンサバイアス推定装置の各部位の機能について、第１センサ１０に対する第２センサ２０の相対バイアスを推定する場合を例に挙げて説明する。このとき、第１センサ１０および第２センサ２０は、代表的な例として、距離、仰角、方位角を観測する３次元センサとする。
【００２１】
まず、第１センサ１０の機能について説明する。
第１信号処理部１１は、センサの受信器（図示せず）から時刻ｋにおける距離、仰角、方位角の観測値を得て、それらの観測値を第１相関処理部１２に出力する。
【００２２】
第１相関処理部１２は、第１信号処理部１１から入力された時刻ｋにおける距離、仰角、方位角の観測値と、第１遅延処理部１５から入力された時刻ｋにおける距離、仰角、方位角の予測値とに基づいて、相関処理を実行する。相関処理とは、観測値の誤差や予測値の誤差を勘案したゲート内において、複数の観測値と目標の予測値との対応づけを予測値と観測値との距離でとり、予測値に一番近い観測値１点または予測値に近い複数点を目標信号の候補として抽出したり、予測値からの距離に応じて重み付け平均した観測値を目標信号の候補として抽出したりする処理である。
【００２３】
また、第１相関処理部１２は、相関処理を実行して得られた時刻ｋにおける相関観測値を、第１データ更新処理部１３および統合相関処理部５０に出力する。ここで、相関観測値は、ゲート内の観測値である。
【００２４】
第１データ更新処理部１３は、第１相関処理部１２から入力された時刻ｋにおける相関観測値のうち、例えば予測値に一番近い相関観測値を用いて、カルマンフィルタのデータ更新処理に基づいて、時刻ｋにおける平滑値を算出する。平滑値は、上述した段落で示した平滑値の状態ベクトルおよびその誤差共分散行列であり、状態ベクトルの次数は、事前に設定した３次元、６次元または９次元とする。以後、平滑値を航跡と呼ぶことがある。なお、誤差共分散行列は、その行および列が状態ベクトルの次数と等しいものとする。
また、第１データ更新処理部１３は、時刻ｋにおける平滑値を、第１予測処理部１４および同期処理部３０に出力する。
【００２５】
第１予測処理部１４は、第１データ更新処理部１３から入力された時刻ｋにおける平滑値の状態ベクトルおよびその誤差共分散行列を、事前に仮定した運動モデルに基づいて、１サンプリング分外挿し、時刻ｋ＋１における予測値を算出する。予測値は、上述した段落で示した予測値の状態ベクトルおよびその誤差共分散行列であり、状態ベクトルの次数は、事前に設定した３次元、６次元または９次元とする。
また、第１予測処理部１４は、時刻ｋ＋１における予測値を、第１遅延処理部１５に出力する。
【００２６】
第１遅延処理部１５は、第１予測処理部１４から入力された時刻ｋ＋１における予測値を１サンプリング分遅延させて、時刻ｋにおける予測値を算出し、算出された予測値を第１相関処理部１２に出力する。
【００２７】
なお、第２センサ２０における第２信号処理部２１、第２相関処理部２２、第２データ更新処理部２３、第２予測処理部２４および第２遅延処理部２５の機能は、上述したように、第１センサ１０における第１信号処理部１１、第１相関処理部１２、第１データ更新処理部１３、第１予測処理部１４および第１遅延処理部１５の機能と同様なので、説明を省略する。
【００２８】
続いて、同期処理部３０は、第１センサ１０から入力された平滑値と、第２センサ２０から入力された平滑値とに基づいて、時刻同期および航跡相関を実行する。すなわち、同期処理部３０は、第１センサ１０から得られた航跡と、第２センサ２０から得られた航跡とに基づいて、時刻同期および航跡相関を実行する。
【００２９】
以下、図２を参照しながら、同期処理部３０による時刻同期について説明する。ここで、第１センサ１０から得られた航跡を第１センサ航跡と呼び、第２センサ２０から得られた航跡を第２センサ航跡と呼ぶ。
図２は、この発明の実施の形態１に係る同期処理部３０による同期処理を説明するためのタイミングチャートである。
【００３０】
図２において、横軸は、第１センサ航跡の同期処理への入力タイミングのタイムライン（Ａ）、第２センサ航跡の同期処理への入力タイミングのタイムライン（Ｂ）、および同期処理での処理タイミングのタイムライン（Ｃ）を示している。また、タイムライン（Ａ）において直交する太棒線は、各時刻における第１センサ航跡を表し、タイムライン（Ｂ）において直交する太棒線は、各時刻における第２センサ航跡を表している。
【００３１】
図２より、第１センサ航跡と第２センサ航跡とは、同期処理への入力タイミングが互いに異なることが分かる。そこで、同時刻の第１センサ航跡と第２センサ航跡とを比較するために、ある基準時刻に時刻合わせを行う必要がある。なお、基準時刻は、タイムライン（Ｃ）によって決まるものとする。
【００３２】
このとき、タイムライン（Ｂ）で航跡が生成される時刻をｔ（ｉ）とし、この時刻ｔ（ｉ）を基準時刻とすると（ｔ（ｋ）≦ｔ（ｉ）≦ｔ（ｋ＋１））、第１センサ航跡が時刻ｔ（ｉ）で得られていないので、同期処理部３０は、時刻ｔ（ｉ）以前に第１センサ航跡が得られた時刻ｔ（ｋ）における第１センサ航跡を時刻ｔ（ｉ）まで保持する。
【００３３】
すなわち、時刻ｔ（ｋ）における第１センサ航跡を、時刻ｔ（ｋ）から時刻ｔ（ｉ）までのサンプリング間隔Δｔ（＝ｔ（ｉ）−ｔ（ｋ））で保持して、時刻ｔ（ｉ）における第１センサ航跡を算出する。
つまり、同期処理部３０は、タイムライン（Ｃ）における（１０）の箇所で時刻同期をとることになる。
【００３４】
次に、図３を参照しながら、同期処理部３０による航跡相関について説明する。
図３は、この発明の実施の形態１に係る同期処理部３０による航跡相関を示す説明図である。
図３において、第１センサ１０からは、第１センサ航跡１０１および第１センサ航跡１０２が得られているものとし、第２センサ２０からは、第２センサ航跡２０１、第２センサ航跡２０２および第２センサ航跡２０３が得られているものとする。また、第１センサ航跡１０１を中心とした第１センサ航跡１０１のゲート、および第１センサ航跡１０２を中心とした第１センサ航跡１０２のゲートを点線で囲まれた範囲で示す。
【００３５】
ここで、第１センサ航跡のゲート内に第２センサ航跡が入っている場合には、同期処理部３０は、第１センサ航跡と第２センサ航跡とが同一航跡であると判定する。図３の場合では、第１センサ航跡１０１と第２センサ航跡２０１とが同一航跡であると判定され、第１センサ航跡１０２と第２センサ航跡２０３とが同一航跡であると判定される。
【００３６】
このような、第１センサ航跡と第２センサ航跡との同一性を判定する処理を航跡相関と呼ぶ。
なお、航跡相関を実行する場合には、第１センサ１０と第２センサ２０との基準座標が互いに異なるので、第１センサ１０または第２センサ２０の何れかの基準座標に座標変換してから航跡相関を実行する。また、ここでは、第１センサ航跡を中心としたゲートを設定したが、これに限定されず、第２センサ航跡を中心としたゲートを設定し、第２センサ航跡を中心としたゲート内に第１センサ航跡が入るか否かで同一航跡を判定してもよい。
【００３７】
同期処理部３０は、まず、第１センサ１０から得られた第１センサ航跡と、第２センサ２０から得られた第２センサ航跡とに基づいて時刻同期を実行し、続いて、航跡相関を実行する。このとき、時刻同期は、各センサの速度を用いて、時刻同期を実行する時刻まで航跡位置を保持する。
【００３８】
そして、同期処理部３０は、同一航跡と判定された第１センサ航跡と第２センサ航跡とのペアを、バイアス格子点探索処理部４０に出力する。また、同期処理部３０は、第１センサ航跡と第２センサ航跡とのペア情報を、統合相関処理部５０に出力する。なお、ペア情報は、同一航跡と判定された第１センサ航跡と第２センサ航跡とのペアだけでなく、同一航跡と判定されなかった第１センサ航跡および第２センサ航跡も含むものとする。
【００３９】
次に、バイアス格子点探索処理部４０は、バイアス格子点探索処理により、第１センサ１０に対する第２センサ２０の相対バイアスを推定する。
以下、図４〜７を参照しながら、バイアス格子点探索処理部４０によるバイアス格子点探索処理について説明する。
【００４０】
図４は、この発明の実施の形態１に係るバイアス格子点探索処理部４０による、距離、仰角、方位角バイアスを持つ場合のバイアス格子点探索処理を示す説明図である。また、図５は、この発明の実施の形態１に係るバイアス格子点探索処理部４０による、Ｘ，Ｙ，Ｚバイアスを持つ場合のバイアス格子点探索処理を示す説明図である。なお、図４と図５とは、成分が異なるだけで処理内容は同様なので、図４を用いてバイアス格子点探索処理を説明する。
【００４１】
図４において、ｚｋＹ（アンダーバー）は、時刻ｋにおける第２センサ２０の航跡を示し、ｚｋ−１Ｙ（アンダーバー）は、時刻ｋ−１における第２センサ２０の航跡を示し、ｚｋ−２Ｙ（アンダーバー）は、時刻ｋ−２における第２センサ２０の航跡を示す。また、ｚｋＸ（アンダーバー）は、時刻ｋにおける第１センサ１０の航跡を示し、ｚｋ−１Ｘ（アンダーバー）は、時刻ｋ−１における第１センサ１０の航跡を示し、ｚｋ−２Ｘ（アンダーバー）は、時刻ｋ−２における第１センサ１０の航跡を示す。
【００４２】
また、ΔｚｉＲ，ｉＥ，Ａｚ（アンダーバー）は、第１センサ１０に対する第２センサ２０のバイアス候補を示す。なお、このバイアス候補は複数存在し、複数のバイアス候補の中から、後述する図７の処理により１つを選択する。
ここで、バイアス候補のイメージは、図４の右側に示すとおりであり、バイアス候補の成分は、距離成分ΔＲｉＲ、仰角成分ΔＥｉＥ、方位角成分ΔＡｚｉＡｚからなり、ｉＲ、ｉＥ、ｉＡｚは、バイアス格子点の位置を示すインデックスである。また、このバイアス候補は、距離、仰角、方位角の各軸からなる格子内で値が変化する。
【００４３】
なお、バイアス格子点の探索範囲は、後述するバイアス推定フィルタ処理部６０からのバイアスベクトル推定値およびその推定誤差共分散行列、または状態ベクトル推定値およびその推定誤差共分散行列に基づいて絞り込まれる。
【００４４】
図６は、この発明の実施の形態１に係るバイアス推定フィルタ処理部６０からのバイアスベクトル推定値およびその推定誤差共分散行列、または状態ベクトル推定値およびその推定誤差共分散行列に基づいて、バイアス格子点の探索範囲を絞り込む処理を示す説明図である。なお、図６では、２次元平面のイメージを示しているが、バイアス格子点は、推定すべきバイアスの数による。例えば、距離バイアス、仰角バイアス、方位角バイアスを推定する場合には、バイアス格子点は３次元空間となる。
【００４５】
このとき、図６のように、バイアス格子点の探索範囲を、バイアスベクトル推定値の推定誤差共分散行列から得られる誤差楕円体、または状態ベクトル推定値の推定誤差共分散行列から得られる誤差楕円体で設定することにより、バイアス格子点の探索範囲を絞り込むことができる。また、バイアス推定フィルタ処理部６０からのバイアス推定値を格子点探索の始点に設定することより、格子点の端から探索する処理が不要となる。
【００４６】
続いて、図７のフローチャートを参照しながら、バイアス格子点探索処理部４０によるバイアス格子点探索処理の手順について説明する。
図７は、この発明の実施の形態１に係るバイアス格子点探索処理部４０によるバイアス格子点探索処理の手順を示すフローチャートである。なお、図８のフローチャートは、上述したバイアス格子点の探索範囲を絞り込む処理以外の処理を示している。
【００４７】
まず、バイアス格子点探索処理部４０は、バイアス候補ΔｚｉＲ，ｉＥ，Ａｚ（アンダーバー）を付加する前、すなわちバイアス候補ΔｚｉＲ，ｉＥ，Ａｚ（アンダーバー）が未設定の第１センサ航跡ｚｋＸ（アンダーバー）を設定する（ステップＳ１）。この第１センサ航跡をバイアス候補未設定第１センサ航跡と呼ぶ。
【００４８】
続いて、バイアス格子点探索処理部４０は、第１センサ航跡ｚｋＸ（アンダーバー）と同一航跡であると判定された第２センサ航跡ｚｋＹ（アンダーバー）を設定する（ステップＳ２）。
【００４９】
次に、バイアス格子点探索処理部４０は、バイアス格子点の位置を表すインデックス（ｉＲ，ｉＥ，ｉＡｚ）に対応する格子内の１点（図４の右側参照）を、バイアス候補ΔｚｉＲ，ｉＥ，Ａｚ（アンダーバー）として設定する（ステップＳ３）。
【００５０】
続いて、バイアス格子点探索処理部４０は、ステップＳ３で設定したバイアス候補ΔｚｉＲ，ｉＥ，Ａｚ（アンダーバー）を、バイアス候補未設定第１センサ航跡ｚｋＸ（アンダーバー）に付加して、バイアス候補設定第１センサ航跡ｚｋＸ（アンダーバー）を設定する（ステップＳ４）。
【００５１】
次に、バイアス格子点探索処理部４０は、時刻ｋ−Ｌ＋１から時刻ｋまでの間のある一時刻（ここでは、時刻ｋとする）におけるバイアス候補設定第１センサ航跡ｚｋＸ（アンダーバー）と第２センサ航跡ｚｋＹ（アンダーバー）との残差εｋｉＲ，ｉＥ，Ａｚを、次式（１）を用いて算出する（ステップＳ５）。
【００５２】
【数１】
2011064484.tif 000003
【００５３】
なお、残差εｋｉＲ，ｉＥ，Ａｚは、式（１）ではなく、次式（２）を用いて算出されてもよい。
【００５４】
【数２】
2011064484.tif 000004
【００５５】
式（１）、（２）において、‖‖は、ノルムを示している。このノルムは、ユークリッドノルムであってもよいし、ベクトルの成分の最大値を取るノルムであってもよい。
以降の説明については、残差εｋｉＲ，ｉＥ，Ａｚとして式（２）を用いて説明する。
【００５６】
続いて、バイアス格子点探索処理部４０は、時刻ｋ−Ｌ＋１から時刻ｋまでのＬサンプル分について、残差を算出したか否かを判定する（ステップＳ６）。
ステップＳ６において、Ｌサンプル分について残差を算出していない（すなわち、Ｎｏ）と判定された場合には、バイアス格子点探索処理部４０は、カウンタｉを１つ進めて（ステップＳ７）、ステップＳ５に移行し、次の時刻における航跡の残差を算出する。
【００５７】
一方、ステップＳ６において、Ｌサンプル分について残差を算出した（すなわち、Ｙｅｓ）と判定された場合には、バイアス格子点探索処理部４０は、Ｌサンプル分の航跡の残差和εｉＲ，ｉＥ，Ａｚを、次式（３）を用いて算出する（ステップＳ８）。
【００５８】
【数３】
2011064484.tif 000005
【００５９】
次に、バイアス格子点探索処理部４０は、他のバイアス候補が存在しないか否かを判定する（ステップＳ９）。ここで、バイアス格子点探索処理部４０は、全てのバイアス格子点が既に使用されている場合には、他のバイアス候補は存在しないと判定し、未使用のバイアス格子点が残っている場合には、他のバイアス候補が存在すると判定する。
【００６０】
ステップＳ９において、他のバイアス候補が存在する（すなわち、Ｎｏ）と判定された場合には、バイアス格子点探索処理部４０は、カウンタｎを１つ進めて（ステップＳ１０）、ステップＳ３に移行し、別のバイアス候補について残差和εｉＲ，ｉＥ，Ａｚを算出する。
【００６１】
一方、ステップＳ９において、他のバイアス候補が存在しない（すなわち、Ｙｅｓ）と判定された場合には、バイアス格子点探索処理部４０は、全てのバイアス候補について算出した残差和に基づいて、残差和が最小となるときのバイアス候補を、バイアス推定値として決定し（ステップＳ１１）、図７の処理を終了する。
【００６２】
なお、上記ステップＳ９〜１１の処理を言い換えると、バイアス格子点探索処理部４０は、図４の右側に示すように、バイアス候補を格子内で移動させることにより、バイアス候補毎に、式（３）を用いて、Ｌサンプル分の航跡の残差和を算出する。そして、バイアス格子点探索処理部４０は、Ｌサンプル分の航跡の残差和が最小となるときのバイアス候補を選択し、バイアス推定値とする。
【００６３】
以上のように、バイアス格子点探索処理部４０は、バイアス格子点探索処理により、バイアス推定値を決定する。また、バイアス格子点探索処理部４０は、決定したバイアス推定値を、バイアス推定フィルタ処理部６０に出力する。
【００６４】
また、統合相関処理部５０は、同期処理部３０から得られたペア情報に基づいて、同一航跡と判定された第１センサ航跡と第２センサ航跡とのペアにおける第１センサ航跡に対応する相関観測値について、バイアス推定フィルタ処理部６０で使用可能か否かを判定する。同様に、統合相関処理部５０は、同期処理部３０から得られたペア情報に基づいて、同一航跡と判定された第１センサ航跡と第２センサ航跡とのペアにおける第２センサ航跡に対応する相関観測値について、バイアス推定フィルタ処理部６０で使用可能か否かを判定する。
また、統合相関処理部５０は、相関観測値がバイアス推定フィルタ処理部６０で使用可能と判定すると、判定毎に相関観測値をバイアス推定フィルタ処理部６０に出力する。
【００６５】
このとき、統合相関処理部５０は、第１センサ航跡に対応する相関観測値および第２センサ航跡に対応する相関観測値が、所定の中心に対する所定のゲート範囲内にある場合に、その相関観測値がバイアス推定フィルタ処理部６０で使用可能であると判定する。
なお、所定の中心は、同期処理部３０で同一航跡と判定された第１センサ航跡と第２センサ航跡との統合航跡の中心であってもよいし、バイアス推定フィルタ処理部６０で推定された目標の位置の中心であってもよい。また、所定のゲート範囲は、同期処理部３０で同一航跡と判定された第１センサ航跡と第２センサ航跡との統合航跡のゲート範囲であってもよいし、任意に設定されるゲート範囲であってもよい。
【００６６】
ここで、相関処理部１２から得られる第１センサ１０に対応する相関観測値と、相関処理部２２から得られる第２センサ２０に対応する相関観測値とは、一般的に、統合相関処理部５０に非同期で入力される。なお、もし、相関処理部１２から得られる第１センサ１０に対応する相関観測値と、相関処理部２２から得られる第２センサ２０に対応する相関観測値とが、統合相関処理部５０に同期して入力される場合には、事前に設定されるセンサの優先順位、例えば、センサ番号の小さいものから順に処理する等の規則に従って、まず第１センサ１０、次に第２センサ２０といった順に処理する。
【００６７】
続いて、バイアス推定フィルタ処理部６０は、統合相関処理部５０からの相関観測値を用いて、カルマンフィルタまたは最小２乗法に基づいて、バイアス推定フィルタ処理による状態推定およびバイアス推定を実行する。
【００６８】
まず、状態推定について、バイアス推定フィルタ処理部６０は、次式（４）に示すように、目標の位置、速度の各成分からなる状態ベクトル、または目標の位置、速度、加速度の各成分からなる状態ベクトルの推定値およびその推定誤差共分散行列を算出する。なお、目標の状態ベクトルの次数について、例えば、式（４）のような６次元の状態ベクトルを推定するか、次式（５）のような９次元の状態ベクトルを推定するかは、事前に設定しておく。
【００６９】
【数４】
2011064484.tif 000006
【００７０】
【数５】
2011064484.tif 000007
【００７１】
式（４）において、（ｋ）は時刻ｋを示し、ｘ（ｋ）（アンダーバー）は時刻ｋの状態ベクトルを示し、記号「´」は行列およびベクトルの転置を示し、状態ベクトルの右辺のｘ（ｋ）はｘ成分の位置成分を示し、ｘ（ｋ）（ドット）はｘ成分の速度成分を示し、ｘ（ｋ）（ツードット）はｘ成分の加速度成分をそれぞれ示す。なお、ｙ成分、ｚ成分についても同様である。
【００７２】
次に、バイアス推定について、バイアス推定フィルタ処理部６０は、次式（６）に示すように、距離、仰角、方位角の各成分からなるバイアスベクトルの推定値およびその推定誤差共分散行列を算出する。
【００７３】
【数６】
2011064484.tif 000008
【００７４】
式（６）において、ｂ（ｍ）（ｋ）（アンダーバー）は、時刻ｋにおけるセンサｍのバイアスベクトルを示す。ここで、ｍはセンサを表す識別子であり、この識別子ｍは、ｍ＝１，２，・・・のように番号で設定されてもよいし、ｍ＝Ａ，Ｂ，・・・Ｘ，Ｙ，Ｚ，・・・のように、記号で設定されてもよい。ここでは、識別子ｍは、第１センサ１０または第２センサ２０の何れかに対応する。
【００７５】
また、式（６）において、ΔＲ（ｍ）はセンサｍの距離バイアスを示し、ΔＥ（ｍ）はセンサｍの仰角バイアスを示し、ΔＡｚ（ｍ）はセンサｍの方位角バイアスをそれぞれ示す。
なお、目標のバイアスベクトルの種類については、式（６）のような３次元のバイアスベクトルであってもよいし、次式（７）、（８）、（９）のような１次元のバイアスベクトルであってもよいし、次式（１０）、（１１）、（１２）のような２次元のバイアスベクトルであってもよく、推定すべきバイアスベクトルの種類は、センサ毎に事前に設定しておく。
【００７６】
【数７】
2011064484.tif 000009
【００７７】
【数８】
2011064484.tif 000010
【００７８】
【数９】
2011064484.tif 000011
【００７９】
【数１０】
2011064484.tif 000012
【００８０】
【数１１】
2011064484.tif 000013
【００８１】
【数１２】
2011064484.tif 000014
【００８２】
ここで、バイアス推定フィルタ処理部６０は、バイアス格子点探索処理部４０から得られる、バイアス格子点探索処理によるバイアス推定値を、バイアス推定におけるバイアスベクトル推定値の初期値として設定する。バイアスベクトルの推定誤差共分散行列の初期値は、前の時刻の値をそのまま引き継いで計算してもよいし、改めて、事前に決めたある値をバイアスベクトルの推定誤差共分散行列初期値に設定してもよい。また、格子点探索処理によるバイアス推定値を１度しか使用しないで、バイアスベクトル推定値の初期値を計算してもよい。
【００８３】
このとき、バイアス推定フィルタ処理部６０による状態推定は、前の時刻の値をそのまま引き継いで状態ベクトル推定値および推定誤差共分散行列を用いて算出してもよいし、現時刻を含む最新のＬサンプルの相関観測値を用いて、現時刻で改めて、状態ベクトル推定値および推定誤差共分散行列を算出してもよい。
【００８４】
最終的に、バイアス推定フィルタ処理部６０は、バイアス推定結果として、カルマンフィルタまたは最小２乗法に基づいて算出したバイアスベクトル推定値を、バイアスＤＢ７０に出力する。また、バイアス推定フィルタ処理部６０は、バイアス格子点探索処理部４０から得られる、バイアス格子点探索処理によるバイアス推定値を、バイアスＤＢ７０に出力する。
さらに、バイアス推定フィルタ処理部６０は、バイアスベクトルの推定値およびその推定誤差共分散行列と、状態ベクトルの推定値およびその推定誤差共分散行列とを、バイアス格子点探索処理部４０に出力する。
【００８５】
バイアスＤＢ７０は、カルマンフィルタまたは最小２乗法に基づいて、バイアス推定フィルタ処理部６０で算出された各時刻のバイアスベクトル推定値と、バイアス格子点探索処理によるバイアス推定値とを、データベースに蓄積する。
また、バイアスＤＢ７０は、バイアスベクトル推定値の時刻による移動平均値と、バイアス推定値の時刻による移動平均値とを算出し、これらの移動平均値をデータベースに蓄積する。
【００８６】
以上のように、実施の形態１によれば、バイアス推定フィルタ処理手段は、カルマンフィルタおよび最小２乗フィルタによるバイアス推定フィルタ処理を実行する場合に、バイアス格子点探索処理手段によるバイアス格子点探索処理によって算出されるバイアス推定値をバイアスベクトル推定値の初期値として設定する。これにより、バイアス推定フィルタ処理によるバイアスベクトル推定値の初期値を適切に設定することができる。そのため、バイアスベクトル推定値の精度を向上させることができるとともに、バイアスベクトル推定値の収束を早くすることができる。
【００８７】
なお、バイアス格子点探索処理のみでもバイアス推定値を算出することは可能であるが、バイアス格子点が粗い場合、本発明は、バイアス格子点探索処理によるバイアス格子よりも細かい精度でバイアス推定値を算出することができる。また、バイアス格子点を細かくした場合、バイアス推定値を高精度に算出できる可能性はあるものの、バイアス格子点探索処理にかかる演算の処理負荷が高くなる。
【００８８】
このとき、バイアス推定フィルタ処理手段から出力されるバイアスベクトル推定値およびその推定誤差共分散行列から得られる誤差楕円体、または状態ベクトル推定値およびその推定誤差共分散行列から得られる誤差楕円体をバイアス格子点の探索範囲として設定することにより、バイアス格子点探索処理の処理負荷を低減することができる。また、バイアス格子点探索処理の探索開始点を、バイアス推定フィルタ処理手段から出力されるバイアスベクトル推定値とすることにより、バイアス格子点探索処理において、バイアス格子点の端から探索することなく、ある程度バイアス真値に近い位置から、バイアス推定値を探索することができる。その結果、バイアス推定精度の早期収束、および高精度なバイアス推定値の算出を実現することができる。
【００８９】
実施の形態２．
図８は、この発明の実施の形態２に係るセンサバイアス推定装置を示すブロック構成図である。
図８において、このセンサバイアス推定装置は、図１に示したセンサバイアス推定装置に加えて、バイアス格子間隔制御処理部８０を備えている。
【００９０】
バイアス格子間隔制御処理部８０は、以下に示す手順で、バイアス格子点探索処理部４０におけるバイアス格子点を設定する。また、バイアス格子点探索処理部４０からバイアス格子間隔制御処理部８０には、現時刻でのバイアス候補、時刻および格子点探索終了フラグが入力される。
ここで、格子点探索終了フラグとは、例えば格子点探索終了フラグが０の場合に、格子点探索中であることを示し、格子点探索終了フラグが１の場合に、格子点の探索が終了していることを示す等、格子点探索中かあるいは格子点探索終了かが分かるようなフラグである。
【００９１】
最初に、バイアス格子間隔制御処理部８０は、バイアス格子点を粗く探索するような格子間隔を設定する制御信号を、バイアス格子点探索処理部４０に出力する。バイアス格子点探索処理部４０は、上記式（３）を用いて、Ｌサンプル分の航跡の残差和εｉＲ，ｉＥ，Ａｚを算出し、Ｌサンプル分の航跡の残差和εｉＲ，ｉＥ，Ａｚが最小となるバイアス候補ΔｚｉＲ，ｉＥ，Ａｚ（アンダーバー）を、バイアス推定値として出力する。
【００９２】
次に、バイアス格子間隔制御処理部８０は、このバイアス候補を中心として、格子間隔を細かく設定する制御信号を、バイアス格子点探索処理部４０に出力する。バイアス格子点探索処理部４０は、バイアス格子間隔を細かく設定して、再度上記式（３）を用いて、Ｌサンプル分の航跡の残差和εｉＲ，ｉＥ，Ａｚを算出する。
【００９３】
続いて、バイアス格子間隔制御処理部８０は、バイアス格子点を粗く探索するような格子間隔を設定する制御信号と、格子間隔を細かく設定する制御信号とを繰り返して出力する。また、バイアス格子間隔制御処理部８０は、前回算出したＬサンプル分の航跡の残差和と今回算出したＬサンプル分の航跡の残差和との差が、事前に決めたしきい値以下である場合には、処理を終了する制御信号を、バイアス格子点探索処理部４０に出力する。
【００９４】
なお、バイアス格子間隔制御処理部８０は、バイアス格子点探索処理部４０から得られる探索開始時刻と現時刻との差が、事前に決めた探索終了判定時刻しきい値よりも大きい場合、かつ格子点探索終了フラグが０の場合に、探索処理が事前に決めた時間内に終了しないと判定する。このとき、バイアス格子間隔制御処理部８０は、現時刻においてＬサンプル分の航跡の残差和が最小となるようなバイアス候補を出力させる制御信号を、バイアス格子点探索処理部４０に出力する。バイアス格子点探索処理部４０は、現時刻（現時点）においてＬサンプル分の航跡の残差和が最小となるようなバイアス候補を、バイアス推定フィルタ処理部６０に出力する。
【００９５】
以上のように、実施の形態２によれば、バイアス格子点探索処理が所定の時間内に終了しない場合であっても、現時刻で最良のバイアス推定値を算出することができる。
【００９６】
実施の形態３．
図９は、この発明の実施の形態３に係るセンサバイアス推定装置を示すブロック構成図である。
図９において、このセンサバイアス推定装置は、図１に示したセンサバイアス推定装置に加えて、バイアス格子間隔再設定処理部９０を備えている。
【００９７】
バイアス格子点探索処理部４０は、現時刻でのバイアス候補、時刻および格子点探索終了フラグを、バイアス格子間隔再設定処理部９０に出力する。
バイアス格子間隔再設定処理部９０は、バイアス格子点探索処理部４０から得られる探索開始時刻と現時刻との差が、事前に決めた格子間隔再設定時刻しきい値よりも大きい場合、かつ格子点探索終了フラグが０の場合に、次時刻のバイアス格子間隔を現時刻のバイアス格子間隔よりも粗く設定する制御信号を、バイアス格子点探索処理部４０に出力する。
【００９８】
以上のように、実施の形態３によれば、現時刻において、バイアス格子点探索処理が所定の時間内に終了する見込みがない場合であっても、現時刻よりもバイアス格子間隔を粗く設定することにより、次時刻以降のバイアス格子点探索処理を、所定の時間内に終了させることができる。すなわち、所定の時間内で、最良のバイアス推定値を算出することができる。
なお、この発明の実施の形態３に係るセンサバイアス推定装置は、実施の形態２と組み合わせることもできる。
【００９９】
実施の形態４．
図１０は、この発明の実施の形態４に係るセンサバイアス推定装置を示すブロック構成図である。
図１０において、このセンサバイアス推定装置は、図１に示したセンサバイアス推定装置に加えて、第１のバイアス格子点探索範囲再設定処理部１００を備えている。
【０１００】
バイアス格子点探索処理部４０は、現時刻でのバイアス候補、時刻および格子点探索終了フラグを、第１のバイアス格子点探索範囲再設定処理部１００に出力する。
第１のバイアス格子点探索範囲再設定処理部１００は、バイアス格子点探索処理部４０から得られる探索開始時刻と現時刻との差が、事前に決めた探索範囲再設定時刻しきい値よりも大きい場合、かつ格子点探索終了フラグが０の場合に、探索処理が事前に決めた時間内に終了しないと判定する。
【０１０１】
このとき、第１のバイアス格子点探索範囲再設定処理部１００は、バイアス格子点の探索範囲を狭くする制御信号を、バイアス格子点探索処理部４０に出力する。バイアス格子点探索処理部４０は、現時刻よりも狭い固定のバイアス格子点探索範囲を設定する。
なお、第１のバイアス格子点探索範囲再設定処理部１００は、図６で示したバイアスベクトル推定値の推定誤差共分散行列から得られる誤差楕円体、または状態ベクトル推定値の推定誤差共分散行列から得られる誤差楕円体に、半径を小さくする係数を掛けて、現時刻よりも狭い可変のバイアス格子点探索範囲を設定してもよい。
【０１０２】
以上のように、実施の形態４によれば、現時刻において、バイアス格子点探索処理が所定の時間内に終了する見込みがない場合であっても、次時刻以降のバイアス格子点の探索範囲を狭く設定することにより、次時刻以降のバイアス格子点探索処理を、所定の時間内に終了させることができる。すなわち、所定の時間内で、最良のバイアス推定値を算出することができる。
なお、この発明の実施の形態４に係るセンサバイアス推定装置は、実施の形態２または３と組み合わせることもできる。
【０１０３】
実施の形態５．
図１１は、この発明の実施の形態５に係るセンサバイアス推定装置を示すブロック構成図である。
図１１において、このセンサバイアス推定装置は、図１に示したセンサバイアス推定装置に加えて、第２のバイアス格子点探索範囲再設定処理部１１０を備えている。
【０１０４】
バイアス格子点探索処理部４０は、現時刻でのバイアス候補、時刻および格子点探索終了フラグを、第２のバイアス格子点探索範囲再設定処理部１１０に出力する。
第２のバイアス格子点探索範囲再設定処理部１１０は、バイアス格子点探索処理部４０から得られる探索開始時刻と現時刻との差が、事前に決めた探索範囲再設定時刻しきい値よりも大きい場合、かつ格子点探索終了フラグが１の場合に、探索処理が事前に決めた時間内に終了したと判定する。
【０１０５】
このとき、第２のバイアス格子点探索範囲再設定処理部１１０は、バイアス格子点の探索範囲を広くする制御信号を、バイアス格子点探索処理部４０に出力する。バイアス格子点探索処理部４０は、現時刻よりも広い固定のバイアス格子点探索範囲を設定する。
なお、第２のバイアス格子点探索範囲再設定処理部１１０は、図６で示したバイアスベクトル推定値の推定誤差共分散行列から得られる誤差楕円体、または状態ベクトル推定値の推定誤差共分散行列から得られる誤差楕円体に、半径を大きくする係数を掛けて、現時刻よりも広い可変のバイアス格子点探索範囲を設定してもよい。
【０１０６】
以上のように、実施の形態５によれば、現時刻において、バイアス格子点探索処理が所定の時間内に終了する見込みがある場合であって、かつ現時刻のバイアス格子点探索範囲内に、実際のバイアス真値がない場合に、次時刻以降のバイアス格子点探索範囲を広く設定することにより、バイアス格子点探索範囲外に存在する実際のバイアス真値を発見することができる。すなわち、バイアス推定精度を向上させることができる。
なお、この発明の実施の形態５に係るセンサバイアス推定装置は、実施の形態２または３と組み合わせることもできる。
【０１０７】
実施の形態６．
図１２は、この発明の実施の形態６に係るセンサバイアス推定装置を示すブロック構成図である。
図１２において、このセンサバイアス推定装置は、図１に示したセンサバイアス推定装置に加えて、バイアス成分推定順序設定処理部１２０を備えている。
【０１０８】
バイアス成分推定順序設定処理部１２０は、バイアス格子点探索処理部４０およびバイアス推定フィルタ処理部６０に対して、ある成分についてバイアス推定値を算出した後に、残りの成分についてバイアス推定値を算出するように制御信号を出力する。
以下、バイアス成分推定順序設定処理部１２０が、バイアス格子点探索処理部４０およびバイアス推定フィルタ処理部６０に対して、仰角、方位角の各成分についてバイアス推定値を算出した後に、距離成分についてバイアス推定値を算出するように制御信号を出力した場合を例に挙げて説明する。
【０１０９】
このとき、バイアス格子点探索処理部４０は、バイアス格子点探索処理を、仰角、方位角からなる範囲と設定して、仰角、方位角バイアス推定値を算出し、算出結果をバイアス推定フィルタ処理部６０に出力する。
【０１１０】
続いて、バイアス推定フィルタ処理部６０は、バイアス格子点探索処理部４０から得られる、仰角、方位角バイアス推定値を、バイアス推定におけるバイアスベクトル推定値の初期値として設定する。また、バイアス推定フィルタ処理部６０は、この初期値に基づいて、上記式（１０）で示した仰角、方位角の各成分からなるバイアスベクトル推定値およびその推定誤差共分散行列と、状態ベクトル推定値およびその推定誤差共分散行列を算出する。
【０１１１】
次に、バイアス格子点探索処理部４０は、仰角、方位角のバイアス格子点探索処理とは別に、距離のバイアス格子点探索処理を実行する。
距離のバイアス格子点探索処理において、バイアス格子点探索処理部４０は、バイアス推定フィルタ処理部６０から得られる仰角、方位角の各成分からなるバイアスベクトル推定値を、バイアス格子点探索処理におけるバイアス候補ΔｚｉＲ，ｉＥ，Ａｚ（アンダーバー）の仰角成分、方位角成分と設定する。
【０１１２】
そして、バイアス格子点探索処理部４０は、距離成分のみについて図７に示したバイアス格子点探索処理を実行して、距離バイアス推定値を算出し、算出結果をバイアス推定フィルタ処理部６０に出力する。
また、バイアス格子点探索処理部４０は、算出した距離バイアス推定値をバイアスＤＢ７０に出力する。
【０１１３】
特に遠距離の場合、角度誤差に起因するクロスレンジ誤差は、距離誤差、すなわちレンジ誤差に比べて悪くなる。その場合、距離バイアスに比べて、角度バイアスの影響が遠距離において支配的になり、バイアス推定フィルタ処理によって、距離バイアスを推定できない場合が考えられる。
そこで、この実施の形態６によれば、例えば仰角、方位角と距離とに分けてバイアス推定値を算出することにより、バイアス推定精度を向上させることができる。
【０１１４】
なお、上記実施の形態６では、バイアス成分の推定順序として、まず、仰角、方位角の各成分についてバイアス推定値を算出した後に、距離成分についてバイアス推定値を算出する場合を示した。この例は、仰角、方位角と距離とに分けてバイアス推定を実行する例である。
しかしながら、これに限定されず、距離、仰角と、方位角とに分けてバイアス推定を実行してもよいし、距離、方位角と仰角とに分けてバイアス推定を実行してもよいし、距離と仰角と方位角とにそれぞれ分けてバイアス推定を実行してもよい。
また、この発明の実施の形態６に係るセンサバイアス推定装置は、実施の形態２〜５と組み合わせることもできる。
【０１１５】
実施の形態７．
図１３は、この発明の実施の形態７に係るセンサバイアス推定装置を示すブロック構成図である。
図１３において、このセンサバイアス推定装置は、図１に示したセンサバイアス推定装置に加えて、バイアス成分次数設定処理部１３０を備えている。
【０１１６】
この実施の形態７において、第１センサ１０の処理は、上述した実施の形態１と同様とする。すなわち、第１センサ１０は、センサの受信器から距離、仰角、方位角の観測値を得て、最終的に、第１データ更新処理部１３から、上記式（４）または（５）に示した成分を有する平滑値の状態ベクトルおよびその誤差共分散行列を出力する。なお、この実施の形態７では、上記式（４）のような速度項までの状態ベクトルについて説明する。
【０１１７】
また、この実施の形態７において、第２センサ２０は、センサの受信器から仰角、方位角の観測値を得て、最終的に、第２データ更新処理部２３から、次式（１３）または（１４）に示す成分を有する平滑値の状態ベクトルおよびその誤差共分散行列を出力する。
【０１１８】
【数１３】
2011064484.tif 000015
【０１１９】
【数１４】
2011064484.tif 000016
【０１２０】
式（１３）、（１４）において、Ｅ（ｋ）は時刻ｋの仰角を示し、Ａｚ（ｋ）は時刻ｋの方位角を示し、Ｅ（ｋ）（ドット）は時刻ｋの仰角速度を示し、Ａｚ（ｋ）（ドット）は時刻ｋの方位角速度を示し、Ｅ（ｋ）（ツードット）は時刻ｋの仰角加速度を示し、Ａｚ（ｋ）（ツードット）は時刻ｋの方位角加速度をそれぞれ示す。なお、この実施の形態７では、上記式（１３）のような速度項までの状態ベクトルについて説明する。
【０１２１】
バイアス成分次数設定処理部１３０は、同期処理部３０、バイアス格子点探索処理部４０、統合相関処理部５０およびバイアス推定フィルタ処理部６０に対して、推定すべきバイアス成分の次数を、観測される成分の次数に合わせるように制御信号を出力する。
【０１２２】
このとき、同期処理部３０は、第１センサ１０から得られる式（４）に示した成分を有する平滑値の状態ベクトル、すなわちｘ，ｙ，ｚの直交座標に関する６次元の航跡と、第２センサ２０から得られる式（１３）に示した成分を有する平滑値の状態ベクトル、すなわち仰角、方位角に関する４次元の航跡とに基づいて、時刻同期および航跡相関を実行する。
【０１２３】
ここで、同期処理部３０による航跡相関は、航跡の位置情報を用いて実行される。このとき、第１センサ１０からのｘ，ｙ，ｚの直交座標に関する６次元の航跡と、第２センサ２０からの仰角、方位角に関する４次元の航跡とは、次元が互いに異なるので、次元が少ない方の基準座標に合わせる。
【０１２４】
すなわち、同期処理部３０は、第１センサ１０からのｘ，ｙ，ｚの直交座標に関する航跡を、第２センサ２０を中心とした第２センサ基準座標に変換し、さらに、第２センサ基準座標において、直交座標から極座標への座標変換を行う。その後、同期処理部３０は、第２センサ基準座標において、仰角、方位角の各成分の情報を用いて、第１センサ１０から得られる航跡と第２センサ２０から得られる航跡との航跡相関を実行する。
【０１２５】
続いて、バイアス格子点探索処理部４０は、同期処理部３０による航跡相関によって得られた第１センサ１０と第２センサ２０との仰角、方位角を各成分とする航跡のペアを用いて、これらの仰角、方位角の各成分からなるバイアス格子点探索範囲を設定する。また、バイアス格子点探索処理部４０は、バイアス格子点探索処理により、仰角、方位角バイアス推定値を算出し、算出結果をバイアス推定フィルタ処理部６０に出力する。
【０１２６】
次に、バイアス推定フィルタ処理部６０は、バイアス格子点探索処理部４０から得られる、仰角、方位角バイアス推定値を、バイアス推定におけるバイアスベクトル推定値の初期値として設定する。また、バイアス推定フィルタ処理部６０は、この初期値に基づいて、仰角、方位角の各成分からなるバイアスベクトル推定値およびその推定誤差共分散行列を算出する。バイアス推定フィルタ処理部６０は、この初期値に基づいて、状態推定処理により、第１センサ１０または第２センサ２０を基準とする直交座標における平滑値の状態ベクトルおよびその誤差共分散行列を算出する。
【０１２７】
なお、統合相関処理部５０から得られる相関観測値は、第１センサ１０からの相関観測値については、ｘ，ｙ，ｚの３次元の直交座標における観測位置であるが、第２センサ２０からの相関観測値については、仰角、方位角の極座標における観測位置である。
【０１２８】
したがって、バイアス推定フィルタ処理部６０が、第２センサ２０からの相関観測値を用いて状態推定処理を実行する場合、状態ベクトルがｘ，ｙ，ｚの３次元の直交座標であるのに対して、第２センサ２０の観測値が仰角、方位角であり、観測ベクトルの成分が仰角、方位角の２次元なので、状態ベクトルと観測ベクトルとの関係を表す観測モデルが非線形となる。そこで、バイアス推定フィルタ処理部６０が、第２センサ２０からの相関観測値を用いて状態推定処理を実行する場合には、拡張カルマンフィルタが用いられる。
【０１２９】
以上のように、実施の形態７によれば、センサが得る観測ベクトルの次数が異なっている場合であっても、バイアス推定精度を向上させることができる。
【０１３０】
なお、上記実施の形態７では、バイアス成分次数設定処理部１３０が、第２センサ２０で得られた仰角、方位角からなる２次元の観測ベクトルに対応した、２次元のバイアスベクトル推定値を算出する場合について説明した。しかしながら、これに限定されず、バイアス成分次数設定処理部１３０は、第２センサ２０で得られた２次元の観測ベクトルよりも次数の低い１次元のバイアスベクトル推定値、例えば仰角成分のみあるいは方位角成分のみを算出するように制御信号を出力してもよい。また、何次元のバイアスベクトル推定値を算出するかについては、事前に設定される。
【０１３１】
また、上記実施の形態７では、第２センサ２０が、仰角、方位角を観測する場合について説明した。しかしながら、これに限定されず、第２センサ２０が、距離のみを観測する場合、仰角のみを観測する場合、方位角のみを観測する場合、距離および仰角を観測する場合、距離および方位角を観測する場合にも、それぞれ適用することができる。
また、この発明の実施の形態７に係るセンサバイアス推定装置は、実施の形態２〜６と組み合わせることもできる。
【符号の説明】
【０１３２】
１０  第１センサ、２０  第２センサ、３０  同期処理部（同期処理手段）、４０  バイアス格子点探索処理部（バイアス格子点探索処理手段）、５０  統合相関処理部（統合相関処理手段）、６０  バイアス推定フィルタ処理部（バイアス推定フィルタ処理手段）、８０  バイアス格子間隔制御処理部（バイアス格子間隔制御処理手段）、９０  バイアス格子間隔再設定処理部（バイアス格子間隔再設定処理手段）、１００  第１のバイアス格子点探索範囲再設定処理部（第１のバイアス格子点探索範囲再設定処理手段）、１１０  第２のバイアス格子点探索範囲再設定処理部（第２のバイアス格子点探索範囲再設定処理手段）、１２０  バイアス成分推定順序設定処理部（バイアス成分推定順序設定処理手段）、１３０  バイアス成分次数設定処理部（バイアス成分次数設定処理手段）。
【図１】
2011064484.tif 000017
【図２】
2011064484.tif 000018
【図３】
2011064484.tif 000019
【図４】
2011064484.tif 000020
【図５】
2011064484.tif 000021
【図６】
2011064484.tif 000022
【図７】
2011064484.tif 000023
【図８】
2011064484.tif 000024
【図９】
2011064484.tif 000025
【図１０】
2011064484.tif 000026
【図１１】
2011064484.tif 000027
【図１２】
2011064484.tif 000028
【図１３】
2011064484.tif 000029
【図１４】
2011064484.tif 000030