JP2004301586A - Moving locus measuring apparatus - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To precisely measure positions and speed even if a mobile unit exists near a sensor by utilizing the arrival time difference of radio waves to each sensor from the mobile unit in addition to the arrival direction of the radio waves. <P>SOLUTION: A moving locus measuring apparatus comprises an angle measuring means 2 for calculating the azimuth angle of radio waves arriving from a plurality of sensors 1-1 to a sensor 1-I; and an arrival time difference measuring means 3 for calculating the time difference, where radio waves arrive from the mobile unit to the sensor, thus performing tracking processing by using the angle measurement value and the arrival time difference. As a result, malfunctions of large measurement error when the position is calculated only by the azimuth angle are eliminated if the mobile unit is located at a position close to a straight line for connecting the plurality of sensors when measuring the azimuth angle of the mobile unit by using the plurality of sensors for tracking the position and speed of the mobile unit from the measurement result. <P>COPYRIGHT: (C)2005,JPO&NCIPI

Description

【００１８】
状態ベクトルｘ _ｋを使用して、移動体が直線コースをほぼ等速で直進運動している際の運動モデル（等速直進モデルと呼ぶ）を式（２）のように定義する。
【数２】

ここで、Φ_ｋは等速直進運動の状態遷移行列であって、式（３）によって与えられる。
【数３】

【００１９】
式（２）において、ｗ _ｋは移動体の実際の運動を等速直進運動と見なしたことに伴う誤差を表現するための加速度相当の駆動雑音ベクトルであって、式（４）、式（５）の性質も持つ正規性白色雑音であるとする。
【数４】

【数５】

ただし、Ｅ［］は集合平均を表す記号である。
【００２０】
またΓ_ｋは駆動雑音の変換行列で、式（６）で表される。
【数６】

【００２１】
次に、観測モデルとして、方位角の観測モデルと、到着時間差（到着距離差）の観測モデルを定義する。まず、到着時間差（到着距離差）の観測モデルを定義する。サンプリング時刻ｔ_ｋにおいて、移動体からの電波がセンサ１−ｉ１及び１−ｉ２に探知される時間（到着時間）の差の真値ΔＴ_{ｋ，（ｉ１，ｉ２）}は、移動体の状態ベクトルｘ _ｋにより、次式によって表わされる。
【数７】

ただし、ｈ_{（ｉ１，ｉ２）}（ｘ _ｋ）は次の通りである。
【数８】

ここで、ｃは光速である。
【００２２】
したがって、センサ１−ｉ１と１−ｉ２の到着時間差の観測モデルは、式（９）によって定義される。
【数９】

【００２３】
ここで、ΔＴ^＊ _{ｋ，（ｉ１，ｉ２）}は移動体からセンサ１−ｉ１及び１−ｉ２への送信信号の到着時間の差の観測値であって、センサ１−ｉ１及び１−ｉ２の探知した電波に基づいて到着時間差計測手段３により算出されるものである。またν_{ΔＴ，ｋ，（ｉ１，ｉ２）}は到着時間差の観測雑音を表す。また観測雑音ν_{ΔＴ，ｋ，（ｉ１，ｉ２）}は、式（１０）、式（１１）の性質を持つ正規性白色雑音であるとする。
【数１０】

【数１１】

【００２４】
次に、方位角の観測モデルについて定義する。サンプリング時刻ｔ_ｋにおいて、センサ１−ｉより見た移動体の方位角の真値θ_ｋ，ｉは、移動体の状態ベクトルｘ _ｋにより、次式のとおり表される。
【数１２】

ただし、ｆ_ｉ（ｘ _ｋ）は式（１３）による。
【数１３】

【００２５】
したがって、センサ１−ｉの観測モデルは式（１４）によって定義される。
【数１４】

ここで、θ^＊ _ｋ，ｉはセンサ１−ｉによって探知された電波に基づいて、測角手段２によって出力される測角値である。またν_{θ，ｋ，ｉ}はこの測角値の観測雑音を表す。また観測雑音ν_{θ，ｋ，ｉ}は、式（１５）、式（１６）の性質を持つ正規性白色雑音であるとする。
【数１５】

【数１６】

【００２６】
次に、この発明において、上記の運動モデルと観測モデルに基づき、拡張カルマンフィルタのアルゴリズムを使用して行う移動体の追尾処理方法を説明する。まず、以下のように変数を定義しておく。
【数１７】

【数１８】

【数１９】

【数２０】

【００２７】
ここで、ｘ ^’ _ｋはサンプリング時刻ｔ_ｋまでの観測情報によりサンプリング時刻ｔ_ｋの状態ベクトルを推定した平滑ベクトルである。またｘ ^” _ｋはサンプリング時刻ｔ_ｋ−１までの観測情報によりサンプリング時刻ｔ_ｋの状態ベクトルを推定した予測ベクトルである。さらにＰ^’ _ｋ，Ｐ^” _ｋは、それぞれ平滑ベクトル、予測ベクトルの誤差共分散行列である。
【００２８】
競走の開始時刻を初期サンプリング時刻ｔ_０とし、平滑ベクトルの初期値ｘ^’ _０および平滑誤差共分散行列の初期値Ｐ^’ _０を予め設定する。ここで、平滑ベクトルの初期値には、通常の目標追尾のように、平滑ベクトルの初期値として、最初の１サンプル目で得られた観測結果より算出した目標の観測位置と、次の２サンプル目で得られた各観測結果より算出した目標の２点の観測位置に基づいて、この２点間の変位と時間間隔より算出した速度を設定する。
【００２９】
以降、時刻ｔ_ｋにセンサ１−１〜１−Ｉのいずれかから、新たな観測値が得られる毎に、サンプリング時刻を１つ進めて、追尾処理を行う。以下に、サンプリング時刻ｔ_ｋにおいて実行される追尾処理の具体的な方法を説明していく。サンプリング時刻ｔ_ｋにおいて、測角値または到着時間差観測値が得られると、まず、拡張カルマンフィルタの処理アルゴリズムに従い、以下の予測処理を行う。
【数２１】

【数２２】

【００３０】
次に、取得された各観測値毎に相関判定および平滑処理を、到着時間差観測値についてＭ＝Ｎ（Ｍ_{ｏｂｓ，ｋ}）回、測角値についてＪ＝Ｎ（Ｉ_{ｏｂｓ，ｋ}）回、合計Ｍ＋Ｊ回繰り返し実行する。到着時間差観測値についての相関判定および平滑処理と、測角値についての相関判定および平滑処理との間には、特に優先順位や依存関係があるわけではない。しかし以下では、説明を簡潔にするために、まず同時刻に得られた到着時間差観測値及び測角値の処理の順番に特に優先順位はない。すなわち、同時刻に得られた到着時間差観測値間、測角値間で処理の順番が入れ替わったり、到着時間差と測角値とで処理の順番が入れ替わってもよい。しかし、以下、及び、実施の形態では、説明を簡潔にするために、まず到着時間差観測値について相関判定及び平滑処理をＭ回行い、その後、測角値について相関判定及び平滑処理をＪ回行うこととする。
【００３１】
以下、表記を簡潔にするため、Ｍ個の到着時間差観測値をΔＴ^＊ _ｋ，１，…，ΔＴ^＊ _ｋ，Ｍと記す。それぞれの値は、あるセンサペアから得られており、以後、必要に応じて、ΔＴ^＊ _ｋ，１，…，ΔＴ^＊ _ｋ，Ｍと、ΔＴ^＊ _{ｋ，（ｉ１，ｉ２）}，…，ΔＴ^＊ _{ｋ，（ｉ２Ｍ−１，ｉ２Ｍ）}の表現を併用する。
以後、必要に応じて、例えば、時刻ｋにおけるｍ個目の到着時間差観測値がセンサａ，ｂより得られる場合、ΔＴ^＊ _ｋ，ｍとΔＴ^＊ _{（ａ，ｂ）}の表現を併用する。その他の変数についても同様である。
【００３２】
時刻ｔ_ｋにおいて、ｍ番目の到着時間差観測値ΔＴ^＊ _ｋ，ｍまでにより平滑処理を行った結果で得られた状態ベクトルの推定値をｘ ^’ _ｋ，ｍ、その誤差共分散行列をＰ ^’ _ｋ，ｍと表す。まず、ｍ＝０の場合の初期値として、式（２３）、式（２４）に示すように、予測ベクトルと予測誤差共分散行列を設定する。
【数２３】

【数２４】

【００３３】
上記初期値を設定した後、次のような到着時間差相関判定処理と到着時間差平滑処理とを、ｍ＝１，…，Ｍについて繰り返し行う。
【００３４】
まず、取得した到着時間差観測値について相関判定を行う。すなわち、この到着時間差観測値が、すでに追尾している移動体から得られる到着時間差値に相当するデータであるか否かを判定する。この到着時間差相関判定処理は、追尾処理を行う上で、極端に精度の悪い到着時間差観測値を除外する効果がある。このような精度の悪い到着時間差観測値が生ずる場合としては、例えば、センサが、移動体から発信された電波の直接波とともに、他の構造物に一旦反射してからセンサに到着する間接波を、混信して受信したことなどが考えられる。到着時間差相関判定処理では、まず、上記ｘ ^’ _{ｋ，ｍ−１}を使用し、式（２５）に従って、移動体の予測到着時間差ΔＴ^” _ｋ，ｍを算出する。
【数２５】

【００３５】
次に、実際の到着時間差観測値と上記により得られた予測到着時間差値との差（残差と呼ぶ）ΔＴ^＊ _ｋ，ｍ−ΔＴ^” _ｋ，ｍの大きさに基づいて、到着時間差観測値ΔＴ^＊ _ｋ，ｍの有効性を判定する。式（９）および式（２５）によれば、残差ΔＴ^＊ _ｋ，ｍ−ΔＴ”_ｋ，ｍは、次式のようになる。
【数２６】

【００３６】
ベクトルｘ ^’ _{ｋ，ｍ−１}と真値ｘ _ｋの誤差が微少であることを仮定すれば、式（２６）右辺の第１項は、式（２７）のように近似することができる。
【数２７】

ここで、Ｈ_ｍ（ｘ ^’ _{ｋ，ｍ−１}）は、式（２８）に示すように、式（８）の関数ｈ_ｍ（ｘ _ｋ）の勾配のｘ ^’ _{ｋ，ｍ−１}における値である。
【数２８】

なお、Ｘ_ｋはｘ_ｋを時間で微分したもの（ｄｘ_ｋ／ｄｔ）である。またＹ_ｋについても同様（ｄｙ_ｋ／ｄｔ）である。
【００３７】
上記、式（２６）に、式（１１）、式（２７）、式（２８）を用いれば、ΔＴ^＊ _ｋ，ｍ−ΔＴ^” _ｋ，ｍの分散値ｓ_ｋ，ｍは、次のように計算できる。
【数２９】

【００３８】
到着時間差相関判定処理では、上記分散値ｓ_ｋ，ｍを使用し、ｄをパラメータとして式（３０）を満たす場合にのみ、この到着時間差観測値ΔＴ’_ｋ，ｍを有効であると判定する。
【数３０】

【００３９】
上記相関判定処理により、到着時間差観測値ΔＴ^＊ _ｋ，ｊが有効であると判断された場合には、拡張カルマンフィルタのアルゴリズムに従い、以下の式（３１）、式（３２）、式（３３）の計算を行う。ただし、Ｉは４×４の単位行列である。
【数３１】

【数３２】

【数３３】

【００４０】
一方、到着時間差観測値ΔＴ^’ _ｋ，ｍが有効でないと判断された場合は、到着時間差観測値を用いた平滑処理は行わず、以下の式に示すように、平滑値および平滑誤差共分散行列を予測値および予測誤差共分散行列で置き換える。
【数３４】

【００４１】
上記、式（２５）〜式（３４）の一連の処理をｍ＝１，．．．，Ｍについて繰り返し実行する。以上が、到着時間差観測値ΔＴ^’ _ｋ，ｍを用いた相関判定および平滑処理の内容である。
【００４２】
次に、測角値について、相関判定及び平滑処理を行う。まず、到着時間差の最終平滑値を以下のとおり設定する。
【数３５】

【００４３】
次に、観測モデル式（１２）〜式（１６）を用いて、測角値θ^＊ _ｋ，ｊ（ｊ＝１，…，Ｊ）について、到着時間差観測値の場合と同様にして以下の処理を行う。まず、上記ｘ^’ _{ｋ，ｊ−１}を使用し、式（３６）に従って、移動体の予測方位θ”_ｋ，ｊを算出する。
【数３６】

【００４４】
実際の測角値と上記得られた予測方位との差（残差と呼ぶ）θ^＊ _ｋ，ｊ−θ^” _ｋ，ｊの大きさに基づいて、測角値θ^＊ _ｋ，ｊの有効性を判定する。到着時間差の場合と同様に、θ^＊ _ｋ，ｊ−θ^” _ｋ，ｊの分散値ｓ_ｋ，ｊは式（３７）と式（３８）を用いて計算する。
【数３７】

【数３８】

【００４５】
測角相関判定処理では、上記分散値ｓ_ｋ，ｊを使用し、ｄをパラメータとして式（３９）を満たす場合にのみ、この測角値θ^＊ _ｋ，ｊを有効であると判定する。
【数３９】

【００４６】
上記相関判定処理により、測角値θ^’ _ｋ，ｊが有効であると判断された場合には、拡張カルマンフィルタのアルゴリズムに従い、以下の式（４０）、式（４１）、式（４２）による計算を行う。ただし、Ｉは４×４の単位行列である。
【数４０】

【数４１】

【数４２】

【００４７】
一方、測角値θ^＊ _ｋ，ｊが有効でないと判断された場合は、測角値を用いた平滑処理は行わず、式（４３）に示すように、平滑値および平滑誤差共分散行列を予測値および予測誤差共分散行列で置き換える。
【数４３】

上記、式（３６）〜式（４３）の一連の処理をｊ＝１，．．．，Ｊについて繰り返し実行する。
【００４８】
この結果、最終的に得られたｘ^’ _ｋ，ＪおよびＰ^’ _ｋ，Ｊを、式（４４）、式（４５）に示すように、サンプリング時刻ｔ_ｋにおける平滑ベクトルおよび平滑誤差共分散行列の算出結果として、サンプリング時刻ｔ_ｋでの処理を終了する。
【数４４】

【数４５】

【００４９】
以上が、この発明における移動体の追尾方法の原理である。次に上記原理に基づいて、実施の形態１による移動軌跡計測装置の動作について説明する。
【００５０】
まず追尾手段４において、追尾の開始時刻である初期サンプリング時刻をｔ_０とする。まず初期値設定手段５は、通常のカルマンフィルタの理論に基づいて、平滑値ｘ ^’ _ｋの初期値ｘ ^’ _０、及び、平滑誤差共分散行列Ｐ^’ _ｋの初期値Ｐ^’ _０を算出し、追尾処理を開始する。
【００５１】
次に、ある時刻ｔ_ｋにおいてセンサ１−１〜１−ｉが移動体からの電波を探知すると、測角手段２は、この移動体からの到着した電波に基づいて測角値を算出する。さらに到着時間差計測手段３は、この移動体からの到着した電波の到着時間差を算出する。追尾手段４は、測角手段２が算出した測角値と到着時間差計測手段３が算出した到着時間差とを観測値として取得し、サンプリング時刻を１つ進めて追尾処理を行う。以下、サンプリング時刻ｔ_ｋにおける処理を説明する。
【００５２】
追尾手段４が観測値を取得すると、予測手段８は、平滑ベクトル用メモリ６より１サンプリング前の平滑ベクトルｘ ^’ _ｋ−１と、平滑誤差共分散行列用メモリ７より１サンプリング前の平滑誤差共分散行列Ｐ^’ _ｋ−１を読み出す。予測手段８は、予め設定された駆動雑音共分散行列Ｑ_ｋを使用して、式（２１）及び式（２２）に基づき、予測ベクトルｘ ^” _ｋと予測誤差共分散行列Ｐ^” _ｋを算出する。
【００５３】
ここで、サンプリング時刻ｔ_ｋに、Ｊ＝Ｎ（Ｉ_{ｏｂｓ，ｋ}）個の測角値θ^＊ _ｋ，ｊ（ｊ＝１，．．，Ｊ）、及び、Ｍ＝Ｎ（Ｍ_{ｏｂｓ，ｋ}）個の到着時間差観測値ΔＴ^＊ _ｋ，ｍ（ｍ＝１，…，Ｍ）が得られたとする。そうすると、測角時間差選別手段１１は、メモリ上の測角値と到着時間差観測値の分類を行う。その結果、測角値θ^＊ _ｋ，ｊ（ｊ＝１，．．，Ｊ）を測角値選択手段１３へ出力し、到着時間差観測値ΔＴ^＊ _ｋ，ｍ（ｍ＝１，…，Ｍ）を、時間差相関判定手段１２へ出力する。
【００５４】
時間差相関判定手段１２は、予測ベクトルｘ ^” _ｋ、予測誤差共分散行列Ｐ^” _ｋ、及び、到着時間差観測値ΔＴ^＊ _ｋ，ｍ（ｍ＝１，…，Ｍ）の情報を取得して、式（２５）により到着時間差予測値を算出する。次に式（２８）及び式（２９）に基づき、予め設定されたｄを用いて、到着時間差観測値が式（３０）を満たすかどうかを判定する。ここで、この判定処理の結果を表す変数として、Ｃｏｒ_ｋ，ｍを用いることとする。判定の結果、式（３０）を満たす場合、Ｃｏｒ_ｋ，ｍに１を設定し、満たさない場合はＣｏｒ_ｋ，ｍに０を設定する。そして時間差相関判定手段１２はＣｏｒ_ｋ，ｍの値を出力する。
【００５５】
測角値選択手段１３は、時間差相関判定手段１２から測角値θ^＊ _ｋ，ｊ（ｊ＝１，…，Ｊ）とＣｏｒ_ｋ，ｍ（ｍ＝１，…，Ｍ）を取得する。続いてＣｏｒ_ｋ，ｍ＝０となるセンサ１−ｉ１と１−ｉ２からの測角値を削除する。その後、削除されなかった測角値について、その数をＪ_ｉｎとし、改めて番号を振りなおし、θ^＊ _ｋ，ｊ（ｊ＝１，…，Ｊ_ｉｎ）を出力する。
【００５６】
次に測角相関判定手段９は、予測手段８より予測ベクトルｘ ^” _ｋと予測誤差共分散行列Ｐ^” _ｋを取得する。また測角値選択手段１３から測角値θ^＊ _ｋ，ｊ（ｊ＝１，…，Ｊ_ｉｎ）を取得する。そして式（３６）に基づき、予測ベクトルｘ ^” _ｋを用いて、測角予測値θ ^” _ｋ，ｊを算出する。さらに式（３７）、式（３８）を用いて、式（３９）を満たすかどうかの判定を行う。式（３９）を満たす場合、θ^＊ _ｋ，ｊを出力する。
【００５７】
続いて測角平滑手段１０は、測角相関判定手段９からθ^＊ _ｋ，ｊを取得する。そして式（４０）〜式（４３）を用いて、平滑処理を行う。さらに処理した測角値θ^＊ _ｋ，ｊが、その時刻の最終測角値でない場合、その平滑ベクトルｘ ^’ _{ｋ， ｊ}及び平滑誤差共分散行列Ｐ^’ _ｋ，ｊを測角相関判定手段９に出力し、その値を用いて、次の測角値に対し相関処理を行う。測角平滑手段１０が、最後の測角値の処理を終了すると、式（４４）及び式（４５）を用いて、ｘ ^’ _ｋとＰ^’ _ｋとを算出する。最後に、測角平滑手段１０は、平滑ベクトル用メモリ６にｘ ^’ _ｋを記憶させ、さらにＰ^’ _ｋを平滑誤差共分散行列用メモリ７に記憶させる。
【００５８】
以上から明らかなように、実施の形態１の移動軌跡計測装置は、到着時間差観測値を、時間差相関判定手段１２によって処理し、時間差相関判定手段１２で到着時間差が有効であると判断されたセンサからの測角値のみを、測角値選択手段１３によって選択することとした。この結果、信頼性の低い測角値を測角平滑処理に用いることがなくなるため、高精度な追尾結果を得ることができる。
【００５９】
なお、実施の形態１において測角時間差選別手段１１は測角手段２の出力と時間差計測手段３の出力が混在した場合に、それらの出力を分類する機能を有するものとしたが、測角手段２の出力が測角値選択手段１３に直接なされ、さらに到着時間差計測手段３の出力が直接時間差相関判定手段１２に直接なされるのであれば、測角時間差選別手段１１は必須の構成要素ではない。
【００６０】
また実施の形態１は、走行トラックを移動する競走馬のような移動体を例に説明した。しかし、その他経路に沿って複数のセンサが配置されている場所（陸上・海上を問わない）を移動する移動体の位置や速度を計測する上でも、実施の形態１による移動軌跡計測装置が適用できることはいうまでもない。
【００６１】
実施の形態２．
実施の形態１による移動軌跡計測装置は、所定の条件に合致しなかった測角値を排除することで、測角平滑処理に用いる測角値の信頼性を向上し、その結果として高精度な追尾結果を得るものであった。これに対して実施の形態２による移動軌跡計測装置は、電波の到着時刻について平滑処理を行う点を特徴とするものである。なお実施の形態２は、実施の形態１と比して、追尾手段４の構成のみが異なるものである。したがって以下では、追尾手段４の詳細な構成と動作についてのみ説明することとする。
【００６２】
図５は、実施の形態２による追尾手段４の詳細な構成を示すブロック図である。図において、時間差平滑手段１４は、移動体から電波がセンサ１−１〜１−Ｉに到着した到着時間差について平滑処理を行うものである。また、実施の形態２では、実施の形態１の測角値選択手段１３を用いない。その他、実施の形態１の追尾手段４と同じ符号を付した構成要素については、実施の形態１と同様であるので説明を省略する。
【００６３】
次に実施の形態２における追尾手段４の動作について説明する。センサ１−１〜１−Ｉが探知した電波について、測角手段２と到着時間差計測手段３が観測値を算出すると、予測手段８は、実施の形態１と同様に、予測ベクトルｘ ^” _ｋと予測誤差共分散行列Ｐ^” _ｋを算出する。そして、測角時間差選別手段１１は、測角値と到着時間差観測値の整理を行い、測角値θ^＊ _ｋ，ｊ（ｊ＝１，…，Ｊ）を測角相関判定手段９に出力し、到着時間差観測値ΔＴ^＊ _ｋ，ｍ（ｍ＝１，…，Ｍ）を、時間差相関判定手段１２に出力する。
【００６４】
続いて、時間差相関判定手段１２は、予測ベクトルｘ ^” _ｋと予測誤差共分散行列Ｐ^” _ｋ、及び到着時間差観測値到着時間差観測値ΔＴ^＊ _ｋ，ｍ（ｍ＝１，…，Ｍ）を取得する。そして式（２５）を用いて到着時間差予測値を算出し、式（２８）、式（２９）、及び式（３０）に従い、予め設定されたｄを用いて、到着時間差観測値ΔＴ^＊ _ｋ，ｍ（ｍ ＝１，．．．，Ｍ）が式（３０）を満たすかどうかの判定を行う。判定の結果、式（３０）を満たす場合、Ｃｏｒ_ｋ，ｍ＝１とし、到着時間差観測値ΔＴ^＊ _ｋ，ｍを、時間差平滑器１４へ出力する。満たさない場合は、Ｃｏｒ_ｋ，ｍ＝０とする。
【００６５】
時間差平滑手段１４は、時間差相関判定手段１２から、到着時間差観測値ΔＴ^＊ _ｋ，ｍ、予測ベクトルｘ ^” _ｋ及び予測誤差共分散行列Ｐ^” _ｋを取得する。その後、まず時刻ｋにおいて初めての到着時間差観測値である場合に、式（２３）及び式（２４）を用いて、ｘ ^’ _{ｋ，ｍ−１}とＰ^’ _{ｋ，ｍ−１}を設定する。そして、式（３１）〜式（３４）に従い、状態ベクトルの推定値ｘ ^’ _ｋ，ｍ及びその誤差共分散行列Ｐ^’ _ｋ，ｍを計算し、時間差相関判定手段１２へ出力する。この一連の動作をすべての到着時間差観測値について行い、時間差平滑手段１４が最後の到着時間差観測値を処理する。さらにそのときの平滑ベクトルｘ ^’ _ｋ，ｍ及びその誤差共分散行列Ｐ^’ _ｋ，ｍを、式（３５）のように設定し、測角相関判定手段９、及び測角平滑手段１０に出力する。
【００６６】
次に測角相関判定手段９は、測角時間差選別手段１１から測角値θ^＊ _ｋ，ｉ（ｉはＩ_{ｏｂｓ，ｋ}の任意の要素）、また、時間差平滑手段１４から、ｘ ^’ _ｋ，ｊ及びその誤差共分散行列Ｐ^’ _ｋ，ｊを取得する。そして、推定ベクトルｘ ^’ _ｋ，ｊを用いて式（３６）から測角予測値θ^＊ _ｋ，ｊを算出する。そして、式（３７）、式（３８）を用いて、式（３９）を満たすかどうかの判定を行う。式（３９）を満たす場合は、θ^＊ _ｋ，ｊを測角平滑手段１０に出力する。
【００６７】
測角平滑手段１０は、測角相関判定手段９から測角値θ^＊ _ｋ，ｊとｘ ^’ _ｋ，ｊ、及びその誤差共分散行列Ｐ^’ _ｋ，ｊを取得すると、式（４０）〜式（４３）による平滑処理を行い、ｘ ^’ _ｋ，ｊ及びＰ^’ _ｋ，ｊを測角相関判定手段９に出力する。測角相関判定手段９と測角平滑手段１０がすべての測角値についての処理を繰り返し行う。測角平滑手段１０が、最後の測角値の処理を終了すると、式（４４）及び式（４５）を用いて、ｘ ^’ _ｋとＰ^’ _ｋとを算出する。最後に、測角平滑手段１０は、平滑ベクトル用メモリ６にｘ ^’ _ｋを記憶させ、さらにＰ^’ _ｋを平滑誤差共分散行列用メモリ７に記憶させる。
【００６８】
以上から明らかなように、実施の形態２の移動軌跡計測装置は、到着時間差観測値について、時間差相関判定手段１２によって相関処理し、さらに時間差平滑手段１４によって平滑処理することとした。この結果、測角値のみを用いる場合に比べ、高精度な追尾結果を得ることができる。
【００６９】
実施の形態３．
なお、実施の形態１による移動軌跡計測装置における測角値選択手段１３と、実施の形態２による移動軌跡計測装置における時間差平滑手段１４の双方を備えるようにしてもよい。図６は、このような移動軌跡計測装置の追尾手段４の構成図である。実施の形態１及び実施の形態２の追尾手段４と同じ符号を付した構成要素については、実施の形態１及び実施の形態２と同様であるので、説明を省略することとする。
【００７０】
次に実施の形態３における追尾手段４の動作について説明する。追尾手段４では、実施の形態２における追尾処理と同様に時間差平滑手段１４による電波の到着時間差の平滑処理を行う。その詳細は実施の形態２と同様である。
【００７１】
さらに加えて、実施の形態３における追尾手段４では、実施の形態１による移動軌跡計測装置における測角値選択手段１３によって、時間差相関結果による測角値の選択処理を行う。具体的には、時間差相関判定手段１２において、式（２８）及び式（２９）に基づき、予め設定されたｄを用いて、到着時間差観測値が式（３０）を満たすかどうかを判定する。次に測角値選択手段１３は、式（３０）を満たす測角値のみを選択して出力する。以降、出力された測角値は測角相関判定手段９に入力され、実施の形態１と同様に平滑値及び平滑誤差共分散行列を算出する。
【００７２】
以上から明らかなように、実施の形態３による移動軌跡計測装置では、到着時間差観測値について、時間差相関判定手段１２が相関処理を行い、時間差平滑手段１４が平滑処理を行うとともに、時間差相関判定手段１２が有効であると判断した到着時間差を算出したセンサからの測角値のみを、測角値選択手段１３が選択することとした。これにより、信頼性の低い測角値を測角平滑処理に用いることがなくなることにより、また測角値のみを用いる場合に比べ、高精度な追尾結果を得ることができる。
【００７３】
実施の形態４．
さらに、追尾処理を行う前に優れた測位精度が得られる観測値のみを選択し、選択された観測値に基づいて追尾処理を行うようにしてもよい。実施の形態４はこのような移動軌跡計測装置に関するものである。
【００７４】
図７は、実施の形態４による移動軌跡計測装置の構成を示すブロック図である。図において、測定値選択手段１５は追尾処理に有意な観測値を選択するものである。ここで、有意な観測値とは、観測値の精度が著しく劣るものや、目標とセンサの距離が長いため、測位にほとんど有用でない観測値などを除いた観測値をいう。その他、図３と同じ符号を付した構成要素については、実施の形態１と同様であるので説明を省略する。また実施の形態４における追尾手段４には、実施の形態１乃至実施の形態３のどの追尾手段を採用してもよい。ただし、実施の形態４における測定値選択手段１５は、観測値を選択する基準として追尾手段４の算出する予測値を用いるので、測定値選択手段１５に追尾手段４における予測手段８の算出結果を入力できるようになっているものとする。
【００７５】
次に測定値選択手段１５の詳細な構成について説明する。図８は、測定値選択手段１５の詳細な構成を示すブロック図である。図において、センサ距離算出手段１６は、移動体とセンサ１−１〜１−Ｉとの距離を算出するものである。センサ選択手段１７は、センサ距離算出手段１６によって算出されたセンサ１−１〜１−Ｉの移動体からの距離が所定値以下の場合に、そのセンサからの観測値を選択するものである。観測値選択手段１８は、測角手段２及び到着時間差計測手段３が出力する観測値のうち、信頼性の高い観測値を選択するものである。
【００７６】
続いて、実施の形態４による移動軌跡計測装置の動作について説明する。なお、実施の形態４における移動軌跡計測装置は実施の形態１と比して、測定値選択手段１５のみが異なるので、以下においては、測定値選択手段１５の動作の説明を中心に行う。
【００７７】
まず測定値選択手段１５は、追尾手段４の出力する予測値ｘ ^” _ｋを取得する。するとセンサ距離算出手段１６は、予測値ｘ ^” _ｋとセンサ１−１〜１−Ｉの位置との距離を算出する。次にセンサ選択手段１７は、センサ距離算出手段１６が算出した各センサ１−１〜１−Ｉの位置と予測値との距離情報、及び、測角値及び到着時間差観測値ΔＴ^＊ _ｋ，ｍを取得する。そして、移動体の位置の予測値との距離が著しく遠いセンサからの測角値及び到着時間差観測値を削除し、残りの観測値を出力する。
【００７８】
観測値選択手段１８は、センサ選択手段１７が出力した観測値を取得する。そして、センサ位置と目標予測値ｘ ^” _ｋの情報を用い、測位精度情報から、優れた推定精度が得られ、かつ無駄のないように観測値の選出を行う。測位精度情報については、例えば、Ｄ． Ｊ． Ｔｏｒｒｉｅｒｉ， ”Ｓｔａｔｉｓｔｉｃａｌ Ｔｈｅｏｒｙ ｏｆ Ｐａｓｓｉｖｅ Ｌｏｃａｔｉｏｎ Ｓｙｓｔｅｍｓ，” ＩＥＥＥ Ｔｒａｎｓ． ｏｎ ＡＥＳ， Ｖｏｌ． ＡＥＳ−２０， Ｎｏ．２， ｐｐ．１８３−ｐｐ．１９８， Ｍａｒ １９８４．に示されている方法を用いて算出を行う。
【００７９】
上記方法による測位精度情報から、優れた測位精度の得られる観測値を選択して、残りの観測値を削除する。そして、選択された測角値及び到着時間差観測値を観測値を追尾手段４に出力する。以後、実施の形態１乃至実施の形態３と同様の処理を行う。
【００８０】
以上から明らかなように、実施の形態４の移動軌跡計測装置によれば、予め有意な観測値のみを選択してから追尾処理を行うようにするので、計算負荷の軽減が可能となり、また追尾処理の精度を向上させることができる。
【００８１】
なお、距離情報に基づく有意な観測値の選択処理、及び、測位精度情報による観測値の選択処理は、それぞれ単独で用いても同様の効果を奏することはいうまでもない。
【００８２】
【発明の効果】
この発明に係る移動軌跡計測装置は、移動体から到着した電波の測角値に加えて、前記複数個のセンサの各々へ到着する前記電波の到着時間差を計測する到着時間差計測手段と、前記到着時間差計測手段が出力する到着時間差と測角値に基づいて移動体の位置及び速度を追尾する追尾手段とを備えることにより、測角値のみでは高い精度が得られない位置に移動体が存在する場合にも、位置と速度の測定を高精度に行うことができるという効果を奏するものである。
【図面の簡単な説明】
【図１】この発明及び従来技術によるセンサと移動体の関係を示す説明図である。
【図２】従来技術による測定誤差の説明図である。
【図３】この発明の実施の形態１の構成を示すブロック図である。
【図４】この発明の実施の形態１の追尾手段４の構成を示すブロック図である。
【図５】この発明の実施の形態２の追尾手段４の構成を示すブロック図である。
【図６】この発明の実施の形態３の追尾手段４の構成を示すブロック図である。
【図７】この発明の実施の形態４の構成を示すブロック図である。
【図８】この発明の実施の形態４の測定値選択手段の構成を示すブロック図である。
【符号の説明】
１−１〜１−Ｉ：センサ、２：測角手段、３：到着時間差計測手段、
４：追尾手段、５：初期値設定手段、６：平滑ベクトル用メモリ、
７：平滑誤差共分散行列用メモリ、８：予測手段、９：測角相関判定手段、
１０：測角平滑手段、１１：測角時間差選別手段、１２：時間差相関判定手段、
１３：測角値選択手段、１４：時間差平滑手段、１５：測定値選択手段、
１６：センサ選択手段、１７：不要観測値削除手段、１８：観測値選択手段[0001]
TECHNICAL FIELD OF THE INVENTION
BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a movement trajectory measuring device that measures the position and speed of a moving object such as a racehorse, bicycle, boat carrier, automobile, etc. at each time by using radio waves, and in particular, in addition to the arrival direction of radio waves, The present invention relates to a movement trajectory measuring device that improves the accuracy of a measured value by using a difference in arrival time at a sensor.
[0002]
[Prior art]
Conventionally, as a method of measuring the position and speed of a moving object such as a racehorse at a racetrack or a boat at a boat racetrack, a plurality of sensors are arranged on a route side band as shown in FIG. There has been a method in which the arrival angles of radio waves arriving from a moving body are obtained and calculated based on these arrival angles. This is based on the principle that a moving object exists at a point where straight lines forming the arrival angles of radio waves at a plurality of sensors intersect (for example, Patent Document 1).
[0003]
[Patent Document 1]
Japanese Patent Application Laid-Open No. H11-248830, "Traveling route measuring device" (FIG. 1, page 3 to page 14)
[0004]
[Problems to be solved by the invention]
There is a measurement error in the measurement of the arrival angle. FIG. 2 is a diagram conceptually showing a position calculation error due to the measurement error. In the figure, a point A is a point sufficiently distant from a straight line connecting the sensor 101 and the sensor 102. When a moving object is present at the point A, the direction of radio wave arrival at the sensor 101 is ideally the direction of a line connecting the point A and the sensor 101.
[0005]
However, in reality, due to an angle measurement error, the result of detecting the arrival direction of the radio wave has a spread such as the angle formed by the line segment a1 and the line segment a2. Similarly, also in the sensor 102, the angle measurement result has a spread such as an angle formed by the line segment b1 and the line segment b2. For this reason, the position of the moving object is detected with an accuracy that makes the square A1 surrounded by the line segments a1, a2, b1, and b2 the maximum error. If the moving body is located at a point B where the distance between the straight line connecting the sensor 101 and the sensor 102 is short, and taking into account the angle measurement error as in the case of the point A, the measurement error of the position of the moving body is the square B1 As a result, the error becomes larger than in the case of the point A.
[0006]
The present invention has been made in order to solve the above-described problem. In addition to the arrival direction of radio waves, a moving object exists at a position close to a sensor using a difference in arrival time of radio waves from the moving object to each sensor. Even in such a case, the position and speed are accurately measured.
[0007]
[Means for Solving the Problems]
A trajectory measurement device according to the present invention includes a plurality of sensors that receive radio waves from a moving object,
Arrival time difference measuring means for measuring an arrival time difference between the radio waves arriving at the plurality of sensors,
Angle measuring means for measuring an azimuth at which the radio wave arrives at the plurality of sensors,
A tracking unit that tracks the moving state of the moving object based on the measurement value output by the arrival time difference measuring unit and the angle measurement unit, and outputs a smoothed value and a predicted value of the moving state.
[0008]
BEST MODE FOR CARRYING OUT THE INVENTION
Hereinafter, embodiments of the present invention will be described.
Embodiment 1 FIG.
FIG. 3 is a block diagram showing a configuration of the movement trajectory measuring device according to the first embodiment of the present invention. In the figure, sensors 1-1 to 1-I detect radio waves arriving from a moving body, and are arranged around a moving path of the moving body. For example, it may be arranged along a running track of a racetrack as shown in FIG. 1 or may be arranged on a roadside zone of an expressway. Here, the moving object means an object (including a creature) that moves on a certain range of a track, such as a racehorse, a bicycle, a boat, a car, and the like. The radio wave from the moving body may be a radio wave transmitted by a transmitter mounted on the moving body itself, or a radar wave source is provided for each of the sensors 1-1 to 1-I. The reflected light may be a reflected wave obtained by reflecting a beam radiated from a moving body.
[0009]
Subsequently, the angle measuring means 2 acquires the direction in which radio waves from the moving body arrive at the sensors 1-1 to 1-I. The arrival time difference measuring means 3 obtains a difference between times when radio waves arrive at the sensors 1-1 to 1-I from a moving body. The tracking means 4 is for tracking the moving object from the outputs of the angle measuring means 2 and the arrival time difference measuring means 3.
[0010]
Next, FIG. 4 is a block diagram showing a detailed configuration of the tracking means 4. In the figure, an initial value setting means 5 sets an initial value of a smooth vector of a Kalman filter and a smooth error covariance matrix. The smoothed vector memory 6 is a storage device that stores a calculated value of the smoothed vector. The smoothing error covariance matrix memory 7 is a storage device that stores a calculated value of the smoothing error covariance matrix. The prediction means 8 performs a process of predicting the motion data of the moving object based on the smoothed vector stored in the smoothed vector memory 6 and the smoothed error covariance matrix stored in the smoothed error covariance matrix memory 7. . The angle measurement correlation determination means 9 determines whether or not the input angle measurement value is valid data for tracking processing. The angle measurement smoothing unit 10 performs a smoothing process using the angle measurement value input from the angle measurement correlation determination unit 9.
[0011]
Subsequently, the angle measuring time difference selecting means 11 has a mixture of the angle measuring value output by the angle measuring means 2 outside the tracking means 4 and the arrival time difference observed value output by the arrival time difference measuring means 3 on the memory. In such a case, each is classified and output. The time difference correlation determination means 12 determines whether or not the arrival time difference observation value output by the angle measurement time difference selection means 11 is valid data for tracking processing. The angle measurement value selection means 13 selects only angle measurement values from the sensors that output the arrival time difference determined to be valid by the time difference correlation determination means 12 among the sensors 1-1 to 1-I.
[0012]
Next, prior to the description of the operation of the first embodiment, the principle of the moving object tracking method according to the present invention will be described. First, the outline is as follows. That is, first, radio waves transmitted from the moving object are received by a plurality of sensors in the racetrack. Next, the extended Kalman filter is used based on the azimuth angle of the moving body as viewed from each sensor and the instantaneous observation result of the difference in the arrival time of the radio wave from each sensor to the moving body. Track position and speed. The algorithm of the extended Kalman filter is detailed in, for example, "Applied Kalman Filter" by Toru Katayama.
[0013]
In the following, for the sake of explanation, a tracking processing method for a single moving object such as a racehorse racing on a running track in a competition field will be described. However, if this processing is executed for each moving object, tracking of a plurality of moving objects will be described. Is possible.
[0014]
Next, the principle will be described in detail. As shown in FIG. 1, a fixed xy orthogonal coordinate system is set in a racetrack. The sensors 1-1 to 1-I are installed at predetermined intervals along a traveling track. Further, among the sensors 1-1 to 1-I, the i-th sensor is represented as 1-i (i = 1,..., I), and the position thereof is [X_i, Y_i]^T(Hereinafter T represents transposition of the matrix). Also, the sampling time since the race started is t_k(K = 0, 1, 2,...).
[0015]
A certain sampling time t_kIn, it is all or a part of the I sensors that detects a radio wave from a moving object. This set of sensor numbers is_{obs, k}Call. I_{obs, k}Is the number of sensors included in N (I_{obs, k}). Sampling time t_k, The sensor 1-i (where i is I_{obs, k}The azimuth of the moving object as seen from the angle θ_{k, i}(I is I_{obs, k}Any element). Azimuth angle θ_kj(I is I_{obs, k}As shown in FIG. 1, the clockwise rotation based on a direction parallel to the y-axis is defined as positive.
[0016]
The sampling time t_kIn, consider obtaining the time difference between arrival from the transmitter to the two sensors. At this time, a set of pairs of numbers of two sensors from which the arrival time difference is obtained is M_{obs, k}And Sampling time t_kHere, the sensors 1-i1 and 1-i2 (where i1 and i2 are M_{obs, k}The arrival time difference of any two elements of the_{k, (i1, i2)}(I1 and i2 are M_{obs, k}Arbitrary two elements). In addition, the number of arrival time differences is represented by N (M_{obs, k}).
[0017]
Sampling time t_kVector of the moving object given by the true value of the position and velocity of the moving object atx _kIs defined by Expression (1). Where dx_k/ Dt, dy_k/ Dt indicates each time derivative.
(Equation 1)

[0018]
State vectorx _kIs used to define a motion model (referred to as a constant-velocity straight-traveling model) when the moving body is linearly traveling at a substantially constant speed on a straight course as shown in Expression (2).
(Equation 2)

Where Φ_kIs a state transition matrix of constant-velocity linear motion, and is given by Expression (3).
(Equation 3)

[0019]
In equation (2),w _kIs a driving noise vector corresponding to acceleration for expressing an error caused by regarding the actual motion of the moving object as a constant-velocity straight-line motion, and has a normality having the properties of Expressions (4) and (5). Assume that the noise is white noise.
(Equation 4)

(Equation 5)

Here, E [] is a symbol representing the collective average.
[0020]
Also_kIs a drive noise conversion matrix, which is represented by equation (6).
(Equation 6)

[0021]
Next, an azimuth angle observation model and an arrival time difference (arrival distance difference) observation model are defined as observation models. First, an observation model of the arrival time difference (arrival distance difference) is defined. Sampling time t_k, The true value ΔT of the difference between the times (arrival times) at which the radio waves from the moving object are detected by the sensors 1-i1 and 1-i2_{k, (i1, i2)}Is the state vector of the moving objectx _kIs represented by the following equation.
(Equation 7)

Where h_{(I1, i2)}(x _k) Is as follows.
(Equation 8)

Here, c is the speed of light.
[0022]
Therefore, the observation model of the arrival time difference between the sensors 1-i1 and 1-i2 is defined by Expression (9).
(Equation 9)

[0023]
Where ΔT^* _{k, (i1, i2)}Is the observed value of the difference between the arrival times of the transmission signals from the moving object to the sensors 1-i1 and 1-i2, and is calculated by the arrival time difference measuring means 3 based on the radio waves detected by the sensors 1-i1 and 1-i2. Is what is done. Also ν_{ΔT, k, (i1, i2)}Represents the observation noise of the arrival time difference. Observation noise ν_{ΔT, k, (i1, i2)}Is normal white noise having the properties of Expressions (10) and (11).
(Equation 10)

(Equation 11)

[0024]
Next, an azimuth observation model is defined. Sampling time t_k, The true value θ of the azimuth of the moving body as viewed from the sensor 1-i_{k, i}Is the state vector of the moving objectx _kIs represented by the following equation.
(Equation 12)

Where f_i(x _k) Is based on equation (13).
(Equation 13)

[0025]
Therefore, the observation model of the sensor 1-i is defined by Expression (14).
[Equation 14]

Where θ^* _{k, i}Is an angle measurement value output by the angle measurement means 2 based on a radio wave detected by the sensor 1-i. Also ν_{θ, k, i}Represents the observation noise of this angle measurement value. Observation noise ν_{θ, k, i}Is normal white noise having the properties of Expressions (15) and (16).
[Equation 15]

(Equation 16)

[0026]
Next, in the present invention, a moving object tracking processing method performed by using an extended Kalman filter algorithm based on the above-described motion model and observation model will be described. First, variables are defined as follows.
[Equation 17]

(Equation 18)

[Equation 19]

(Equation 20)

[0027]
here,x ^' _kIs the sampling time t_kSampling time t based on observation information up to_kIs a smooth vector obtained by estimating the state vector. Alsox ^" _kIs the sampling time t_k-1Sampling time t based on observation information up to_kIs a prediction vector obtained by estimating the state vector. Further P^' _k, P^" _kIs the error covariance matrix of the smooth vector and the prediction vector, respectively.
[0028]
The start time of the race is set to the initial sampling time t₀And the initial value x of the smooth vector^' ₀And the initial value P of the smooth error covariance matrix^' ₀Is set in advance. Here, the initial value of the smoothing vector includes the target observation position calculated from the observation result obtained in the first sample and the next two samples as the initial value of the smoothing vector as in the case of normal target tracking. Based on the observation positions of the two target points calculated from the observation results obtained by eyes, the speed calculated from the displacement between these two points and the time interval is set.
[0029]
Thereafter, at time t_kEach time a new observation value is obtained from any of the sensors 1-1 to 1-I, the sampling time is advanced by one and the tracking process is performed. Below, sampling time t_kThe following describes a specific method of the tracking processing executed in step (1). Sampling time t_kIn, when the angle measurement value or the arrival time difference observation value is obtained, first, the following prediction processing is performed according to the processing algorithm of the extended Kalman filter.
(Equation 21)

(Equation 22)

[0030]
Next, a correlation determination and a smoothing process are performed for each of the obtained observation values, and M = N (M_{obs, k}) Times, J = N (I_{obs, k}) Times, a total of M + J times. There is no particular priority or dependency between the correlation determination and smoothing processing for the arrival time difference observation value and the correlation determination and smoothing processing for the angle measurement value. However, in the following, for the sake of simplicity, first, there is no particular priority in the order of processing the arrival time difference observation value and the angle measurement value obtained at the same time. That is, the order of processing may be switched between the arrival time difference observation values and the angle measurement values obtained at the same time, or the order of processing may be switched between the arrival time difference and the angle measurement value. However, in the following and in the embodiments, for simplicity of description, first, correlation determination and smoothing processing are performed M times on arrival time difference observation values, and then correlation determination and smoothing processing are performed J times on angle measurement values. It shall be.
[0031]
Hereinafter, in order to simplify the notation, the M arrival time difference observation values are represented by ΔT^* _{k, 1}, ..., ΔT^* _{k, M}It is written. Each value is obtained from a certain sensor pair, and if necessary, ΔT^* _{k, 1}, ..., ΔT^* _{k, M}And ΔT^* _{k, (i1, i2)}, ..., ΔT^* _{k, (i2M-1, i2M)}Is used together.
Thereafter, if necessary, for example, when the mth arrival time difference observation value at time k is obtained from the sensors a and b, ΔT^* _{k, m}And ΔT^* _{(A, b)}Is used together. The same applies to other variables.
[0032]
Time t_kAt the m-th observed time difference of arrival ΔT^* _{k, m}The estimated value of the state vector obtained as a result of smoothingx ^' _{k, m}And its error covariance matrixP ^' _{k, m}It expresses. First, as an initial value when m = 0, a prediction vector and a prediction error covariance matrix are set as shown in Expressions (23) and (24).
(Equation 23)

[Equation 24]

[0033]
After setting the initial values, the following arrival time difference correlation determination processing and arrival time difference smoothing processing are repeatedly performed for m = 1,.
[0034]
First, a correlation determination is performed for the acquired arrival time difference observation value. That is, it is determined whether or not the arrival time difference observation value is data corresponding to the arrival time difference value obtained from the mobile that has already been tracked. The arrival time difference correlation determination processing has an effect of excluding extremely low precision arrival time difference observation values in performing the tracking processing. As a case where such an inaccurate arrival time difference observation value occurs, for example, the sensor may generate an indirect wave that is reflected by another structure and then arrives at the sensor together with the direct wave of the radio wave transmitted from the moving body. It is possible that the signal was received after interference. In the arrival time difference correlation determination process, first,x ^' _{k, m-1}And the predicted arrival time difference ΔT of the mobile object according to equation (25).^" _{k, m}Is calculated.
(Equation 25)

[0035]
Next, the difference (called a residual) ΔT between the actual arrival time difference observed value and the predicted arrival time difference value obtained as described above.^* _{k, m}−ΔT^" _{k, m}Time difference observation value ΔT based on the magnitude of^* _{k, m}Judge the validity of According to equations (9) and (25), the residual ΔT^* _{k, m}−ΔT ”_{k, m}Is as follows:
(Equation 26)

[0036]
vectorx ^' _{k, m-1}And true valuex _kIs small, the first term on the right side of equation (26) can be approximated as in equation (27).
[Equation 27]

Where H_m(x ^' _{k, m-1}) Is the function h of equation (8), as shown in equation (28)._m(x _k) Of the gradientx ^' _{k, m-1}Is the value at.
[Equation 28]

Note that X_kIs x_kIs differentiated with respect to time (dx_k/ Dt). Also Y_kThe same applies to (dy_k/ Dt).
[0037]
By using the equations (11), (27), and (28) in the above equation (26), ΔT^* _{k, m}−ΔT^" _{k, m}Variance s_{k, m}Can be calculated as follows:
(Equation 29)

[0038]
In the arrival time difference correlation determination process, the variance s_{k, m}And the arrival time difference observed value ΔT ′ is used only when Expression (30) is satisfied using d as a parameter._{k, m}Is determined to be valid.
[Equation 30]

[0039]
By the above-described correlation determination processing, the arrival time difference observed value ΔT^* _{k, j}Is determined to be valid, the following equations (31), (32), and (33) are calculated according to the algorithm of the extended Kalman filter. Here, I is a 4 × 4 unit matrix.
[Equation 31]

(Equation 32)

[Equation 33]

[0040]
On the other hand, the arrival time difference observed value ΔT^' _{k, m}Is determined to be invalid, the smoothing process using the arrival time difference observation value is not performed, and the smoothed value and the smoothed error covariance matrix are replaced with the predicted value and the forecasted error covariance matrix as shown in the following equation. .
[Equation 34]

[0041]
The above-described series of processing of Expressions (25) to (34) is performed with m = 1,. . . , M are repeatedly executed. The above is the arrival time difference observed value ΔT^' _{k, m}4 shows the contents of the correlation determination and the smoothing process using.
[0042]
Next, correlation determination and smoothing processing are performed on the angle measurement values. First, the final smoothed value of the arrival time difference is set as follows.
(Equation 35)

[0043]
Next, the angle measurement value θ is calculated using the observation model equations (12) to (16).^* _{k, j}The following processing is performed for (j = 1,..., J) in the same manner as in the case of the arrival time difference observation value. First, x^' _{k, j-1}And the predicted azimuth θ ″ of the moving object according to equation (36)._{k, j}Is calculated.
[Equation 36]

[0044]
The difference between the actual angle measurement value and the obtained predicted orientation (referred to as residual) θ^* _{k, j}−θ^" _{k, j}Angle measurement θ based on the size of^* _{k, j}Judge the validity of As with the arrival time difference, θ^* _{k, j}−θ^" _{k, j}Variance s_{k, j}Is calculated using Expression (37) and Expression (38).
(37)

[Equation 38]

[0045]
In the angle measurement correlation determination process, the variance s_{k, j}This angle measurement value θ is used only when Expression (39) is satisfied using d as a parameter.^* _{k, j}Is determined to be valid.
[Equation 39]

[0046]
By the above-described correlation determination processing, the angle measurement value θ^' _{k, j}Is determined to be valid, the following equations (40), (41), and (42) are calculated according to the algorithm of the extended Kalman filter. Here, I is a 4 × 4 unit matrix.
(Equation 40)

(Equation 41)

(Equation 42)

[0047]
On the other hand, the angle measurement value θ^* _{k, j}Is not valid, the smoothing process using the angle measurement value is not performed, and the smoothed value and the smoothed error covariance matrix are replaced with the predicted value and the predicted error covariance matrix as shown in Expression (43). .
[Equation 43]

The above-described series of processing of Expressions (36) to (43) is performed by j = 1,. . . , J repeatedly.
[0048]
As a result, the finally obtained x^' _{k, J}And P^' _{k, J}At the sampling time t as shown in equations (44) and (45)._kAt the sampling time t_kIs completed.
[Equation 44]

[Equation 45]

[0049]
The above is the principle of the moving object tracking method according to the present invention. Next, the operation of the movement trajectory measuring device according to the first embodiment will be described based on the above principle.
[0050]
First, the tracking means 4 sets the initial sampling time, which is the tracking start time, to t.₀And First, the initial value setting means 5 calculates a smoothed value based on the theory of a normal Kalman filter.x ^' _kInitial value ofx ^' ₀, And the smooth error covariance matrix P^' _kInitial value P^' ₀Is calculated, and the tracking process is started.
[0051]
Next, at a certain time t_kWhen the sensors 1-1 to 1-i detect a radio wave from the moving body, the angle measuring means 2 calculates an angle measurement value based on the radio wave arriving from the moving body. Further, the arrival time difference measuring means 3 calculates the arrival time difference of the radio wave arriving from the moving body. The tracking means 4 acquires the angle measurement value calculated by the angle measurement means 2 and the arrival time difference calculated by the arrival time difference measurement means 3 as observation values, and performs tracking processing by advancing the sampling time by one. Hereinafter, the sampling time t_kWill be described.
[0052]
When the tracking means 4 acquires the observation value, the prediction means 8 sends the smoothed vector from the smoothed vector memory 6 one sample before.x ^' _k-1And the smoothed error covariance matrix P one sample before the smoothed error covariance matrix memory 7^' _k-1Is read. The predicting means 8 calculates a driving noise covariance matrix Q_kAnd the prediction vector based on equations (21) and (22)x ^" _kAnd the prediction error covariance matrix P^" _kIs calculated.
[0053]
Here, sampling time t_kAnd J = N (I_{obs, k}) Angle measurement values θ^* _{k, j}(J = 1,..., J) and M = N (M_{obs, k}) Observations of arrival time difference ΔT^* _{k, m}(M = 1,..., M) are obtained. Then, the angle measurement time difference selection means 11 classifies the angle measurement value in the memory and the arrival time difference observation value. As a result, the angle measurement value θ^* _{k, j}(J = 1,..., J) is output to the angle measurement value selection means 13 and the arrival time difference observed value ΔT^* _{k, m}(M = 1,..., M) are output to the time difference correlation determination means 12.
[0054]
The time difference correlation determination means 12 calculates the prediction vectorx ^" _k, The prediction error covariance matrix P^" _k, And arrival time difference observed value ΔT^* _{k, m}The information of (m = 1,..., M) is acquired, and the arrival time difference predicted value is calculated by Expression (25). Next, based on Expressions (28) and (29), it is determined whether or not the observed arrival time difference satisfies Expression (30) using d set in advance. Here, as a variable representing the result of this determination processing, Cor_{k, m}Shall be used. If the result of the determination satisfies Expression (30), Cor_{k, m}Is set to 1, and if not satisfied, Cor_{k, m}Is set to 0. Then, the time difference correlation determination means 12_{k, m}The value of is output.
[0055]
The angle measurement value selection means 13 receives the angle measurement value θ from the time difference correlation determination means 12.^* _{k, j}(J = 1, ..., J) and Cor_{k, m}(M = 1,..., M). Then Cor_{k, m}The angle measurement values from the sensors 1-i1 and 1-i2 for which = 0 are deleted. Then, for the angle measurement values that have not been deleted,_inAnd renumbered again, θ^* _{k, j}(J = 1, ..., J_in) Is output.
[0056]
Next, the angle-measurement correlation judging means 9 sends the prediction vectorx ^" _kAnd the prediction error covariance matrix P^" _kTo get. In addition, the angle measurement value θ^* _{k, j}(J = 1, ..., J_in) To get. Then, based on equation (36), the prediction vectorx ^" _kIs used to estimate the angle measurementθ ^" _{k, j}Is calculated. Further, it is determined whether Expression (39) is satisfied by using Expressions (37) and (38). When equation (39) is satisfied, θ^* _{k, j}Is output.
[0057]
Subsequently, the angle measurement smoothing means 10 sends the angle measurement correlation determination means 9^* _{k, j}To get. Then, a smoothing process is performed using Expressions (40) to (43). Further processed angle value θ^* _{k, j}Is not the last angle measurement at that time, its smooth vectorx ^' _{k, j}And the smooth error covariance matrix P^' _{k, j}Is output to the angle-measuring correlation determining means 9 and the value is used to perform a correlation process on the next angle-measuring value. When the angle measurement smoothing means 10 completes the processing of the last angle measurement value, using the equations (44) and (45),x ^' _kAnd P^' _kIs calculated. Finally, the angle measurement smoothing means 10 stores thex ^' _kAnd furthermore, P^' _kIs stored in the smoothing error covariance matrix memory 7.
[0058]
As is clear from the above description, the movement trajectory measuring device of the first embodiment processes the arrival time difference observation value by the time difference correlation determining means 12, and the time difference correlation determining means 12 determines that the arrival time difference is valid. Is selected by the angle measurement value selection means 13. As a result, since a low-reliability angle measurement value is not used for the angle measurement smoothing process, a highly accurate tracking result can be obtained.
[0059]
In the first embodiment, when the output of the angle measuring unit 2 and the output of the time difference measuring unit 3 are mixed, the angle measuring time difference selecting unit 11 has a function of classifying the outputs. 2 is output directly to the angle measurement value selection means 13 and the output of the arrival time difference measurement means 3 is directly output to the time difference correlation determination means 12, the angle measurement time difference selection means 11 is not an essential component. .
[0060]
In the first embodiment, a moving body such as a racehorse moving on a running track has been described as an example. However, the moving trajectory measuring device according to the first embodiment is also applicable to measuring the position and speed of a moving object moving at a place (regardless of whether it is on land or at sea) where a plurality of sensors are arranged along a route. It goes without saying that you can do it.
[0061]
Embodiment 2 FIG.
The movement trajectory measuring device according to the first embodiment improves the reliability of the angle measurement values used for the angle measurement smoothing process by eliminating the angle measurement values that do not match the predetermined condition, and as a result, achieves a highly accurate measurement. The tracking result was obtained. On the other hand, the moving trajectory measuring apparatus according to the second embodiment is characterized in that smoothing processing is performed on the arrival time of radio waves. Note that the second embodiment is different from the first embodiment only in the configuration of the tracking means 4. Therefore, only the detailed configuration and operation of the tracking means 4 will be described below.
[0062]
FIG. 5 is a block diagram showing a detailed configuration of the tracking means 4 according to the second embodiment. In the figure, a time difference smoothing means 14 performs a smoothing process on an arrival time difference when a radio wave arrives at a sensor 1-1 to 1-I from a moving body. In the second embodiment, the angle measurement value selection unit 13 of the first embodiment is not used. The other components having the same reference numerals as those of the tracking means 4 of the first embodiment are the same as those of the first embodiment, and the description thereof will be omitted.
[0063]
Next, the operation of the tracking means 4 according to the second embodiment will be described. When the angle measuring means 2 and the arrival time difference measuring means 3 calculate the observed value for the radio waves detected by the sensors 1-1 to 1-I, the predicting means 8 sets the prediction vector as in the first embodiment.x ^" _kAnd the prediction error covariance matrix P^" _kIs calculated. Then, the angle measurement time difference selection means 11 sorts the angle measurement value and the arrival time difference observation value, and calculates the angle measurement value θ.^* _{k, j}(J = 1,..., J) is output to the angle-measuring correlation determining means 9 and the arrival time difference observed value ΔT^* _{k, m}(M = 1,..., M) are output to the time difference correlation determining means 12.
[0064]
Subsequently, the time difference correlation determining means 12 calculates the prediction vectorx ^" _kAnd the prediction error covariance matrix P^" _k, And arrival time difference observation value arrival time difference observation value ΔT^* _{k, m}(M = 1,..., M). Then, the estimated arrival time difference is calculated using the equation (25), and according to the equations (28), (29) and (30), using the preset d, the arrival time difference observed value ΔT^* _{k, m}It is determined whether (m = 1,..., M) satisfies Expression (30). If the result of the determination satisfies Expression (30), Cor_{k, m}= 1 and arrival time difference observed value ΔT^* _{k, m}Is output to the time difference smoother 14. If not, Cor_{k, m}= 0.
[0065]
The time difference smoothing means 14 receives the arrival time difference observed value ΔT^* _{k, m}, Prediction vectorx ^" _kAnd the prediction error covariance matrix P^" _kTo get. Then, first, at time k, if it is the first arrival time difference observed value, using equations (23) and (24),x ^' _{k, m-1}And P^' _{k, m-1}Set. Then, according to equations (31) to (34), the estimated value of the state vectorx ^' _{k, m}And its error covariance matrix P^' _{k, m}Is calculated and output to the time difference correlation determination means 12. This series of operations is performed for all the arrival time difference observation values, and the time difference smoothing means 14 processes the last arrival time difference observation value. Furthermore, the smooth vector at that timex ^' _{k, m}And its error covariance matrix P^' _{k, m}Is set as in Expression (35), and is output to the angle measurement correlation determination means 9 and the angle measurement smoothing means 10.
[0066]
Next, the angle-measuring correlation determining means 9 receives the angle-measuring value θ from the angle-measuring time difference selecting means 11.^* _{k, i}(I is I_{obs, k}From the time difference smoothing means 14,x ^' _{k, j}And its error covariance matrix P^' _{k, j}To get. And the estimated vectorx ^' _{k, j}From equation (36), the angle measurement predicted value θ^* _{k, j}Is calculated. Then, it is determined whether Expression (39) is satisfied by using Expressions (37) and (38). When equation (39) is satisfied, θ^* _{k, j}Is output to the angle measurement smoothing means 10.
[0067]
The angle measurement smoothing means 10 receives the angle measurement value θ from the angle measurement correlation determination means 9.^* _{k, j}Whenx ^' _{k, j}, And its error covariance matrix P^' _{k, j}Is obtained, a smoothing process is performed by Expressions (40) to (43),x ^' _{k, j}And P^' _{k, j}Is output to the angle measurement correlation determination means 9. The angle-measuring correlation determining means 9 and the angle-measuring smoothing means 10 repeat the processing for all the angle-measuring values. When the angle measurement smoothing means 10 completes the processing of the last angle measurement value, using the equations (44) and (45),x ^' _kAnd P^' _kIs calculated. Finally, the angle measurement smoothing means 10 stores thex ^' _kAnd furthermore, P^' _kIs stored in the smoothing error covariance matrix memory 7.
[0068]
As is clear from the above description, the movement trajectory measuring device according to the second embodiment performs the correlation process on the observed arrival time difference by the time difference correlation determination unit 12 and further performs the smoothing process by the time difference smoothing unit 14. As a result, a more accurate tracking result can be obtained as compared with the case where only the angle measurement value is used.
[0069]
Embodiment 3 FIG.
Note that both the angle measurement value selecting unit 13 in the moving locus measuring device according to the first embodiment and the time difference smoothing unit 14 in the moving locus measuring device according to the second embodiment may be provided. FIG. 6 is a configuration diagram of the tracking means 4 of such a movement trajectory measuring device. The components denoted by the same reference numerals as those of the tracking means 4 of the first and second embodiments are the same as those of the first and second embodiments, and thus the description thereof will be omitted.
[0070]
Next, the operation of the tracking means 4 in the third embodiment will be described. The tracking means 4 performs the smoothing processing of the arrival time difference of the radio wave by the time difference smoothing means 14 similarly to the tracking processing in the second embodiment. The details are the same as in the second embodiment.
[0071]
In addition, in the tracking means 4 according to the third embodiment, the angle measurement value selecting means 13 in the movement trajectory measuring device according to the first embodiment performs an angle measurement value selection process based on the time difference correlation result. More specifically, the time difference correlation determining means 12 determines whether or not the arrival time difference observation value satisfies Expression (30), using d set in advance, based on Expressions (28) and (29). Next, the angle measurement value selection unit 13 selects and outputs only the angle measurement values that satisfy Expression (30). Thereafter, the output angle measurement value is input to the angle measurement correlation determination unit 9 and calculates a smoothed value and a smoothed error covariance matrix as in the first embodiment.
[0072]
As is apparent from the above description, in the movement trajectory measuring device according to the third embodiment, the time difference correlation determination means 12 performs the correlation process on the arrival time difference observed value, the time difference smoothing means 14 performs the smoothing process, and the time difference correlation determination means The angle measurement value selection means 13 selects only the angle measurement value from the sensor that has calculated the arrival time difference that is determined to be valid. This eliminates the use of a low-reliability angle measurement value for the angle measurement smoothing process, and enables a more accurate tracking result to be obtained as compared with a case where only the angle measurement value is used.
[0073]
Embodiment 4 FIG.
Furthermore, before performing the tracking process, only the observation values that provide excellent positioning accuracy may be selected, and the tracking process may be performed based on the selected observation values. Embodiment 4 relates to such a movement locus measuring device.
[0074]
FIG. 7 is a block diagram showing a configuration of a movement locus measuring device according to the fourth embodiment. In the figure, a measurement value selection means 15 selects an observation value that is significant for tracking processing. Here, a significant observation value refers to an observation value excluding observation values that are not very useful for positioning because the accuracy of the observation value is extremely poor or the distance between the target and the sensor is long. The other components denoted by the same reference numerals as those in FIG. 3 are the same as those in the first embodiment, and a description thereof will not be repeated. Also, any of the tracking units of the first to third embodiments may be adopted as the tracking unit 4 in the fourth embodiment. However, since the measured value selecting means 15 in the fourth embodiment uses the predicted value calculated by the tracking means 4 as a reference for selecting an observed value, the measured value selecting means 15 uses the calculated result of the predicting means 8 in the tracking means 4 for the measured value selecting means 15. It is assumed that input can be performed.
[0075]
Next, a detailed configuration of the measurement value selection unit 15 will be described. FIG. 8 is a block diagram illustrating a detailed configuration of the measurement value selection unit 15. In the figure, a sensor distance calculating means 16 calculates a distance between a moving body and sensors 1-1 to 1-I. The sensor selecting unit 17 selects an observation value from the sensor 1-1 to 1-1-I when the distance from the moving body calculated by the sensor distance calculating unit 16 is equal to or less than a predetermined value. The observation value selection unit 18 selects an observation value with high reliability from the observation values output by the angle measurement unit 2 and the arrival time difference measurement unit 3.
[0076]
Next, the operation of the movement trajectory measuring device according to the fourth embodiment will be described. Note that the movement trajectory measuring device according to the fourth embodiment differs from the first embodiment only in the measured value selecting unit 15, and therefore the following description focuses on the operation of the measured value selecting unit 15.
[0077]
First, the measured value selecting unit 15 calculates the predicted value output from the tracking unit 4.x ^" _kTo get. Then, the sensor distance calculating means 16 calculates the predicted valuex ^" _kAnd the distance between the sensor 1-1 and the position of the sensor 1-1 are calculated. Next, the sensor selecting means 17 calculates the distance information between the position of each of the sensors 1-1 to 1-I calculated by the sensor distance calculating means 16 and the predicted value, and the angle measurement value and the arrival time difference observed value ΔT.^* _{k, m}To get. Then, the angle measurement value and the arrival time difference observation value from the sensor whose distance from the predicted value of the position of the moving object is extremely long are deleted, and the remaining observation values are output.
[0078]
The observation value selection unit 18 acquires the observation value output by the sensor selection unit 17. Then, the sensor position and the target predicted valuex ^" _kUsing this information, an excellent estimation accuracy is obtained from the positioning accuracy information, and observation values are selected without waste. Regarding the positioning accuracy information, for example, D.I. J. Torreri, "Statistical Theory of Passive Location Systems," IEEE Trans. on AES, Vol. AES-20, No. 2, pp. 183-pp. 198, Mar 1984. The calculation is performed using the method described in (1).
[0079]
From the positioning accuracy information obtained by the above method, an observation value with excellent positioning accuracy is selected, and the remaining observation values are deleted. Then, the selected angle measurement value and arrival time difference observation value are output to the tracking means 4 as observation values. Thereafter, the same processing as in the first to third embodiments is performed.
[0080]
As is clear from the above, according to the movement trajectory measuring device of the fourth embodiment, only the significant observation value is selected in advance and then the tracking processing is performed, so that the calculation load can be reduced, and the tracking can be performed. Processing accuracy can be improved.
[0081]
Note that it goes without saying that the processing for selecting a significant observation value based on the distance information and the processing for selecting the observation value based on the positioning accuracy information have the same effect even when used alone.
[0082]
【The invention's effect】
The moving trajectory measuring device according to the present invention includes an arrival time difference measuring unit that measures an arrival time difference between the radio waves arriving at each of the plurality of sensors, in addition to an angle measurement value of a radio wave arriving from a moving body, By providing tracking means for tracking the position and speed of the moving object based on the arrival time difference output by the time difference measuring means and the angle measurement value, the moving object exists at a position where high accuracy cannot be obtained only by the angle measurement value Also in this case, there is an effect that the position and speed can be measured with high accuracy.
[Brief description of the drawings]
FIG. 1 is an explanatory diagram showing a relationship between a sensor and a moving body according to the present invention and a conventional technique.
FIG. 2 is an explanatory diagram of a measurement error according to the related art.
FIG. 3 is a block diagram showing a configuration of the first embodiment of the present invention.
FIG. 4 is a block diagram showing a configuration of a tracking means 4 according to the first embodiment of the present invention.
FIG. 5 is a block diagram showing a configuration of a tracking means 4 according to Embodiment 2 of the present invention.
FIG. 6 is a block diagram showing a configuration of a tracking means 4 according to Embodiment 3 of the present invention.
FIG. 7 is a block diagram showing a configuration of a fourth embodiment of the present invention.
FIG. 8 is a block diagram illustrating a configuration of a measurement value selection unit according to a fourth embodiment of the present invention.
[Explanation of symbols]
1-1 to 1-I: sensor, 2: angle measuring means, 3: arrival time difference measuring means,
4: Tracking means, 5: Initial value setting means, 6: Smooth vector memory,
7: memory for a smooth error covariance matrix, 8: prediction means, 9: angle measurement correlation determination means,
10: angle measurement smoothing means, 11: angle measurement time difference selection means, 12: time difference correlation determination means,
13: angle measurement value selection means, 14: time difference smoothing means, 15: measurement value selection means,
16: sensor selection means, 17: unnecessary observation value deletion means, 18: observation value selection means

Claims (7)

A plurality of sensors for receiving radio waves from the moving object,
Arrival time difference measuring means for measuring the arrival time difference of the radio waves arriving at the plurality of sensors,
Angle measuring means for measuring an azimuth at which the radio wave arrives at the plurality of sensors,
Tracking means for tracking the moving state of the moving body based on the measurement value output by the arrival time difference measuring means and the angle measuring means, and outputting a smoothed value and a predicted value of the moving state. Moving trajectory measuring device. A plurality of sensors for receiving radio waves from the moving object,
Arrival time difference measuring means for measuring the arrival time difference of the radio waves arriving at the plurality of sensors,
Angle measuring means for measuring an azimuth at which the radio wave arrives at the plurality of sensors,
A measurement value selection unit that selects and outputs only a measurement value that is significant for tracking the moving object from the measurement values output by the arrival time difference measurement unit and the angle measurement unit,
A movement trajectory measuring device comprising: a tracking unit that tracks a moving state of a moving object based on a measurement value output by the measurement value selection unit and outputs a smoothed value and a predicted value of the moving state. The measurement value selection means, sensor distance calculation means for calculating the distance between the position of the moving object included in the movement state up to the previous sampling output by the tracking means and the position of the plurality of sensors,
From among the plurality of sensors, a sensor whose distance calculated by the sensor distance calculating means is equal to or smaller than a predetermined value is selected, and a measurement value for the selected sensor is obtained from the arrival time difference measuring means and the angle measuring means. 3. The moving trajectory measuring device according to claim 2, further comprising a sensor selecting unit that outputs the moving trajectory. The measurement value selection unit selects only a measurement value whose positioning accuracy is a predetermined estimation accuracy among the measurement values of the plurality of sensors, based on the movement state up to the previous sampling output by the tracking unit. The moving trajectory measuring device according to claim 2, further comprising an observation value selecting unit that performs the measurement. The tracking unit uses a smoothed vector and a predicted value of a Kalman filter based on a smoothed error covariance matrix up to the previous sampling of the moving object, and determines whether or not the measured value of the arrival time difference measuring unit is valid. Correlation determination means;
Among the measurement values of the angle measurement means, an angle measurement value selection means for selecting only the measurement values from the sensor determined that the time difference correlation determination means is valid,
Angle measurement correlation determination means for determining whether there is a correlation between the measurement value selected by the angle measurement value selection means and the predicted value,
Angle measurement smoothing means for calculating a smoothed vector and a smooth error covariance matrix for the current sampling for the measurement value of the angle measurement means that the angle measurement correlation determination means has determined to have a correlation. The moving trajectory measuring device according to claim 1. The tracking unit uses a smoothed vector and a predicted value of a Kalman filter based on a smoothed error covariance matrix up to the previous sampling of the moving object, and determines whether or not the measured value of the arrival time difference measuring unit is valid. Correlation determination means;
Among the measured values of the angle measuring means, for the measured values of the arrival time difference measuring means determined by the time difference correlation determining means to be valid, a time difference smoothing for calculating a smoothed vector and a smoothed error covariance matrix for the current sampling. Means,
A measurement value of the angle measurement means, a smoothed vector and a smooth error covariance matrix for the current sampling calculated by the time difference smoothing means, and an angle measurement correlation determination means for determining whether there is a correlation with the predicted value,
Angle measurement smoothing means for calculating a smoothed vector and a smooth error covariance matrix for the current sampling for the measurement value of the angle measurement means that the angle measurement correlation determination means has determined to have a correlation. The moving trajectory measuring device according to claim 1. The tracking unit uses a smoothed vector and a predicted value of a Kalman filter based on a smoothed error covariance matrix up to the previous sampling of the moving object, and determines whether or not the measured value of the arrival time difference measuring unit is valid. Correlation determination means;
Among the measured values of the angle measuring means, for the measured values of the arrival time difference measuring means determined by the time difference correlation determining means to be valid, a time difference smoothing for calculating a smoothed vector and a smoothed error covariance matrix for the current sampling. Means,
Among the measurement values of the angle measurement means, an angle measurement value selection means for selecting only the measurement values from the sensor determined that the time difference correlation determination means is valid,
Angle measurement correlation determination means for determining whether or not there is a correlation between the measurement value selected by the angle measurement value selection means, the smoothed vector and the smooth error covariance matrix for the current sampling calculated by the time difference smoothing means, and the predicted value. When,
2. The angle measuring unit, wherein the angle measuring unit determines that the angle measuring correlation determining unit has a correlation, further comprising angle measuring smoothing unit that calculates a smooth vector and a smooth error covariance matrix for the current sampling. The movement trajectory measuring device according to claim 4.

Images