JP2009300380A - 目標追尾装置 - Google Patents
目標追尾装置 Download PDFInfo
- Publication number
- JP2009300380A JP2009300380A JP2008158106A JP2008158106A JP2009300380A JP 2009300380 A JP2009300380 A JP 2009300380A JP 2008158106 A JP2008158106 A JP 2008158106A JP 2008158106 A JP2008158106 A JP 2008158106A JP 2009300380 A JP2009300380 A JP 2009300380A
- Authority
- JP
- Japan
- Prior art keywords
- tracking
- target
- predicted value
- value
- positioning result
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
【課題】アンセンテッドカルマンフィルタの推定精度を、観測条件によらずに安定化させることのできる目標追尾装置を得る。
【解決手段】追尾フィルタにおける運動モデルと観測モデルが非線形関数で表される場合に、非線形関数をアンセンテッド変換により近似するアンセンテッドカルマンフィルタを適用した追尾フィルタ２を備え、追尾フィルタ２は、受信信号に基づいて算出された目標の測位結果を逐次読み込み、測位結果からアンセンテッドカルマンフィルタを適用して算出した予測値と測位結果とに基づいて、測位結果に対する予測値の適合度を逐次算出する予測値適合度評価部２２と、予測値適合度評価部２２で逐次算出された適合度に基づいて、アンセンテッドカルマンフィルタにおけるアンセンテッド変換のスケーリングパラメータを逐次更新するパラメータ設定部２４とを備える。
【選択図】図５
【解決手段】追尾フィルタにおける運動モデルと観測モデルが非線形関数で表される場合に、非線形関数をアンセンテッド変換により近似するアンセンテッドカルマンフィルタを適用した追尾フィルタ２を備え、追尾フィルタ２は、受信信号に基づいて算出された目標の測位結果を逐次読み込み、測位結果からアンセンテッドカルマンフィルタを適用して算出した予測値と測位結果とに基づいて、測位結果に対する予測値の適合度を逐次算出する予測値適合度評価部２２と、予測値適合度評価部２２で逐次算出された適合度に基づいて、アンセンテッドカルマンフィルタにおけるアンセンテッド変換のスケーリングパラメータを逐次更新するパラメータ設定部２４とを備える。
【選択図】図５
Description
本発明は、観測装置が観測した目標を追尾する目標追尾装置に関する。
従来の目標追尾装置においては、次のような方法で、目標の位置および速度を推定している。まず、異なる位置に配置された複数の受信局によりネットワークを構成し、受信局間での電波到来時間差（Ｔｉｍｅ Ｄｉｆｆｅｒｅｎｃｅ Ｏｆ Ａｒｒｉｖａｌ：以下、ＴＤＯＡと称す）より換算される各受信局−目標間の距離の差（以下、距離差と称す）を求める。
さらに、ドップラー周波数差（Ｆｒｅｑｕｅｎｃｙ Ｄｉｆｆｅｒｅｎｃｅ Ｏｆ Ａｒｒｉｖａｌ：以下、ＦＤＯＡと称す）より換算される各受信局−目標間のラジアル速度の差（以下、ラジアル速度差と称す）を求める。そして、目標追尾装置は、上述のようにして求められた距離差およびラジアル速度差を非同期に入力し、追尾処理を行うことで目標の位置および速度を推定する。
目標の状態変数ベクトルを、北基準直交座標系における位置、速度で定義すると、ＴＤＯＡおよびＦＤＯＡは、非線形関数となる。そして、追尾目標の運動モデルは、非線形関数式（１）で表され、観測モデルは、非線形関数式（２）で表される。
上式（１）および（２）において、ｘｋはサンプリング時刻ｔｋにおける目標の状態変数ベクトル、ｗｋは駆動雑音ベクトル（平均０、共分散行列Ｑｋ）、Γ１は駆動雑音ベクトルに対する変換行列を表す。また、式（２）において、ｚｋはサンプリング時刻ｔｋにおける目標の観測値ベクトル、ｖｋは観測雑音ベクトル（平均０、共分散行列Ｒｋ）を表す。
このように、運動モデルと観測モデルが非線形関数となる場合、従来技術では、モデルの非線形性を考慮し、追尾フィルタとして拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ：以下、ＥＫＦと称す）を適用するのが一般的である（例えば、特許文献１参照）。
しかしながら、従来技術には次のような課題がある。
ＥＫＦでは、非線形関数に対する部分的な線形一次近似によって、カルマンフィルタの理論を適用する。このため、問題の非線形性が強い場合には、線形一次近似による真値からの誤差が大きくなり、追尾フィルタによる推定値が真の期待値および誤差共分散から大きく外れる可能性がある。
ＥＫＦでは、非線形関数に対する部分的な線形一次近似によって、カルマンフィルタの理論を適用する。このため、問題の非線形性が強い場合には、線形一次近似による真値からの誤差が大きくなり、追尾フィルタによる推定値が真の期待値および誤差共分散から大きく外れる可能性がある。
このようなＥＫＦにおける非線形関数の線形一次近似の問題を軽減するため、追尾フィルタとしてアンセンテッドカルマンフィルタ（Ｕｎｓｃｅｎｔｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ：以下、ＵＫＦと称す）を適用する。
ＵＫＦでは、アンセンテッド変換（Ｕｎｓｃｅｎｔｅｄ Ｔｒａｎｓｆｏｒｍａｔｉｏｎ：以下、ＵＴと称す）により、非線形関数をテーラー展開の３次の項まで近似することができる。このため、ＥＫＦを適用する場合と比較して、非線形関数となるモデルにおける推定精度を、より高くすることが期待できる
ＵＴでは、まず、２ｎ＋１個のシグマポイントを、下式（３）〜（５）に従って算出する。
上式（３）〜（５）において、ｘ（＾）ｋ｜ｋは、時刻ｔｋにおける追尾フィルタによる平滑値ベクトル、Ｐｋ｜ｋは、平滑誤差共分散行列を表す（ここで、（＾）という表記は、かっこの前の文字の上部に＾が付されることを意味している）。特に、平滑誤差共分散行列Ｐｋ｜ｋについては、下三角行列√（Ｐｋ｜ｋ）と上三角行列（√（Ｐｋ｜ｋ））Ｔに分解し、下式（６）のように表す。
また、上式（４）、（５）におけるλは、スケーリングパラメータであり、［√｛（ｎ＋λ）Ｐｋ｜ｋ｝］ｉは、下三角行列√｛（ｎ＋λ）Ｐｋ｜ｋ｝の第ｉ列を表す。
ＵＫＦでは、ＵＴによって上式（３）〜（５）で算出される２ｎ＋１個のシグマポイントが、モデルの非線形関数の非線形性を軽減する項となる。
しかし、上式（４）、（５）に示されるように、シグマポイントにはスケーリングパラメータλが含まれている。そして、適切なスケーリングパラメータの値の設定は、観測精度やサンプリングレートなどの観測条件によって異なる。従って、設定が不適切な場合には、推定値が発散する、あるいは、推定値の精度劣化を引き起こすなどの課題がある。
本発明は、前記のような課題を解決するためになされたものであり、アンセンテッドカルマンフィルタ（ＵＫＦ）の推定精度を、観測条件によらずに安定化させることのできる目標追尾装置を得ることを目的とする。
本発明に係る目標追尾装置は、位置が未知である目標から送出される電波を位置が既知である複数の受信局で受信した受信信号に基づいて、目標の位置、速度の運動諸元を推定する追尾フィルタを備え、追尾フィルタにおける運動モデルと観測モデルが非線形関数で表される場合に、前記追尾フィルタとして非線形関数をアンセンテッド変換により近似するアンセンテッドカルマンフィルタを適用した目標追尾装置であって、追尾フィルタは、受信信号に基づいて算出された目標の測位結果を逐次読み込み、測位結果からアンセンテッドカルマンフィルタを適用して算出した予測値と測位結果とに基づいて、測位結果に対する予測値の適合度を逐次算出する予測値適合度評価部と、予測値適合度評価部で逐次算出された適合度に基づいて、アンセンテッドカルマンフィルタにおけるアンセンテッド変換のスケーリングパラメータを逐次更新するパラメータ設定部とを備えるものである。
本発明に係る目標追尾装置によれば、運動モデルおよび観測モデルが非線形性関数となる場合において、測位結果と推定値に基づいて、観測毎にアンセンテッドカルマンフィルタ（ＵＫＦ）のスケーリングパラメータを更新する構成を備えることにより、ＵＫＦの推定精度を、観測条件によらずに安定化させることのできる目標追尾装置を得ることができる。
以下、本発明の目標追尾装置の好適な実施の形態につき図面を用いて説明する。
実施の形態１．
図１は、本発明の実施の形態１における目標追尾装置の構成の一例を示すブロック図である。この図１の目標追尾装置は、目標測位装置１、Ｎ個の追尾フィルタ２（１）〜２（Ｎ）、統合平滑処理部３、状態量更新部４、および表示部５を備えている。
図１は、本発明の実施の形態１における目標追尾装置の構成の一例を示すブロック図である。この図１の目標追尾装置は、目標測位装置１、Ｎ個の追尾フィルタ２（１）〜２（Ｎ）、統合平滑処理部３、状態量更新部４、および表示部５を備えている。
ここで、ｍ＝１〜Ｎとすると、各追尾フィルタ２（ｍ）は、予測値算出部２１（ｍ）、予測値適合度評価部２２（ｍ）、平滑値算出部２３（ｍ）、およびパラメータ設定部２４（ｍ）を備えている。
次に、各構成要素の機能について説明する。
目標測位装置１は、受信局によって受信された信号に基づいて、目標の位置、速度などの情報を測位結果として算出する。各追尾フィルタ２（ｍ）は、目標測位装置１から、算出された測位結果および受信局の位置情報を入力し、目標の位置および速度を推定する。
目標測位装置１は、受信局によって受信された信号に基づいて、目標の位置、速度などの情報を測位結果として算出する。各追尾フィルタ２（ｍ）は、目標測位装置１から、算出された測位結果および受信局の位置情報を入力し、目標の位置および速度を推定する。
追尾フィルタ２（ｍ）において、予測値算出部２１（ｍ）は、目標測位装置１より目標の測位結果と受信局の位置情報を入力し、パラメータ設定部２４（ｍ）よりＵＫＦのスケーリングパラメータの設定を入力する。そして、予測値算出部２１（ｍ）は、パラメータ設定部２４（ｍ）で設定されたスケーリングパラメータを持つＵＫＦにより、目標の位置および速度の予測値を算出する。
予測値適合度評価部２２（ｍ）は、目標測位装置１より目標の測位結果を入力するとともに、予測値算出部２１（ｍ）より目標の位置および速度の予測値を入力する。そして、予測値適合度評価部２２（ｍ）は、予測値と観測値の差あるいは距離に基づいて、測位結果に対する予測値の適合度を算出し、適合度を統合平滑処理部３に対して出力する。
また、予測値適合度評価部２２（ｍ）は、適合度が所定値よりも低い場合には、スケーリングパラメータの設定を修正し、パラメータ設定部２４（ｍ）におけるスケーリングパラメータの設定を更新する。
平滑値算出部２３（ｍ）は、予測値算出部２１（ｍ）より目標の位置および速度の予測値を入力し、ＵＫＦの手順に従って目標位置の平滑値を算出する。
統合平滑処理部３は、Ｎ個の平滑値算出部２３（１）〜２３（Ｎ）より目標の位置および速度の平滑値を入力するとともに、Ｎ個の予測値適合度評価部２２（１）〜２２（Ｎ）より予測値の適合度を入力する。そして、統合平滑処理部３は、Ｎ個の平滑値をそれぞれに対応する適合度によって重み付けし、さらに平滑値の重み付け平均値を算出して統合平滑値とし、状態量更新部４および表示部５に対して出力する。
状態量更新部４は、統合平滑処理部３で算出されたＮ個の追尾フィルタ２（１）〜２（Ｎ）の統合平滑値を入力し、統合平滑値より距離差およびラジアル速度差の統合予測値を算出する。そして、状態量更新部４は、算出した統合予測値を目標測位装置１にフィードバックする。
表示部５は、統合平滑処理部３で算出されたＮ個の追尾フィルタ２（１）〜２（Ｎ）の統合平滑値を入力し、統合平滑値を現時刻における目標の位置および速度の推定値として表示する。
次に、ＴＤＯＡおよびＦＤＯＡが得られ、目標と受信局の間の距離差およびラジアル速度差を観測値とする場合を例として、並列ＵＫＦによって目標を追尾するための各種定義事項について説明する。
図２は、本発明の実施の形態１における受信局と信号源である目標との位置関係を示した図である。ここでは、目標の３次元運動を推定する場合を仮定する。
図２において、ｓａ、ｓｂは、それぞれ受信局ａ、受信局ｂの位置ベクトルを表す。また、ｒａは、受信局ａから信号源までの距離、ｒｂは、受信局ｂから信号源までの距離をそれぞれ表す。ｕは、目標（信号源）の位置ベクトルを表す。
ここで、目標の運動モデルとして、下式（７）で表される等速直線運動を仮定する。
上式（７）において、ｘｋは、位置および速度ベクトルで構成される目標の状態変数ベクトルを表す。また、上式（７）の各要素は、それぞれ下式（８）〜（１３）で表される。
ここで、ｕｋは、信号源の位置ベクトルを表し、ｕｋ（・）は、信号源の速度ベクトルを表す（ここで、（・）という表記は、かっこの前の文字の上部に・が付されることを意味している）。Φｋは、推移行列を表す。ｗｋは、駆動雑音ベクトル、Γ1、ｋは、駆動雑音ベクトルの変換行列である。
次に、観測モデルを定義する。
まず、ＴＤＯＡが得られる場合における追尾方式（以下、ＴＤＯＡ追尾方式と称す）の観測モデルを示す。
まず、ＴＤＯＡが得られる場合における追尾方式（以下、ＴＤＯＡ追尾方式と称す）の観測モデルを示す。
ＴＤＯＡ方式における観測値ベクトルは、受信局間のＴＤＯＡより換算される受信局と信号源との距離ｒａ、ｒｂの差（距離差）である。この距離差を観測値として用いる場合の観測モデルを、下式（１４）、（１５）で定義する。
上式（１５）におけるｒａｂは、距離差を表す。また、上式（１４）におけるｈ（ｘｋ）は、下式（１６）で定義される。
また、上式（１４）における観測雑音ベクトルｖｋについては、下式（１７）のように距離差の成分として与える。
次に、ＴＤＯＡとＦＤＯＡが得られる場合における追尾方式（以下、ＴＤＯＡ／ＦＤＯＡ追尾方式と称す）の観測モデルを示す。
ＴＤＯＡ／ＦＤＯＡ追尾方式における観測値ベクトルは、距離差とＦＤＯＡから換算される受信局と信号源との距離ｒａ、ｒｂの変化率の差（ラジアル速度差）であり、距離差とラジアル速度差を観測値として用いる場合の観測モデルを下式（１８）〜（２０）で定義する。
上式（１８）における観測雑音ベクトルｖｋについては、下式（２１）のように距離差、ラジアル速度差の成分を持つベクトルとして与える。
次に、並列ＵＫＦの、各追尾フィルタ２（ｍ）における追尾処理について説明する。
各追尾フィルタ２（ｍ）において、まず、２ｎ＋１個のシグマポイントを、下式（２２）〜（２４）に従って算出する。
各追尾フィルタ２（ｍ）において、まず、２ｎ＋１個のシグマポイントを、下式（２２）〜（２４）に従って算出する。
上式（２２）〜（２４）において、ｘ（＾）ｍ、ｋ｜ｋは、時刻ｔｋにおける追尾フィルタ２（ｍ）による平滑値ベクトル、Ｐｍ、ｋ｜ｋは、追尾フィルタ２（ｍ）による平滑誤差共分散行列を表す。特に、平滑誤差共分散行列Ｐｍ、ｋ｜ｋについては、下三角行列√（Ｐｍ、ｋ｜ｋ）と上三角行列（√（Ｐｍ、ｋ｜ｋ））Ｔに分解し、下式（２５）のように表す。
また、上式（２３）、（２４）におけるλｍは、追尾フィルタ２（ｍ）のパラメータ設定部２４（ｍ）により設定されるスケーリングパラメータであり、［√｛（ｎ＋λｍ）Ｐｍ、ｋ｜ｋ｝］ｉは、下三角行列√｛（ｎ＋λｍ）Ｐｍ、ｋ｜ｋ｝の第ｉ列を表す。
次に、上式（２２）〜（２４）によって得たシグマポイントごとに、予測処理を下式（２６）に従って行うとともに、予測値の統合処理を下式（２７）、（２８）に従って行う。
ここで、上式（２７）、（２８）におけるωｍ、ｉは、下式（２９）、（３０）に従って算出する。
次に、上式（２７）〜（３０）の予測処理および予測値の統合処理を実施した後、ｒａとｒｂの距離差の予測値ｒ（〜）ｍ、ａｂ、およびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂを、上式（２７）で得た予測値ベクトルにより、下式（３１）、（３２）に従って算出する（ここで、（〜）という表記は、かっこの前の文字の上部に〜が付されることを意味し、（・）（〜）という表記は、かっこの前の文字の上部に・が付されたものにさらに〜が付されることを意味している）。
次に、新たな観測値ベクトルが得られた場合の位置と速度の平滑値を得る、データ更新について説明する。まず、シグマポイント毎に得た予測値ベクトルｘ（〜）ｍ、ｋ＋１｜ｋ、ｉ、および統合後の予測値ベクトルｘ（〜）ｍ、ｋ＋１｜ｋより、下式（３３）、（３４）を計算する。
次に、Ｐｍ、ＸＺ、Ｐｍ、ＺＺおよび観測誤差共分散行列Ｒｍ、ｋによって残差共分散行列Ｓｍ、ｋ｜ｋ-1、ゲイン行列Ｋｍ、ｋをそれぞれ下式（３５）、（３６）に従って算出する。
次に、平滑値ベクトル、平滑誤差共分散行列を下式（３７）、（３８）に従って算出する。
上述した式（２２）〜（３８）までに示した処理が、各追尾フィルタ２（ｍ）において並列に行われるものとする。
次に、本実施の形態１における目標追尾装置の一連の動作について説明する。
図３は、本発明の実施の形態１の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。図３において、ステップＳＴ２ａ（ｍ）〜ステップＳＴ２ｅ（ｍ）（ｍ＝１〜Ｎ）は、図１における追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）のそれぞれの動作を示す。以下、各ステップの動作について説明する。
図３は、本発明の実施の形態１の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。図３において、ステップＳＴ２ａ（ｍ）〜ステップＳＴ２ｅ（ｍ）（ｍ＝１〜Ｎ）は、図１における追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）のそれぞれの動作を示す。以下、各ステップの動作について説明する。
ステップＳＴ１の動作
目標測位装置１は、複数の受信局による観測の結果、ＴＤＯＡが得られている場合には、ＴＤＯＡから距離差ｒａｂを算出する。また、目標測位装置１は、ＴＤＯＡとＦＤＯＡが得られている場合には、ＴＤＯＡ／ＦＤＯＡから距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂを算出する。
目標測位装置１は、複数の受信局による観測の結果、ＴＤＯＡが得られている場合には、ＴＤＯＡから距離差ｒａｂを算出する。また、目標測位装置１は、ＴＤＯＡとＦＤＯＡが得られている場合には、ＴＤＯＡ／ＦＤＯＡから距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂを算出する。
そして、目標測位装置１は、距離差およびラジアル速度差に対して、相関ゲートによりマルチパス排除を行った結果を、目標の測位結果として追尾フィルタ２（ｍ）に対して出力する。また、受信局の位置情報ｓａ、ｓｂを追尾フィルタ２（ｍ）に対して出力する。
ステップＳＴ２ａ（ｍ）（ｍ＝１〜Ｎ）の動作
追尾フィルタ２（ｍ）において、予測値算出部２１（ｍ）は、目標測位装置１により算出された距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂおよび受信局の位置情報を入力する。
追尾フィルタ２（ｍ）において、予測値算出部２１（ｍ）は、目標測位装置１により算出された距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂおよび受信局の位置情報を入力する。
ステップＳＴ２ｂ（ｍ）（ｍ＝１〜Ｎ）の動作
追尾フィルタ２（ｍ）において、予測値算出部２１（ｍ）は、目標測位装置１より目標の測位結果（距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂ）と受信局の位置情報（ｓａ、ｓｂ）を入力する。さらに、予測値算出部２１（ｍ）は、パラメータ設定部２４（ｍ）よりＵＫＦのスケーリングパラメータの設定λｍを入力する。
追尾フィルタ２（ｍ）において、予測値算出部２１（ｍ）は、目標測位装置１より目標の測位結果（距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂ）と受信局の位置情報（ｓａ、ｓｂ）を入力する。さらに、予測値算出部２１（ｍ）は、パラメータ設定部２４（ｍ）よりＵＫＦのスケーリングパラメータの設定λｍを入力する。
そして、予測値算出部２１（ｍ）は、パラメータ設定部２４（ｍ）で設定されたスケーリングパラメータを持つＵＫＦにより、目標の位置と速度の予測値ｘ（〜）ｍ、ｋ＋１｜ｋおよび予測誤差共分散行列Ｐｍ、ｋ＋１｜ｋを算出する。さらに、予測値算出部２１（ｍ）は、算出結果を平滑値算出部２３（ｍ）に対して出力する。
追尾フィルタの運動モデルとしては、上式（７）〜（１３）で定義される運動モデルを使用する。また、観測モデルとしては、ＴＤＯＡが得られる場合には、上式（１４）〜（１７）、ＴＤＯＡ／ＦＤＯＡが得られる場合には、上式（１８）〜（２１）を使用する。
目標の位置および速度の予測値を算出する手順は、上式（２２）〜（３０）による。
目標の位置および速度の予測値より、距離差の予測値ｒ（〜）ｍ、ａｂ、およびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂを算出する。そして、予測値適合度評価部２２（ｍ）に対して出力する。
目標の位置および速度の予測値より、距離差の予測値ｒ（〜）ｍ、ａｂ、およびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂを算出する。そして、予測値適合度評価部２２（ｍ）に対して出力する。
目標の位置および速度の予測値から距離差の予測値およびラジアル速度差の予測値を算出する手順は、上式（３１）、（３２）による。
ステップＳＴ２ｃ（ｍ）（ｍ＝１〜Ｎ）の動作
予測値適合度評価部２２（ｍ）は、予測値算出部２１（ｍ）によって算出された距離差の予測値ｒ（〜）ｍ、ａｂおよびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂの適合度を評価する。さらに、予測値適合度評価部２２（ｍ）は、評価した適合度に応じてパラメータ設定部２４（ｍ）におけるスケーリングパラメータλｍの設定を変更する。
予測値適合度評価部２２（ｍ）は、予測値算出部２１（ｍ）によって算出された距離差の予測値ｒ（〜）ｍ、ａｂおよびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂの適合度を評価する。さらに、予測値適合度評価部２２（ｍ）は、評価した適合度に応じてパラメータ設定部２４（ｍ）におけるスケーリングパラメータλｍの設定を変更する。
図４は、本発明の実施の形態１の予測値適合度評価部２２（ｍ）における適合度を評価の過程およびスケーリングパラメータλｍの設定を変更する過程の説明図である。予測値適合度評価部２２（ｍ）は、目標測位装置１より距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂを入力する。さらに、予測値適合度評価部２２（ｍ）は、予測値算出部２１（ｍ）より距離差の予測値ｒ（〜）ｍ、ａｂおよびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂを入力する。
そして、予測値適合度評価部２２（ｍ）は、予測値と観測値の差あるいは距離に基づいて、予測値の測位結果に対する適合度を算出する。そして、予測値適合度評価部２２（ｍ）は、算出した適合度を統合平滑処理部３に対して出力する。
予測値の適合度の算出方法としては、例えば、次のものが考えられる。予測値適合度評価部２２（ｍ）は、各追尾フィルタ２（ｍ）のパラメータ設定部２４（ｍ）において設定されているスケーリングパラメータλｍに基づいて、追尾フィルタ２（ｍ）の予測値を中心とするＮ個の確率密度関数式である下式（３９）に、目標測位装置による測位結果を代入し、尤度として算出することができる。
ステップＳＴ２ｄ（ｍ）（ｍ＝１〜Ｎ）の動作
予測値適合度評価部２２（ｍ）は、上式（３９）あるいはその他の計算式によって算出される予測値適合度を評価するための閾値Ｔｈｐｌを有している。そして、予測値適合度評価部２２（ｍ）は、上式（３９）による計算結果がＴｈｐｌ以上の場合には、スケーリングパラメータの設定を存続させる。
予測値適合度評価部２２（ｍ）は、上式（３９）あるいはその他の計算式によって算出される予測値適合度を評価するための閾値Ｔｈｐｌを有している。そして、予測値適合度評価部２２（ｍ）は、上式（３９）による計算結果がＴｈｐｌ以上の場合には、スケーリングパラメータの設定を存続させる。
一方、予測値適合度評価部２２（ｍ）は、上式（３９）による計算結果がＴｈｐｌ未満の場合には、スケーリングパラメータの設定を修正し、修正の結果をパラメータ設定部２４（ｍ）に対して出力する。この結果、パラメータ設定部２４（ｍ）におけるスケーリングパラメータの設定が更新される。ここで、閾値Ｔｈｐｌは、あらかじめ設定するパラメータである。
スケーリングパラメータλｍの修正方法は、例えば、下式（４０）による。下式（４０）の右辺第２項のΔλは、スケーリングパラメータの修正量であり、例えば、下式（４１）を用いる。下式（４１）の右辺におけるεは、あらかじめ設定するパラメータである。
ステップＳＴ２ｅ（ｍ）（ｍ＝１〜Ｎ）の動作
平滑値算出部２３（ｍ）は、予測値算出部２１（ｍ）より目標の位置と速度の予測値ｘ（〜）ｍ、ｋ＋１｜ｋおよび予測誤差共分散行列Ｐｍ、ｋ＋１｜ｋを入力し、ＵＫＦの手順に従って目標位置の平滑値ｘ（＾）ｍ、ｋ｜ｋおよび平滑誤差共分散行列Ｐｍ、ｋ｜ｋを算出する。平滑値および平滑誤差共分散行列の算出手順は、上式（３３）〜（３８）による。
平滑値算出部２３（ｍ）は、予測値算出部２１（ｍ）より目標の位置と速度の予測値ｘ（〜）ｍ、ｋ＋１｜ｋおよび予測誤差共分散行列Ｐｍ、ｋ＋１｜ｋを入力し、ＵＫＦの手順に従って目標位置の平滑値ｘ（＾）ｍ、ｋ｜ｋおよび平滑誤差共分散行列Ｐｍ、ｋ｜ｋを算出する。平滑値および平滑誤差共分散行列の算出手順は、上式（３３）〜（３８）による。
本実施の形態１における目標追尾装置では、各追尾フィルタ２（ｍ）において、上述したステップＳＴ２ａ（ｍ）からステップＳＴ２ｅ（ｍ）（ｍ＝１〜Ｎ）までが並列に動作しているものとする。
ステップＳＴ３の動作
統合平滑処理部３は、平滑値算出部２３（ｍ）（ｍ＝１〜Ｎ）より目標の位置と速度の平滑値ｘ（＾）ｍ、ｋ｜ｋを入力し、予測値適合度評価部２２（ｍ）より予測値の適合度Ｐｍ（λｍ｜ｒａｂ、ｒ（・）ａｂ）を入力する。そして、統合平滑処理部３は、Ｎ個の平滑値をそれぞれに対応する適合度によって重み付けし、平滑値の重み付け平均値ｘ（＾）ｋ｜ｋを算出する。さらに、統合平滑処理部３は、重み付け平均値を統合平滑値として、状態量更新部４および表示部５に対して出力する。
統合平滑処理部３は、平滑値算出部２３（ｍ）（ｍ＝１〜Ｎ）より目標の位置と速度の平滑値ｘ（＾）ｍ、ｋ｜ｋを入力し、予測値適合度評価部２２（ｍ）より予測値の適合度Ｐｍ（λｍ｜ｒａｂ、ｒ（・）ａｂ）を入力する。そして、統合平滑処理部３は、Ｎ個の平滑値をそれぞれに対応する適合度によって重み付けし、平滑値の重み付け平均値ｘ（＾）ｋ｜ｋを算出する。さらに、統合平滑処理部３は、重み付け平均値を統合平滑値として、状態量更新部４および表示部５に対して出力する。
重み付け平均値の算出方法としては、適合度の算出方法が上式（３９）である場合には、例えば、下式（４２）による。
ステップＳＴ４の動作
状態量更新部４は、統合平滑処理部３より追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の統合平滑値ｘ（＾）ｋ｜ｋを入力する。そして、状態量更新部４は、統合平滑値より距離差の統合予測値ｒ（〜）ａｂ、およびラジアル速度差の統合予測値ｒ（・）（〜）ａｂを算出し、これらの統合予測値を、目標測位装置１にフィードバックする。統合予測値の算出方法としては、下式（４３）、（４４）による。
状態量更新部４は、統合平滑処理部３より追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の統合平滑値ｘ（＾）ｋ｜ｋを入力する。そして、状態量更新部４は、統合平滑値より距離差の統合予測値ｒ（〜）ａｂ、およびラジアル速度差の統合予測値ｒ（・）（〜）ａｂを算出し、これらの統合予測値を、目標測位装置１にフィードバックする。統合予測値の算出方法としては、下式（４３）、（４４）による。
ステップＳＴ５の動作
表示部５は、統合平滑処理部３より追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の統合平滑値ｘ（＾）ｋ｜ｋを入力し、統合平滑値を現時刻における目標の位置および速度の推定値として表示する。
表示部５は、統合平滑処理部３より追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の統合平滑値ｘ（＾）ｋ｜ｋを入力し、統合平滑値を現時刻における目標の位置および速度の推定値として表示する。
以上のように、実施の形態１によれば、運動モデルおよび観測モデルが非線形性関数となる場合において、異なるスケーリングパラメータの設定を持つ複数のＵＫＦを用いて、それぞれのＵＫＦによる目標の位置および速度の予測値の適合度に基づいて、観測毎にそれぞれのＵＫＦのスケーリングパラメータを更新している。このような構成により、観測条件によって適切なスケーリングパラメータの設定が異なるＵＫＦを用いた追尾フィルタによる目標の位置と速度の推定精度を、観測条件によらずに安定させることができる。
実施の形態２．
図５は、本発明の実施の形態２における目標追尾装置の構成の一例を示すブロック図である。本実施の形態２における目標追尾装置は、先の実施の形態１におけるＮ個の追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）を１個とし、統合平滑処理部３を省いた場合に相当する。
図５は、本発明の実施の形態２における目標追尾装置の構成の一例を示すブロック図である。本実施の形態２における目標追尾装置は、先の実施の形態１におけるＮ個の追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）を１個とし、統合平滑処理部３を省いた場合に相当する。
追尾フィルタ２における予測値算出部２１とパラメータ設定部２４の機能は、それぞれ先の実施の形態１における予測値算出部２１（ｍ）、パラメータ設定部２４（ｍ）と同一である。
予測値適合度評価部２２は、目標測位装置１より目標の測位結果を入力し、予測値算出部２１より目標の位置および速度の予測値を入力する。そして、予測値適合度評価部２２は、予測値の測位結果に対する残差を計算し、残差に基づいてパラメータ設定部２４（ｍ）におけるスケーリングパラメータの設定を更新する。
平滑値算出部２３は、予測値算出部２１より目標の位置および速度の予測値を入力し、ＵＫＦの手順に従って目標位置の平滑値を算出する。そして、平滑値算出部２３は、平滑値を状態量更新部４および表示部５に対して出力する。
また、状態量更新部４は、平滑値を目標測位装置１にフィードバックする。表示部５は、追尾フィルタ２の平滑値算出部２３より追尾フィルタの平滑値を入力し、平滑値を現時刻における目標の位置および速度の推定値として表示する。
次に、本実施の形態２における目標追尾装置の動作について説明する。図６は、本発明の実施の形態２の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。以下、各ステップの動作について説明する。
ステップＳＴ１〜ステップＳＴ２ｂの動作
この図６におけるステップＳＴ１〜ステップＳＴ２ｂの動作は、それぞれ、先の実施の形態１におけるステップＳＴ１〜ステップＳＴ２ｂ（ｍ）と共通である。予測処理に用いる数式としては、上式（２２）〜（３２）において、ｍ＝１のみとした場合に相当する。
この図６におけるステップＳＴ１〜ステップＳＴ２ｂの動作は、それぞれ、先の実施の形態１におけるステップＳＴ１〜ステップＳＴ２ｂ（ｍ）と共通である。予測処理に用いる数式としては、上式（２２）〜（３２）において、ｍ＝１のみとした場合に相当する。
ステップＳＴ２ｃの動作
予測値適合度評価部２２は、目標測位装置１より距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂを入力し、予測値算出部２１より距離差の予測値ｒ（〜）ｍ、ａｂおよびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂを入力する。そして、予測値適合度評価部２２は、予測値と測位結果との残差を計算する。
予測値適合度評価部２２は、目標測位装置１より距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂを入力し、予測値算出部２１より距離差の予測値ｒ（〜）ｍ、ａｂおよびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂを入力する。そして、予測値適合度評価部２２は、予測値と測位結果との残差を計算する。
ステップＳＴ２ｄの動作
予測値適合度評価部２２は、ステップＳＴ２ｃにおいて計算した残差に基づいて、パラメータ設定部２４におけるスケーリングパラメータの設定を更新する。スケーリングパラメータの修正方法としては、例えば、下式（４５）による。下式（４５）の右辺第２項のΔλ２は、スケーリングパラメータの修正量であり、例えば、下式（４６）を用いる。下式（４６）の右辺におけるε２は、あらかじめ設定するパラメータである。
予測値適合度評価部２２は、ステップＳＴ２ｃにおいて計算した残差に基づいて、パラメータ設定部２４におけるスケーリングパラメータの設定を更新する。スケーリングパラメータの修正方法としては、例えば、下式（４５）による。下式（４５）の右辺第２項のΔλ２は、スケーリングパラメータの修正量であり、例えば、下式（４６）を用いる。下式（４６）の右辺におけるε２は、あらかじめ設定するパラメータである。
ステップＳＴ２ｅの動作
平滑値算出部２３は、予測値算出部２１より目標の位置と速度の予測値ｘ（〜）ｋ＋１｜ｋおよび予測誤差共分散行列Ｐｋ＋１｜ｋを入力する。そして、平滑値算出部２３は、ＵＫＦの手順に従って、目標位置の平滑値ｘ（＾）ｋ｜ｋおよび平滑誤差共分散行列Ｐｋ｜ｋを算出し、状態量更新部４および表示部５に対して出力する。平滑値および平滑誤差共分散行列の算出には、上式（３３）〜（３８）において、ｍ＝１のみとした場合のものを用いる。
平滑値算出部２３は、予測値算出部２１より目標の位置と速度の予測値ｘ（〜）ｋ＋１｜ｋおよび予測誤差共分散行列Ｐｋ＋１｜ｋを入力する。そして、平滑値算出部２３は、ＵＫＦの手順に従って、目標位置の平滑値ｘ（＾）ｋ｜ｋおよび平滑誤差共分散行列Ｐｋ｜ｋを算出し、状態量更新部４および表示部５に対して出力する。平滑値および平滑誤差共分散行列の算出には、上式（３３）〜（３８）において、ｍ＝１のみとした場合のものを用いる。
ステップＳＴ４の動作
状態量更新部４は、平滑値算出部２３より追尾フィルタ２の平滑値ｘ（＾）ｋ｜ｋを入力し、平滑値より距離差の予測値ｒ（〜）ａｂ、およびラジアル速度差の予測値ｒ（・）（〜）ａｂを算出する。そして、状態量更新部４は、これらの予測値を目標測位装置１にフィードバックする。距離差およびラジアル速度差の予測値の算出方法は、上式（４３）、（４４）による。
状態量更新部４は、平滑値算出部２３より追尾フィルタ２の平滑値ｘ（＾）ｋ｜ｋを入力し、平滑値より距離差の予測値ｒ（〜）ａｂ、およびラジアル速度差の予測値ｒ（・）（〜）ａｂを算出する。そして、状態量更新部４は、これらの予測値を目標測位装置１にフィードバックする。距離差およびラジアル速度差の予測値の算出方法は、上式（４３）、（４４）による。
以上のように、実施の形態２によれば、追尾フィルタには単一のＵＫＦを用いつつ、観測毎にＵＫＦのスケーリングパラメータを更新することにより、観測条件によらずに安定した推定精度を得ることができる。さらに、先の実施の形態１に比べ、目標追尾装置の構成を簡略化し、追尾処理の負荷を低減できる。
なお、本実施の形態２おけるスケーリングパラメータの修正方法の例としては、予測値の残差に基づく方法を示した。しかしながら、スケーリングパラメータの修正方法は、他のものであってもよい。
実施の形態３．
図７は、本発明の実施の形態３における目標追尾装置の構成の一例を示すブロック図である。本実施の形態３における目標追尾装置は、先の実施の形態１の目標追尾装置に対して、追尾フィルタ制御部６をさらに備えている。そして、目標測位装置１から、追尾フィルタ制御部６を介して各追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の予測値算出部２１（ｍ）に接続されている。
図７は、本発明の実施の形態３における目標追尾装置の構成の一例を示すブロック図である。本実施の形態３における目標追尾装置は、先の実施の形態１の目標追尾装置に対して、追尾フィルタ制御部６をさらに備えている。そして、目標測位装置１から、追尾フィルタ制御部６を介して各追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の予測値算出部２１（ｍ）に接続されている。
本実施の形態３における目標測位装置１、予測値算出部２１（ｍ）、平滑値算出部２３（ｍ）、パラメータ設定部２４（ｍ）、状態量更新部４の機能は、それぞれ、先の実施の形態１における目標追尾装置のものと同一である。
また、統合平滑処理部３の機能は、追尾フィルタ制御部６によって動作する追尾フィルタが１つに限定されるまでは、先の実施の形態１における目標追尾装置のものと同一である。追尾フィルタ制御部６によって動作する追尾フィルタが１つに限定された後における、統合平滑処理部３の機能については、後述する。
本実施の形態３における予測値適合度評価部２２（ｍ）は、先の実施の形態１における予測値適合度評価部２２（ｍ）の機能を有するのに加え、算出した予測値の適合度を追尾フィルタ制御部６に入力する機能を有している。
追尾フィルタ制御部６は、目標測位装置１から目標の測位結果と受信機の位置情報を入力し、追尾処理を開始してからあらかじめ設定した時間が経過するまでは、全ての追尾フィルタ＃ｍ（ｍ＝１〜Ｎ）に、測位結果と受信機の位置情報を入力する。
また、追尾フィルタ制御部６は、各追尾フィルタ２（ｍ）の予測値適合度評価部２２（ｍ）より、追尾フィルタ２（ｍ）による予測値の測位結果に対する適合度を入力する。そして、追尾フィルタ制御部６は、追尾処理を開始してからあらかじめ設定した時間が経過した時点で、最も予測値の適合度が高い追尾フィルタ１つのみを選択し、残る（Ｎ−１）個の追尾フィルタの動作を停止させる。
動作する追尾フィルタが１つに限定された後では、統合平滑処理部３は、動作している追尾フィルタによる平滑値をそのまま統合平滑値とし、状態量更新部４および表示部５に対して出力する。
次に、本実施の形態３における目標追尾装置の動作について説明する。図８は、本発明の実施の形態３の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。以下、各ステップの動作について説明する。
ステップＳＴ１、ステップＳＴ２ａ（ｍ）〜ステップＳＴ２ｅ（ｍ）、ステップＳＴ４、ステップＳＴ５の動作
これらの動作は、先の実施の形態１における図３のものと同一である。
これらの動作は、先の実施の形態１における図３のものと同一である。
ステップＳＴ６の動作
追尾フィルタ制御部６は、追尾処理を開始してからの時間を計測し、所定の時間ｔｃｈａｎｇｅが経過した後、出力先として、かかる時点で最も予測値の適合度が高い追尾フィルタ１つのみを選択し、残るＮ−１個の追尾フィルタの動作を停止させる。時間ｔｃｈａｎｇｅは、あらかじめ追尾フィルタ制御部６に設定するパラメータである。
追尾フィルタ制御部６は、追尾処理を開始してからの時間を計測し、所定の時間ｔｃｈａｎｇｅが経過した後、出力先として、かかる時点で最も予測値の適合度が高い追尾フィルタ１つのみを選択し、残るＮ−１個の追尾フィルタの動作を停止させる。時間ｔｃｈａｎｇｅは、あらかじめ追尾フィルタ制御部６に設定するパラメータである。
ステップＳＴ３の動作
統合平滑処理部３の動作は、追尾フィルタ制御部６によって動作する追尾フィルタが１つに限定されるまでは、先の実施の形態１における目標追尾装置のものと同一である。一方、追尾フィルタ制御部６によって動作する追尾フィルタが１つに限定された後では、動作している追尾フィルタによる平滑値をそのまま統合平滑値とする。
統合平滑処理部３の動作は、追尾フィルタ制御部６によって動作する追尾フィルタが１つに限定されるまでは、先の実施の形態１における目標追尾装置のものと同一である。一方、追尾フィルタ制御部６によって動作する追尾フィルタが１つに限定された後では、動作している追尾フィルタによる平滑値をそのまま統合平滑値とする。
以上のように、実施の形態３によれば、追尾精度が安定しない追尾開始直後の時間帯に並列ＵＫＦによる追尾を行う過程でＵＫＦのスケーリングパラメータとして最適な値を算出する。そして、一定時間経過後は、最適なスケーリングパラメータを持ったＵＫＦによる追尾フィルタのみを動作させる。これにより、観測条件によらずに安定した推定精度を得ることができると同時に、追尾開始直後の時間帯より後の追尾処理にかかわる負荷を低減することができる。
実施の形態４．
図９は、本発明の実施の形態４における目標追尾装置の構成の一例を示すブロック図である。本実施の形態４における目標追尾装置は、先の実施の形態１の目標測位装置に対して、遅延観測値処理部７をさらに備えている。そして、目標測位装置１から、遅延観測値処理部７を介して各追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の予測値算出部２１（ｍ）に接続されている。
図９は、本発明の実施の形態４における目標追尾装置の構成の一例を示すブロック図である。本実施の形態４における目標追尾装置は、先の実施の形態１の目標測位装置に対して、遅延観測値処理部７をさらに備えている。そして、目標測位装置１から、遅延観測値処理部７を介して各追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の予測値算出部２１（ｍ）に接続されている。
本実施の形態４における目標測位装置１は、実施の形態１における目標測位装置と同様に目標を測位し、測位結果を遅延観測値処理部７に対して出力する。遅延観測値処理部７は、目標測位装置１より目標に対する測位結果を入力し、状態量更新部４より統合平滑処理後の統合予測値を入力し、測位結果の時刻と最新の統合予測値の時刻とを比較する。
測位結果に付与されている時刻が最新の統合予測値に付与されている時刻よりも新しい場合（すなわち、測位結果が追尾フィルタに対して遅延なく入力された場合）には、遅延観測値処理部７は、測位結果をそのまま各追尾フィルタ２（ｍ）の予測値算出部２１（ｍ）と予測値適合度評価部２２（ｍ）に対して出力する。
一方、測位結果に付与されている時刻が最新の統合予測値に付与されている時刻よりも古い場合には、遅延観測値処理部７は、最新の統合予測値に基づいて、受信機毎の目標に対するラジアル速度とラジアル加速度を推定する。
遅延観測値処理部７は、ラジアル速度、ラジアル加速度、および最新の統合予測値に付与されている時刻と測位結果に付与されている時刻との時間差により、観測値であるＴＤＯＡおよびＦＤＯＡを外挿する。
遅延観測値処理部７は、外挿により得られたＴＤＯＡより距離差を算出し、ＴＤＯＡ／ＦＤＯＡよりラジアル速度差を算出する。そして、遅延観測値処理部７は、予測値算出部２１（ｍ）および予測値適合度評価部２２（ｍ）に対して算出結果を出力する。
本実施の形態４における追尾フィルタ２（ｍ）および追尾フィルタ２（ｍ）の各部、表示部５の機能は、先の実施の形態１と同様である。
統合平滑処理部３は、先の実施の形態１と同様の機能を持つのに加え、各追尾フィルタ２（ｍ）の予測値の適合度、各追尾フィルタ２（ｍ）のパラメータ設定部２４（ｍ）におけるスケーリングパラメータの値を状態量更新部４に対して出力する。
状態量更新部４は、統合平滑処理部３より追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の統合平滑値、予測値の適合度を入力する。そして、状態量更新部４は、統合平滑値より距離差およびラジアル速度差の統合予測値を算出し、統合予測値を目標測位装置１と遅延観測値処理部７にフィードバックする。
次に、本実施の形態４における目標追尾装置の動作について説明する。図１０は、本発明の実施の形態４の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。以下、各ステップの動作について説明する。
ステップＳＴ２ａ（ｍ）〜ステップＳＴ２ｅ（ｍ）、ステップＳＴ３、ステップＳＴ４、ステップＳＴ５の動作
これらの動作は、先の実施の形態１における図３のものと同一である。
これらの動作は、先の実施の形態１における図３のものと同一である。
ステップＳＴ７ａの動作
遅延観測値処理部７は、目標測位装置１より目標に対する測位結果を入力し、状態量更新部４より平滑値を入力し、測位結果の時刻と最新の平滑値の時刻とを比較する。
遅延観測値処理部７は、目標測位装置１より目標に対する測位結果を入力し、状態量更新部４より平滑値を入力し、測位結果の時刻と最新の平滑値の時刻とを比較する。
測位結果に付与されている時刻が最新の平滑値に付与されている時刻よりも新しい場合（Ｙｅｓの側のフロー）、遅延観測値処理部７は、測位結果をそのまま各追尾フィルタ２（ｍ）の予測値算出部２１（ｍ）と予測値適合度評価部２２（ｍ）に対して出力する。
ステップＳＴ７ｂの動作
測位結果に付与されている時刻が最新の平滑値に付与されている時刻よりも古い場合（（ステップＳＴ７ａ）におけるＮｏの側のフロー）、遅延観測値処理部７は、平滑値に基づいて、受信機毎の目標に対するラジアル速度とラジアル加速度を推定する。
測位結果に付与されている時刻が最新の平滑値に付与されている時刻よりも古い場合（（ステップＳＴ７ａ）におけるＮｏの側のフロー）、遅延観測値処理部７は、平滑値に基づいて、受信機毎の目標に対するラジアル速度とラジアル加速度を推定する。
遅延観測値処理部７は、推定によるラジアル速度、ラジアル加速度、および最新の平滑値に付与されている時刻と測位結果に付与されている時刻との時間差により、平滑値の時刻よりも遅延した測位結果に対する時刻補正を行う。
遅延観測値処理部７は、外挿により得られたＴＤＯＡより距離差を算出し、ＴＤＯＡ／ＦＤＯＡよりラジアル速度差を算出する。そして、遅延観測値処理部７は、予測値算出部２１（ｍ）および予測値適合度評価部２２（ｍ）に対して出力する。
遅延観測値処理部７におけるラジアル速度とラジアル加速度の推定、および遅延した測位結果に対する時刻補正は、例えば、次の手順による。まず、追尾フィルタ２（ｍ）による平滑値に付与されている時刻ｔｋと、測位結果に付与されている時刻ｔｋ'より、下式（４７）より時刻差Δｔを得る。
測位結果の伝送に遅延が生じた場合は、Δｔ＜０であり、この場合はステップＳＴ７ｂとして、下記の観測値補正処理を実施する。ここでは、ＴＤＯＡが得られている場合の、遅延した測位結果に対する時刻補正の手順を、数式を用いて説明する。
距離差に関する観測精度が時間によらず一定であると見なし、まず、下式（４８）を計算することにより、時刻ｔｋ'におけるラジアル速度差を推定する。
上式（４８）におけるｓａ、ｋ'、ｓｂ、ｋ'は、時刻ｔｋ'におけるセンサの位置ベクトルであり、ｓ（・）ａ、ｋ'、ｓ（・）ｂ、ｋ'は、時刻ｔｋ'におけるセンサの速度ベクトルであり、いずれも既知量である。
上式（４８）におけるｕ（〜）ｋ'｜ｋ、ｕ（・）（〜）ｋ'｜ｋは、それぞれ時刻ｔｋ'における目標の位置ベクトルおよび速度ベクトルの推定値であり、時刻ｔｋにおける目標の位置の平滑値および速度ベクトルの平滑値から算出する。ｕ（〜）ｋ'｜ｋ、ｕ（・）（〜）ｋ'｜ｋの算出方法としては、例えば、下式（４９）、（５０）による。
次に、時刻ｔｋにおける観測モデルを下式（５１）で定義する。
上式（５１）の右辺第１項のｚｋ'は、時刻ｔｋ'における観測値ベクトルである。また、上式（５１）の右辺第２項のｖｋ'は、観測雑音ベクトルである。さらに、上式（５１）の右辺第３項の括弧内に、上式（４８）によって算出された時刻ｔｋ'におけるラジアル速度差の推定値を代入し、遅延して得られた観測値ベクトルｚｋ'の時刻を現時刻ｔｋに補正した結果を得る。
次に、ＴＤＯＡ／ＦＤＯＡが得られている場合の、遅延した測位結果に対する時刻補正の手順を、数式を用いて説明する。速度差に関する観測精度が時間によらず一定であると見なし、まず、下式（５２）を計算することにより、時刻ｔｋ'におけるラジアル加速度の差を推定する。
式（５２）におけるｓａ、ｋ'、ｓｂ、ｋ'は、時刻ｔｋ'におけるセンサの位置ベクトルであり、ｓ（・）ａ、ｋ'、ｓ（・）ｂ、ｋ'は、時刻ｔｋ'におけるセンサの速度ベクトルであり、いずれも既知量である。
上式（５２）におけるｕ（〜）ｋ'｜ｋ、ｕ（・）（〜）ｋ'｜ｋは、それぞれ時刻ｔｋ'における目標の位置ベクトルおよび速度ベクトルであり、時刻ｔｋにおける目標の位置の平滑値および速度ベクトルの平滑値から算出する。ｕ（〜）ｋ'｜ｋ、ｕ（・）（〜）ｋ'｜ｋの算出方法としては、例えば、上式（４９）、（５０）による。
次に、時刻ｔｋにおける観測モデルを式（５３）で定義する。
上式（５３）の右辺第１項のｚｋ'は、時刻ｔｋ'における観測値ベクトルである。また、上式（５３）の右辺第２項のｖｋ'は、観測雑音ベクトルである。さらに、上式（５３）の右辺第３項の括弧内に、式（５２）によって算出された時刻ｔｋ'におけるラジアル加速度の差の推定値を代入し、遅延して得られた観測値ベクトルｚｋ'の時刻を現時刻ｔｋに補正した結果を得る。
以上のように、実施の形態４によれば、複数の受信局間でのＴＤＯＡあるいはＴＤＯＡ／ＦＤＯＡが得られる。この結果、距離差およびラジアル速度差を観測値ベクトルとしてＵＫＦによって目標の位置と速度の推定を行う場合に、観測条件によらずに安定した推定精度を得ることができる。さらに、ネットワークの状況によって測位結果の転送に遅延が生じ、同時刻に得られる測位結果データの一部に欠落を生じた場合でも、追尾を維持することができる。
なお、上述の実施の形態１〜４では、複数の受信局間でのＴＤＯＡあるいはＴＤＯＡ／ＦＤＯＡが得られ、距離差およびラジアル速度差を観測値ベクトルとして目標の位置と速度を推定する例を示した。しかしながら、観測装置の形式は、他のものでもよく、観測値ベクトルの要素は、距離差およびラジアル速度差でなく、例えば、位置あるいは位置と速度で構成されていてもよい。
また、観測モデルにおける非線形項は、上式（１６）や（２０）の形式に限らず、他の形式のものであってもよい。
また、上述の実施の形態１〜４における予測値の観測値に対する適合度を算出方法の例としては、パラメータ設定部２４（ｍ）において設定されているスケーリングパラメータλｍに基づく、追尾フィルタ２（ｍ）の予測値を中心とするＮ個の確率密度関数によって尤度として算出する方法を示した。しかしながら、適合度の算出方法は、これに限定されず、他のものであってもよい。
また、上述の実施の形態１〜４において、予測値の観測値に対する適合度の低い追尾フィルタ２（ｍ）のスケーリングパラメータの変更方法の例として、予測値の残差の２次形式に基づく数式を挙げた。しかしながら、変更方法は、他のものであってもよい。
１ 目標測位装置、２、２（１）〜２（Ｎ） 追尾フィルタ、２１、２１（１）〜２１（Ｎ） 予測値算出部、２２、２２（１）〜２２（Ｎ） 予測値適合度評価部、２３、２３（１）〜２３（Ｎ） 平滑値算出部、２４、２４（１）〜２４（Ｎ） パラメータ設定部、３ 統合平滑処理部、４ 状態量更新部、５ 表示部、６ 追尾フィルタ制御部、７ 遅延観測値処理部。
Claims (5)
- 位置が未知である目標から送出される電波を位置が既知である複数の受信局で受信した受信信号に基づいて、前記目標の位置、速度の運動諸元を推定する追尾フィルタを備え、前記追尾フィルタにおける運動モデルと観測モデルが非線形関数で表される場合に、前記追尾フィルタとして前記非線形関数をアンセンテッド変換により近似するアンセンテッドカルマンフィルタを適用した目標追尾装置であって、
前記追尾フィルタは、
前記受信信号に基づいて算出された前記目標の測位結果を逐次読み込み、前記測位結果から前記アンセンテッドカルマンフィルタを適用して算出した予測値と前記測位結果とに基づいて、前記測位結果に対する前記予測値の適合度を逐次算出する予測値適合度評価部と、
前記予測値適合度評価部で逐次算出された前記適合度に基づいて、前記アンセンテッドカルマンフィルタにおける前記アンセンテッド変換のスケーリングパラメータを逐次更新するパラメータ設定部と
を備えることを特徴とする目標追尾装置。 - 請求項１に記載の目標追尾装置において、
前記予測値適合度評価部は、前記予測値と前記測位結果との残差に基づいて、前記測位結果に対する前記予測値の適合度を逐次算出することを特徴とする目標追尾装置。 - 請求項１に記載の目標追尾装置において、
前記追尾フィルタは、前記スケーリングパラメータが異なる複数の追尾フィルタで構成され、
前記複数の追尾フィルタのそれぞれは、算出した前記予測値の平滑値を算出する平滑値算出部をさらに備え、
前記複数の追尾フィルタのそれぞれの平滑値算出部で算出された複数の平滑値と、前記複数の追尾フィルタのそれぞれの予測値適合度評価部で算出された複数の適合度とを読み込み、前記複数の平滑値を前記複数の適合度により加重平均化した統合平滑値を算出することにより前記目標の最終的な予測値を算出する統合平滑処理部
をさらに備えることを特徴とする目標追尾装置。 - 請求項３に記載の目標追尾装置において、
前記複数の追尾フィルタの前段に設けられ、前記受信信号に基づいて算出された前記目標の測位結果を読み込むとともに、前記複数の追尾フィルタのそれぞれの予測値適合度評価部で算出された複数の適合度を読み込み、追尾処理開始から所定時間経過するまでは、前記複数の追尾フィルタの全てに対して前記測位結果を出力して前記複数の追尾フィルタの全てを動作させ、前記所定時間経過後は、読み込んだ前記複数の適合度に基づいて最も適合度の高い１つの追尾フィルタを特定し、特定した前記１つの追尾フィルタに対して前記測位結果を出力して前記１つの追尾フィルタのみを動作させて残りの追尾フィルタの動作を停止させる追尾フィルタ制御部をさらに備え、
前記統合平滑処理部は、前記所定時間経過後においては、特定された前記１つの追尾フィルタに対応する平滑値算出部で算出された平滑値を前記目標の最終的な予測値として算出する
ことを特徴とする目標追尾装置。 - 請求項３に記載の目標追尾装置において、
前記複数の追尾フィルタの前段に設けられ、前記受信信号に基づいて算出された前記目標の測位結果を読み込むとともに、前記統合平滑処理部で算出された前記目標の最終的な予測値を読み込み、前記測位結果に付与された第１時刻と前記最終的な予測値に付与された第２時刻とを比較し、前記第２時刻が前記第１時刻よりも過去の時刻である場合には、前記複数の追尾フィルタの全てに対して前記測位結果を出力し、前記第１時刻が前記第２時刻よりも過去の時刻である場合には、前記第１時刻における測位結果に基づいて前記第２時刻における推定測位結果を求め、前記複数の追尾フィルタの全てに対して前記推定測位結果を前記第２時刻における測位結果として出力する遅延観測値処理部をさらに備えることを特徴とする目標追尾装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2008158106A JP2009300380A (ja) | 2008-06-17 | 2008-06-17 | 目標追尾装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2008158106A JP2009300380A (ja) | 2008-06-17 | 2008-06-17 | 目標追尾装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2009300380A true JP2009300380A (ja) | 2009-12-24 |
Family
ID=41547426
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2008158106A Pending JP2009300380A (ja) | 2008-06-17 | 2008-06-17 | 目標追尾装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2009300380A (ja) |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102025430A (zh) * | 2010-11-19 | 2011-04-20 | 中兴通讯股份有限公司 | 基于闭环的自动校准方法和设备 |
| CN102680762A (zh) * | 2011-10-11 | 2012-09-19 | 国电联合动力技术有限公司 | 一种无迹卡尔曼滤波的风电场机端电压测量方法及其应用 |
| CN102930171A (zh) * | 2012-11-15 | 2013-02-13 | 北京理工大学 | 一种基于多胞型微分包含的非线性滤波方法 |
| JP2013074365A (ja) * | 2011-09-27 | 2013-04-22 | Internatl Business Mach Corp <Ibm> | カルマン・フィルタの処理方法、プログラム及びシステム |
| JP2013149203A (ja) * | 2012-01-23 | 2013-08-01 | Nippon Telegr & Teleph Corp <Ntt> | 最適モデル推定装置、方法、及びプログラム |
| JP2015532485A (ja) * | 2012-10-01 | 2015-11-09 | スネクマ | マルチセンサー測定方法及びシステム |
| WO2019167268A1 (ja) * | 2018-03-02 | 2019-09-06 | 三菱電機株式会社 | 目標監視装置および目標監視システム |
| CN110277978A (zh) * | 2019-07-04 | 2019-09-24 | 苏州妙益科技股份有限公司 | 一种基于可变渐消因子的卡尔曼滤波电流采集方法 |
| CN111489377A (zh) * | 2020-03-27 | 2020-08-04 | 北京迈格威科技有限公司 | 目标跟踪自评测方法及装置 |
| WO2020166058A1 (ja) * | 2019-02-15 | 2020-08-20 | 三菱電機株式会社 | 測位装置、測位システム、移動端末及び測位方法 |
| CN112526450A (zh) * | 2020-11-19 | 2021-03-19 | 西安电子科技大学 | 基于时钟偏差和站址误差的时差定位跟踪方法 |
| CN115792796A (zh) * | 2023-02-13 | 2023-03-14 | 鹏城实验室 | 基于相对观测等效模型的协同定位方法、装置及终端 |
| CN116482724A (zh) * | 2023-03-09 | 2023-07-25 | 北京理工大学 | 一种导航接收机高精度观测量计算方法 |
-
2008
- 2008-06-17 JP JP2008158106A patent/JP2009300380A/ja active Pending
Cited By (20)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102025430A (zh) * | 2010-11-19 | 2011-04-20 | 中兴通讯股份有限公司 | 基于闭环的自动校准方法和设备 |
| JP2013074365A (ja) * | 2011-09-27 | 2013-04-22 | Internatl Business Mach Corp <Ibm> | カルマン・フィルタの処理方法、プログラム及びシステム |
| CN102680762A (zh) * | 2011-10-11 | 2012-09-19 | 国电联合动力技术有限公司 | 一种无迹卡尔曼滤波的风电场机端电压测量方法及其应用 |
| JP2013149203A (ja) * | 2012-01-23 | 2013-08-01 | Nippon Telegr & Teleph Corp <Ntt> | 最適モデル推定装置、方法、及びプログラム |
| JP2015532485A (ja) * | 2012-10-01 | 2015-11-09 | スネクマ | マルチセンサー測定方法及びシステム |
| CN102930171A (zh) * | 2012-11-15 | 2013-02-13 | 北京理工大学 | 一种基于多胞型微分包含的非线性滤波方法 |
| JP6625295B1 (ja) * | 2018-03-02 | 2019-12-25 | 三菱電機株式会社 | 目標監視装置および目標監視システム |
| WO2019167268A1 (ja) * | 2018-03-02 | 2019-09-06 | 三菱電機株式会社 | 目標監視装置および目標監視システム |
| WO2020166058A1 (ja) * | 2019-02-15 | 2020-08-20 | 三菱電機株式会社 | 測位装置、測位システム、移動端末及び測位方法 |
| JPWO2020166058A1 (ja) * | 2019-02-15 | 2021-05-20 | 三菱電機株式会社 | 測位装置、測位システム、移動端末及び測位方法 |
| GB2594413A (en) * | 2019-02-15 | 2021-10-27 | Mitsubishi Electric Corp | Positioning device, positioning system, mobile terminal, and positioning method |
| GB2594413B (en) * | 2019-02-15 | 2022-04-20 | Mitsubishi Electric Corp | Positioning device, positioning system, mobile terminal, and positioning method |
| CN110277978A (zh) * | 2019-07-04 | 2019-09-24 | 苏州妙益科技股份有限公司 | 一种基于可变渐消因子的卡尔曼滤波电流采集方法 |
| CN111489377A (zh) * | 2020-03-27 | 2020-08-04 | 北京迈格威科技有限公司 | 目标跟踪自评测方法及装置 |
| CN111489377B (zh) * | 2020-03-27 | 2023-11-10 | 北京迈格威科技有限公司 | 目标跟踪自评测方法及装置 |
| CN112526450A (zh) * | 2020-11-19 | 2021-03-19 | 西安电子科技大学 | 基于时钟偏差和站址误差的时差定位跟踪方法 |
| CN112526450B (zh) * | 2020-11-19 | 2022-10-21 | 西安电子科技大学 | 基于时钟偏差和站址误差的时差定位跟踪方法 |
| CN115792796A (zh) * | 2023-02-13 | 2023-03-14 | 鹏城实验室 | 基于相对观测等效模型的协同定位方法、装置及终端 |
| CN116482724A (zh) * | 2023-03-09 | 2023-07-25 | 北京理工大学 | 一种导航接收机高精度观测量计算方法 |
| CN116482724B (zh) * | 2023-03-09 | 2024-05-03 | 北京理工大学 | 一种导航接收机高精度观测量计算方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP2009300380A (ja) | 目標追尾装置 | |
| CN109472418B (zh) | 基于卡尔曼滤波的机动目标状态预测优化方法 | |
| US10415975B2 (en) | Motion tracking with reduced on-body sensors set | |
| JP4775478B2 (ja) | 位置算出方法及び位置算出装置 | |
| JP6191103B2 (ja) | 移動状態算出方法及び移動状態算出装置 | |
| US8560234B2 (en) | System and method of navigation based on state estimation using a stepped filter | |
| JP2011064484A (ja) | センサバイアス推定装置 | |
| US9977113B2 (en) | Method of determining position and cooperative positioning system using same | |
| US20140214317A1 (en) | Position calculating method and position calculating device | |
| KR102172145B1 (ko) | 추측 항법 시스템에서의 밀결합 측위 방법 및 그 장치 | |
| CN109827579B (zh) | 一种组合定位中滤波模型实时校正的方法和*** | |
| JP2011127939A (ja) | 移動***置推定追尾装置、移動***置推定追尾方法、及び移動***置推定追尾プログラム | |
| JP5151833B2 (ja) | 移動***置推定システム、移動***置推定方法、及び移動***置推定プログラム | |
| KR20090019488A (ko) | 무선 측위 장치 및 방법 | |
| CN112034445B (zh) | 基于毫米波雷达的车辆运动轨迹跟踪方法和*** | |
| CN110007298B (zh) | 一种目标超前预测跟踪方法 | |
| JP4882544B2 (ja) | 追尾処理装置及びその方法並びにプログラム | |
| JP4080722B2 (ja) | 目標追尾装置及び目標追尾方法 | |
| JP4262964B2 (ja) | 移動局の位置測定方法 | |
| JP6041547B2 (ja) | 追尾装置 | |
| JP2011013228A (ja) | 位置算出方法及び位置算出装置 | |
| CN114611068A (zh) | 一种高机动目标跟踪方法 | |
| CN111123323A (zh) | 一种提高便携设备定位精度的方法 | |
| CN113030945A (zh) | 一种基于线性序贯滤波的相控阵雷达目标跟踪方法 | |
| JP2005331248A (ja) | 目標追尾装置及び目標追尾方法 |