センサバイアス推定装置
【課題】バイアスベクトル推定値を高精度に算出可能なセンサバイアス推定装置を得る。
【解決手段】それぞれのセンサが、観測値と予測値とに基づいて相関観測値を算出するとともに、相関観測値に基づいてセンサ航跡を算出し、算出結果を出力する装置に適用され、複数のセンサ10、20のバイアスを推定するセンサバイアス推定装置であって、センサ航跡に基づいて、時刻同期および航跡相関を実行し、同一航跡と判定されたセンサ航跡の組を出力する同期処理部30と、同一航跡と判定されたセンサ航跡の組に対してバイアス格子点探索処理を実行し、バイアス推定値を算出するバイアス格子点探索処理部40と、相関観測値に基づいて、バイアス推定フィルタ処理を実行し、バイアスベクトル推定値を算出するバイアス推定フィルタ処理部60とを備え、バイアス推定フィルタ処理部60は、バイアス推定値をバイアスベクトル推定値の初期値として設定するものである。
【解決手段】それぞれのセンサが、観測値と予測値とに基づいて相関観測値を算出するとともに、相関観測値に基づいてセンサ航跡を算出し、算出結果を出力する装置に適用され、複数のセンサ10、20のバイアスを推定するセンサバイアス推定装置であって、センサ航跡に基づいて、時刻同期および航跡相関を実行し、同一航跡と判定されたセンサ航跡の組を出力する同期処理部30と、同一航跡と判定されたセンサ航跡の組に対してバイアス格子点探索処理を実行し、バイアス推定値を算出するバイアス格子点探索処理部40と、相関観測値に基づいて、バイアス推定フィルタ処理を実行し、バイアスベクトル推定値を算出するバイアス推定フィルタ処理部60とを備え、バイアス推定フィルタ処理部60は、バイアス推定値をバイアスベクトル推定値の初期値として設定するものである。
【発明の詳細な説明】
【技術分野】
【0001】
この発明は、例えば複数のセンサを有する追尾装置に適用され、それぞれのセンサのバイアス(バイアス誤差)を推定するセンサバイアス推定装置に関する。
【背景技術】
【0002】
従来のセンサのバイアス誤差推定装置は、複数の目標に対して、それぞれ第1および第2のセンサからのデータ入力を観測する第1および第2の観測器の時間毎の出力に基づいて、観測行列を生成する観測行列生成器と、観測行列出力と観測雑音の共分散とからバイアス誤差推定値の共分散行列を算出する推定値評価器と、観測行列と推定値評価器出力とから仮のバイアス誤差値を算出する推定値算出器と、観測行列生成器と推定値評価器との間に設けられ、観測行列とその転置行列との積の最小固有値が所定の定数よりも大きい場合にのみ、観測行列を出力する階数判定器とを備え、累積演算結果からカルマンフィルタによりセンサのバイアス誤差を推定している(例えば、特許文献1参照)。
【0003】
次に、図14を参照しながら、バイアス(バイアス誤差)の補正を実行する目的について説明する。なお、本明細書において、バイアスとバイアス誤差とは同義とする。
図14は、2つのセンサのバイアスをそれぞれ補正する様子を、PPI(Plan Position Indicator)表示で例示する説明図である。
図14において、2つのセンサを構成するセンサAおよびセンサBは、それぞれ方位角方向にバイアスを有しており、センサAおよびセンサBともに、目標1、2、3を同時に観測している(図中(a)の状態)。
【0004】
このとき、センサAおよびBは、それぞれ方位角方向にバイアスを有しているので、センサAおよびBで同一の目標を観測していたとしても、センサAおよびBの観測結果を重ね合わせた場合、目標同士が重なり合わず、だぶって見えることとなる(図中(b)の状態)。そこで、このような状況を改善するために、センサAおよびBのバイアスを推定して、バイアスを補正することにより、センサAおよびBの観測結果を重ね合わせた場合に、目標同士を1つに重ね合わせることができる(図中(c)の状態)。
すなわち、センサAおよびBで観測した目標の位置、速度、加速度等からなる目標の状態量を一致させることが、バイアスの補正を実行する目的である。
【0005】
続いて、特許文献1に示したようなカルマンフィルタにより、距離、仰角、方位角の各成分からなるバイアスベクトルと、目標の位置、速度の各成分からなる状態ベクトルとを推定することを考える。
【0006】
ここで、バイアスベクトルは、距離、方位角の各成分で構成されてもよいし、仰角、方位角の各成分で構成されてもよいし、距離、仰角の各成分で構成されてもよいし、距離成分のみで構成されてもよいし、仰角成分のみで構成されてもよいし、方位角成分のみで構成されてもよい。すなわち、バイアスベクトルの次数およびその成分は、事前に任意に設定してカルマンフィルタを構成することができる。
また、バイアスベクトルは、X,Y,Zの各成分で構成されてもよいし、バイアス推定を実行する構成は、上記距離、仰角、方位角の各成分の組み合わせと同様に、様々な組み合わせによって実現することができる。
【0007】
同様に、状態ベクトルは、それぞれ3次元のX,Y,Z成分からなる位置、速度の6次元の状態ベクトルとして構成されてもよいし、それぞれ3次元のX,Y,Z成分からなる位置、速度、加速度の9次元の状態ベクトルとして構成されてもよいし、それぞれ2次元のX,Y成分からなる位置、速度の4次元の状態ベクトルとして構成されてもよいし、それぞれ2次元のX,Y成分からなる位置、速度、加速度の6次元の状態ベクトルとして構成されてもよい。すなわち、状態ベクトルの次数およびその成分は、事前に任意に設定してカルマンフィルタを構成することができる。
【先行技術文献】
【特許文献】
【0008】
【特許文献1】特許第3302870号公報
【発明の概要】
【発明が解決しようとする課題】
【0009】
しかしながら、従来技術には、以下のような課題がある。
カルマンフィルタおよび最小2乗フィルタによりバイアス推定を実行する場合には、バイアスベクトル推定値の初期値が必要になる。ここで、従来のセンサのバイアス誤差推定装置では、バイアスベクトル推定値の初期値の設定方法が明らかにされていない。
そのため、バイアスベクトル推定値の初期値が、バイアスベクトルの真値から大きく外れていた場合には、バイアスベクトル推定値の精度が低下したり、バイアスベクトル推定値の収束が遅くなったりするという問題がある。
【0010】
この発明は、上記のような課題を解決するためになされたものであり、バイアスベクトル推定値を高精度に算出することができるセンサバイアス推定装置を得ることを目的とする。
【課題を解決するための手段】
【0011】
この発明に係るセンサバイアス推定装置は、複数のセンサを有する装置であり、それぞれのセンサが、観測値と予測値とに基づいて相関処理を実行して相関観測値を算出するとともに、相関観測値に基づいてセンサ航跡を算出し、相関観測値およびセンサ航跡を出力する装置に適用され、複数のセンサのバイアスを推定するセンサバイアス推定装置であって、センサ航跡に基づいて、時刻同期および航跡相関を実行し、同一航跡と判定されたセンサ航跡の組を出力する同期処理手段と、同一航跡と判定されたセンサ航跡の組に対してバイアス格子点探索処理を実行し、バイアス推定値を算出するバイアス格子点探索処理手段と、相関観測値に基づいて、バイアス推定フィルタ処理を実行し、バイアスベクトル推定値を算出するバイアス推定フィルタ処理手段と、を備え、バイアス推定フィルタ処理手段は、バイアス推定値をバイアスベクトル推定値の初期値として設定するものである。
【発明の効果】
【0012】
この発明に係るセンサバイアス推定装置によれば、センサからの相関観測値に基づいてバイアス推定フィルタ処理を実行し、バイアスベクトル推定値を算出するバイアス推定フィルタ処理手段は、バイアス格子点探索処理手段によるバイアス格子点探索処理によって算出されるバイアス推定値をバイアスベクトル推定値の初期値として設定する。そのため、バイアスベクトル推定値の初期値を適切に設定することができ、バイアスベクトル推定値を高精度に算出することができるセンサバイアス推定装置を得ることができる。
【図面の簡単な説明】
【0013】
【図1】この発明の実施の形態1に係るセンサバイアス推定装置を示すブロック構成図である。
【図2】この発明の実施の形態1に係る同期処理部による同期処理を説明するためのタイミングチャートである。
【図3】この発明の実施の形態1に係る同期処理部による航跡相関を示す説明図である。
【図4】この発明の実施の形態1に係るバイアス格子点探索処理部による、距離、仰角、方位角バイアスを持つ場合のバイアス格子点探索処理を示す説明図である。
【図5】この発明の実施の形態1に係るバイアス格子点探索処理部による、X,Y,Zバイアスを持つ場合のバイアス格子点探索処理を示す説明図である。
【図6】この発明の実施の形態1に係るバイアス推定フィルタ処理部からのバイアスベクトル推定値およびその推定誤差共分散行列、または状態ベクトル推定値およびその推定誤差共分散行列に基づいて、バイアス格子点の探索範囲を絞り込む処理を示す説明図である。
【図7】この発明の実施の形態1に係るバイアス格子点探索処理部によるバイアス格子点探索処理の手順を示すフローチャートである。
【図8】この発明の実施の形態2に係るセンサバイアス推定装置を示すブロック構成図である。
【図9】この発明の実施の形態3に係るセンサバイアス推定装置を示すブロック構成図である。
【図10】この発明の実施の形態4に係るセンサバイアス推定装置を示すブロック構成図である。
【図11】この発明の実施の形態5に係るセンサバイアス推定装置を示すブロック構成図である。
【図12】この発明の実施の形態6に係るセンサバイアス推定装置を示すブロック構成図である。
【図13】この発明の実施の形態7に係るセンサバイアス推定装置を示すブロック構成図である。
【図14】2つのセンサのバイアスをそれぞれ補正する様子を、PPI表示で例示する説明図である。
【発明を実施するための形態】
【0014】
以下、この発明のセンサバイアス推定装置の好適な実施の形態につき図面を用いて説明するが、各図において同一、または相当する部分については、同一符号を付して説明する。
【0015】
実施の形態1.
図1は、この発明の実施の形態1に係るセンサバイアス推定装置を示すブロック構成図である。
図1において、このセンサバイアス推定装置は、第1センサ10、第2センサ20、同期処理部30、バイアス格子点探索処理部40、統合相関処理部50、バイアス推定フィルタ処理部60およびバイアスデータベース(バイアスDB)70とを備えている。
【0016】
第1センサ10は、第1信号処理部11、第1相関処理部12、第1データ更新処理部13、第1予測処理部14および第1遅延処理部15を有している。また、第2センサ20は、第2信号処理部21、第2相関処理部22、第2データ更新処理部23、第2予測処理部24および第2遅延処理部25を有している。なお、第1センサ10における第1信号処理部11から第1遅延処理部15までの処理と、第2センサ20における第2信号処理部21から第2遅延処理部25までの処理とは、互いに同一の処理である。
【0017】
第1センサ10および第2センサ20は、絶対座標に対してバイアスを有している。また、第1センサ10からバイアスDB70までの処理により、第1センサ10および第2センサ20の両方のバイアスを推定することもできるし、第1センサ10に対する第2センサ20のバイアスを推定することもできる。
【0018】
ここで、絶対座標に対する第1センサ10および第2センサ20のバイアスを絶対バイアスと呼び、絶対座標に対する第1センサ10の絶対バイアスを0とした場合の第2センサ20の絶対バイアスを相対バイアスと呼ぶ。また、絶対座標を第1センサ10とすれば、第1センサ10の絶対バイアスが0でない場合でも、第1センサ10に対する第2センサ20のバイアスを相対バイアスと呼ぶことができる。
【0019】
また、第1センサ10および第2センサ20は、距離、仰角、方位角を観測するレーダのような3次元のセンサであってもよいし、仰角、方位角を観測するIR(Infra Red)センサ、またはESM(Electric Support Measures)センサのような2次元のセンサであってもよい。また、第1センサ10および第2センサ20は、距離、方位角を観測する2次元のセンサであってもよいし、距離のみを観測する1次元のセンサであってもよいし、方位角のみを観測する1次元のセンサであってもよいし、仰角のみを観測する1次元のセンサであってもよい。
【0020】
以下、このセンサバイアス推定装置の各部位の機能について、第1センサ10に対する第2センサ20の相対バイアスを推定する場合を例に挙げて説明する。このとき、第1センサ10および第2センサ20は、代表的な例として、距離、仰角、方位角を観測する3次元センサとする。
【0021】
まず、第1センサ10の機能について説明する。
第1信号処理部11は、センサの受信器(図示せず)から時刻kにおける距離、仰角、方位角の観測値を得て、それらの観測値を第1相関処理部12に出力する。
【0022】
第1相関処理部12は、第1信号処理部11から入力された時刻kにおける距離、仰角、方位角の観測値と、第1遅延処理部15から入力された時刻kにおける距離、仰角、方位角の予測値とに基づいて、相関処理を実行する。相関処理とは、観測値の誤差や予測値の誤差を勘案したゲート内において、複数の観測値と目標の予測値との対応づけを予測値と観測値との距離でとり、予測値に一番近い観測値1点または予測値に近い複数点を目標信号の候補として抽出したり、予測値からの距離に応じて重み付け平均した観測値を目標信号の候補として抽出したりする処理である。
【0023】
また、第1相関処理部12は、相関処理を実行して得られた時刻kにおける相関観測値を、第1データ更新処理部13および統合相関処理部50に出力する。ここで、相関観測値は、ゲート内の観測値である。
【0024】
第1データ更新処理部13は、第1相関処理部12から入力された時刻kにおける相関観測値のうち、例えば予測値に一番近い相関観測値を用いて、カルマンフィルタのデータ更新処理に基づいて、時刻kにおける平滑値を算出する。平滑値は、上述した段落で示した平滑値の状態ベクトルおよびその誤差共分散行列であり、状態ベクトルの次数は、事前に設定した3次元、6次元または9次元とする。以後、平滑値を航跡と呼ぶことがある。なお、誤差共分散行列は、その行および列が状態ベクトルの次数と等しいものとする。
また、第1データ更新処理部13は、時刻kにおける平滑値を、第1予測処理部14および同期処理部30に出力する。
【0025】
第1予測処理部14は、第1データ更新処理部13から入力された時刻kにおける平滑値の状態ベクトルおよびその誤差共分散行列を、事前に仮定した運動モデルに基づいて、1サンプリング分外挿し、時刻k+1における予測値を算出する。予測値は、上述した段落で示した予測値の状態ベクトルおよびその誤差共分散行列であり、状態ベクトルの次数は、事前に設定した3次元、6次元または9次元とする。
また、第1予測処理部14は、時刻k+1における予測値を、第1遅延処理部15に出力する。
【0026】
第1遅延処理部15は、第1予測処理部14から入力された時刻k+1における予測値を1サンプリング分遅延させて、時刻kにおける予測値を算出し、算出された予測値を第1相関処理部12に出力する。
【0027】
なお、第2センサ20における第2信号処理部21、第2相関処理部22、第2データ更新処理部23、第2予測処理部24および第2遅延処理部25の機能は、上述したように、第1センサ10における第1信号処理部11、第1相関処理部12、第1データ更新処理部13、第1予測処理部14および第1遅延処理部15の機能と同様なので、説明を省略する。
【0028】
続いて、同期処理部30は、第1センサ10から入力された平滑値と、第2センサ20から入力された平滑値とに基づいて、時刻同期および航跡相関を実行する。すなわち、同期処理部30は、第1センサ10から得られた航跡と、第2センサ20から得られた航跡とに基づいて、時刻同期および航跡相関を実行する。
【0029】
以下、図2を参照しながら、同期処理部30による時刻同期について説明する。ここで、第1センサ10から得られた航跡を第1センサ航跡と呼び、第2センサ20から得られた航跡を第2センサ航跡と呼ぶ。
図2は、この発明の実施の形態1に係る同期処理部30による同期処理を説明するためのタイミングチャートである。
【0030】
図2において、横軸は、第1センサ航跡の同期処理への入力タイミングのタイムライン(A)、第2センサ航跡の同期処理への入力タイミングのタイムライン(B)、および同期処理での処理タイミングのタイムライン(C)を示している。また、タイムライン(A)において直交する太棒線は、各時刻における第1センサ航跡を表し、タイムライン(B)において直交する太棒線は、各時刻における第2センサ航跡を表している。
【0031】
図2より、第1センサ航跡と第2センサ航跡とは、同期処理への入力タイミングが互いに異なることが分かる。そこで、同時刻の第1センサ航跡と第2センサ航跡とを比較するために、ある基準時刻に時刻合わせを行う必要がある。なお、基準時刻は、タイムライン(C)によって決まるものとする。
【0032】
このとき、タイムライン(B)で航跡が生成される時刻をt(i)とし、この時刻t(i)を基準時刻とすると(t(k)≦t(i)≦t(k+1))、第1センサ航跡が時刻t(i)で得られていないので、同期処理部30は、時刻t(i)以前に第1センサ航跡が得られた時刻t(k)における第1センサ航跡を時刻t(i)まで保持する。
【0033】
すなわち、時刻t(k)における第1センサ航跡を、時刻t(k)から時刻t(i)までのサンプリング間隔Δt(=t(i)−t(k))で保持して、時刻t(i)における第1センサ航跡を算出する。
つまり、同期処理部30は、タイムライン(C)における(10)の箇所で時刻同期をとることになる。
【0034】
次に、図3を参照しながら、同期処理部30による航跡相関について説明する。
図3は、この発明の実施の形態1に係る同期処理部30による航跡相関を示す説明図である。
図3において、第1センサ10からは、第1センサ航跡101および第1センサ航跡102が得られているものとし、第2センサ20からは、第2センサ航跡201、第2センサ航跡202および第2センサ航跡203が得られているものとする。また、第1センサ航跡101を中心とした第1センサ航跡101のゲート、および第1センサ航跡102を中心とした第1センサ航跡102のゲートを点線で囲まれた範囲で示す。
【0035】
ここで、第1センサ航跡のゲート内に第2センサ航跡が入っている場合には、同期処理部30は、第1センサ航跡と第2センサ航跡とが同一航跡であると判定する。図3の場合では、第1センサ航跡101と第2センサ航跡201とが同一航跡であると判定され、第1センサ航跡102と第2センサ航跡203とが同一航跡であると判定される。
【0036】
このような、第1センサ航跡と第2センサ航跡との同一性を判定する処理を航跡相関と呼ぶ。
なお、航跡相関を実行する場合には、第1センサ10と第2センサ20との基準座標が互いに異なるので、第1センサ10または第2センサ20の何れかの基準座標に座標変換してから航跡相関を実行する。また、ここでは、第1センサ航跡を中心としたゲートを設定したが、これに限定されず、第2センサ航跡を中心としたゲートを設定し、第2センサ航跡を中心としたゲート内に第1センサ航跡が入るか否かで同一航跡を判定してもよい。
【0037】
同期処理部30は、まず、第1センサ10から得られた第1センサ航跡と、第2センサ20から得られた第2センサ航跡とに基づいて時刻同期を実行し、続いて、航跡相関を実行する。このとき、時刻同期は、各センサの速度を用いて、時刻同期を実行する時刻まで航跡位置を保持する。
【0038】
そして、同期処理部30は、同一航跡と判定された第1センサ航跡と第2センサ航跡とのペアを、バイアス格子点探索処理部40に出力する。また、同期処理部30は、第1センサ航跡と第2センサ航跡とのペア情報を、統合相関処理部50に出力する。なお、ペア情報は、同一航跡と判定された第1センサ航跡と第2センサ航跡とのペアだけでなく、同一航跡と判定されなかった第1センサ航跡および第2センサ航跡も含むものとする。
【0039】
次に、バイアス格子点探索処理部40は、バイアス格子点探索処理により、第1センサ10に対する第2センサ20の相対バイアスを推定する。
以下、図4〜7を参照しながら、バイアス格子点探索処理部40によるバイアス格子点探索処理について説明する。
【0040】
図4は、この発明の実施の形態1に係るバイアス格子点探索処理部40による、距離、仰角、方位角バイアスを持つ場合のバイアス格子点探索処理を示す説明図である。また、図5は、この発明の実施の形態1に係るバイアス格子点探索処理部40による、X,Y,Zバイアスを持つ場合のバイアス格子点探索処理を示す説明図である。なお、図4と図5とは、成分が異なるだけで処理内容は同様なので、図4を用いてバイアス格子点探索処理を説明する。
【0041】
図4において、zkY(アンダーバー)は、時刻kにおける第2センサ20の航跡を示し、zk−1Y(アンダーバー)は、時刻k−1における第2センサ20の航跡を示し、zk−2Y(アンダーバー)は、時刻k−2における第2センサ20の航跡を示す。また、zkX(アンダーバー)は、時刻kにおける第1センサ10の航跡を示し、zk−1X(アンダーバー)は、時刻k−1における第1センサ10の航跡を示し、zk−2X(アンダーバー)は、時刻k−2における第1センサ10の航跡を示す。
【0042】
また、ΔziR,iE,Az(アンダーバー)は、第1センサ10に対する第2センサ20のバイアス候補を示す。なお、このバイアス候補は複数存在し、複数のバイアス候補の中から、後述する図7の処理により1つを選択する。
ここで、バイアス候補のイメージは、図4の右側に示すとおりであり、バイアス候補の成分は、距離成分ΔRiR、仰角成分ΔEiE、方位角成分ΔAziAzからなり、iR、iE、iAzは、バイアス格子点の位置を示すインデックスである。また、このバイアス候補は、距離、仰角、方位角の各軸からなる格子内で値が変化する。
【0043】
なお、バイアス格子点の探索範囲は、後述するバイアス推定フィルタ処理部60からのバイアスベクトル推定値およびその推定誤差共分散行列、または状態ベクトル推定値およびその推定誤差共分散行列に基づいて絞り込まれる。
【0044】
図6は、この発明の実施の形態1に係るバイアス推定フィルタ処理部60からのバイアスベクトル推定値およびその推定誤差共分散行列、または状態ベクトル推定値およびその推定誤差共分散行列に基づいて、バイアス格子点の探索範囲を絞り込む処理を示す説明図である。なお、図6では、2次元平面のイメージを示しているが、バイアス格子点は、推定すべきバイアスの数による。例えば、距離バイアス、仰角バイアス、方位角バイアスを推定する場合には、バイアス格子点は3次元空間となる。
【0045】
このとき、図6のように、バイアス格子点の探索範囲を、バイアスベクトル推定値の推定誤差共分散行列から得られる誤差楕円体、または状態ベクトル推定値の推定誤差共分散行列から得られる誤差楕円体で設定することにより、バイアス格子点の探索範囲を絞り込むことができる。また、バイアス推定フィルタ処理部60からのバイアス推定値を格子点探索の始点に設定することより、格子点の端から探索する処理が不要となる。
【0046】
続いて、図7のフローチャートを参照しながら、バイアス格子点探索処理部40によるバイアス格子点探索処理の手順について説明する。
図7は、この発明の実施の形態1に係るバイアス格子点探索処理部40によるバイアス格子点探索処理の手順を示すフローチャートである。なお、図8のフローチャートは、上述したバイアス格子点の探索範囲を絞り込む処理以外の処理を示している。
【0047】
まず、バイアス格子点探索処理部40は、バイアス候補ΔziR,iE,Az(アンダーバー)を付加する前、すなわちバイアス候補ΔziR,iE,Az(アンダーバー)が未設定の第1センサ航跡zkX(アンダーバー)を設定する(ステップS1)。この第1センサ航跡をバイアス候補未設定第1センサ航跡と呼ぶ。
【0048】
続いて、バイアス格子点探索処理部40は、第1センサ航跡zkX(アンダーバー)と同一航跡であると判定された第2センサ航跡zkY(アンダーバー)を設定する(ステップS2)。
【0049】
次に、バイアス格子点探索処理部40は、バイアス格子点の位置を表すインデックス(iR,iE,iAz)に対応する格子内の1点(図4の右側参照)を、バイアス候補ΔziR,iE,Az(アンダーバー)として設定する(ステップS3)。
【0050】
続いて、バイアス格子点探索処理部40は、ステップS3で設定したバイアス候補ΔziR,iE,Az(アンダーバー)を、バイアス候補未設定第1センサ航跡zkX(アンダーバー)に付加して、バイアス候補設定第1センサ航跡zkX(アンダーバー)を設定する(ステップS4)。
【0051】
次に、バイアス格子点探索処理部40は、時刻k−L+1から時刻kまでの間のある一時刻(ここでは、時刻kとする)におけるバイアス候補設定第1センサ航跡zkX(アンダーバー)と第2センサ航跡zkY(アンダーバー)との残差εkiR,iE,Azを、次式(1)を用いて算出する(ステップS5)。
【0052】
【数1】
【0053】
なお、残差εkiR,iE,Azは、式(1)ではなく、次式(2)を用いて算出されてもよい。
【0054】
【数2】
【0055】
式(1)、(2)において、‖‖は、ノルムを示している。このノルムは、ユークリッドノルムであってもよいし、ベクトルの成分の最大値を取るノルムであってもよい。
以降の説明については、残差εkiR,iE,Azとして式(2)を用いて説明する。
【0056】
続いて、バイアス格子点探索処理部40は、時刻k−L+1から時刻kまでのLサンプル分について、残差を算出したか否かを判定する(ステップS6)。
ステップS6において、Lサンプル分について残差を算出していない(すなわち、No)と判定された場合には、バイアス格子点探索処理部40は、カウンタiを1つ進めて(ステップS7)、ステップS5に移行し、次の時刻における航跡の残差を算出する。
【0057】
一方、ステップS6において、Lサンプル分について残差を算出した(すなわち、Yes)と判定された場合には、バイアス格子点探索処理部40は、Lサンプル分の航跡の残差和εiR,iE,Azを、次式(3)を用いて算出する(ステップS8)。
【0058】
【数3】
【0059】
次に、バイアス格子点探索処理部40は、他のバイアス候補が存在しないか否かを判定する(ステップS9)。ここで、バイアス格子点探索処理部40は、全てのバイアス格子点が既に使用されている場合には、他のバイアス候補は存在しないと判定し、未使用のバイアス格子点が残っている場合には、他のバイアス候補が存在すると判定する。
【0060】
ステップS9において、他のバイアス候補が存在する(すなわち、No)と判定された場合には、バイアス格子点探索処理部40は、カウンタnを1つ進めて(ステップS10)、ステップS3に移行し、別のバイアス候補について残差和εiR,iE,Azを算出する。
【0061】
一方、ステップS9において、他のバイアス候補が存在しない(すなわち、Yes)と判定された場合には、バイアス格子点探索処理部40は、全てのバイアス候補について算出した残差和に基づいて、残差和が最小となるときのバイアス候補を、バイアス推定値として決定し(ステップS11)、図7の処理を終了する。
【0062】
なお、上記ステップS9〜11の処理を言い換えると、バイアス格子点探索処理部40は、図4の右側に示すように、バイアス候補を格子内で移動させることにより、バイアス候補毎に、式(3)を用いて、Lサンプル分の航跡の残差和を算出する。そして、バイアス格子点探索処理部40は、Lサンプル分の航跡の残差和が最小となるときのバイアス候補を選択し、バイアス推定値とする。
【0063】
以上のように、バイアス格子点探索処理部40は、バイアス格子点探索処理により、バイアス推定値を決定する。また、バイアス格子点探索処理部40は、決定したバイアス推定値を、バイアス推定フィルタ処理部60に出力する。
【0064】
また、統合相関処理部50は、同期処理部30から得られたペア情報に基づいて、同一航跡と判定された第1センサ航跡と第2センサ航跡とのペアにおける第1センサ航跡に対応する相関観測値について、バイアス推定フィルタ処理部60で使用可能か否かを判定する。同様に、統合相関処理部50は、同期処理部30から得られたペア情報に基づいて、同一航跡と判定された第1センサ航跡と第2センサ航跡とのペアにおける第2センサ航跡に対応する相関観測値について、バイアス推定フィルタ処理部60で使用可能か否かを判定する。
また、統合相関処理部50は、相関観測値がバイアス推定フィルタ処理部60で使用可能と判定すると、判定毎に相関観測値をバイアス推定フィルタ処理部60に出力する。
【0065】
このとき、統合相関処理部50は、第1センサ航跡に対応する相関観測値および第2センサ航跡に対応する相関観測値が、所定の中心に対する所定のゲート範囲内にある場合に、その相関観測値がバイアス推定フィルタ処理部60で使用可能であると判定する。
なお、所定の中心は、同期処理部30で同一航跡と判定された第1センサ航跡と第2センサ航跡との統合航跡の中心であってもよいし、バイアス推定フィルタ処理部60で推定された目標の位置の中心であってもよい。また、所定のゲート範囲は、同期処理部30で同一航跡と判定された第1センサ航跡と第2センサ航跡との統合航跡のゲート範囲であってもよいし、任意に設定されるゲート範囲であってもよい。
【0066】
ここで、相関処理部12から得られる第1センサ10に対応する相関観測値と、相関処理部22から得られる第2センサ20に対応する相関観測値とは、一般的に、統合相関処理部50に非同期で入力される。なお、もし、相関処理部12から得られる第1センサ10に対応する相関観測値と、相関処理部22から得られる第2センサ20に対応する相関観測値とが、統合相関処理部50に同期して入力される場合には、事前に設定されるセンサの優先順位、例えば、センサ番号の小さいものから順に処理する等の規則に従って、まず第1センサ10、次に第2センサ20といった順に処理する。
【0067】
続いて、バイアス推定フィルタ処理部60は、統合相関処理部50からの相関観測値を用いて、カルマンフィルタまたは最小2乗法に基づいて、バイアス推定フィルタ処理による状態推定およびバイアス推定を実行する。
【0068】
まず、状態推定について、バイアス推定フィルタ処理部60は、次式(4)に示すように、目標の位置、速度の各成分からなる状態ベクトル、または目標の位置、速度、加速度の各成分からなる状態ベクトルの推定値およびその推定誤差共分散行列を算出する。なお、目標の状態ベクトルの次数について、例えば、式(4)のような6次元の状態ベクトルを推定するか、次式(5)のような9次元の状態ベクトルを推定するかは、事前に設定しておく。
【0069】
【数4】
【0070】
【数5】
【0071】
式(4)において、(k)は時刻kを示し、x(k)(アンダーバー)は時刻kの状態ベクトルを示し、記号「´」は行列およびベクトルの転置を示し、状態ベクトルの右辺のx(k)はx成分の位置成分を示し、x(k)(ドット)はx成分の速度成分を示し、x(k)(ツードット)はx成分の加速度成分をそれぞれ示す。なお、y成分、z成分についても同様である。
【0072】
次に、バイアス推定について、バイアス推定フィルタ処理部60は、次式(6)に示すように、距離、仰角、方位角の各成分からなるバイアスベクトルの推定値およびその推定誤差共分散行列を算出する。
【0073】
【数6】
【0074】
式(6)において、b(m)(k)(アンダーバー)は、時刻kにおけるセンサmのバイアスベクトルを示す。ここで、mはセンサを表す識別子であり、この識別子mは、m=1,2,・・・のように番号で設定されてもよいし、m=A,B,・・・X,Y,Z,・・・のように、記号で設定されてもよい。ここでは、識別子mは、第1センサ10または第2センサ20の何れかに対応する。
【0075】
また、式(6)において、ΔR(m)はセンサmの距離バイアスを示し、ΔE(m)はセンサmの仰角バイアスを示し、ΔAz(m)はセンサmの方位角バイアスをそれぞれ示す。
なお、目標のバイアスベクトルの種類については、式(6)のような3次元のバイアスベクトルであってもよいし、次式(7)、(8)、(9)のような1次元のバイアスベクトルであってもよいし、次式(10)、(11)、(12)のような2次元のバイアスベクトルであってもよく、推定すべきバイアスベクトルの種類は、センサ毎に事前に設定しておく。
【0076】
【数7】
【0077】
【数8】
【0078】
【数9】
【0079】
【数10】
【0080】
【数11】
【0081】
【数12】
【0082】
ここで、バイアス推定フィルタ処理部60は、バイアス格子点探索処理部40から得られる、バイアス格子点探索処理によるバイアス推定値を、バイアス推定におけるバイアスベクトル推定値の初期値として設定する。バイアスベクトルの推定誤差共分散行列の初期値は、前の時刻の値をそのまま引き継いで計算してもよいし、改めて、事前に決めたある値をバイアスベクトルの推定誤差共分散行列初期値に設定してもよい。また、格子点探索処理によるバイアス推定値を1度しか使用しないで、バイアスベクトル推定値の初期値を計算してもよい。
【0083】
このとき、バイアス推定フィルタ処理部60による状態推定は、前の時刻の値をそのまま引き継いで状態ベクトル推定値および推定誤差共分散行列を用いて算出してもよいし、現時刻を含む最新のLサンプルの相関観測値を用いて、現時刻で改めて、状態ベクトル推定値および推定誤差共分散行列を算出してもよい。
【0084】
最終的に、バイアス推定フィルタ処理部60は、バイアス推定結果として、カルマンフィルタまたは最小2乗法に基づいて算出したバイアスベクトル推定値を、バイアスDB70に出力する。また、バイアス推定フィルタ処理部60は、バイアス格子点探索処理部40から得られる、バイアス格子点探索処理によるバイアス推定値を、バイアスDB70に出力する。
さらに、バイアス推定フィルタ処理部60は、バイアスベクトルの推定値およびその推定誤差共分散行列と、状態ベクトルの推定値およびその推定誤差共分散行列とを、バイアス格子点探索処理部40に出力する。
【0085】
バイアスDB70は、カルマンフィルタまたは最小2乗法に基づいて、バイアス推定フィルタ処理部60で算出された各時刻のバイアスベクトル推定値と、バイアス格子点探索処理によるバイアス推定値とを、データベースに蓄積する。
また、バイアスDB70は、バイアスベクトル推定値の時刻による移動平均値と、バイアス推定値の時刻による移動平均値とを算出し、これらの移動平均値をデータベースに蓄積する。
【0086】
以上のように、実施の形態1によれば、バイアス推定フィルタ処理手段は、カルマンフィルタおよび最小2乗フィルタによるバイアス推定フィルタ処理を実行する場合に、バイアス格子点探索処理手段によるバイアス格子点探索処理によって算出されるバイアス推定値をバイアスベクトル推定値の初期値として設定する。これにより、バイアス推定フィルタ処理によるバイアスベクトル推定値の初期値を適切に設定することができる。そのため、バイアスベクトル推定値の精度を向上させることができるとともに、バイアスベクトル推定値の収束を早くすることができる。
【0087】
なお、バイアス格子点探索処理のみでもバイアス推定値を算出することは可能であるが、バイアス格子点が粗い場合、本発明は、バイアス格子点探索処理によるバイアス格子よりも細かい精度でバイアス推定値を算出することができる。また、バイアス格子点を細かくした場合、バイアス推定値を高精度に算出できる可能性はあるものの、バイアス格子点探索処理にかかる演算の処理負荷が高くなる。
【0088】
このとき、バイアス推定フィルタ処理手段から出力されるバイアスベクトル推定値およびその推定誤差共分散行列から得られる誤差楕円体、または状態ベクトル推定値およびその推定誤差共分散行列から得られる誤差楕円体をバイアス格子点の探索範囲として設定することにより、バイアス格子点探索処理の処理負荷を低減することができる。また、バイアス格子点探索処理の探索開始点を、バイアス推定フィルタ処理手段から出力されるバイアスベクトル推定値とすることにより、バイアス格子点探索処理において、バイアス格子点の端から探索することなく、ある程度バイアス真値に近い位置から、バイアス推定値を探索することができる。その結果、バイアス推定精度の早期収束、および高精度なバイアス推定値の算出を実現することができる。
【0089】
実施の形態2.
図8は、この発明の実施の形態2に係るセンサバイアス推定装置を示すブロック構成図である。
図8において、このセンサバイアス推定装置は、図1に示したセンサバイアス推定装置に加えて、バイアス格子間隔制御処理部80を備えている。
【0090】
バイアス格子間隔制御処理部80は、以下に示す手順で、バイアス格子点探索処理部40におけるバイアス格子点を設定する。また、バイアス格子点探索処理部40からバイアス格子間隔制御処理部80には、現時刻でのバイアス候補、時刻および格子点探索終了フラグが入力される。
ここで、格子点探索終了フラグとは、例えば格子点探索終了フラグが0の場合に、格子点探索中であることを示し、格子点探索終了フラグが1の場合に、格子点の探索が終了していることを示す等、格子点探索中かあるいは格子点探索終了かが分かるようなフラグである。
【0091】
最初に、バイアス格子間隔制御処理部80は、バイアス格子点を粗く探索するような格子間隔を設定する制御信号を、バイアス格子点探索処理部40に出力する。バイアス格子点探索処理部40は、上記式(3)を用いて、Lサンプル分の航跡の残差和εiR,iE,Azを算出し、Lサンプル分の航跡の残差和εiR,iE,Azが最小となるバイアス候補ΔziR,iE,Az(アンダーバー)を、バイアス推定値として出力する。
【0092】
次に、バイアス格子間隔制御処理部80は、このバイアス候補を中心として、格子間隔を細かく設定する制御信号を、バイアス格子点探索処理部40に出力する。バイアス格子点探索処理部40は、バイアス格子間隔を細かく設定して、再度上記式(3)を用いて、Lサンプル分の航跡の残差和εiR,iE,Azを算出する。
【0093】
続いて、バイアス格子間隔制御処理部80は、バイアス格子点を粗く探索するような格子間隔を設定する制御信号と、格子間隔を細かく設定する制御信号とを繰り返して出力する。また、バイアス格子間隔制御処理部80は、前回算出したLサンプル分の航跡の残差和と今回算出したLサンプル分の航跡の残差和との差が、事前に決めたしきい値以下である場合には、処理を終了する制御信号を、バイアス格子点探索処理部40に出力する。
【0094】
なお、バイアス格子間隔制御処理部80は、バイアス格子点探索処理部40から得られる探索開始時刻と現時刻との差が、事前に決めた探索終了判定時刻しきい値よりも大きい場合、かつ格子点探索終了フラグが0の場合に、探索処理が事前に決めた時間内に終了しないと判定する。このとき、バイアス格子間隔制御処理部80は、現時刻においてLサンプル分の航跡の残差和が最小となるようなバイアス候補を出力させる制御信号を、バイアス格子点探索処理部40に出力する。バイアス格子点探索処理部40は、現時刻(現時点)においてLサンプル分の航跡の残差和が最小となるようなバイアス候補を、バイアス推定フィルタ処理部60に出力する。
【0095】
以上のように、実施の形態2によれば、バイアス格子点探索処理が所定の時間内に終了しない場合であっても、現時刻で最良のバイアス推定値を算出することができる。
【0096】
実施の形態3.
図9は、この発明の実施の形態3に係るセンサバイアス推定装置を示すブロック構成図である。
図9において、このセンサバイアス推定装置は、図1に示したセンサバイアス推定装置に加えて、バイアス格子間隔再設定処理部90を備えている。
【0097】
バイアス格子点探索処理部40は、現時刻でのバイアス候補、時刻および格子点探索終了フラグを、バイアス格子間隔再設定処理部90に出力する。
バイアス格子間隔再設定処理部90は、バイアス格子点探索処理部40から得られる探索開始時刻と現時刻との差が、事前に決めた格子間隔再設定時刻しきい値よりも大きい場合、かつ格子点探索終了フラグが0の場合に、次時刻のバイアス格子間隔を現時刻のバイアス格子間隔よりも粗く設定する制御信号を、バイアス格子点探索処理部40に出力する。
【0098】
以上のように、実施の形態3によれば、現時刻において、バイアス格子点探索処理が所定の時間内に終了する見込みがない場合であっても、現時刻よりもバイアス格子間隔を粗く設定することにより、次時刻以降のバイアス格子点探索処理を、所定の時間内に終了させることができる。すなわち、所定の時間内で、最良のバイアス推定値を算出することができる。
なお、この発明の実施の形態3に係るセンサバイアス推定装置は、実施の形態2と組み合わせることもできる。
【0099】
実施の形態4.
図10は、この発明の実施の形態4に係るセンサバイアス推定装置を示すブロック構成図である。
図10において、このセンサバイアス推定装置は、図1に示したセンサバイアス推定装置に加えて、第1のバイアス格子点探索範囲再設定処理部100を備えている。
【0100】
バイアス格子点探索処理部40は、現時刻でのバイアス候補、時刻および格子点探索終了フラグを、第1のバイアス格子点探索範囲再設定処理部100に出力する。
第1のバイアス格子点探索範囲再設定処理部100は、バイアス格子点探索処理部40から得られる探索開始時刻と現時刻との差が、事前に決めた探索範囲再設定時刻しきい値よりも大きい場合、かつ格子点探索終了フラグが0の場合に、探索処理が事前に決めた時間内に終了しないと判定する。
【0101】
このとき、第1のバイアス格子点探索範囲再設定処理部100は、バイアス格子点の探索範囲を狭くする制御信号を、バイアス格子点探索処理部40に出力する。バイアス格子点探索処理部40は、現時刻よりも狭い固定のバイアス格子点探索範囲を設定する。
なお、第1のバイアス格子点探索範囲再設定処理部100は、図6で示したバイアスベクトル推定値の推定誤差共分散行列から得られる誤差楕円体、または状態ベクトル推定値の推定誤差共分散行列から得られる誤差楕円体に、半径を小さくする係数を掛けて、現時刻よりも狭い可変のバイアス格子点探索範囲を設定してもよい。
【0102】
以上のように、実施の形態4によれば、現時刻において、バイアス格子点探索処理が所定の時間内に終了する見込みがない場合であっても、次時刻以降のバイアス格子点の探索範囲を狭く設定することにより、次時刻以降のバイアス格子点探索処理を、所定の時間内に終了させることができる。すなわち、所定の時間内で、最良のバイアス推定値を算出することができる。
なお、この発明の実施の形態4に係るセンサバイアス推定装置は、実施の形態2または3と組み合わせることもできる。
【0103】
実施の形態5.
図11は、この発明の実施の形態5に係るセンサバイアス推定装置を示すブロック構成図である。
図11において、このセンサバイアス推定装置は、図1に示したセンサバイアス推定装置に加えて、第2のバイアス格子点探索範囲再設定処理部110を備えている。
【0104】
バイアス格子点探索処理部40は、現時刻でのバイアス候補、時刻および格子点探索終了フラグを、第2のバイアス格子点探索範囲再設定処理部110に出力する。
第2のバイアス格子点探索範囲再設定処理部110は、バイアス格子点探索処理部40から得られる探索開始時刻と現時刻との差が、事前に決めた探索範囲再設定時刻しきい値よりも大きい場合、かつ格子点探索終了フラグが1の場合に、探索処理が事前に決めた時間内に終了したと判定する。
【0105】
このとき、第2のバイアス格子点探索範囲再設定処理部110は、バイアス格子点の探索範囲を広くする制御信号を、バイアス格子点探索処理部40に出力する。バイアス格子点探索処理部40は、現時刻よりも広い固定のバイアス格子点探索範囲を設定する。
なお、第2のバイアス格子点探索範囲再設定処理部110は、図6で示したバイアスベクトル推定値の推定誤差共分散行列から得られる誤差楕円体、または状態ベクトル推定値の推定誤差共分散行列から得られる誤差楕円体に、半径を大きくする係数を掛けて、現時刻よりも広い可変のバイアス格子点探索範囲を設定してもよい。
【0106】
以上のように、実施の形態5によれば、現時刻において、バイアス格子点探索処理が所定の時間内に終了する見込みがある場合であって、かつ現時刻のバイアス格子点探索範囲内に、実際のバイアス真値がない場合に、次時刻以降のバイアス格子点探索範囲を広く設定することにより、バイアス格子点探索範囲外に存在する実際のバイアス真値を発見することができる。すなわち、バイアス推定精度を向上させることができる。
なお、この発明の実施の形態5に係るセンサバイアス推定装置は、実施の形態2または3と組み合わせることもできる。
【0107】
実施の形態6.
図12は、この発明の実施の形態6に係るセンサバイアス推定装置を示すブロック構成図である。
図12において、このセンサバイアス推定装置は、図1に示したセンサバイアス推定装置に加えて、バイアス成分推定順序設定処理部120を備えている。
【0108】
バイアス成分推定順序設定処理部120は、バイアス格子点探索処理部40およびバイアス推定フィルタ処理部60に対して、ある成分についてバイアス推定値を算出した後に、残りの成分についてバイアス推定値を算出するように制御信号を出力する。
以下、バイアス成分推定順序設定処理部120が、バイアス格子点探索処理部40およびバイアス推定フィルタ処理部60に対して、仰角、方位角の各成分についてバイアス推定値を算出した後に、距離成分についてバイアス推定値を算出するように制御信号を出力した場合を例に挙げて説明する。
【0109】
このとき、バイアス格子点探索処理部40は、バイアス格子点探索処理を、仰角、方位角からなる範囲と設定して、仰角、方位角バイアス推定値を算出し、算出結果をバイアス推定フィルタ処理部60に出力する。
【0110】
続いて、バイアス推定フィルタ処理部60は、バイアス格子点探索処理部40から得られる、仰角、方位角バイアス推定値を、バイアス推定におけるバイアスベクトル推定値の初期値として設定する。また、バイアス推定フィルタ処理部60は、この初期値に基づいて、上記式(10)で示した仰角、方位角の各成分からなるバイアスベクトル推定値およびその推定誤差共分散行列と、状態ベクトル推定値およびその推定誤差共分散行列を算出する。
【0111】
次に、バイアス格子点探索処理部40は、仰角、方位角のバイアス格子点探索処理とは別に、距離のバイアス格子点探索処理を実行する。
距離のバイアス格子点探索処理において、バイアス格子点探索処理部40は、バイアス推定フィルタ処理部60から得られる仰角、方位角の各成分からなるバイアスベクトル推定値を、バイアス格子点探索処理におけるバイアス候補ΔziR,iE,Az(アンダーバー)の仰角成分、方位角成分と設定する。
【0112】
そして、バイアス格子点探索処理部40は、距離成分のみについて図7に示したバイアス格子点探索処理を実行して、距離バイアス推定値を算出し、算出結果をバイアス推定フィルタ処理部60に出力する。
また、バイアス格子点探索処理部40は、算出した距離バイアス推定値をバイアスDB70に出力する。
【0113】
特に遠距離の場合、角度誤差に起因するクロスレンジ誤差は、距離誤差、すなわちレンジ誤差に比べて悪くなる。その場合、距離バイアスに比べて、角度バイアスの影響が遠距離において支配的になり、バイアス推定フィルタ処理によって、距離バイアスを推定できない場合が考えられる。
そこで、この実施の形態6によれば、例えば仰角、方位角と距離とに分けてバイアス推定値を算出することにより、バイアス推定精度を向上させることができる。
【0114】
なお、上記実施の形態6では、バイアス成分の推定順序として、まず、仰角、方位角の各成分についてバイアス推定値を算出した後に、距離成分についてバイアス推定値を算出する場合を示した。この例は、仰角、方位角と距離とに分けてバイアス推定を実行する例である。
しかしながら、これに限定されず、距離、仰角と、方位角とに分けてバイアス推定を実行してもよいし、距離、方位角と仰角とに分けてバイアス推定を実行してもよいし、距離と仰角と方位角とにそれぞれ分けてバイアス推定を実行してもよい。
また、この発明の実施の形態6に係るセンサバイアス推定装置は、実施の形態2〜5と組み合わせることもできる。
【0115】
実施の形態7.
図13は、この発明の実施の形態7に係るセンサバイアス推定装置を示すブロック構成図である。
図13において、このセンサバイアス推定装置は、図1に示したセンサバイアス推定装置に加えて、バイアス成分次数設定処理部130を備えている。
【0116】
この実施の形態7において、第1センサ10の処理は、上述した実施の形態1と同様とする。すなわち、第1センサ10は、センサの受信器から距離、仰角、方位角の観測値を得て、最終的に、第1データ更新処理部13から、上記式(4)または(5)に示した成分を有する平滑値の状態ベクトルおよびその誤差共分散行列を出力する。なお、この実施の形態7では、上記式(4)のような速度項までの状態ベクトルについて説明する。
【0117】
また、この実施の形態7において、第2センサ20は、センサの受信器から仰角、方位角の観測値を得て、最終的に、第2データ更新処理部23から、次式(13)または(14)に示す成分を有する平滑値の状態ベクトルおよびその誤差共分散行列を出力する。
【0118】
【数13】
【0119】
【数14】
【0120】
式(13)、(14)において、E(k)は時刻kの仰角を示し、Az(k)は時刻kの方位角を示し、E(k)(ドット)は時刻kの仰角速度を示し、Az(k)(ドット)は時刻kの方位角速度を示し、E(k)(ツードット)は時刻kの仰角加速度を示し、Az(k)(ツードット)は時刻kの方位角加速度をそれぞれ示す。なお、この実施の形態7では、上記式(13)のような速度項までの状態ベクトルについて説明する。
【0121】
バイアス成分次数設定処理部130は、同期処理部30、バイアス格子点探索処理部40、統合相関処理部50およびバイアス推定フィルタ処理部60に対して、推定すべきバイアス成分の次数を、観測される成分の次数に合わせるように制御信号を出力する。
【0122】
このとき、同期処理部30は、第1センサ10から得られる式(4)に示した成分を有する平滑値の状態ベクトル、すなわちx,y,zの直交座標に関する6次元の航跡と、第2センサ20から得られる式(13)に示した成分を有する平滑値の状態ベクトル、すなわち仰角、方位角に関する4次元の航跡とに基づいて、時刻同期および航跡相関を実行する。
【0123】
ここで、同期処理部30による航跡相関は、航跡の位置情報を用いて実行される。このとき、第1センサ10からのx,y,zの直交座標に関する6次元の航跡と、第2センサ20からの仰角、方位角に関する4次元の航跡とは、次元が互いに異なるので、次元が少ない方の基準座標に合わせる。
【0124】
すなわち、同期処理部30は、第1センサ10からのx,y,zの直交座標に関する航跡を、第2センサ20を中心とした第2センサ基準座標に変換し、さらに、第2センサ基準座標において、直交座標から極座標への座標変換を行う。その後、同期処理部30は、第2センサ基準座標において、仰角、方位角の各成分の情報を用いて、第1センサ10から得られる航跡と第2センサ20から得られる航跡との航跡相関を実行する。
【0125】
続いて、バイアス格子点探索処理部40は、同期処理部30による航跡相関によって得られた第1センサ10と第2センサ20との仰角、方位角を各成分とする航跡のペアを用いて、これらの仰角、方位角の各成分からなるバイアス格子点探索範囲を設定する。また、バイアス格子点探索処理部40は、バイアス格子点探索処理により、仰角、方位角バイアス推定値を算出し、算出結果をバイアス推定フィルタ処理部60に出力する。
【0126】
次に、バイアス推定フィルタ処理部60は、バイアス格子点探索処理部40から得られる、仰角、方位角バイアス推定値を、バイアス推定におけるバイアスベクトル推定値の初期値として設定する。また、バイアス推定フィルタ処理部60は、この初期値に基づいて、仰角、方位角の各成分からなるバイアスベクトル推定値およびその推定誤差共分散行列を算出する。バイアス推定フィルタ処理部60は、この初期値に基づいて、状態推定処理により、第1センサ10または第2センサ20を基準とする直交座標における平滑値の状態ベクトルおよびその誤差共分散行列を算出する。
【0127】
なお、統合相関処理部50から得られる相関観測値は、第1センサ10からの相関観測値については、x,y,zの3次元の直交座標における観測位置であるが、第2センサ20からの相関観測値については、仰角、方位角の極座標における観測位置である。
【0128】
したがって、バイアス推定フィルタ処理部60が、第2センサ20からの相関観測値を用いて状態推定処理を実行する場合、状態ベクトルがx,y,zの3次元の直交座標であるのに対して、第2センサ20の観測値が仰角、方位角であり、観測ベクトルの成分が仰角、方位角の2次元なので、状態ベクトルと観測ベクトルとの関係を表す観測モデルが非線形となる。そこで、バイアス推定フィルタ処理部60が、第2センサ20からの相関観測値を用いて状態推定処理を実行する場合には、拡張カルマンフィルタが用いられる。
【0129】
以上のように、実施の形態7によれば、センサが得る観測ベクトルの次数が異なっている場合であっても、バイアス推定精度を向上させることができる。
【0130】
なお、上記実施の形態7では、バイアス成分次数設定処理部130が、第2センサ20で得られた仰角、方位角からなる2次元の観測ベクトルに対応した、2次元のバイアスベクトル推定値を算出する場合について説明した。しかしながら、これに限定されず、バイアス成分次数設定処理部130は、第2センサ20で得られた2次元の観測ベクトルよりも次数の低い1次元のバイアスベクトル推定値、例えば仰角成分のみあるいは方位角成分のみを算出するように制御信号を出力してもよい。また、何次元のバイアスベクトル推定値を算出するかについては、事前に設定される。
【0131】
また、上記実施の形態7では、第2センサ20が、仰角、方位角を観測する場合について説明した。しかしながら、これに限定されず、第2センサ20が、距離のみを観測する場合、仰角のみを観測する場合、方位角のみを観測する場合、距離および仰角を観測する場合、距離および方位角を観測する場合にも、それぞれ適用することができる。
また、この発明の実施の形態7に係るセンサバイアス推定装置は、実施の形態2〜6と組み合わせることもできる。
【符号の説明】
【0132】
10 第1センサ、20 第2センサ、30 同期処理部(同期処理手段)、40 バイアス格子点探索処理部(バイアス格子点探索処理手段)、50 統合相関処理部(統合相関処理手段)、60 バイアス推定フィルタ処理部(バイアス推定フィルタ処理手段)、80 バイアス格子間隔制御処理部(バイアス格子間隔制御処理手段)、90 バイアス格子間隔再設定処理部(バイアス格子間隔再設定処理手段)、100 第1のバイアス格子点探索範囲再設定処理部(第1のバイアス格子点探索範囲再設定処理手段)、110 第2のバイアス格子点探索範囲再設定処理部(第2のバイアス格子点探索範囲再設定処理手段)、120 バイアス成分推定順序設定処理部(バイアス成分推定順序設定処理手段)、130 バイアス成分次数設定処理部(バイアス成分次数設定処理手段)。
【技術分野】
【0001】
この発明は、例えば複数のセンサを有する追尾装置に適用され、それぞれのセンサのバイアス(バイアス誤差)を推定するセンサバイアス推定装置に関する。
【背景技術】
【0002】
従来のセンサのバイアス誤差推定装置は、複数の目標に対して、それぞれ第1および第2のセンサからのデータ入力を観測する第1および第2の観測器の時間毎の出力に基づいて、観測行列を生成する観測行列生成器と、観測行列出力と観測雑音の共分散とからバイアス誤差推定値の共分散行列を算出する推定値評価器と、観測行列と推定値評価器出力とから仮のバイアス誤差値を算出する推定値算出器と、観測行列生成器と推定値評価器との間に設けられ、観測行列とその転置行列との積の最小固有値が所定の定数よりも大きい場合にのみ、観測行列を出力する階数判定器とを備え、累積演算結果からカルマンフィルタによりセンサのバイアス誤差を推定している(例えば、特許文献1参照)。
【0003】
次に、図14を参照しながら、バイアス(バイアス誤差)の補正を実行する目的について説明する。なお、本明細書において、バイアスとバイアス誤差とは同義とする。
図14は、2つのセンサのバイアスをそれぞれ補正する様子を、PPI(Plan Position Indicator)表示で例示する説明図である。
図14において、2つのセンサを構成するセンサAおよびセンサBは、それぞれ方位角方向にバイアスを有しており、センサAおよびセンサBともに、目標1、2、3を同時に観測している(図中(a)の状態)。
【0004】
このとき、センサAおよびBは、それぞれ方位角方向にバイアスを有しているので、センサAおよびBで同一の目標を観測していたとしても、センサAおよびBの観測結果を重ね合わせた場合、目標同士が重なり合わず、だぶって見えることとなる(図中(b)の状態)。そこで、このような状況を改善するために、センサAおよびBのバイアスを推定して、バイアスを補正することにより、センサAおよびBの観測結果を重ね合わせた場合に、目標同士を1つに重ね合わせることができる(図中(c)の状態)。
すなわち、センサAおよびBで観測した目標の位置、速度、加速度等からなる目標の状態量を一致させることが、バイアスの補正を実行する目的である。
【0005】
続いて、特許文献1に示したようなカルマンフィルタにより、距離、仰角、方位角の各成分からなるバイアスベクトルと、目標の位置、速度の各成分からなる状態ベクトルとを推定することを考える。
【0006】
ここで、バイアスベクトルは、距離、方位角の各成分で構成されてもよいし、仰角、方位角の各成分で構成されてもよいし、距離、仰角の各成分で構成されてもよいし、距離成分のみで構成されてもよいし、仰角成分のみで構成されてもよいし、方位角成分のみで構成されてもよい。すなわち、バイアスベクトルの次数およびその成分は、事前に任意に設定してカルマンフィルタを構成することができる。
また、バイアスベクトルは、X,Y,Zの各成分で構成されてもよいし、バイアス推定を実行する構成は、上記距離、仰角、方位角の各成分の組み合わせと同様に、様々な組み合わせによって実現することができる。
【0007】
同様に、状態ベクトルは、それぞれ3次元のX,Y,Z成分からなる位置、速度の6次元の状態ベクトルとして構成されてもよいし、それぞれ3次元のX,Y,Z成分からなる位置、速度、加速度の9次元の状態ベクトルとして構成されてもよいし、それぞれ2次元のX,Y成分からなる位置、速度の4次元の状態ベクトルとして構成されてもよいし、それぞれ2次元のX,Y成分からなる位置、速度、加速度の6次元の状態ベクトルとして構成されてもよい。すなわち、状態ベクトルの次数およびその成分は、事前に任意に設定してカルマンフィルタを構成することができる。
【先行技術文献】
【特許文献】
【0008】
【特許文献1】特許第3302870号公報
【発明の概要】
【発明が解決しようとする課題】
【0009】
しかしながら、従来技術には、以下のような課題がある。
カルマンフィルタおよび最小2乗フィルタによりバイアス推定を実行する場合には、バイアスベクトル推定値の初期値が必要になる。ここで、従来のセンサのバイアス誤差推定装置では、バイアスベクトル推定値の初期値の設定方法が明らかにされていない。
そのため、バイアスベクトル推定値の初期値が、バイアスベクトルの真値から大きく外れていた場合には、バイアスベクトル推定値の精度が低下したり、バイアスベクトル推定値の収束が遅くなったりするという問題がある。
【0010】
この発明は、上記のような課題を解決するためになされたものであり、バイアスベクトル推定値を高精度に算出することができるセンサバイアス推定装置を得ることを目的とする。
【課題を解決するための手段】
【0011】
この発明に係るセンサバイアス推定装置は、複数のセンサを有する装置であり、それぞれのセンサが、観測値と予測値とに基づいて相関処理を実行して相関観測値を算出するとともに、相関観測値に基づいてセンサ航跡を算出し、相関観測値およびセンサ航跡を出力する装置に適用され、複数のセンサのバイアスを推定するセンサバイアス推定装置であって、センサ航跡に基づいて、時刻同期および航跡相関を実行し、同一航跡と判定されたセンサ航跡の組を出力する同期処理手段と、同一航跡と判定されたセンサ航跡の組に対してバイアス格子点探索処理を実行し、バイアス推定値を算出するバイアス格子点探索処理手段と、相関観測値に基づいて、バイアス推定フィルタ処理を実行し、バイアスベクトル推定値を算出するバイアス推定フィルタ処理手段と、を備え、バイアス推定フィルタ処理手段は、バイアス推定値をバイアスベクトル推定値の初期値として設定するものである。
【発明の効果】
【0012】
この発明に係るセンサバイアス推定装置によれば、センサからの相関観測値に基づいてバイアス推定フィルタ処理を実行し、バイアスベクトル推定値を算出するバイアス推定フィルタ処理手段は、バイアス格子点探索処理手段によるバイアス格子点探索処理によって算出されるバイアス推定値をバイアスベクトル推定値の初期値として設定する。そのため、バイアスベクトル推定値の初期値を適切に設定することができ、バイアスベクトル推定値を高精度に算出することができるセンサバイアス推定装置を得ることができる。
【図面の簡単な説明】
【0013】
【図1】この発明の実施の形態1に係るセンサバイアス推定装置を示すブロック構成図である。
【図2】この発明の実施の形態1に係る同期処理部による同期処理を説明するためのタイミングチャートである。
【図3】この発明の実施の形態1に係る同期処理部による航跡相関を示す説明図である。
【図4】この発明の実施の形態1に係るバイアス格子点探索処理部による、距離、仰角、方位角バイアスを持つ場合のバイアス格子点探索処理を示す説明図である。
【図5】この発明の実施の形態1に係るバイアス格子点探索処理部による、X,Y,Zバイアスを持つ場合のバイアス格子点探索処理を示す説明図である。
【図6】この発明の実施の形態1に係るバイアス推定フィルタ処理部からのバイアスベクトル推定値およびその推定誤差共分散行列、または状態ベクトル推定値およびその推定誤差共分散行列に基づいて、バイアス格子点の探索範囲を絞り込む処理を示す説明図である。
【図7】この発明の実施の形態1に係るバイアス格子点探索処理部によるバイアス格子点探索処理の手順を示すフローチャートである。
【図8】この発明の実施の形態2に係るセンサバイアス推定装置を示すブロック構成図である。
【図9】この発明の実施の形態3に係るセンサバイアス推定装置を示すブロック構成図である。
【図10】この発明の実施の形態4に係るセンサバイアス推定装置を示すブロック構成図である。
【図11】この発明の実施の形態5に係るセンサバイアス推定装置を示すブロック構成図である。
【図12】この発明の実施の形態6に係るセンサバイアス推定装置を示すブロック構成図である。
【図13】この発明の実施の形態7に係るセンサバイアス推定装置を示すブロック構成図である。
【図14】2つのセンサのバイアスをそれぞれ補正する様子を、PPI表示で例示する説明図である。
【発明を実施するための形態】
【0014】
以下、この発明のセンサバイアス推定装置の好適な実施の形態につき図面を用いて説明するが、各図において同一、または相当する部分については、同一符号を付して説明する。
【0015】
実施の形態1.
図1は、この発明の実施の形態1に係るセンサバイアス推定装置を示すブロック構成図である。
図1において、このセンサバイアス推定装置は、第1センサ10、第2センサ20、同期処理部30、バイアス格子点探索処理部40、統合相関処理部50、バイアス推定フィルタ処理部60およびバイアスデータベース(バイアスDB)70とを備えている。
【0016】
第1センサ10は、第1信号処理部11、第1相関処理部12、第1データ更新処理部13、第1予測処理部14および第1遅延処理部15を有している。また、第2センサ20は、第2信号処理部21、第2相関処理部22、第2データ更新処理部23、第2予測処理部24および第2遅延処理部25を有している。なお、第1センサ10における第1信号処理部11から第1遅延処理部15までの処理と、第2センサ20における第2信号処理部21から第2遅延処理部25までの処理とは、互いに同一の処理である。
【0017】
第1センサ10および第2センサ20は、絶対座標に対してバイアスを有している。また、第1センサ10からバイアスDB70までの処理により、第1センサ10および第2センサ20の両方のバイアスを推定することもできるし、第1センサ10に対する第2センサ20のバイアスを推定することもできる。
【0018】
ここで、絶対座標に対する第1センサ10および第2センサ20のバイアスを絶対バイアスと呼び、絶対座標に対する第1センサ10の絶対バイアスを0とした場合の第2センサ20の絶対バイアスを相対バイアスと呼ぶ。また、絶対座標を第1センサ10とすれば、第1センサ10の絶対バイアスが0でない場合でも、第1センサ10に対する第2センサ20のバイアスを相対バイアスと呼ぶことができる。
【0019】
また、第1センサ10および第2センサ20は、距離、仰角、方位角を観測するレーダのような3次元のセンサであってもよいし、仰角、方位角を観測するIR(Infra Red)センサ、またはESM(Electric Support Measures)センサのような2次元のセンサであってもよい。また、第1センサ10および第2センサ20は、距離、方位角を観測する2次元のセンサであってもよいし、距離のみを観測する1次元のセンサであってもよいし、方位角のみを観測する1次元のセンサであってもよいし、仰角のみを観測する1次元のセンサであってもよい。
【0020】
以下、このセンサバイアス推定装置の各部位の機能について、第1センサ10に対する第2センサ20の相対バイアスを推定する場合を例に挙げて説明する。このとき、第1センサ10および第2センサ20は、代表的な例として、距離、仰角、方位角を観測する3次元センサとする。
【0021】
まず、第1センサ10の機能について説明する。
第1信号処理部11は、センサの受信器(図示せず)から時刻kにおける距離、仰角、方位角の観測値を得て、それらの観測値を第1相関処理部12に出力する。
【0022】
第1相関処理部12は、第1信号処理部11から入力された時刻kにおける距離、仰角、方位角の観測値と、第1遅延処理部15から入力された時刻kにおける距離、仰角、方位角の予測値とに基づいて、相関処理を実行する。相関処理とは、観測値の誤差や予測値の誤差を勘案したゲート内において、複数の観測値と目標の予測値との対応づけを予測値と観測値との距離でとり、予測値に一番近い観測値1点または予測値に近い複数点を目標信号の候補として抽出したり、予測値からの距離に応じて重み付け平均した観測値を目標信号の候補として抽出したりする処理である。
【0023】
また、第1相関処理部12は、相関処理を実行して得られた時刻kにおける相関観測値を、第1データ更新処理部13および統合相関処理部50に出力する。ここで、相関観測値は、ゲート内の観測値である。
【0024】
第1データ更新処理部13は、第1相関処理部12から入力された時刻kにおける相関観測値のうち、例えば予測値に一番近い相関観測値を用いて、カルマンフィルタのデータ更新処理に基づいて、時刻kにおける平滑値を算出する。平滑値は、上述した段落で示した平滑値の状態ベクトルおよびその誤差共分散行列であり、状態ベクトルの次数は、事前に設定した3次元、6次元または9次元とする。以後、平滑値を航跡と呼ぶことがある。なお、誤差共分散行列は、その行および列が状態ベクトルの次数と等しいものとする。
また、第1データ更新処理部13は、時刻kにおける平滑値を、第1予測処理部14および同期処理部30に出力する。
【0025】
第1予測処理部14は、第1データ更新処理部13から入力された時刻kにおける平滑値の状態ベクトルおよびその誤差共分散行列を、事前に仮定した運動モデルに基づいて、1サンプリング分外挿し、時刻k+1における予測値を算出する。予測値は、上述した段落で示した予測値の状態ベクトルおよびその誤差共分散行列であり、状態ベクトルの次数は、事前に設定した3次元、6次元または9次元とする。
また、第1予測処理部14は、時刻k+1における予測値を、第1遅延処理部15に出力する。
【0026】
第1遅延処理部15は、第1予測処理部14から入力された時刻k+1における予測値を1サンプリング分遅延させて、時刻kにおける予測値を算出し、算出された予測値を第1相関処理部12に出力する。
【0027】
なお、第2センサ20における第2信号処理部21、第2相関処理部22、第2データ更新処理部23、第2予測処理部24および第2遅延処理部25の機能は、上述したように、第1センサ10における第1信号処理部11、第1相関処理部12、第1データ更新処理部13、第1予測処理部14および第1遅延処理部15の機能と同様なので、説明を省略する。
【0028】
続いて、同期処理部30は、第1センサ10から入力された平滑値と、第2センサ20から入力された平滑値とに基づいて、時刻同期および航跡相関を実行する。すなわち、同期処理部30は、第1センサ10から得られた航跡と、第2センサ20から得られた航跡とに基づいて、時刻同期および航跡相関を実行する。
【0029】
以下、図2を参照しながら、同期処理部30による時刻同期について説明する。ここで、第1センサ10から得られた航跡を第1センサ航跡と呼び、第2センサ20から得られた航跡を第2センサ航跡と呼ぶ。
図2は、この発明の実施の形態1に係る同期処理部30による同期処理を説明するためのタイミングチャートである。
【0030】
図2において、横軸は、第1センサ航跡の同期処理への入力タイミングのタイムライン(A)、第2センサ航跡の同期処理への入力タイミングのタイムライン(B)、および同期処理での処理タイミングのタイムライン(C)を示している。また、タイムライン(A)において直交する太棒線は、各時刻における第1センサ航跡を表し、タイムライン(B)において直交する太棒線は、各時刻における第2センサ航跡を表している。
【0031】
図2より、第1センサ航跡と第2センサ航跡とは、同期処理への入力タイミングが互いに異なることが分かる。そこで、同時刻の第1センサ航跡と第2センサ航跡とを比較するために、ある基準時刻に時刻合わせを行う必要がある。なお、基準時刻は、タイムライン(C)によって決まるものとする。
【0032】
このとき、タイムライン(B)で航跡が生成される時刻をt(i)とし、この時刻t(i)を基準時刻とすると(t(k)≦t(i)≦t(k+1))、第1センサ航跡が時刻t(i)で得られていないので、同期処理部30は、時刻t(i)以前に第1センサ航跡が得られた時刻t(k)における第1センサ航跡を時刻t(i)まで保持する。
【0033】
すなわち、時刻t(k)における第1センサ航跡を、時刻t(k)から時刻t(i)までのサンプリング間隔Δt(=t(i)−t(k))で保持して、時刻t(i)における第1センサ航跡を算出する。
つまり、同期処理部30は、タイムライン(C)における(10)の箇所で時刻同期をとることになる。
【0034】
次に、図3を参照しながら、同期処理部30による航跡相関について説明する。
図3は、この発明の実施の形態1に係る同期処理部30による航跡相関を示す説明図である。
図3において、第1センサ10からは、第1センサ航跡101および第1センサ航跡102が得られているものとし、第2センサ20からは、第2センサ航跡201、第2センサ航跡202および第2センサ航跡203が得られているものとする。また、第1センサ航跡101を中心とした第1センサ航跡101のゲート、および第1センサ航跡102を中心とした第1センサ航跡102のゲートを点線で囲まれた範囲で示す。
【0035】
ここで、第1センサ航跡のゲート内に第2センサ航跡が入っている場合には、同期処理部30は、第1センサ航跡と第2センサ航跡とが同一航跡であると判定する。図3の場合では、第1センサ航跡101と第2センサ航跡201とが同一航跡であると判定され、第1センサ航跡102と第2センサ航跡203とが同一航跡であると判定される。
【0036】
このような、第1センサ航跡と第2センサ航跡との同一性を判定する処理を航跡相関と呼ぶ。
なお、航跡相関を実行する場合には、第1センサ10と第2センサ20との基準座標が互いに異なるので、第1センサ10または第2センサ20の何れかの基準座標に座標変換してから航跡相関を実行する。また、ここでは、第1センサ航跡を中心としたゲートを設定したが、これに限定されず、第2センサ航跡を中心としたゲートを設定し、第2センサ航跡を中心としたゲート内に第1センサ航跡が入るか否かで同一航跡を判定してもよい。
【0037】
同期処理部30は、まず、第1センサ10から得られた第1センサ航跡と、第2センサ20から得られた第2センサ航跡とに基づいて時刻同期を実行し、続いて、航跡相関を実行する。このとき、時刻同期は、各センサの速度を用いて、時刻同期を実行する時刻まで航跡位置を保持する。
【0038】
そして、同期処理部30は、同一航跡と判定された第1センサ航跡と第2センサ航跡とのペアを、バイアス格子点探索処理部40に出力する。また、同期処理部30は、第1センサ航跡と第2センサ航跡とのペア情報を、統合相関処理部50に出力する。なお、ペア情報は、同一航跡と判定された第1センサ航跡と第2センサ航跡とのペアだけでなく、同一航跡と判定されなかった第1センサ航跡および第2センサ航跡も含むものとする。
【0039】
次に、バイアス格子点探索処理部40は、バイアス格子点探索処理により、第1センサ10に対する第2センサ20の相対バイアスを推定する。
以下、図4〜7を参照しながら、バイアス格子点探索処理部40によるバイアス格子点探索処理について説明する。
【0040】
図4は、この発明の実施の形態1に係るバイアス格子点探索処理部40による、距離、仰角、方位角バイアスを持つ場合のバイアス格子点探索処理を示す説明図である。また、図5は、この発明の実施の形態1に係るバイアス格子点探索処理部40による、X,Y,Zバイアスを持つ場合のバイアス格子点探索処理を示す説明図である。なお、図4と図5とは、成分が異なるだけで処理内容は同様なので、図4を用いてバイアス格子点探索処理を説明する。
【0041】
図4において、zkY(アンダーバー)は、時刻kにおける第2センサ20の航跡を示し、zk−1Y(アンダーバー)は、時刻k−1における第2センサ20の航跡を示し、zk−2Y(アンダーバー)は、時刻k−2における第2センサ20の航跡を示す。また、zkX(アンダーバー)は、時刻kにおける第1センサ10の航跡を示し、zk−1X(アンダーバー)は、時刻k−1における第1センサ10の航跡を示し、zk−2X(アンダーバー)は、時刻k−2における第1センサ10の航跡を示す。
【0042】
また、ΔziR,iE,Az(アンダーバー)は、第1センサ10に対する第2センサ20のバイアス候補を示す。なお、このバイアス候補は複数存在し、複数のバイアス候補の中から、後述する図7の処理により1つを選択する。
ここで、バイアス候補のイメージは、図4の右側に示すとおりであり、バイアス候補の成分は、距離成分ΔRiR、仰角成分ΔEiE、方位角成分ΔAziAzからなり、iR、iE、iAzは、バイアス格子点の位置を示すインデックスである。また、このバイアス候補は、距離、仰角、方位角の各軸からなる格子内で値が変化する。
【0043】
なお、バイアス格子点の探索範囲は、後述するバイアス推定フィルタ処理部60からのバイアスベクトル推定値およびその推定誤差共分散行列、または状態ベクトル推定値およびその推定誤差共分散行列に基づいて絞り込まれる。
【0044】
図6は、この発明の実施の形態1に係るバイアス推定フィルタ処理部60からのバイアスベクトル推定値およびその推定誤差共分散行列、または状態ベクトル推定値およびその推定誤差共分散行列に基づいて、バイアス格子点の探索範囲を絞り込む処理を示す説明図である。なお、図6では、2次元平面のイメージを示しているが、バイアス格子点は、推定すべきバイアスの数による。例えば、距離バイアス、仰角バイアス、方位角バイアスを推定する場合には、バイアス格子点は3次元空間となる。
【0045】
このとき、図6のように、バイアス格子点の探索範囲を、バイアスベクトル推定値の推定誤差共分散行列から得られる誤差楕円体、または状態ベクトル推定値の推定誤差共分散行列から得られる誤差楕円体で設定することにより、バイアス格子点の探索範囲を絞り込むことができる。また、バイアス推定フィルタ処理部60からのバイアス推定値を格子点探索の始点に設定することより、格子点の端から探索する処理が不要となる。
【0046】
続いて、図7のフローチャートを参照しながら、バイアス格子点探索処理部40によるバイアス格子点探索処理の手順について説明する。
図7は、この発明の実施の形態1に係るバイアス格子点探索処理部40によるバイアス格子点探索処理の手順を示すフローチャートである。なお、図8のフローチャートは、上述したバイアス格子点の探索範囲を絞り込む処理以外の処理を示している。
【0047】
まず、バイアス格子点探索処理部40は、バイアス候補ΔziR,iE,Az(アンダーバー)を付加する前、すなわちバイアス候補ΔziR,iE,Az(アンダーバー)が未設定の第1センサ航跡zkX(アンダーバー)を設定する(ステップS1)。この第1センサ航跡をバイアス候補未設定第1センサ航跡と呼ぶ。
【0048】
続いて、バイアス格子点探索処理部40は、第1センサ航跡zkX(アンダーバー)と同一航跡であると判定された第2センサ航跡zkY(アンダーバー)を設定する(ステップS2)。
【0049】
次に、バイアス格子点探索処理部40は、バイアス格子点の位置を表すインデックス(iR,iE,iAz)に対応する格子内の1点(図4の右側参照)を、バイアス候補ΔziR,iE,Az(アンダーバー)として設定する(ステップS3)。
【0050】
続いて、バイアス格子点探索処理部40は、ステップS3で設定したバイアス候補ΔziR,iE,Az(アンダーバー)を、バイアス候補未設定第1センサ航跡zkX(アンダーバー)に付加して、バイアス候補設定第1センサ航跡zkX(アンダーバー)を設定する(ステップS4)。
【0051】
次に、バイアス格子点探索処理部40は、時刻k−L+1から時刻kまでの間のある一時刻(ここでは、時刻kとする)におけるバイアス候補設定第1センサ航跡zkX(アンダーバー)と第2センサ航跡zkY(アンダーバー)との残差εkiR,iE,Azを、次式(1)を用いて算出する(ステップS5)。
【0052】
【数1】
【0053】
なお、残差εkiR,iE,Azは、式(1)ではなく、次式(2)を用いて算出されてもよい。
【0054】
【数2】
【0055】
式(1)、(2)において、‖‖は、ノルムを示している。このノルムは、ユークリッドノルムであってもよいし、ベクトルの成分の最大値を取るノルムであってもよい。
以降の説明については、残差εkiR,iE,Azとして式(2)を用いて説明する。
【0056】
続いて、バイアス格子点探索処理部40は、時刻k−L+1から時刻kまでのLサンプル分について、残差を算出したか否かを判定する(ステップS6)。
ステップS6において、Lサンプル分について残差を算出していない(すなわち、No)と判定された場合には、バイアス格子点探索処理部40は、カウンタiを1つ進めて(ステップS7)、ステップS5に移行し、次の時刻における航跡の残差を算出する。
【0057】
一方、ステップS6において、Lサンプル分について残差を算出した(すなわち、Yes)と判定された場合には、バイアス格子点探索処理部40は、Lサンプル分の航跡の残差和εiR,iE,Azを、次式(3)を用いて算出する(ステップS8)。
【0058】
【数3】
【0059】
次に、バイアス格子点探索処理部40は、他のバイアス候補が存在しないか否かを判定する(ステップS9)。ここで、バイアス格子点探索処理部40は、全てのバイアス格子点が既に使用されている場合には、他のバイアス候補は存在しないと判定し、未使用のバイアス格子点が残っている場合には、他のバイアス候補が存在すると判定する。
【0060】
ステップS9において、他のバイアス候補が存在する(すなわち、No)と判定された場合には、バイアス格子点探索処理部40は、カウンタnを1つ進めて(ステップS10)、ステップS3に移行し、別のバイアス候補について残差和εiR,iE,Azを算出する。
【0061】
一方、ステップS9において、他のバイアス候補が存在しない(すなわち、Yes)と判定された場合には、バイアス格子点探索処理部40は、全てのバイアス候補について算出した残差和に基づいて、残差和が最小となるときのバイアス候補を、バイアス推定値として決定し(ステップS11)、図7の処理を終了する。
【0062】
なお、上記ステップS9〜11の処理を言い換えると、バイアス格子点探索処理部40は、図4の右側に示すように、バイアス候補を格子内で移動させることにより、バイアス候補毎に、式(3)を用いて、Lサンプル分の航跡の残差和を算出する。そして、バイアス格子点探索処理部40は、Lサンプル分の航跡の残差和が最小となるときのバイアス候補を選択し、バイアス推定値とする。
【0063】
以上のように、バイアス格子点探索処理部40は、バイアス格子点探索処理により、バイアス推定値を決定する。また、バイアス格子点探索処理部40は、決定したバイアス推定値を、バイアス推定フィルタ処理部60に出力する。
【0064】
また、統合相関処理部50は、同期処理部30から得られたペア情報に基づいて、同一航跡と判定された第1センサ航跡と第2センサ航跡とのペアにおける第1センサ航跡に対応する相関観測値について、バイアス推定フィルタ処理部60で使用可能か否かを判定する。同様に、統合相関処理部50は、同期処理部30から得られたペア情報に基づいて、同一航跡と判定された第1センサ航跡と第2センサ航跡とのペアにおける第2センサ航跡に対応する相関観測値について、バイアス推定フィルタ処理部60で使用可能か否かを判定する。
また、統合相関処理部50は、相関観測値がバイアス推定フィルタ処理部60で使用可能と判定すると、判定毎に相関観測値をバイアス推定フィルタ処理部60に出力する。
【0065】
このとき、統合相関処理部50は、第1センサ航跡に対応する相関観測値および第2センサ航跡に対応する相関観測値が、所定の中心に対する所定のゲート範囲内にある場合に、その相関観測値がバイアス推定フィルタ処理部60で使用可能であると判定する。
なお、所定の中心は、同期処理部30で同一航跡と判定された第1センサ航跡と第2センサ航跡との統合航跡の中心であってもよいし、バイアス推定フィルタ処理部60で推定された目標の位置の中心であってもよい。また、所定のゲート範囲は、同期処理部30で同一航跡と判定された第1センサ航跡と第2センサ航跡との統合航跡のゲート範囲であってもよいし、任意に設定されるゲート範囲であってもよい。
【0066】
ここで、相関処理部12から得られる第1センサ10に対応する相関観測値と、相関処理部22から得られる第2センサ20に対応する相関観測値とは、一般的に、統合相関処理部50に非同期で入力される。なお、もし、相関処理部12から得られる第1センサ10に対応する相関観測値と、相関処理部22から得られる第2センサ20に対応する相関観測値とが、統合相関処理部50に同期して入力される場合には、事前に設定されるセンサの優先順位、例えば、センサ番号の小さいものから順に処理する等の規則に従って、まず第1センサ10、次に第2センサ20といった順に処理する。
【0067】
続いて、バイアス推定フィルタ処理部60は、統合相関処理部50からの相関観測値を用いて、カルマンフィルタまたは最小2乗法に基づいて、バイアス推定フィルタ処理による状態推定およびバイアス推定を実行する。
【0068】
まず、状態推定について、バイアス推定フィルタ処理部60は、次式(4)に示すように、目標の位置、速度の各成分からなる状態ベクトル、または目標の位置、速度、加速度の各成分からなる状態ベクトルの推定値およびその推定誤差共分散行列を算出する。なお、目標の状態ベクトルの次数について、例えば、式(4)のような6次元の状態ベクトルを推定するか、次式(5)のような9次元の状態ベクトルを推定するかは、事前に設定しておく。
【0069】
【数4】
【0070】
【数5】
【0071】
式(4)において、(k)は時刻kを示し、x(k)(アンダーバー)は時刻kの状態ベクトルを示し、記号「´」は行列およびベクトルの転置を示し、状態ベクトルの右辺のx(k)はx成分の位置成分を示し、x(k)(ドット)はx成分の速度成分を示し、x(k)(ツードット)はx成分の加速度成分をそれぞれ示す。なお、y成分、z成分についても同様である。
【0072】
次に、バイアス推定について、バイアス推定フィルタ処理部60は、次式(6)に示すように、距離、仰角、方位角の各成分からなるバイアスベクトルの推定値およびその推定誤差共分散行列を算出する。
【0073】
【数6】
【0074】
式(6)において、b(m)(k)(アンダーバー)は、時刻kにおけるセンサmのバイアスベクトルを示す。ここで、mはセンサを表す識別子であり、この識別子mは、m=1,2,・・・のように番号で設定されてもよいし、m=A,B,・・・X,Y,Z,・・・のように、記号で設定されてもよい。ここでは、識別子mは、第1センサ10または第2センサ20の何れかに対応する。
【0075】
また、式(6)において、ΔR(m)はセンサmの距離バイアスを示し、ΔE(m)はセンサmの仰角バイアスを示し、ΔAz(m)はセンサmの方位角バイアスをそれぞれ示す。
なお、目標のバイアスベクトルの種類については、式(6)のような3次元のバイアスベクトルであってもよいし、次式(7)、(8)、(9)のような1次元のバイアスベクトルであってもよいし、次式(10)、(11)、(12)のような2次元のバイアスベクトルであってもよく、推定すべきバイアスベクトルの種類は、センサ毎に事前に設定しておく。
【0076】
【数7】
【0077】
【数8】
【0078】
【数9】
【0079】
【数10】
【0080】
【数11】
【0081】
【数12】
【0082】
ここで、バイアス推定フィルタ処理部60は、バイアス格子点探索処理部40から得られる、バイアス格子点探索処理によるバイアス推定値を、バイアス推定におけるバイアスベクトル推定値の初期値として設定する。バイアスベクトルの推定誤差共分散行列の初期値は、前の時刻の値をそのまま引き継いで計算してもよいし、改めて、事前に決めたある値をバイアスベクトルの推定誤差共分散行列初期値に設定してもよい。また、格子点探索処理によるバイアス推定値を1度しか使用しないで、バイアスベクトル推定値の初期値を計算してもよい。
【0083】
このとき、バイアス推定フィルタ処理部60による状態推定は、前の時刻の値をそのまま引き継いで状態ベクトル推定値および推定誤差共分散行列を用いて算出してもよいし、現時刻を含む最新のLサンプルの相関観測値を用いて、現時刻で改めて、状態ベクトル推定値および推定誤差共分散行列を算出してもよい。
【0084】
最終的に、バイアス推定フィルタ処理部60は、バイアス推定結果として、カルマンフィルタまたは最小2乗法に基づいて算出したバイアスベクトル推定値を、バイアスDB70に出力する。また、バイアス推定フィルタ処理部60は、バイアス格子点探索処理部40から得られる、バイアス格子点探索処理によるバイアス推定値を、バイアスDB70に出力する。
さらに、バイアス推定フィルタ処理部60は、バイアスベクトルの推定値およびその推定誤差共分散行列と、状態ベクトルの推定値およびその推定誤差共分散行列とを、バイアス格子点探索処理部40に出力する。
【0085】
バイアスDB70は、カルマンフィルタまたは最小2乗法に基づいて、バイアス推定フィルタ処理部60で算出された各時刻のバイアスベクトル推定値と、バイアス格子点探索処理によるバイアス推定値とを、データベースに蓄積する。
また、バイアスDB70は、バイアスベクトル推定値の時刻による移動平均値と、バイアス推定値の時刻による移動平均値とを算出し、これらの移動平均値をデータベースに蓄積する。
【0086】
以上のように、実施の形態1によれば、バイアス推定フィルタ処理手段は、カルマンフィルタおよび最小2乗フィルタによるバイアス推定フィルタ処理を実行する場合に、バイアス格子点探索処理手段によるバイアス格子点探索処理によって算出されるバイアス推定値をバイアスベクトル推定値の初期値として設定する。これにより、バイアス推定フィルタ処理によるバイアスベクトル推定値の初期値を適切に設定することができる。そのため、バイアスベクトル推定値の精度を向上させることができるとともに、バイアスベクトル推定値の収束を早くすることができる。
【0087】
なお、バイアス格子点探索処理のみでもバイアス推定値を算出することは可能であるが、バイアス格子点が粗い場合、本発明は、バイアス格子点探索処理によるバイアス格子よりも細かい精度でバイアス推定値を算出することができる。また、バイアス格子点を細かくした場合、バイアス推定値を高精度に算出できる可能性はあるものの、バイアス格子点探索処理にかかる演算の処理負荷が高くなる。
【0088】
このとき、バイアス推定フィルタ処理手段から出力されるバイアスベクトル推定値およびその推定誤差共分散行列から得られる誤差楕円体、または状態ベクトル推定値およびその推定誤差共分散行列から得られる誤差楕円体をバイアス格子点の探索範囲として設定することにより、バイアス格子点探索処理の処理負荷を低減することができる。また、バイアス格子点探索処理の探索開始点を、バイアス推定フィルタ処理手段から出力されるバイアスベクトル推定値とすることにより、バイアス格子点探索処理において、バイアス格子点の端から探索することなく、ある程度バイアス真値に近い位置から、バイアス推定値を探索することができる。その結果、バイアス推定精度の早期収束、および高精度なバイアス推定値の算出を実現することができる。
【0089】
実施の形態2.
図8は、この発明の実施の形態2に係るセンサバイアス推定装置を示すブロック構成図である。
図8において、このセンサバイアス推定装置は、図1に示したセンサバイアス推定装置に加えて、バイアス格子間隔制御処理部80を備えている。
【0090】
バイアス格子間隔制御処理部80は、以下に示す手順で、バイアス格子点探索処理部40におけるバイアス格子点を設定する。また、バイアス格子点探索処理部40からバイアス格子間隔制御処理部80には、現時刻でのバイアス候補、時刻および格子点探索終了フラグが入力される。
ここで、格子点探索終了フラグとは、例えば格子点探索終了フラグが0の場合に、格子点探索中であることを示し、格子点探索終了フラグが1の場合に、格子点の探索が終了していることを示す等、格子点探索中かあるいは格子点探索終了かが分かるようなフラグである。
【0091】
最初に、バイアス格子間隔制御処理部80は、バイアス格子点を粗く探索するような格子間隔を設定する制御信号を、バイアス格子点探索処理部40に出力する。バイアス格子点探索処理部40は、上記式(3)を用いて、Lサンプル分の航跡の残差和εiR,iE,Azを算出し、Lサンプル分の航跡の残差和εiR,iE,Azが最小となるバイアス候補ΔziR,iE,Az(アンダーバー)を、バイアス推定値として出力する。
【0092】
次に、バイアス格子間隔制御処理部80は、このバイアス候補を中心として、格子間隔を細かく設定する制御信号を、バイアス格子点探索処理部40に出力する。バイアス格子点探索処理部40は、バイアス格子間隔を細かく設定して、再度上記式(3)を用いて、Lサンプル分の航跡の残差和εiR,iE,Azを算出する。
【0093】
続いて、バイアス格子間隔制御処理部80は、バイアス格子点を粗く探索するような格子間隔を設定する制御信号と、格子間隔を細かく設定する制御信号とを繰り返して出力する。また、バイアス格子間隔制御処理部80は、前回算出したLサンプル分の航跡の残差和と今回算出したLサンプル分の航跡の残差和との差が、事前に決めたしきい値以下である場合には、処理を終了する制御信号を、バイアス格子点探索処理部40に出力する。
【0094】
なお、バイアス格子間隔制御処理部80は、バイアス格子点探索処理部40から得られる探索開始時刻と現時刻との差が、事前に決めた探索終了判定時刻しきい値よりも大きい場合、かつ格子点探索終了フラグが0の場合に、探索処理が事前に決めた時間内に終了しないと判定する。このとき、バイアス格子間隔制御処理部80は、現時刻においてLサンプル分の航跡の残差和が最小となるようなバイアス候補を出力させる制御信号を、バイアス格子点探索処理部40に出力する。バイアス格子点探索処理部40は、現時刻(現時点)においてLサンプル分の航跡の残差和が最小となるようなバイアス候補を、バイアス推定フィルタ処理部60に出力する。
【0095】
以上のように、実施の形態2によれば、バイアス格子点探索処理が所定の時間内に終了しない場合であっても、現時刻で最良のバイアス推定値を算出することができる。
【0096】
実施の形態3.
図9は、この発明の実施の形態3に係るセンサバイアス推定装置を示すブロック構成図である。
図9において、このセンサバイアス推定装置は、図1に示したセンサバイアス推定装置に加えて、バイアス格子間隔再設定処理部90を備えている。
【0097】
バイアス格子点探索処理部40は、現時刻でのバイアス候補、時刻および格子点探索終了フラグを、バイアス格子間隔再設定処理部90に出力する。
バイアス格子間隔再設定処理部90は、バイアス格子点探索処理部40から得られる探索開始時刻と現時刻との差が、事前に決めた格子間隔再設定時刻しきい値よりも大きい場合、かつ格子点探索終了フラグが0の場合に、次時刻のバイアス格子間隔を現時刻のバイアス格子間隔よりも粗く設定する制御信号を、バイアス格子点探索処理部40に出力する。
【0098】
以上のように、実施の形態3によれば、現時刻において、バイアス格子点探索処理が所定の時間内に終了する見込みがない場合であっても、現時刻よりもバイアス格子間隔を粗く設定することにより、次時刻以降のバイアス格子点探索処理を、所定の時間内に終了させることができる。すなわち、所定の時間内で、最良のバイアス推定値を算出することができる。
なお、この発明の実施の形態3に係るセンサバイアス推定装置は、実施の形態2と組み合わせることもできる。
【0099】
実施の形態4.
図10は、この発明の実施の形態4に係るセンサバイアス推定装置を示すブロック構成図である。
図10において、このセンサバイアス推定装置は、図1に示したセンサバイアス推定装置に加えて、第1のバイアス格子点探索範囲再設定処理部100を備えている。
【0100】
バイアス格子点探索処理部40は、現時刻でのバイアス候補、時刻および格子点探索終了フラグを、第1のバイアス格子点探索範囲再設定処理部100に出力する。
第1のバイアス格子点探索範囲再設定処理部100は、バイアス格子点探索処理部40から得られる探索開始時刻と現時刻との差が、事前に決めた探索範囲再設定時刻しきい値よりも大きい場合、かつ格子点探索終了フラグが0の場合に、探索処理が事前に決めた時間内に終了しないと判定する。
【0101】
このとき、第1のバイアス格子点探索範囲再設定処理部100は、バイアス格子点の探索範囲を狭くする制御信号を、バイアス格子点探索処理部40に出力する。バイアス格子点探索処理部40は、現時刻よりも狭い固定のバイアス格子点探索範囲を設定する。
なお、第1のバイアス格子点探索範囲再設定処理部100は、図6で示したバイアスベクトル推定値の推定誤差共分散行列から得られる誤差楕円体、または状態ベクトル推定値の推定誤差共分散行列から得られる誤差楕円体に、半径を小さくする係数を掛けて、現時刻よりも狭い可変のバイアス格子点探索範囲を設定してもよい。
【0102】
以上のように、実施の形態4によれば、現時刻において、バイアス格子点探索処理が所定の時間内に終了する見込みがない場合であっても、次時刻以降のバイアス格子点の探索範囲を狭く設定することにより、次時刻以降のバイアス格子点探索処理を、所定の時間内に終了させることができる。すなわち、所定の時間内で、最良のバイアス推定値を算出することができる。
なお、この発明の実施の形態4に係るセンサバイアス推定装置は、実施の形態2または3と組み合わせることもできる。
【0103】
実施の形態5.
図11は、この発明の実施の形態5に係るセンサバイアス推定装置を示すブロック構成図である。
図11において、このセンサバイアス推定装置は、図1に示したセンサバイアス推定装置に加えて、第2のバイアス格子点探索範囲再設定処理部110を備えている。
【0104】
バイアス格子点探索処理部40は、現時刻でのバイアス候補、時刻および格子点探索終了フラグを、第2のバイアス格子点探索範囲再設定処理部110に出力する。
第2のバイアス格子点探索範囲再設定処理部110は、バイアス格子点探索処理部40から得られる探索開始時刻と現時刻との差が、事前に決めた探索範囲再設定時刻しきい値よりも大きい場合、かつ格子点探索終了フラグが1の場合に、探索処理が事前に決めた時間内に終了したと判定する。
【0105】
このとき、第2のバイアス格子点探索範囲再設定処理部110は、バイアス格子点の探索範囲を広くする制御信号を、バイアス格子点探索処理部40に出力する。バイアス格子点探索処理部40は、現時刻よりも広い固定のバイアス格子点探索範囲を設定する。
なお、第2のバイアス格子点探索範囲再設定処理部110は、図6で示したバイアスベクトル推定値の推定誤差共分散行列から得られる誤差楕円体、または状態ベクトル推定値の推定誤差共分散行列から得られる誤差楕円体に、半径を大きくする係数を掛けて、現時刻よりも広い可変のバイアス格子点探索範囲を設定してもよい。
【0106】
以上のように、実施の形態5によれば、現時刻において、バイアス格子点探索処理が所定の時間内に終了する見込みがある場合であって、かつ現時刻のバイアス格子点探索範囲内に、実際のバイアス真値がない場合に、次時刻以降のバイアス格子点探索範囲を広く設定することにより、バイアス格子点探索範囲外に存在する実際のバイアス真値を発見することができる。すなわち、バイアス推定精度を向上させることができる。
なお、この発明の実施の形態5に係るセンサバイアス推定装置は、実施の形態2または3と組み合わせることもできる。
【0107】
実施の形態6.
図12は、この発明の実施の形態6に係るセンサバイアス推定装置を示すブロック構成図である。
図12において、このセンサバイアス推定装置は、図1に示したセンサバイアス推定装置に加えて、バイアス成分推定順序設定処理部120を備えている。
【0108】
バイアス成分推定順序設定処理部120は、バイアス格子点探索処理部40およびバイアス推定フィルタ処理部60に対して、ある成分についてバイアス推定値を算出した後に、残りの成分についてバイアス推定値を算出するように制御信号を出力する。
以下、バイアス成分推定順序設定処理部120が、バイアス格子点探索処理部40およびバイアス推定フィルタ処理部60に対して、仰角、方位角の各成分についてバイアス推定値を算出した後に、距離成分についてバイアス推定値を算出するように制御信号を出力した場合を例に挙げて説明する。
【0109】
このとき、バイアス格子点探索処理部40は、バイアス格子点探索処理を、仰角、方位角からなる範囲と設定して、仰角、方位角バイアス推定値を算出し、算出結果をバイアス推定フィルタ処理部60に出力する。
【0110】
続いて、バイアス推定フィルタ処理部60は、バイアス格子点探索処理部40から得られる、仰角、方位角バイアス推定値を、バイアス推定におけるバイアスベクトル推定値の初期値として設定する。また、バイアス推定フィルタ処理部60は、この初期値に基づいて、上記式(10)で示した仰角、方位角の各成分からなるバイアスベクトル推定値およびその推定誤差共分散行列と、状態ベクトル推定値およびその推定誤差共分散行列を算出する。
【0111】
次に、バイアス格子点探索処理部40は、仰角、方位角のバイアス格子点探索処理とは別に、距離のバイアス格子点探索処理を実行する。
距離のバイアス格子点探索処理において、バイアス格子点探索処理部40は、バイアス推定フィルタ処理部60から得られる仰角、方位角の各成分からなるバイアスベクトル推定値を、バイアス格子点探索処理におけるバイアス候補ΔziR,iE,Az(アンダーバー)の仰角成分、方位角成分と設定する。
【0112】
そして、バイアス格子点探索処理部40は、距離成分のみについて図7に示したバイアス格子点探索処理を実行して、距離バイアス推定値を算出し、算出結果をバイアス推定フィルタ処理部60に出力する。
また、バイアス格子点探索処理部40は、算出した距離バイアス推定値をバイアスDB70に出力する。
【0113】
特に遠距離の場合、角度誤差に起因するクロスレンジ誤差は、距離誤差、すなわちレンジ誤差に比べて悪くなる。その場合、距離バイアスに比べて、角度バイアスの影響が遠距離において支配的になり、バイアス推定フィルタ処理によって、距離バイアスを推定できない場合が考えられる。
そこで、この実施の形態6によれば、例えば仰角、方位角と距離とに分けてバイアス推定値を算出することにより、バイアス推定精度を向上させることができる。
【0114】
なお、上記実施の形態6では、バイアス成分の推定順序として、まず、仰角、方位角の各成分についてバイアス推定値を算出した後に、距離成分についてバイアス推定値を算出する場合を示した。この例は、仰角、方位角と距離とに分けてバイアス推定を実行する例である。
しかしながら、これに限定されず、距離、仰角と、方位角とに分けてバイアス推定を実行してもよいし、距離、方位角と仰角とに分けてバイアス推定を実行してもよいし、距離と仰角と方位角とにそれぞれ分けてバイアス推定を実行してもよい。
また、この発明の実施の形態6に係るセンサバイアス推定装置は、実施の形態2〜5と組み合わせることもできる。
【0115】
実施の形態7.
図13は、この発明の実施の形態7に係るセンサバイアス推定装置を示すブロック構成図である。
図13において、このセンサバイアス推定装置は、図1に示したセンサバイアス推定装置に加えて、バイアス成分次数設定処理部130を備えている。
【0116】
この実施の形態7において、第1センサ10の処理は、上述した実施の形態1と同様とする。すなわち、第1センサ10は、センサの受信器から距離、仰角、方位角の観測値を得て、最終的に、第1データ更新処理部13から、上記式(4)または(5)に示した成分を有する平滑値の状態ベクトルおよびその誤差共分散行列を出力する。なお、この実施の形態7では、上記式(4)のような速度項までの状態ベクトルについて説明する。
【0117】
また、この実施の形態7において、第2センサ20は、センサの受信器から仰角、方位角の観測値を得て、最終的に、第2データ更新処理部23から、次式(13)または(14)に示す成分を有する平滑値の状態ベクトルおよびその誤差共分散行列を出力する。
【0118】
【数13】
【0119】
【数14】
【0120】
式(13)、(14)において、E(k)は時刻kの仰角を示し、Az(k)は時刻kの方位角を示し、E(k)(ドット)は時刻kの仰角速度を示し、Az(k)(ドット)は時刻kの方位角速度を示し、E(k)(ツードット)は時刻kの仰角加速度を示し、Az(k)(ツードット)は時刻kの方位角加速度をそれぞれ示す。なお、この実施の形態7では、上記式(13)のような速度項までの状態ベクトルについて説明する。
【0121】
バイアス成分次数設定処理部130は、同期処理部30、バイアス格子点探索処理部40、統合相関処理部50およびバイアス推定フィルタ処理部60に対して、推定すべきバイアス成分の次数を、観測される成分の次数に合わせるように制御信号を出力する。
【0122】
このとき、同期処理部30は、第1センサ10から得られる式(4)に示した成分を有する平滑値の状態ベクトル、すなわちx,y,zの直交座標に関する6次元の航跡と、第2センサ20から得られる式(13)に示した成分を有する平滑値の状態ベクトル、すなわち仰角、方位角に関する4次元の航跡とに基づいて、時刻同期および航跡相関を実行する。
【0123】
ここで、同期処理部30による航跡相関は、航跡の位置情報を用いて実行される。このとき、第1センサ10からのx,y,zの直交座標に関する6次元の航跡と、第2センサ20からの仰角、方位角に関する4次元の航跡とは、次元が互いに異なるので、次元が少ない方の基準座標に合わせる。
【0124】
すなわち、同期処理部30は、第1センサ10からのx,y,zの直交座標に関する航跡を、第2センサ20を中心とした第2センサ基準座標に変換し、さらに、第2センサ基準座標において、直交座標から極座標への座標変換を行う。その後、同期処理部30は、第2センサ基準座標において、仰角、方位角の各成分の情報を用いて、第1センサ10から得られる航跡と第2センサ20から得られる航跡との航跡相関を実行する。
【0125】
続いて、バイアス格子点探索処理部40は、同期処理部30による航跡相関によって得られた第1センサ10と第2センサ20との仰角、方位角を各成分とする航跡のペアを用いて、これらの仰角、方位角の各成分からなるバイアス格子点探索範囲を設定する。また、バイアス格子点探索処理部40は、バイアス格子点探索処理により、仰角、方位角バイアス推定値を算出し、算出結果をバイアス推定フィルタ処理部60に出力する。
【0126】
次に、バイアス推定フィルタ処理部60は、バイアス格子点探索処理部40から得られる、仰角、方位角バイアス推定値を、バイアス推定におけるバイアスベクトル推定値の初期値として設定する。また、バイアス推定フィルタ処理部60は、この初期値に基づいて、仰角、方位角の各成分からなるバイアスベクトル推定値およびその推定誤差共分散行列を算出する。バイアス推定フィルタ処理部60は、この初期値に基づいて、状態推定処理により、第1センサ10または第2センサ20を基準とする直交座標における平滑値の状態ベクトルおよびその誤差共分散行列を算出する。
【0127】
なお、統合相関処理部50から得られる相関観測値は、第1センサ10からの相関観測値については、x,y,zの3次元の直交座標における観測位置であるが、第2センサ20からの相関観測値については、仰角、方位角の極座標における観測位置である。
【0128】
したがって、バイアス推定フィルタ処理部60が、第2センサ20からの相関観測値を用いて状態推定処理を実行する場合、状態ベクトルがx,y,zの3次元の直交座標であるのに対して、第2センサ20の観測値が仰角、方位角であり、観測ベクトルの成分が仰角、方位角の2次元なので、状態ベクトルと観測ベクトルとの関係を表す観測モデルが非線形となる。そこで、バイアス推定フィルタ処理部60が、第2センサ20からの相関観測値を用いて状態推定処理を実行する場合には、拡張カルマンフィルタが用いられる。
【0129】
以上のように、実施の形態7によれば、センサが得る観測ベクトルの次数が異なっている場合であっても、バイアス推定精度を向上させることができる。
【0130】
なお、上記実施の形態7では、バイアス成分次数設定処理部130が、第2センサ20で得られた仰角、方位角からなる2次元の観測ベクトルに対応した、2次元のバイアスベクトル推定値を算出する場合について説明した。しかしながら、これに限定されず、バイアス成分次数設定処理部130は、第2センサ20で得られた2次元の観測ベクトルよりも次数の低い1次元のバイアスベクトル推定値、例えば仰角成分のみあるいは方位角成分のみを算出するように制御信号を出力してもよい。また、何次元のバイアスベクトル推定値を算出するかについては、事前に設定される。
【0131】
また、上記実施の形態7では、第2センサ20が、仰角、方位角を観測する場合について説明した。しかしながら、これに限定されず、第2センサ20が、距離のみを観測する場合、仰角のみを観測する場合、方位角のみを観測する場合、距離および仰角を観測する場合、距離および方位角を観測する場合にも、それぞれ適用することができる。
また、この発明の実施の形態7に係るセンサバイアス推定装置は、実施の形態2〜6と組み合わせることもできる。
【符号の説明】
【0132】
10 第1センサ、20 第2センサ、30 同期処理部(同期処理手段)、40 バイアス格子点探索処理部(バイアス格子点探索処理手段)、50 統合相関処理部(統合相関処理手段)、60 バイアス推定フィルタ処理部(バイアス推定フィルタ処理手段)、80 バイアス格子間隔制御処理部(バイアス格子間隔制御処理手段)、90 バイアス格子間隔再設定処理部(バイアス格子間隔再設定処理手段)、100 第1のバイアス格子点探索範囲再設定処理部(第1のバイアス格子点探索範囲再設定処理手段)、110 第2のバイアス格子点探索範囲再設定処理部(第2のバイアス格子点探索範囲再設定処理手段)、120 バイアス成分推定順序設定処理部(バイアス成分推定順序設定処理手段)、130 バイアス成分次数設定処理部(バイアス成分次数設定処理手段)。
【特許請求の範囲】
【請求項1】
複数のセンサを有する装置であり、それぞれのセンサが、観測値と予測値とに基づいて相関処理を実行して相関観測値を算出するとともに、前記相関観測値に基づいてセンサ航跡を算出し、前記相関観測値および前記センサ航跡を出力する装置に適用され、前記複数のセンサのバイアスを推定するセンサバイアス推定装置であって、
前記センサ航跡に基づいて、時刻同期および航跡相関を実行し、同一航跡と判定されたセンサ航跡の組を出力する同期処理手段と、
前記同一航跡と判定されたセンサ航跡の組に対してバイアス格子点探索処理を実行し、バイアス推定値を算出するバイアス格子点探索処理手段と、
前記相関観測値に基づいて、バイアス推定フィルタ処理を実行し、バイアスベクトル推定値を算出するバイアス推定フィルタ処理手段と、を備え、
前記バイアス推定フィルタ処理手段は、前記バイアス推定値を前記バイアスベクトル推定値の初期値として設定する
ことを特徴とするセンサバイアス推定装置。
【請求項2】
前記複数のセンサと前記バイアス推定フィルタ処理手段との間に設けられ、前記センサ航跡に対応する前記相関観測値が、所定の中心に対する所定のゲート範囲内にある場合に、前記相関観測値を前記バイアス推定フィルタ処理手段に出力する統合相関処理手段をさらに備えたことを特徴とする請求項1に記載のセンサバイアス推定装置。
【請求項3】
前記バイアス推定フィルタ処理手段は、前記バイアスベクトル推定値およびその推定誤差共分散行列と、状態ベクトル推定値およびその推定誤差共分散行列とを前記バイアス格子点探索処理手段に出力し、
前記バイアス格子点探索処理手段は、前記バイアスベクトル推定値およびその推定誤差共分散行列と、前記状態ベクトル推定値およびその推定誤差共分散行列とに基づいて、前記バイアス格子点探索処理の探索範囲を設定する
ことを特徴とする請求項1または請求項2に記載のセンサバイアス推定装置。
【請求項4】
前記バイアス格子点探索処理におけるバイアス格子間隔を、探索の開始時には粗く設定し、探索が進むにつれて細かく設定する制御信号を前記バイアス格子点探索処理手段に出力するとともに、前記バイアス格子点探索処理が終了しておらず、かつ前記バイアス格子点探索処理手段から得られる探索開始時刻と現時刻との差が、事前に決めた探索終了判定時刻しきい値よりも大きい場合に、現時点において最良のバイアス候補を出力させる制御信号を前記バイアス格子点探索処理手段に出力するバイアス格子間隔制御処理手段をさらに備えたことを特徴とする請求項1から請求項3までの何れか1項に記載のセンサバイアス推定装置。
【請求項5】
前記バイアス格子点探索処理が終了しておらず、かつ前記バイアス格子点探索処理手段から得られる探索開始時刻と現時刻との差が、事前に決めた格子間隔再設定時刻しきい値よりも大きい場合に、次時刻のバイアス格子間隔を現時刻のバイアス格子間隔よりも粗く設定する制御信号を前記バイアス格子点探索処理手段に出力するバイアス格子間隔再設定処理手段をさらに備えたことを特徴とする請求項1から請求項4までの何れか1項に記載に記載のセンサバイアス推定装置。
【請求項6】
前記バイアス格子点探索処理が終了しておらず、かつ前記バイアス格子点探索処理手段から得られる探索開始時刻と現時刻との差が、事前に決めた探索範囲再設定時刻しきい値よりも大きい場合に、前記バイアス格子点探索処理の探索範囲を狭くする制御信号を前記バイアス格子点探索処理手段に出力する第1のバイアス格子点探索範囲再設定処理手段をさらに備えたことを特徴とする請求項3から請求項5までの何れか1項に記載に記載のセンサバイアス推定装置。
【請求項7】
前記バイアス格子点探索処理が終了し、かつ前記バイアス格子点探索処理手段から得られる探索開始時刻と現時刻との差が、事前に決めた探索範囲再設定時刻しきい値よりも大きい場合に、前記バイアス格子点探索処理の探索範囲を広くする制御信号を前記バイアス格子点探索処理手段に出力する第2のバイアス格子点探索範囲再設定処理手段をさらに備えたことを特徴とする請求項3から請求項5までの何れか1項に記載に記載のセンサバイアス推定装置。
【請求項8】
前記バイアス推定値の推定順序として、ある成分についてバイアス推定値を算出した後に、残りの成分についてバイアス推定値を算出させる制御信号を、前記バイアス格子点探索処理手段および前記バイアス推定フィルタ処理手段に出力するバイアス成分推定順序設定処理手段をさらに備えたことを特徴とする請求項1から請求項7までの何れか1項に記載に記載のセンサバイアス推定装置。
【請求項9】
前記複数のセンサから得られる観測ベクトルの次数が互いに異なる場合に、推定すべきバイアス成分の次数を、観測される成分の次数に合わせるような制御信号を、前記同期処理手段、前記バイアス格子点探索処理手段および前記バイアス推定フィルタ処理手段に出力するバイアス成分次数設定処理手段をさらに備えたことを特徴とする請求項1から請求項8までの何れか1項に記載のセンサバイアス推定装置。
【請求項1】
複数のセンサを有する装置であり、それぞれのセンサが、観測値と予測値とに基づいて相関処理を実行して相関観測値を算出するとともに、前記相関観測値に基づいてセンサ航跡を算出し、前記相関観測値および前記センサ航跡を出力する装置に適用され、前記複数のセンサのバイアスを推定するセンサバイアス推定装置であって、
前記センサ航跡に基づいて、時刻同期および航跡相関を実行し、同一航跡と判定されたセンサ航跡の組を出力する同期処理手段と、
前記同一航跡と判定されたセンサ航跡の組に対してバイアス格子点探索処理を実行し、バイアス推定値を算出するバイアス格子点探索処理手段と、
前記相関観測値に基づいて、バイアス推定フィルタ処理を実行し、バイアスベクトル推定値を算出するバイアス推定フィルタ処理手段と、を備え、
前記バイアス推定フィルタ処理手段は、前記バイアス推定値を前記バイアスベクトル推定値の初期値として設定する
ことを特徴とするセンサバイアス推定装置。
【請求項2】
前記複数のセンサと前記バイアス推定フィルタ処理手段との間に設けられ、前記センサ航跡に対応する前記相関観測値が、所定の中心に対する所定のゲート範囲内にある場合に、前記相関観測値を前記バイアス推定フィルタ処理手段に出力する統合相関処理手段をさらに備えたことを特徴とする請求項1に記載のセンサバイアス推定装置。
【請求項3】
前記バイアス推定フィルタ処理手段は、前記バイアスベクトル推定値およびその推定誤差共分散行列と、状態ベクトル推定値およびその推定誤差共分散行列とを前記バイアス格子点探索処理手段に出力し、
前記バイアス格子点探索処理手段は、前記バイアスベクトル推定値およびその推定誤差共分散行列と、前記状態ベクトル推定値およびその推定誤差共分散行列とに基づいて、前記バイアス格子点探索処理の探索範囲を設定する
ことを特徴とする請求項1または請求項2に記載のセンサバイアス推定装置。
【請求項4】
前記バイアス格子点探索処理におけるバイアス格子間隔を、探索の開始時には粗く設定し、探索が進むにつれて細かく設定する制御信号を前記バイアス格子点探索処理手段に出力するとともに、前記バイアス格子点探索処理が終了しておらず、かつ前記バイアス格子点探索処理手段から得られる探索開始時刻と現時刻との差が、事前に決めた探索終了判定時刻しきい値よりも大きい場合に、現時点において最良のバイアス候補を出力させる制御信号を前記バイアス格子点探索処理手段に出力するバイアス格子間隔制御処理手段をさらに備えたことを特徴とする請求項1から請求項3までの何れか1項に記載のセンサバイアス推定装置。
【請求項5】
前記バイアス格子点探索処理が終了しておらず、かつ前記バイアス格子点探索処理手段から得られる探索開始時刻と現時刻との差が、事前に決めた格子間隔再設定時刻しきい値よりも大きい場合に、次時刻のバイアス格子間隔を現時刻のバイアス格子間隔よりも粗く設定する制御信号を前記バイアス格子点探索処理手段に出力するバイアス格子間隔再設定処理手段をさらに備えたことを特徴とする請求項1から請求項4までの何れか1項に記載に記載のセンサバイアス推定装置。
【請求項6】
前記バイアス格子点探索処理が終了しておらず、かつ前記バイアス格子点探索処理手段から得られる探索開始時刻と現時刻との差が、事前に決めた探索範囲再設定時刻しきい値よりも大きい場合に、前記バイアス格子点探索処理の探索範囲を狭くする制御信号を前記バイアス格子点探索処理手段に出力する第1のバイアス格子点探索範囲再設定処理手段をさらに備えたことを特徴とする請求項3から請求項5までの何れか1項に記載に記載のセンサバイアス推定装置。
【請求項7】
前記バイアス格子点探索処理が終了し、かつ前記バイアス格子点探索処理手段から得られる探索開始時刻と現時刻との差が、事前に決めた探索範囲再設定時刻しきい値よりも大きい場合に、前記バイアス格子点探索処理の探索範囲を広くする制御信号を前記バイアス格子点探索処理手段に出力する第2のバイアス格子点探索範囲再設定処理手段をさらに備えたことを特徴とする請求項3から請求項5までの何れか1項に記載に記載のセンサバイアス推定装置。
【請求項8】
前記バイアス推定値の推定順序として、ある成分についてバイアス推定値を算出した後に、残りの成分についてバイアス推定値を算出させる制御信号を、前記バイアス格子点探索処理手段および前記バイアス推定フィルタ処理手段に出力するバイアス成分推定順序設定処理手段をさらに備えたことを特徴とする請求項1から請求項7までの何れか1項に記載に記載のセンサバイアス推定装置。
【請求項9】
前記複数のセンサから得られる観測ベクトルの次数が互いに異なる場合に、推定すべきバイアス成分の次数を、観測される成分の次数に合わせるような制御信号を、前記同期処理手段、前記バイアス格子点探索処理手段および前記バイアス推定フィルタ処理手段に出力するバイアス成分次数設定処理手段をさらに備えたことを特徴とする請求項1から請求項8までの何れか1項に記載のセンサバイアス推定装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【公開番号】特開2011−64484(P2011−64484A)
【公開日】平成23年3月31日(2011.3.31)
【国際特許分類】
【出願番号】特願2009−213149(P2009−213149)
【出願日】平成21年9月15日(2009.9.15)
【出願人】(000006013)三菱電機株式会社 (33,312)
【Fターム(参考)】
【公開日】平成23年3月31日(2011.3.31)
【国際特許分類】
【出願日】平成21年9月15日(2009.9.15)
【出願人】(000006013)三菱電機株式会社 (33,312)
【Fターム(参考)】
[ Back to top ]