(57)【要約】【課題】アンセンテッドカルマンフィルタの推定精度を、観測条件によらずに安定化させることのできる目標追尾装置を得る。【解決手段】追尾フィルタにおける運動モデルと観測モデルが非線形関数で表される場合に、非線形関数をアンセンテッド変換により近似するアンセンテッドカルマンフィルタを適用した追尾フィルタ２を備え、追尾フィルタ２は、受信信号に基づいて算出された目標の測位結果を逐次読み込み、測位結果からアンセンテッドカルマンフィルタを適用して算出した予測値と測位結果とに基づいて、測位結果に対する予測値の適合度を逐次算出する予測値適合度評価部２２と、予測値適合度評価部２２で逐次算出された適合度に基づいて、アンセンテッドカルマンフィルタにおけるアンセンテッド変換のスケーリングパラメータを逐次更新するパラメータ設定部２４とを備える。【選択図】図５
2009300380.tif 000002
【特許請求の範囲】
【請求項１】
位置が未知である目標から送出される電波を位置が既知である複数の受信局で受信した受信信号に基づいて、前記目標の位置、速度の運動諸元を推定する追尾フィルタを備え、前記追尾フィルタにおける運動モデルと観測モデルが非線形関数で表される場合に、前記追尾フィルタとして前記非線形関数をアンセンテッド変換により近似するアンセンテッドカルマンフィルタを適用した目標追尾装置であって、
前記追尾フィルタは、
前記受信信号に基づいて算出された前記目標の測位結果を逐次読み込み、前記測位結果から前記アンセンテッドカルマンフィルタを適用して算出した予測値と前記測位結果とに基づいて、前記測位結果に対する前記予測値の適合度を逐次算出する予測値適合度評価部と、
前記予測値適合度評価部で逐次算出された前記適合度に基づいて、前記アンセンテッドカルマンフィルタにおける前記アンセンテッド変換のスケーリングパラメータを逐次更新するパラメータ設定部と
を備えることを特徴とする目標追尾装置。
【請求項２】
請求項１に記載の目標追尾装置において、
前記予測値適合度評価部は、前記予測値と前記測位結果との残差に基づいて、前記測位結果に対する前記予測値の適合度を逐次算出することを特徴とする目標追尾装置。
【請求項３】
請求項１に記載の目標追尾装置において、
前記追尾フィルタは、前記スケーリングパラメータが異なる複数の追尾フィルタで構成され、
前記複数の追尾フィルタのそれぞれは、算出した前記予測値の平滑値を算出する平滑値算出部をさらに備え、
前記複数の追尾フィルタのそれぞれの平滑値算出部で算出された複数の平滑値と、前記複数の追尾フィルタのそれぞれの予測値適合度評価部で算出された複数の適合度とを読み込み、前記複数の平滑値を前記複数の適合度により加重平均化した統合平滑値を算出することにより前記目標の最終的な予測値を算出する統合平滑処理部
をさらに備えることを特徴とする目標追尾装置。
【請求項４】
請求項３に記載の目標追尾装置において、
前記複数の追尾フィルタの前段に設けられ、前記受信信号に基づいて算出された前記目標の測位結果を読み込むとともに、前記複数の追尾フィルタのそれぞれの予測値適合度評価部で算出された複数の適合度を読み込み、追尾処理開始から所定時間経過するまでは、前記複数の追尾フィルタの全てに対して前記測位結果を出力して前記複数の追尾フィルタの全てを動作させ、前記所定時間経過後は、読み込んだ前記複数の適合度に基づいて最も適合度の高い１つの追尾フィルタを特定し、特定した前記１つの追尾フィルタに対して前記測位結果を出力して前記１つの追尾フィルタのみを動作させて残りの追尾フィルタの動作を停止させる追尾フィルタ制御部をさらに備え、
前記統合平滑処理部は、前記所定時間経過後においては、特定された前記１つの追尾フィルタに対応する平滑値算出部で算出された平滑値を前記目標の最終的な予測値として算出する
ことを特徴とする目標追尾装置。
【請求項５】
請求項３に記載の目標追尾装置において、
前記複数の追尾フィルタの前段に設けられ、前記受信信号に基づいて算出された前記目標の測位結果を読み込むとともに、前記統合平滑処理部で算出された前記目標の最終的な予測値を読み込み、前記測位結果に付与された第１時刻と前記最終的な予測値に付与された第２時刻とを比較し、前記第２時刻が前記第１時刻よりも過去の時刻である場合には、前記複数の追尾フィルタの全てに対して前記測位結果を出力し、前記第１時刻が前記第２時刻よりも過去の時刻である場合には、前記第１時刻における測位結果に基づいて前記第２時刻における推定測位結果を求め、前記複数の追尾フィルタの全てに対して前記推定測位結果を前記第２時刻における測位結果として出力する遅延観測値処理部をさらに備えることを特徴とする目標追尾装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、観測装置が観測した目標を追尾する目標追尾装置に関する。
【背景技術】
【０００２】
従来の目標追尾装置においては、次のような方法で、目標の位置および速度を推定している。まず、異なる位置に配置された複数の受信局によりネットワークを構成し、受信局間での電波到来時間差（Ｔｉｍｅ  Ｄｉｆｆｅｒｅｎｃｅ  Ｏｆ  Ａｒｒｉｖａｌ：以下、ＴＤＯＡと称す）より換算される各受信局−目標間の距離の差（以下、距離差と称す）を求める。
【０００３】
さらに、ドップラー周波数差（Ｆｒｅｑｕｅｎｃｙ  Ｄｉｆｆｅｒｅｎｃｅ  Ｏｆ  Ａｒｒｉｖａｌ：以下、ＦＤＯＡと称す）より換算される各受信局−目標間のラジアル速度の差（以下、ラジアル速度差と称す）を求める。そして、目標追尾装置は、上述のようにして求められた距離差およびラジアル速度差を非同期に入力し、追尾処理を行うことで目標の位置および速度を推定する。
【０００４】
目標の状態変数ベクトルを、北基準直交座標系における位置、速度で定義すると、ＴＤＯＡおよびＦＤＯＡは、非線形関数となる。そして、追尾目標の運動モデルは、非線形関数式（１）で表され、観測モデルは、非線形関数式（２）で表される。
【０００５】
【数１】
2009300380.tif 000003
【０００６】
上式（１）および（２）において、ｘｋはサンプリング時刻ｔｋにおける目標の状態変数ベクトル、ｗｋは駆動雑音ベクトル（平均０、共分散行列Ｑｋ）、Γ１は駆動雑音ベクトルに対する変換行列を表す。また、式（２）において、ｚｋはサンプリング時刻ｔｋにおける目標の観測値ベクトル、ｖｋは観測雑音ベクトル（平均０、共分散行列Ｒｋ）を表す。
【０００７】
このように、運動モデルと観測モデルが非線形関数となる場合、従来技術では、モデルの非線形性を考慮し、追尾フィルタとして拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒ：以下、ＥＫＦと称す）を適用するのが一般的である（例えば、特許文献１参照）。
【０００８】
【特許文献１】特開２００６−２０１０８４号公報
【発明の開示】
【発明が解決しようとする課題】
【０００９】
しかしながら、従来技術には次のような課題がある。
ＥＫＦでは、非線形関数に対する部分的な線形一次近似によって、カルマンフィルタの理論を適用する。このため、問題の非線形性が強い場合には、線形一次近似による真値からの誤差が大きくなり、追尾フィルタによる推定値が真の期待値および誤差共分散から大きく外れる可能性がある。
【００１０】
このようなＥＫＦにおける非線形関数の線形一次近似の問題を軽減するため、追尾フィルタとしてアンセンテッドカルマンフィルタ（Ｕｎｓｃｅｎｔｅｄ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒ：以下、ＵＫＦと称す）を適用する。
【００１１】
ＵＫＦでは、アンセンテッド変換（Ｕｎｓｃｅｎｔｅｄ  Ｔｒａｎｓｆｏｒｍａｔｉｏｎ：以下、ＵＴと称す）により、非線形関数をテーラー展開の３次の項まで近似することができる。このため、ＥＫＦを適用する場合と比較して、非線形関数となるモデルにおける推定精度を、より高くすることが期待できる
【００１２】
ＵＴでは、まず、２ｎ＋１個のシグマポイントを、下式（３）〜（５）に従って算出する。
【００１３】
【数２】
2009300380.tif 000004
【００１４】
上式（３）〜（５）において、ｘ（＾）ｋ｜ｋは、時刻ｔｋにおける追尾フィルタによる平滑値ベクトル、Ｐｋ｜ｋは、平滑誤差共分散行列を表す（ここで、（＾）という表記は、かっこの前の文字の上部に＾が付されることを意味している）。特に、平滑誤差共分散行列Ｐｋ｜ｋについては、下三角行列√（Ｐｋ｜ｋ）と上三角行列（√（Ｐｋ｜ｋ））Ｔに分解し、下式（６）のように表す。
【００１５】
【数３】
2009300380.tif 000005
【００１６】
また、上式（４）、（５）におけるλは、スケーリングパラメータであり、［√｛（ｎ＋λ）Ｐｋ｜ｋ｝］ｉは、下三角行列√｛（ｎ＋λ）Ｐｋ｜ｋ｝の第ｉ列を表す。
【００１７】
ＵＫＦでは、ＵＴによって上式（３）〜（５）で算出される２ｎ＋１個のシグマポイントが、モデルの非線形関数の非線形性を軽減する項となる。
【００１８】
しかし、上式（４）、（５）に示されるように、シグマポイントにはスケーリングパラメータλが含まれている。そして、適切なスケーリングパラメータの値の設定は、観測精度やサンプリングレートなどの観測条件によって異なる。従って、設定が不適切な場合には、推定値が発散する、あるいは、推定値の精度劣化を引き起こすなどの課題がある。
【００１９】
本発明は、前記のような課題を解決するためになされたものであり、アンセンテッドカルマンフィルタ（ＵＫＦ）の推定精度を、観測条件によらずに安定化させることのできる目標追尾装置を得ることを目的とする。
【課題を解決するための手段】
【００２０】
本発明に係る目標追尾装置は、位置が未知である目標から送出される電波を位置が既知である複数の受信局で受信した受信信号に基づいて、目標の位置、速度の運動諸元を推定する追尾フィルタを備え、追尾フィルタにおける運動モデルと観測モデルが非線形関数で表される場合に、前記追尾フィルタとして非線形関数をアンセンテッド変換により近似するアンセンテッドカルマンフィルタを適用した目標追尾装置であって、追尾フィルタは、受信信号に基づいて算出された目標の測位結果を逐次読み込み、測位結果からアンセンテッドカルマンフィルタを適用して算出した予測値と測位結果とに基づいて、測位結果に対する予測値の適合度を逐次算出する予測値適合度評価部と、予測値適合度評価部で逐次算出された適合度に基づいて、アンセンテッドカルマンフィルタにおけるアンセンテッド変換のスケーリングパラメータを逐次更新するパラメータ設定部とを備えるものである。
【発明の効果】
【００２１】
本発明に係る目標追尾装置によれば、運動モデルおよび観測モデルが非線形性関数となる場合において、測位結果と推定値に基づいて、観測毎にアンセンテッドカルマンフィルタ（ＵＫＦ）のスケーリングパラメータを更新する構成を備えることにより、ＵＫＦの推定精度を、観測条件によらずに安定化させることのできる目標追尾装置を得ることができる。
【発明を実施するための最良の形態】
【００２２】
以下、本発明の目標追尾装置の好適な実施の形態につき図面を用いて説明する。
【００２３】
実施の形態１．
図１は、本発明の実施の形態１における目標追尾装置の構成の一例を示すブロック図である。この図１の目標追尾装置は、目標測位装置１、Ｎ個の追尾フィルタ２（１）〜２（Ｎ）、統合平滑処理部３、状態量更新部４、および表示部５を備えている。
【００２４】
ここで、ｍ＝１〜Ｎとすると、各追尾フィルタ２（ｍ）は、予測値算出部２１（ｍ）、予測値適合度評価部２２（ｍ）、平滑値算出部２３（ｍ）、およびパラメータ設定部２４（ｍ）を備えている。
【００２５】
次に、各構成要素の機能について説明する。
目標測位装置１は、受信局によって受信された信号に基づいて、目標の位置、速度などの情報を測位結果として算出する。各追尾フィルタ２（ｍ）は、目標測位装置１から、算出された測位結果および受信局の位置情報を入力し、目標の位置および速度を推定する。
【００２６】
追尾フィルタ２（ｍ）において、予測値算出部２１（ｍ）は、目標測位装置１より目標の測位結果と受信局の位置情報を入力し、パラメータ設定部２４（ｍ）よりＵＫＦのスケーリングパラメータの設定を入力する。そして、予測値算出部２１（ｍ）は、パラメータ設定部２４（ｍ）で設定されたスケーリングパラメータを持つＵＫＦにより、目標の位置および速度の予測値を算出する。
【００２７】
予測値適合度評価部２２（ｍ）は、目標測位装置１より目標の測位結果を入力するとともに、予測値算出部２１（ｍ）より目標の位置および速度の予測値を入力する。そして、予測値適合度評価部２２（ｍ）は、予測値と観測値の差あるいは距離に基づいて、測位結果に対する予測値の適合度を算出し、適合度を統合平滑処理部３に対して出力する。
【００２８】
また、予測値適合度評価部２２（ｍ）は、適合度が所定値よりも低い場合には、スケーリングパラメータの設定を修正し、パラメータ設定部２４（ｍ）におけるスケーリングパラメータの設定を更新する。
【００２９】
平滑値算出部２３（ｍ）は、予測値算出部２１（ｍ）より目標の位置および速度の予測値を入力し、ＵＫＦの手順に従って目標位置の平滑値を算出する。
【００３０】
統合平滑処理部３は、Ｎ個の平滑値算出部２３（１）〜２３（Ｎ）より目標の位置および速度の平滑値を入力するとともに、Ｎ個の予測値適合度評価部２２（１）〜２２（Ｎ）より予測値の適合度を入力する。そして、統合平滑処理部３は、Ｎ個の平滑値をそれぞれに対応する適合度によって重み付けし、さらに平滑値の重み付け平均値を算出して統合平滑値とし、状態量更新部４および表示部５に対して出力する。
【００３１】
状態量更新部４は、統合平滑処理部３で算出されたＮ個の追尾フィルタ２（１）〜２（Ｎ）の統合平滑値を入力し、統合平滑値より距離差およびラジアル速度差の統合予測値を算出する。そして、状態量更新部４は、算出した統合予測値を目標測位装置１にフィードバックする。
【００３２】
表示部５は、統合平滑処理部３で算出されたＮ個の追尾フィルタ２（１）〜２（Ｎ）の統合平滑値を入力し、統合平滑値を現時刻における目標の位置および速度の推定値として表示する。
【００３３】
次に、ＴＤＯＡおよびＦＤＯＡが得られ、目標と受信局の間の距離差およびラジアル速度差を観測値とする場合を例として、並列ＵＫＦによって目標を追尾するための各種定義事項について説明する。
【００３４】
図２は、本発明の実施の形態１における受信局と信号源である目標との位置関係を示した図である。ここでは、目標の３次元運動を推定する場合を仮定する。
【００３５】
図２において、ｓａ、ｓｂは、それぞれ受信局ａ、受信局ｂの位置ベクトルを表す。また、ｒａは、受信局ａから信号源までの距離、ｒｂは、受信局ｂから信号源までの距離をそれぞれ表す。ｕは、目標（信号源）の位置ベクトルを表す。
【００３６】
ここで、目標の運動モデルとして、下式（７）で表される等速直線運動を仮定する。
【００３７】
【数４】
2009300380.tif 000006
【００３８】
上式（７）において、ｘｋは、位置および速度ベクトルで構成される目標の状態変数ベクトルを表す。また、上式（７）の各要素は、それぞれ下式（８）〜（１３）で表される。
【００３９】
【数５】
2009300380.tif 000007
【００４０】
ここで、ｕｋは、信号源の位置ベクトルを表し、ｕｋ（・）は、信号源の速度ベクトルを表す（ここで、（・）という表記は、かっこの前の文字の上部に・が付されることを意味している）。Φｋは、推移行列を表す。ｗｋは、駆動雑音ベクトル、Γ1、ｋは、駆動雑音ベクトルの変換行列である。
【００４１】
次に、観測モデルを定義する。
まず、ＴＤＯＡが得られる場合における追尾方式（以下、ＴＤＯＡ追尾方式と称す）の観測モデルを示す。
【００４２】
ＴＤＯＡ方式における観測値ベクトルは、受信局間のＴＤＯＡより換算される受信局と信号源との距離ｒａ、ｒｂの差（距離差）である。この距離差を観測値として用いる場合の観測モデルを、下式（１４）、（１５）で定義する。
【００４３】
【数６】
2009300380.tif 000008
【００４４】
上式（１５）におけるｒａｂは、距離差を表す。また、上式（１４）におけるｈ（ｘｋ）は、下式（１６）で定義される。
【００４５】
【数７】
2009300380.tif 000009
【００４６】
また、上式（１４）における観測雑音ベクトルｖｋについては、下式（１７）のように距離差の成分として与える。
【００４７】
【数８】
2009300380.tif 000010
【００４８】
次に、ＴＤＯＡとＦＤＯＡが得られる場合における追尾方式（以下、ＴＤＯＡ／ＦＤＯＡ追尾方式と称す）の観測モデルを示す。
【００４９】
ＴＤＯＡ／ＦＤＯＡ追尾方式における観測値ベクトルは、距離差とＦＤＯＡから換算される受信局と信号源との距離ｒａ、ｒｂの変化率の差（ラジアル速度差）であり、距離差とラジアル速度差を観測値として用いる場合の観測モデルを下式（１８）〜（２０）で定義する。
【００５０】
【数９】
2009300380.tif 000011
【００５１】
上式（１８）における観測雑音ベクトルｖｋについては、下式（２１）のように距離差、ラジアル速度差の成分を持つベクトルとして与える。
【００５２】
【数１０】
2009300380.tif 000012
【００５３】
次に、並列ＵＫＦの、各追尾フィルタ２（ｍ）における追尾処理について説明する。
各追尾フィルタ２（ｍ）において、まず、２ｎ＋１個のシグマポイントを、下式（２２）〜（２４）に従って算出する。
【００５４】
【数１１】
2009300380.tif 000013
【００５５】
上式（２２）〜（２４）において、ｘ（＾）ｍ、ｋ｜ｋは、時刻ｔｋにおける追尾フィルタ２（ｍ）による平滑値ベクトル、Ｐｍ、ｋ｜ｋは、追尾フィルタ２（ｍ）による平滑誤差共分散行列を表す。特に、平滑誤差共分散行列Ｐｍ、ｋ｜ｋについては、下三角行列√（Ｐｍ、ｋ｜ｋ）と上三角行列（√（Ｐｍ、ｋ｜ｋ））Ｔに分解し、下式（２５）のように表す。
【００５６】
【数１２】
2009300380.tif 000014
【００５７】
また、上式（２３）、（２４）におけるλｍは、追尾フィルタ２（ｍ）のパラメータ設定部２４（ｍ）により設定されるスケーリングパラメータであり、［√｛（ｎ＋λｍ）Ｐｍ、ｋ｜ｋ｝］ｉは、下三角行列√｛（ｎ＋λｍ）Ｐｍ、ｋ｜ｋ｝の第ｉ列を表す。
【００５８】
次に、上式（２２）〜（２４）によって得たシグマポイントごとに、予測処理を下式（２６）に従って行うとともに、予測値の統合処理を下式（２７）、（２８）に従って行う。
【００５９】
【数１３】
2009300380.tif 000015
【００６０】
ここで、上式（２７）、（２８）におけるωｍ、ｉは、下式（２９）、（３０）に従って算出する。
【００６１】
【数１４】
2009300380.tif 000016
【００６２】
次に、上式（２７）〜（３０）の予測処理および予測値の統合処理を実施した後、ｒａとｒｂの距離差の予測値ｒ（〜）ｍ、ａｂ、およびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂを、上式（２７）で得た予測値ベクトルにより、下式（３１）、（３２）に従って算出する（ここで、（〜）という表記は、かっこの前の文字の上部に〜が付されることを意味し、（・）（〜）という表記は、かっこの前の文字の上部に・が付されたものにさらに〜が付されることを意味している）。
【００６３】
【数１５】
2009300380.tif 000017
【００６４】
次に、新たな観測値ベクトルが得られた場合の位置と速度の平滑値を得る、データ更新について説明する。まず、シグマポイント毎に得た予測値ベクトルｘ（〜）ｍ、ｋ＋１｜ｋ、ｉ、および統合後の予測値ベクトルｘ（〜）ｍ、ｋ＋１｜ｋより、下式（３３）、（３４）を計算する。
【００６５】
【数１６】
2009300380.tif 000018
【００６６】
次に、Ｐｍ、ＸＺ、Ｐｍ、ＺＺおよび観測誤差共分散行列Ｒｍ、ｋによって残差共分散行列Ｓｍ、ｋ｜ｋ-1、ゲイン行列Ｋｍ、ｋをそれぞれ下式（３５）、（３６）に従って算出する。
【００６７】
【数１７】
2009300380.tif 000019
【００６８】
次に、平滑値ベクトル、平滑誤差共分散行列を下式（３７）、（３８）に従って算出する。
【００６９】
【数１８】
2009300380.tif 000020
【００７０】
上述した式（２２）〜（３８）までに示した処理が、各追尾フィルタ２（ｍ）において並列に行われるものとする。
【００７１】
次に、本実施の形態１における目標追尾装置の一連の動作について説明する。
図３は、本発明の実施の形態１の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。図３において、ステップＳＴ２ａ（ｍ）〜ステップＳＴ２ｅ（ｍ）（ｍ＝１〜Ｎ）は、図１における追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）のそれぞれの動作を示す。以下、各ステップの動作について説明する。
【００７２】
ステップＳＴ１の動作
目標測位装置１は、複数の受信局による観測の結果、ＴＤＯＡが得られている場合には、ＴＤＯＡから距離差ｒａｂを算出する。また、目標測位装置１は、ＴＤＯＡとＦＤＯＡが得られている場合には、ＴＤＯＡ／ＦＤＯＡから距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂを算出する。
【００７３】
そして、目標測位装置１は、距離差およびラジアル速度差に対して、相関ゲートによりマルチパス排除を行った結果を、目標の測位結果として追尾フィルタ２（ｍ）に対して出力する。また、受信局の位置情報ｓａ、ｓｂを追尾フィルタ２（ｍ）に対して出力する。
【００７４】
ステップＳＴ２ａ（ｍ）（ｍ＝１〜Ｎ）の動作
追尾フィルタ２（ｍ）において、予測値算出部２１（ｍ）は、目標測位装置１により算出された距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂおよび受信局の位置情報を入力する。
【００７５】
ステップＳＴ２ｂ（ｍ）（ｍ＝１〜Ｎ）の動作
追尾フィルタ２（ｍ）において、予測値算出部２１（ｍ）は、目標測位装置１より目標の測位結果（距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂ）と受信局の位置情報（ｓａ、ｓｂ）を入力する。さらに、予測値算出部２１（ｍ）は、パラメータ設定部２４（ｍ）よりＵＫＦのスケーリングパラメータの設定λｍを入力する。
【００７６】
そして、予測値算出部２１（ｍ）は、パラメータ設定部２４（ｍ）で設定されたスケーリングパラメータを持つＵＫＦにより、目標の位置と速度の予測値ｘ（〜）ｍ、ｋ＋１｜ｋおよび予測誤差共分散行列Ｐｍ、ｋ＋１｜ｋを算出する。さらに、予測値算出部２１（ｍ）は、算出結果を平滑値算出部２３（ｍ）に対して出力する。
【００７７】
追尾フィルタの運動モデルとしては、上式（７）〜（１３）で定義される運動モデルを使用する。また、観測モデルとしては、ＴＤＯＡが得られる場合には、上式（１４）〜（１７）、ＴＤＯＡ／ＦＤＯＡが得られる場合には、上式（１８）〜（２１）を使用する。
【００７８】
目標の位置および速度の予測値を算出する手順は、上式（２２）〜（３０）による。
目標の位置および速度の予測値より、距離差の予測値ｒ（〜）ｍ、ａｂ、およびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂを算出する。そして、予測値適合度評価部２２（ｍ）に対して出力する。
【００７９】
目標の位置および速度の予測値から距離差の予測値およびラジアル速度差の予測値を算出する手順は、上式（３１）、（３２）による。
【００８０】
ステップＳＴ２ｃ（ｍ）（ｍ＝１〜Ｎ）の動作
予測値適合度評価部２２（ｍ）は、予測値算出部２１（ｍ）によって算出された距離差の予測値ｒ（〜）ｍ、ａｂおよびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂの適合度を評価する。さらに、予測値適合度評価部２２（ｍ）は、評価した適合度に応じてパラメータ設定部２４（ｍ）におけるスケーリングパラメータλｍの設定を変更する。
【００８１】
図４は、本発明の実施の形態１の予測値適合度評価部２２（ｍ）における適合度を評価の過程およびスケーリングパラメータλｍの設定を変更する過程の説明図である。予測値適合度評価部２２（ｍ）は、目標測位装置１より距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂを入力する。さらに、予測値適合度評価部２２（ｍ）は、予測値算出部２１（ｍ）より距離差の予測値ｒ（〜）ｍ、ａｂおよびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂを入力する。
【００８２】
そして、予測値適合度評価部２２（ｍ）は、予測値と観測値の差あるいは距離に基づいて、予測値の測位結果に対する適合度を算出する。そして、予測値適合度評価部２２（ｍ）は、算出した適合度を統合平滑処理部３に対して出力する。
【００８３】
予測値の適合度の算出方法としては、例えば、次のものが考えられる。予測値適合度評価部２２（ｍ）は、各追尾フィルタ２（ｍ）のパラメータ設定部２４（ｍ）において設定されているスケーリングパラメータλｍに基づいて、追尾フィルタ２（ｍ）の予測値を中心とするＮ個の確率密度関数式である下式（３９）に、目標測位装置による測位結果を代入し、尤度として算出することができる。
【００８４】
【数１９】
2009300380.tif 000021
【００８５】
ステップＳＴ２ｄ（ｍ）（ｍ＝１〜Ｎ）の動作
予測値適合度評価部２２（ｍ）は、上式（３９）あるいはその他の計算式によって算出される予測値適合度を評価するための閾値Ｔｈｐｌを有している。そして、予測値適合度評価部２２（ｍ）は、上式（３９）による計算結果がＴｈｐｌ以上の場合には、スケーリングパラメータの設定を存続させる。
【００８６】
一方、予測値適合度評価部２２（ｍ）は、上式（３９）による計算結果がＴｈｐｌ未満の場合には、スケーリングパラメータの設定を修正し、修正の結果をパラメータ設定部２４（ｍ）に対して出力する。この結果、パラメータ設定部２４（ｍ）におけるスケーリングパラメータの設定が更新される。ここで、閾値Ｔｈｐｌは、あらかじめ設定するパラメータである。
【００８７】
スケーリングパラメータλｍの修正方法は、例えば、下式（４０）による。下式（４０）の右辺第２項のΔλは、スケーリングパラメータの修正量であり、例えば、下式（４１）を用いる。下式（４１）の右辺におけるεは、あらかじめ設定するパラメータである。
【００８８】
【数２０】
2009300380.tif 000022
【００８９】
ステップＳＴ２ｅ（ｍ）（ｍ＝１〜Ｎ）の動作
平滑値算出部２３（ｍ）は、予測値算出部２１（ｍ）より目標の位置と速度の予測値ｘ（〜）ｍ、ｋ＋１｜ｋおよび予測誤差共分散行列Ｐｍ、ｋ＋１｜ｋを入力し、ＵＫＦの手順に従って目標位置の平滑値ｘ（＾）ｍ、ｋ｜ｋおよび平滑誤差共分散行列Ｐｍ、ｋ｜ｋを算出する。平滑値および平滑誤差共分散行列の算出手順は、上式（３３）〜（３８）による。
【００９０】
本実施の形態１における目標追尾装置では、各追尾フィルタ２（ｍ）において、上述したステップＳＴ２ａ（ｍ）からステップＳＴ２ｅ（ｍ）（ｍ＝１〜Ｎ）までが並列に動作しているものとする。
【００９１】
ステップＳＴ３の動作
統合平滑処理部３は、平滑値算出部２３（ｍ）（ｍ＝１〜Ｎ）より目標の位置と速度の平滑値ｘ（＾）ｍ、ｋ｜ｋを入力し、予測値適合度評価部２２（ｍ）より予測値の適合度Ｐｍ（λｍ｜ｒａｂ、ｒ（・）ａｂ）を入力する。そして、統合平滑処理部３は、Ｎ個の平滑値をそれぞれに対応する適合度によって重み付けし、平滑値の重み付け平均値ｘ（＾）ｋ｜ｋを算出する。さらに、統合平滑処理部３は、重み付け平均値を統合平滑値として、状態量更新部４および表示部５に対して出力する。
【００９２】
重み付け平均値の算出方法としては、適合度の算出方法が上式（３９）である場合には、例えば、下式（４２）による。
【００９３】
【数２１】
2009300380.tif 000023
【００９４】
ステップＳＴ４の動作
状態量更新部４は、統合平滑処理部３より追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の統合平滑値ｘ（＾）ｋ｜ｋを入力する。そして、状態量更新部４は、統合平滑値より距離差の統合予測値ｒ（〜）ａｂ、およびラジアル速度差の統合予測値ｒ（・）（〜）ａｂを算出し、これらの統合予測値を、目標測位装置１にフィードバックする。統合予測値の算出方法としては、下式（４３）、（４４）による。
【００９５】
【数２２】
2009300380.tif 000024
【００９６】
ステップＳＴ５の動作
表示部５は、統合平滑処理部３より追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の統合平滑値ｘ（＾）ｋ｜ｋを入力し、統合平滑値を現時刻における目標の位置および速度の推定値として表示する。
【００９７】
以上のように、実施の形態１によれば、運動モデルおよび観測モデルが非線形性関数となる場合において、異なるスケーリングパラメータの設定を持つ複数のＵＫＦを用いて、それぞれのＵＫＦによる目標の位置および速度の予測値の適合度に基づいて、観測毎にそれぞれのＵＫＦのスケーリングパラメータを更新している。このような構成により、観測条件によって適切なスケーリングパラメータの設定が異なるＵＫＦを用いた追尾フィルタによる目標の位置と速度の推定精度を、観測条件によらずに安定させることができる。
【００９８】
実施の形態２．
図５は、本発明の実施の形態２における目標追尾装置の構成の一例を示すブロック図である。本実施の形態２における目標追尾装置は、先の実施の形態１におけるＮ個の追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）を１個とし、統合平滑処理部３を省いた場合に相当する。
【００９９】
追尾フィルタ２における予測値算出部２１とパラメータ設定部２４の機能は、それぞれ先の実施の形態１における予測値算出部２１（ｍ）、パラメータ設定部２４（ｍ）と同一である。
【０１００】
予測値適合度評価部２２は、目標測位装置１より目標の測位結果を入力し、予測値算出部２１より目標の位置および速度の予測値を入力する。そして、予測値適合度評価部２２は、予測値の測位結果に対する残差を計算し、残差に基づいてパラメータ設定部２４（ｍ）におけるスケーリングパラメータの設定を更新する。
【０１０１】
平滑値算出部２３は、予測値算出部２１より目標の位置および速度の予測値を入力し、ＵＫＦの手順に従って目標位置の平滑値を算出する。そして、平滑値算出部２３は、平滑値を状態量更新部４および表示部５に対して出力する。
【０１０２】
また、状態量更新部４は、平滑値を目標測位装置１にフィードバックする。表示部５は、追尾フィルタ２の平滑値算出部２３より追尾フィルタの平滑値を入力し、平滑値を現時刻における目標の位置および速度の推定値として表示する。
【０１０３】
次に、本実施の形態２における目標追尾装置の動作について説明する。図６は、本発明の実施の形態２の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。以下、各ステップの動作について説明する。
【０１０４】
ステップＳＴ１〜ステップＳＴ２ｂの動作
この図６におけるステップＳＴ１〜ステップＳＴ２ｂの動作は、それぞれ、先の実施の形態１におけるステップＳＴ１〜ステップＳＴ２ｂ（ｍ）と共通である。予測処理に用いる数式としては、上式（２２）〜（３２）において、ｍ＝１のみとした場合に相当する。
【０１０５】
ステップＳＴ２ｃの動作
予測値適合度評価部２２は、目標測位装置１より距離差ｒａｂとラジアル速度差ｒ（・）ａ、ｂを入力し、予測値算出部２１より距離差の予測値ｒ（〜）ｍ、ａｂおよびラジアル速度差の予測値ｒ（・）（〜）ｍ、ａｂを入力する。そして、予測値適合度評価部２２は、予測値と測位結果との残差を計算する。
【０１０６】
ステップＳＴ２ｄの動作
予測値適合度評価部２２は、ステップＳＴ２ｃにおいて計算した残差に基づいて、パラメータ設定部２４におけるスケーリングパラメータの設定を更新する。スケーリングパラメータの修正方法としては、例えば、下式（４５）による。下式（４５）の右辺第２項のΔλ２は、スケーリングパラメータの修正量であり、例えば、下式（４６）を用いる。下式（４６）の右辺におけるε２は、あらかじめ設定するパラメータである。
【０１０７】
【数２３】
2009300380.tif 000025
【０１０８】
ステップＳＴ２ｅの動作
平滑値算出部２３は、予測値算出部２１より目標の位置と速度の予測値ｘ（〜）ｋ＋１｜ｋおよび予測誤差共分散行列Ｐｋ＋１｜ｋを入力する。そして、平滑値算出部２３は、ＵＫＦの手順に従って、目標位置の平滑値ｘ（＾）ｋ｜ｋおよび平滑誤差共分散行列Ｐｋ｜ｋを算出し、状態量更新部４および表示部５に対して出力する。平滑値および平滑誤差共分散行列の算出には、上式（３３）〜（３８）において、ｍ＝１のみとした場合のものを用いる。
【０１０９】
ステップＳＴ４の動作
状態量更新部４は、平滑値算出部２３より追尾フィルタ２の平滑値ｘ（＾）ｋ｜ｋを入力し、平滑値より距離差の予測値ｒ（〜）ａｂ、およびラジアル速度差の予測値ｒ（・）（〜）ａｂを算出する。そして、状態量更新部４は、これらの予測値を目標測位装置１にフィードバックする。距離差およびラジアル速度差の予測値の算出方法は、上式（４３）、（４４）による。
【０１１０】
以上のように、実施の形態２によれば、追尾フィルタには単一のＵＫＦを用いつつ、観測毎にＵＫＦのスケーリングパラメータを更新することにより、観測条件によらずに安定した推定精度を得ることができる。さらに、先の実施の形態１に比べ、目標追尾装置の構成を簡略化し、追尾処理の負荷を低減できる。
【０１１１】
なお、本実施の形態２おけるスケーリングパラメータの修正方法の例としては、予測値の残差に基づく方法を示した。しかしながら、スケーリングパラメータの修正方法は、他のものであってもよい。
【０１１２】
実施の形態３．
図７は、本発明の実施の形態３における目標追尾装置の構成の一例を示すブロック図である。本実施の形態３における目標追尾装置は、先の実施の形態１の目標追尾装置に対して、追尾フィルタ制御部６をさらに備えている。そして、目標測位装置１から、追尾フィルタ制御部６を介して各追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の予測値算出部２１（ｍ）に接続されている。
【０１１３】
本実施の形態３における目標測位装置１、予測値算出部２１（ｍ）、平滑値算出部２３（ｍ）、パラメータ設定部２４（ｍ）、状態量更新部４の機能は、それぞれ、先の実施の形態１における目標追尾装置のものと同一である。
【０１１４】
また、統合平滑処理部３の機能は、追尾フィルタ制御部６によって動作する追尾フィルタが１つに限定されるまでは、先の実施の形態１における目標追尾装置のものと同一である。追尾フィルタ制御部６によって動作する追尾フィルタが１つに限定された後における、統合平滑処理部３の機能については、後述する。
【０１１５】
本実施の形態３における予測値適合度評価部２２（ｍ）は、先の実施の形態１における予測値適合度評価部２２（ｍ）の機能を有するのに加え、算出した予測値の適合度を追尾フィルタ制御部６に入力する機能を有している。
【０１１６】
追尾フィルタ制御部６は、目標測位装置１から目標の測位結果と受信機の位置情報を入力し、追尾処理を開始してからあらかじめ設定した時間が経過するまでは、全ての追尾フィルタ＃ｍ（ｍ＝１〜Ｎ）に、測位結果と受信機の位置情報を入力する。
【０１１７】
また、追尾フィルタ制御部６は、各追尾フィルタ２（ｍ）の予測値適合度評価部２２（ｍ）より、追尾フィルタ２（ｍ）による予測値の測位結果に対する適合度を入力する。そして、追尾フィルタ制御部６は、追尾処理を開始してからあらかじめ設定した時間が経過した時点で、最も予測値の適合度が高い追尾フィルタ１つのみを選択し、残る（Ｎ−１）個の追尾フィルタの動作を停止させる。
【０１１８】
動作する追尾フィルタが１つに限定された後では、統合平滑処理部３は、動作している追尾フィルタによる平滑値をそのまま統合平滑値とし、状態量更新部４および表示部５に対して出力する。
【０１１９】
次に、本実施の形態３における目標追尾装置の動作について説明する。図８は、本発明の実施の形態３の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。以下、各ステップの動作について説明する。
【０１２０】
ステップＳＴ１、ステップＳＴ２ａ（ｍ）〜ステップＳＴ２ｅ（ｍ）、ステップＳＴ４、ステップＳＴ５の動作
これらの動作は、先の実施の形態１における図３のものと同一である。
【０１２１】
ステップＳＴ６の動作
追尾フィルタ制御部６は、追尾処理を開始してからの時間を計測し、所定の時間ｔｃｈａｎｇｅが経過した後、出力先として、かかる時点で最も予測値の適合度が高い追尾フィルタ１つのみを選択し、残るＮ−１個の追尾フィルタの動作を停止させる。時間ｔｃｈａｎｇｅは、あらかじめ追尾フィルタ制御部６に設定するパラメータである。
【０１２２】
ステップＳＴ３の動作
統合平滑処理部３の動作は、追尾フィルタ制御部６によって動作する追尾フィルタが１つに限定されるまでは、先の実施の形態１における目標追尾装置のものと同一である。一方、追尾フィルタ制御部６によって動作する追尾フィルタが１つに限定された後では、動作している追尾フィルタによる平滑値をそのまま統合平滑値とする。
【０１２３】
以上のように、実施の形態３によれば、追尾精度が安定しない追尾開始直後の時間帯に並列ＵＫＦによる追尾を行う過程でＵＫＦのスケーリングパラメータとして最適な値を算出する。そして、一定時間経過後は、最適なスケーリングパラメータを持ったＵＫＦによる追尾フィルタのみを動作させる。これにより、観測条件によらずに安定した推定精度を得ることができると同時に、追尾開始直後の時間帯より後の追尾処理にかかわる負荷を低減することができる。
【０１２４】
実施の形態４．
図９は、本発明の実施の形態４における目標追尾装置の構成の一例を示すブロック図である。本実施の形態４における目標追尾装置は、先の実施の形態１の目標測位装置に対して、遅延観測値処理部７をさらに備えている。そして、目標測位装置１から、遅延観測値処理部７を介して各追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の予測値算出部２１（ｍ）に接続されている。
【０１２５】
本実施の形態４における目標測位装置１は、実施の形態１における目標測位装置と同様に目標を測位し、測位結果を遅延観測値処理部７に対して出力する。遅延観測値処理部７は、目標測位装置１より目標に対する測位結果を入力し、状態量更新部４より統合平滑処理後の統合予測値を入力し、測位結果の時刻と最新の統合予測値の時刻とを比較する。
【０１２６】
測位結果に付与されている時刻が最新の統合予測値に付与されている時刻よりも新しい場合（すなわち、測位結果が追尾フィルタに対して遅延なく入力された場合）には、遅延観測値処理部７は、測位結果をそのまま各追尾フィルタ２（ｍ）の予測値算出部２１（ｍ）と予測値適合度評価部２２（ｍ）に対して出力する。
【０１２７】
一方、測位結果に付与されている時刻が最新の統合予測値に付与されている時刻よりも古い場合には、遅延観測値処理部７は、最新の統合予測値に基づいて、受信機毎の目標に対するラジアル速度とラジアル加速度を推定する。
【０１２８】
遅延観測値処理部７は、ラジアル速度、ラジアル加速度、および最新の統合予測値に付与されている時刻と測位結果に付与されている時刻との時間差により、観測値であるＴＤＯＡおよびＦＤＯＡを外挿する。
【０１２９】
遅延観測値処理部７は、外挿により得られたＴＤＯＡより距離差を算出し、ＴＤＯＡ／ＦＤＯＡよりラジアル速度差を算出する。そして、遅延観測値処理部７は、予測値算出部２１（ｍ）および予測値適合度評価部２２（ｍ）に対して算出結果を出力する。
【０１３０】
本実施の形態４における追尾フィルタ２（ｍ）および追尾フィルタ２（ｍ）の各部、表示部５の機能は、先の実施の形態１と同様である。
【０１３１】
統合平滑処理部３は、先の実施の形態１と同様の機能を持つのに加え、各追尾フィルタ２（ｍ）の予測値の適合度、各追尾フィルタ２（ｍ）のパラメータ設定部２４（ｍ）におけるスケーリングパラメータの値を状態量更新部４に対して出力する。
【０１３２】
状態量更新部４は、統合平滑処理部３より追尾フィルタ２（ｍ）（ｍ＝１〜Ｎ）の統合平滑値、予測値の適合度を入力する。そして、状態量更新部４は、統合平滑値より距離差およびラジアル速度差の統合予測値を算出し、統合予測値を目標測位装置１と遅延観測値処理部７にフィードバックする。
【０１３３】
次に、本実施の形態４における目標追尾装置の動作について説明する。図１０は、本発明の実施の形態４の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。以下、各ステップの動作について説明する。
【０１３４】
ステップＳＴ２ａ（ｍ）〜ステップＳＴ２ｅ（ｍ）、ステップＳＴ３、ステップＳＴ４、ステップＳＴ５の動作
これらの動作は、先の実施の形態１における図３のものと同一である。
【０１３５】
ステップＳＴ７ａの動作
遅延観測値処理部７は、目標測位装置１より目標に対する測位結果を入力し、状態量更新部４より平滑値を入力し、測位結果の時刻と最新の平滑値の時刻とを比較する。
【０１３６】
測位結果に付与されている時刻が最新の平滑値に付与されている時刻よりも新しい場合（Ｙｅｓの側のフロー）、遅延観測値処理部７は、測位結果をそのまま各追尾フィルタ２（ｍ）の予測値算出部２１（ｍ）と予測値適合度評価部２２（ｍ）に対して出力する。
【０１３７】
ステップＳＴ７ｂの動作
測位結果に付与されている時刻が最新の平滑値に付与されている時刻よりも古い場合（（ステップＳＴ７ａ）におけるＮｏの側のフロー）、遅延観測値処理部７は、平滑値に基づいて、受信機毎の目標に対するラジアル速度とラジアル加速度を推定する。
【０１３８】
遅延観測値処理部７は、推定によるラジアル速度、ラジアル加速度、および最新の平滑値に付与されている時刻と測位結果に付与されている時刻との時間差により、平滑値の時刻よりも遅延した測位結果に対する時刻補正を行う。
【０１３９】
遅延観測値処理部７は、外挿により得られたＴＤＯＡより距離差を算出し、ＴＤＯＡ／ＦＤＯＡよりラジアル速度差を算出する。そして、遅延観測値処理部７は、予測値算出部２１（ｍ）および予測値適合度評価部２２（ｍ）に対して出力する。
【０１４０】
遅延観測値処理部７におけるラジアル速度とラジアル加速度の推定、および遅延した測位結果に対する時刻補正は、例えば、次の手順による。まず、追尾フィルタ２（ｍ）による平滑値に付与されている時刻ｔｋと、測位結果に付与されている時刻ｔｋ'より、下式（４７）より時刻差Δｔを得る。
【０１４１】
【数２４】
2009300380.tif 000026
【０１４２】
測位結果の伝送に遅延が生じた場合は、Δｔ＜０であり、この場合はステップＳＴ７ｂとして、下記の観測値補正処理を実施する。ここでは、ＴＤＯＡが得られている場合の、遅延した測位結果に対する時刻補正の手順を、数式を用いて説明する。
【０１４３】
距離差に関する観測精度が時間によらず一定であると見なし、まず、下式（４８）を計算することにより、時刻ｔｋ'におけるラジアル速度差を推定する。
【０１４４】
【数２５】
2009300380.tif 000027
【０１４５】
上式（４８）におけるｓａ、ｋ'、ｓｂ、ｋ'は、時刻ｔｋ'におけるセンサの位置ベクトルであり、ｓ（・）ａ、ｋ'、ｓ（・）ｂ、ｋ'は、時刻ｔｋ'におけるセンサの速度ベクトルであり、いずれも既知量である。
【０１４６】
上式（４８）におけるｕ（〜）ｋ'｜ｋ、ｕ（・）（〜）ｋ'｜ｋは、それぞれ時刻ｔｋ'における目標の位置ベクトルおよび速度ベクトルの推定値であり、時刻ｔｋにおける目標の位置の平滑値および速度ベクトルの平滑値から算出する。ｕ（〜）ｋ'｜ｋ、ｕ（・）（〜）ｋ'｜ｋの算出方法としては、例えば、下式（４９）、（５０）による。
【０１４７】
【数２６】
2009300380.tif 000028
【０１４８】
次に、時刻ｔｋにおける観測モデルを下式（５１）で定義する。
【０１４９】
【数２７】
2009300380.tif 000029
【０１５０】
上式（５１）の右辺第１項のｚｋ'は、時刻ｔｋ'における観測値ベクトルである。また、上式（５１）の右辺第２項のｖｋ'は、観測雑音ベクトルである。さらに、上式（５１）の右辺第３項の括弧内に、上式（４８）によって算出された時刻ｔｋ'におけるラジアル速度差の推定値を代入し、遅延して得られた観測値ベクトルｚｋ'の時刻を現時刻ｔｋに補正した結果を得る。
【０１５１】
次に、ＴＤＯＡ／ＦＤＯＡが得られている場合の、遅延した測位結果に対する時刻補正の手順を、数式を用いて説明する。速度差に関する観測精度が時間によらず一定であると見なし、まず、下式（５２）を計算することにより、時刻ｔｋ'におけるラジアル加速度の差を推定する。
【０１５２】
【数２８】
2009300380.tif 000030
【０１５３】
式（５２）におけるｓａ、ｋ'、ｓｂ、ｋ'は、時刻ｔｋ'におけるセンサの位置ベクトルであり、ｓ（・）ａ、ｋ'、ｓ（・）ｂ、ｋ'は、時刻ｔｋ'におけるセンサの速度ベクトルであり、いずれも既知量である。
【０１５４】
上式（５２）におけるｕ（〜）ｋ'｜ｋ、ｕ（・）（〜）ｋ'｜ｋは、それぞれ時刻ｔｋ'における目標の位置ベクトルおよび速度ベクトルであり、時刻ｔｋにおける目標の位置の平滑値および速度ベクトルの平滑値から算出する。ｕ（〜）ｋ'｜ｋ、ｕ（・）（〜）ｋ'｜ｋの算出方法としては、例えば、上式（４９）、（５０）による。
【０１５５】
次に、時刻ｔｋにおける観測モデルを式（５３）で定義する。
【０１５６】
【数２９】
2009300380.tif 000031
【０１５７】
上式（５３）の右辺第１項のｚｋ'は、時刻ｔｋ'における観測値ベクトルである。また、上式（５３）の右辺第２項のｖｋ'は、観測雑音ベクトルである。さらに、上式（５３）の右辺第３項の括弧内に、式（５２）によって算出された時刻ｔｋ'におけるラジアル加速度の差の推定値を代入し、遅延して得られた観測値ベクトルｚｋ'の時刻を現時刻ｔｋに補正した結果を得る。
【０１５８】
以上のように、実施の形態４によれば、複数の受信局間でのＴＤＯＡあるいはＴＤＯＡ／ＦＤＯＡが得られる。この結果、距離差およびラジアル速度差を観測値ベクトルとしてＵＫＦによって目標の位置と速度の推定を行う場合に、観測条件によらずに安定した推定精度を得ることができる。さらに、ネットワークの状況によって測位結果の転送に遅延が生じ、同時刻に得られる測位結果データの一部に欠落を生じた場合でも、追尾を維持することができる。
【０１５９】
なお、上述の実施の形態１〜４では、複数の受信局間でのＴＤＯＡあるいはＴＤＯＡ／ＦＤＯＡが得られ、距離差およびラジアル速度差を観測値ベクトルとして目標の位置と速度を推定する例を示した。しかしながら、観測装置の形式は、他のものでもよく、観測値ベクトルの要素は、距離差およびラジアル速度差でなく、例えば、位置あるいは位置と速度で構成されていてもよい。
【０１６０】
また、観測モデルにおける非線形項は、上式（１６）や（２０）の形式に限らず、他の形式のものであってもよい。
【０１６１】
また、上述の実施の形態１〜４における予測値の観測値に対する適合度を算出方法の例としては、パラメータ設定部２４（ｍ）において設定されているスケーリングパラメータλｍに基づく、追尾フィルタ２（ｍ）の予測値を中心とするＮ個の確率密度関数によって尤度として算出する方法を示した。しかしながら、適合度の算出方法は、これに限定されず、他のものであってもよい。
【０１６２】
また、上述の実施の形態１〜４において、予測値の観測値に対する適合度の低い追尾フィルタ２（ｍ）のスケーリングパラメータの変更方法の例として、予測値の残差の２次形式に基づく数式を挙げた。しかしながら、変更方法は、他のものであってもよい。
【図面の簡単な説明】
【０１６３】
【図１】本発明の実施の形態１における目標追尾装置の構成の一例を示すブロック図である。
【図２】本発明の実施の形態１における受信局と信号源である目標との位置関係を示した図である。
【図３】本発明の実施の形態１の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。
【図４】本発明の実施の形態１の予測値適合度評価部２２（ｍ）における適合度を評価の過程およびスケーリングパラメータλｍの設定を変更する過程の説明図である。
【図５】本発明の実施の形態２における目標追尾装置の構成の一例を示すブロック図である。
【図６】本発明の実施の形態２の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。
【図７】本発明の実施の形態３における目標追尾装置の構成の一例を示すブロック図である。
【図８】本発明の実施の形態３の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。
【図９】本発明の実施の形態４における目標追尾装置の構成の一例を示すブロック図である。
【図１０】本発明の実施の形態４の目標追尾装置において、時刻ｔｋでの目標の位置および速度を推定する流れの一例を示すフローチャートである。
【符号の説明】
【０１６４】
１  目標測位装置、２、２（１）〜２（Ｎ）  追尾フィルタ、２１、２１（１）〜２１（Ｎ）  予測値算出部、２２、２２（１）〜２２（Ｎ）  予測値適合度評価部、２３、２３（１）〜２３（Ｎ）  平滑値算出部、２４、２４（１）〜２４（Ｎ）  パラメータ設定部、３  統合平滑処理部、４  状態量更新部、５  表示部、６  追尾フィルタ制御部、７  遅延観測値処理部。
【図１】
2009300380.tif 000032
【図２】
2009300380.tif 000033
【図３】
2009300380.tif 000034
【図４】
2009300380.tif 000035
【図５】
2009300380.tif 000036
【図６】
2009300380.tif 000037
【図７】
2009300380.tif 000038
【図８】
2009300380.tif 000039
【図９】
2009300380.tif 000040
【図１０】
2009300380.tif 000041