運動諸元推定装置
【課題】複数のセンサが非同期に観測した場合においても目標の運動諸元を推定することができる運動諸元推定装置を提供する。
【解決手段】複数のセンサ対による異なる各時刻での観測結果から検出された各センサ対のセンサ間の目標との距離差及び速度差の観測値と、予め仮定した目標の運動方程式に従い基準時刻における目標の運動諸元の初期推定値に対し該基準時刻との時刻差を反映させて算出した各時刻での各センサ対のセンサ間の目標との距離差及び速度差の概略推定値と、の差分により目標の運動諸元の修正量を算出し、収束演算によって差分が収束するまで運動諸元を修正することで基準時刻における目標の運動諸元を推定する。
【解決手段】複数のセンサ対による異なる各時刻での観測結果から検出された各センサ対のセンサ間の目標との距離差及び速度差の観測値と、予め仮定した目標の運動方程式に従い基準時刻における目標の運動諸元の初期推定値に対し該基準時刻との時刻差を反映させて算出した各時刻での各センサ対のセンサ間の目標との距離差及び速度差の概略推定値と、の差分により目標の運動諸元の修正量を算出し、収束演算によって差分が収束するまで運動諸元を修正することで基準時刻における目標の運動諸元を推定する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、レーダセンサ等の複数のセンサを用いて、航空機、船舶、車両などの移動体と複数のセンサのうちの任意の２つのセンサの距離差やドップラ周波数差等の観測情報を入力し、これら観測情報に基づいて移動体の真の位置や速度等の運動諸元を推定する運動諸元推定装置に関するものである。
【背景技術】
【０００２】
レーダセンサ等を含む複数のセンサによる目標との距離差やドップラ周波数差の観測結果によって目標の運動諸元を推定する従来の運動諸元推定装置では、一般的に複数のセンサ間で同期して得られた観測情報を用いていた。ここで、同期とは、複数センサ間で目標の観測が同一時刻に行われることを表す。また、非同期とは複数のセンサ間で目標の観測を異なる時刻に行うことを意味する。
【０００３】
非特許文献１に開示される従来の運動諸元推定装置について説明する。
ここでは、説明の簡単のため、複数のセンサで構成される観測装置の観測モデルを下記式（１）のように仮定する。
【数１】
ここで、ｚi,j（ｔ）は時刻ｔにおけるセンサｉ及びセンサｊ間の目標との距離差（又は時刻ｔにおけるセンサｉ及びセンサｊ間の目標とのドップラ周波数差）の観測値である。ｈi,j（ｘ（ｔ））は時刻ｔにおけるセンサｉ及びセンサｊ間の目標との距離差の真値を表す観測式であり、ｌi,j（ｘ（ｔ））は時刻ｔにおけるセンサｉ及びセンサｊ間の目標とのドップラ周波数差の真値を表す観測式である。ｖi,j（ｔ）は、時刻ｔにおけるセンサｉ及びセンサｊ間の目標との距離差の観測誤差（又は時刻ｔにおけるセンサｉ及びセンサｊ間の目標とのドップラ周波数差の観測誤差）である。
【０００４】
例えば、観測装置によってｘ−ｙの２次元直交座標系におけるセンサｉ及びセンサｊ間の目標位置との距離差の観測結果が得られたとする。このとき、観測値の真値ｈi,j（ｘ（ｔ））を下記式（２）及び下記式（４）のように定義する。また、観測装置によってセンサｉ及びセンサｊ間の目標位置とのドップラ周波数差の観測結果が得られた場合は、観測値の真値ｌi,j（ｘ（ｔ））は、下記式（３）、下記式（２）及び下記式（５）で定義される。
【数２】
【０００５】
なお、ｓi（ｔ），ｓj（ｔ）は、それぞれ時刻ｔにおけるセンサｉ，ｊの位置ベクトルを表し、ｓi（ｔ）ドット及びｓj（ｔ）ドット（以下、出願書類の処理の関係上、読みで表記する）は、それぞれ時刻ｔにおけるセンサｉ，ｊの速度を示している。また、ｘ（ｔ）は、上記式（６）で表される目標の状態ベクトルであり、運動諸元推定装置によって求める運動諸元を表している。
【０００６】
上記のような観測モデルの仮定の下、従来の運動諸元推定装置では、以下に示すアルゴリズムで、状態ベクトルの推定を行う。
センサが３つ以上の場合、上記式（２）（又は上記式（３））の方程式が２式以上得られるため、この連立方程式を解くことで状態ベクトルを求めることができる。しかしながら、観測誤差等の影響により、実際には上記式（１）となるため、一意に解が求まらない。そこで、下記のような方法を採用する。
【０００７】
先ず、上記式（２）（又は上記式（３））の目標の状態ベクトルに対して適当な初期値を定め、上記式（２）（又は上記式（３））の左辺の初期値を与える。次に、複数のセンサによって実際の距離差（又はドップラ周波数差）の観測値が得られると、上記初期値との誤差信号が得られる。
【０００８】
ここで、複数のセンサにより実際の距離差の観測値が得られた場合について説明する。上記式（２）より下記式（７）〜（１０）の関係を導き出すことができる。これらより、距離差の真値及びその誤差信号と目標の状態ベクトルの修正量との関係が得られるので、上記式（２）を目標の状態ベクトルの初期値で近似して（下記式（７）参照）、真値への修正量δｘアンダーバー（以下、出願書類の処理の関係上、読みで表記する）が決まる。
【数３】
【０００９】
上記式（７）を最小二乗法を用いて解くと、修正量δｘアンダーバーは下記式（１１）で与えられる。これにより、上記式（１０）を解く。
【数４】
【００１０】
以上の処理を修正量δｘアンダーバーが収束するまで繰り返すと、状態ベクトルが推定される。つまり、収束判定と判断された場合、現在の目標の状態ベクトルに修正量を加えたものが目標の状態ベクトルとなる。一方、収束判定とみなされない場合においては、状態ベクトルに修正量を付加し、上述したアルゴリズに従った処理を繰り返す。なお、ドップラ周波数差の観測値が得られた場合についても上述と同様なアルゴリズムを適用することができる。
【００１１】
【非特許文献１】K. C. HO: "An Accurate Algebraic Solution for Moving Source Location Using TDOA and FDOA Measurements" IEEE Trans. On Signal Processing, Vol.52, No.9, Sep. 2004
【発明の開示】
【発明が解決しようとする課題】
【００１２】
従来の運動諸元推定装置では、複数のセンサが同じ時刻に観測した場合を前提として目標の運動諸元を推定している。しかし、実際には観測タイミングのずれや伝送遅延等により複数のセンサ間で観測時刻が異なる場合が多くなることが想定される。従って、従来の運動諸元推定装置では目標の運動諸元の推定が困難となり、後段処理である目標追尾等の処理に移行できないという課題がある。このため、異なる時刻に距離差やドップラ周波数差が得られた場合においても目標の運動諸元を推定できることが望まれていた。
【００１３】
この発明は、上記のような課題を解決するためになされたものであり、複数のセンサが非同期に観測した場合においても目標の運動諸元を推定することができる運動諸元推定装置を得ることを目的とする。
【課題を解決するための手段】
【００１４】
この発明に係る運動諸元推定装置は、目標の運動を観測する複数のセンサを有する受信手段と、受信手段の複数のセンサのうち任意のセンサ対による同一時刻での観測結果から、該センサ対のセンサ間の目標との距離差及び速度差の観測値を検出する検出手段と、複数のセンサ対による異なる各時刻での観測結果から検出された各センサ対のセンサ間の目標との距離差及び速度差の観測値と、予め仮定した目標の運動方程式に従い基準時刻における目標の運動諸元の初期値に対し該基準時刻との時刻差を反映させて算出した各時刻での各センサ対のセンサ間の目標との距離差及び速度差の概略推定値と、の差分より目標の運動諸元の修正量を算出し、収束演算によって差分が収束するまで運動諸元を修正することで基準時刻における目標の運動諸元を推定する非同期測位手段とを備えるものである。
【発明の効果】
【００１５】
この発明によれば、複数のセンサ対による異なる各時刻での観測結果から検出された各センサ対のセンサ間の目標との距離差及び速度差の観測値と、予め仮定した目標の運動方程式に従い基準時刻における目標の運動諸元の初期推定値に対し該基準時刻との時刻差を反映させて算出した各時刻での各センサ対のセンサ間の目標との距離差及び速度差の概略推定値と、の差分により目標の運動諸元の修正量を算出し、収束演算によって差分が収束するまで運動諸元を修正することで基準時刻における目標の運動諸元を推定するので、複数のセンサが非同期、すなわち異なる時刻で観測した結果からでも目標の運動諸元を推定することができ、該運動諸元を用いて目標を早期に追尾処理等することができるという効果がある。
【発明を実施するための最良の形態】
【００１６】
実施の形態１．
図１は、この発明の実施の形態１による運動諸元推定装置の構成を示す図である。実施の形態１による運動諸元推定装置は、受信手段１、距離差／速度差観測値検出手段（検出手段）２、非同期測位手段４を備える。受信手段１は、センサ１１〜１Ｍ（Ｍは、１より大きい整数）を備え、各センサ１１〜１Ｍは、目標とセンサ１１〜１Ｍのうちの任意の２センサ間（図１の例では、センサ１１とセンサ１２，・・・，センサ１（Ｍ−１）とセンサ１Ｍ）との距離差やドップラ周波数差を観測する。
【００１７】
距離差／速度差観測値検出手段２は、距離差／速度差観測値検出部２１〜２Ｎ（Ｎは、Ｍ−１の整数）を備える。距離差／速度差観測値検出部２１〜２Ｎは、受信手段１を構成するセンサ１１〜１Ｍから任意の２つのセンサごとにセンサ間の目標との距離差及びドップラ周波数差の観測結果を検出し、該２センサ間の距離差及びドップラ周波数差から求めた速度差とその観測時刻を出力する。
【００１８】
非同期測位手段４は、距離差／速度差観測値検出手段２から出力される、受信手段１における任意の２センサ間の目標との距離差及びドップラ周波数差から得られる速度差を入力して、非同期を考慮して目標の状態ベクトルを推定する。また、非同期測位手段４は、図１に示すように、初期状態量設定部４１、時刻差算出部４２１〜４２Ｎ、距離差／速度差概略値算出部４３１〜４３Ｎ、修正量算出部４４１〜４４Ｎ、修正状態ベクトル算出部４５、及び収束判定部４６を備える。
【００１９】
初期状態量設定部４１は、修正状態ベクトル算出部４５により目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。時刻差算出部４２１〜４２Ｎは、初期状態量設定部４１により設定された基準時刻と観測時刻との時刻差を算出する。
【００２０】
距離差／速度差概略値算出部４３１〜４３Ｎは、時刻差算出部４２１〜４２Ｎにより算出された時刻差及び初期状態量設定部４１により設定された基準時刻における目標の初期値（若しくは収束判定部４６からの修正量を初期値として）を用いて予め仮定した運動方程式に沿った概略値を算出する。
【００２１】
修正量算出部４４１〜４４Ｎは、距離差／速度差概略値算出部４３１〜４３Ｎにより算出された概略値と距離差／速度差観測値検出手段２から得られた観測値との差分を算出し、この差分を用いて目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部４５は、修正量算出部４４１〜４４Ｎから修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。
【００２２】
収束判定部４６では、修正状態ベクトル算出部４５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差／速度差概略値算出部４３１〜４３Ｎにフィードバックしたり、収束計算を終了して修正状態ベクトルを測位結果として出力する。
【００２３】
本発明による運動諸元推定装置では、非同期に得られた距離差や速度差の観測情報を、予め仮定した運動方程式（例えば、等速直線運動）に当てはめて、基準時刻（目標の運動諸元を推定したい指定時刻）における目標の運動諸元として目標の状態ベクトル（位置、速度等）を推定する。なお、この推定には、例えば最小二乗法を用いる。
【００２４】
また、以降の説明では目標の運動モデルを下記式（１２）のように仮定する。
但し、ｔは基準時刻であり、ｘ（ｔ）は基準時刻ｔにおける目標の運動諸元の真値を表す状態ベクトルである。また、ΔＴは、基準時刻ｔからの時刻差である。
【数５】
【００２５】
例えば、目標の基本的な運動を等速直線運動と仮定し、ｘ−ｙの２次元直交座標系における目標の位置と速度を推定する場合、目標の状態ベクトルｘ（ｔ）と運動モデルは下記式（１３）及び下記式（１４）のように定義される。但し、Ｔはベクトルや行列の転置を示しており、ドットは時間微分を示している。また、Ｉn×nはｎ行ｎ列の単位行列であり、Ｏn×nは、ｎ行ｎ列の零行列である。
【数６】
【００２６】
次に、上記式（１）と上記式（１２）より、２センサ間の目標との距離差について下記式（１５）が定義される。
【数７】
【００２７】
例えば、図２に示すように目標が航空機である場合、当該目標についてそれぞれ異なる時刻ｔ1〜ｔ3で基準時刻ｔを含めて計４回の観測で観測値が得られたとすると、上記式（１５）より下記式（１６）が導出される。なお、時刻ｔk（ｋ＝１〜３）は、下記式（１７）より基準時刻ｔからΔｔk前の時刻である。
【数８】
【００２８】
上記式（１６）より最小二乗法による収束計算を用いて、基準時刻ｔにおける状態ベクトル（図２中、時刻ｔにおいて太線の四角と線で表したもの）の推定値を算出する。なお、最小二乗法による収束計算は、例えば非特許文献１に開示される従来の運動諸元推定装置と同様の処理を実施する。
【００２９】
次に動作について説明する。
センサ１１〜１Ｍのうちの任意の２センサ間の目標との距離差及びドップラ周波数差の観測値は、受信手段１から距離差／速度差観測値検出手段２に出力される。距離差／速度差観測値検出手段２では、受信手段１における任意の２センサによる観測値を距離差／速度差観測値検出部２１〜２Ｎが入力する。図１の例では、受信手段１のセンサ１１〜１Ｍのうち、センサ１１及びセンサ１２のセンサ出力が距離差／速度差観測値検出部２１に入力され、センサ１２及びセンサ１３のセンサ出力が距離差／速度差観測値検出部２２に入力され、同様に２センサの出力が距離差／速度差観測値検出部に逐次入力される。
【００３０】
距離差／速度差観測値検出部２１〜２Ｎでは、センサ１１〜１Ｍのうちの２センサからそれぞれ入力した検知結果に基づいて、２センサ間の目標との距離差を検出し、２センサ間の目標とのドップラ周波数差から速度差を求める。この後、距離差／速度差観測値検出部２１〜２Ｎは、２センサ間の目標との距離差及び速度差を、非同期測位手段４内の修正量算出部４４１〜４４Ｎにそれぞれ出力し、各センサの観測時刻も非同期測位手段４内の時刻差算出部４２１〜４２Ｎにそれぞれ出力する。
【００３１】
非同期測位手段４内の初期状態量設定部４１は、基準時刻ｔを時刻差算出部４２１〜４２Ｎにそれぞれ出力し、目標の状態ベクトルの初期値ｘ（ｔ）を距離差／速度差概略値算出部４３１〜４３Ｎにそれぞれ出力する。時刻差算出部４２１〜４２Ｎは、基準時刻ｔと観測時刻とを用い、上記式（１７）により基準時刻ｔと観測時刻ｔkの時刻差Δｔkを算出し、距離差／速度差概略値算出部４３１〜４３Ｎにそれぞれ出力する。
【００３２】
距離差／速度差概略値算出部４３１〜４３Ｎでは、初期状態量設定部４１から入力した目標の状態ベクトルｘ（ｔ）の初期値と時刻差算出部４２１〜４２Ｎから入力した時刻差を用い、上記式（１２）に従ってそれぞれ異なる時刻における目標の状態ベクトルｘ（ｔ）とセンサｉ，ｊ間の目標との距離差又は速度差の概略値（上記式（１６）のｇi,jに相当するもの）とを算出して修正量算出部４４１〜４４Ｎにそれぞれ出力する。
【００３３】
このように、距離差／速度差概略値算出部４３１〜４３Ｎは、予め仮定した目標の運動方程式である上記式（１２）〜（１７）に従って、基準時刻ｔにおける目標の運動諸元の初期値ｘ（ｔ）に対し基準時刻ｔとの時刻差ΔＴを反映させて各時刻での各センサ対のセンサ間の目標との距離差及び速度差の概略値（概略推定値）を算出する。
【００３４】
修正量算出部４４１〜４４Ｎでは、距離差／速度差概略値算出部４３１〜４３Ｎからの概略値ｇi,jと、距離差／速度差観測値検出部２１〜２Ｎからの観測値ｚi,j（距離差、速度差の観測値）とを入力し、概略値ｇi,jと観測値ｚi,jとの誤差（距離差の誤差Δｈi,j、速度差の誤差Δｌi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部４５では、修正量算出部４４１〜４４Ｎから異なる時刻における修正量をそれぞれ入力し、最小二乗法による収束計算を施して、目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【００３５】
収束判定部４６では、修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値ｘ（ｔ）の代わりに距離差／速度差概略値算出部４３１〜４３Ｎの入力へフィードバックし、上記処理を繰り返す。修正量が閾値未満になれば、修正状態ベクトルを測位結果として出力する。
【００３６】
以上のように、この実施の形態１によれば、目標の運動を観測する複数のセンサ１１〜１Ｍを有する受信手段１と、受信手段１の複数のセンサ１１〜１Ｍのうち任意のセンサ対による同一時刻での観測結果から該センサ対のセンサ間の目標との距離差及び速度差の観測値を検出する距離差／速度差観測値検出手段２と、複数のセンサ対による異なる各時刻での観測結果から検出された各センサ対のセンサ間の目標との距離差及び速度差の観測値と、予め仮定した目標の運動方程式に従い基準時刻における目標の運動諸元の初期値に対し該基準時刻との時刻差を反映させて算出した各時刻での各センサ対のセンサ間の目標との距離差及び速度差の概略値と、の差分により目標の運動諸元の修正量を算出し、収束演算によって差分が収束するまで運動諸元を修正することで基準時刻における目標の運動諸元を推定する非同期測位手段４とを備えるので、観測タイミングのずれや伝送遅延等により複数のセンサ間で観測時刻が異なり、観測値が非同期になっても、信頼性の高い目標の運動諸元推定が可能である。
【００３７】
また、上記実施の形態１では、ｘ−ｙの２次元直交座標系における目標の位置と速度を推定する場合を示したが、未知数を増やすことで３次元での目標の位置と速度を推定することもできる。この場合は、観測値の数を増やすことで、目標の状態ベクトルを推定できる。
【００３８】
実施の形態２．
図３は、この発明の実施の形態２による運動諸元推定装置の構成を示す図である。図３において、実施の形態２による運動諸元推定装置の基本的な構成は、上記実施の形態１の図１で示したものと同様であるが、非同期測位手段５が距離差／速度差観測値検出部２１〜２Ｎから観測精度を入力する点で異なる。
【００３９】
非同期測位手段５の修正状態ベクトル算出部５５は、距離差／速度差観測値検出部２１〜２Ｎから入力した観測精度を考慮して、修正量算出部５４１〜５４Ｎからの修正量に対し最小二乗法を用いて目標の運動諸元を推定する。つまり、修正状態ベクトル算出部５５が、状態ベクトルの修正量を算出するにあたり、上記式（１１）について距離差及びドップラ周波数差の観測精度を重み付けの項として加える。
【００４０】
初期状態量設定部５１は、修正状態ベクトル算出部５５により目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。時刻差算出部５２１〜５２Ｎは、初期状態量設定部５１により設定された基準時刻と観測時刻との時刻差を算出する。
【００４１】
距離差／速度差概略値算出部５３１〜５３Ｎは、時刻差算出部５２１〜５２Ｎにより算出された時刻差及び初期状態量設定部５１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部５６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。
【００４２】
収束判定部５６は、修正状態ベクトル算出部５５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差／速度差概略値算出部５３１〜５３Ｎにフィードバックしたり、収束計算を終了して修正状態ベクトルを測位結果として出力する。
なお、他の構成は、図１と同一又はこれに相当する構成であるので同一符号を付して説明を省略する。
【００４３】
次に動作について説明する。
上記実施の形態１と異なる動作について主に説明する。
修正状態ベクトル算出部５５は、修正量算出部５４１〜５４Ｎから入力した状態ベクトルの修正量と、距離差／速度差観測値検出部２１〜２Ｎから入力した距離差及びドップラ周波数差の観測精度を用い、該観測精度を重み付けした最小二乗法により目標の状態ベクトルの修正量とこれを状態ベクトルに反映させた修正状態ベクトルを出力する。例えば、下記式（１８）のように観測精度Ｓを重み付けした式に従って目標の状態ベクトルの修正量と修正状態ベクトルを算出する。
【数９】
【００４４】
収束判定部５６は、修正状態ベクトル算出部５５から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標状態ベクトルの初期値ｘ（ｔ）の代わりに距離差／速度差概略値算出部５３１〜５３Ｎの入力にフィードバックし、上記処理を繰り返す。修正量が閾値未満になれば、修正状態ベクトルを測位結果として出力する。
【００４５】
以上のように、この実施の形態２によれば、非同期測位手段５が、観測値と概略値（概略推定値）との誤差（差分）に対してセンサ１１〜１Ｍの観測精度を重み付けして目標の運動諸元の修正量を算出するので、観測値が非同期であっても目標の状態ベクトルを推定することができる。また、観測精度を考慮することにより、より信頼性の高い推定を実施することが可能である。
【００４６】
実施の形態３．
図４は、この発明の実施の形態３による運動諸元推定装置の構成を示す図である。図４に示すように、実施の形態３による運動諸元推定装置は、受信手段１、距離差／速度差観測値検出手段２、仮測位／初期値算出手段６及び非同期測位手段７を備える。なお、図４において、図１と同一又はこれに相当する構成要素に同一符号を付して説明を省略する。
【００４７】
仮測位／初期値算出手段６は、初期状態量設定部６１、距離差／速度差概略値算出部６２１，６２２、修正量算出部６３１，６３２、修正状態ベクトル算出部６４、及び収束判定部６５を備える。初期状態量設定部６１は、修正状態ベクトル算出部６４が目標の状態ベクトルの修正量を最小二乗法により算出する際に用いる初期値として目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。
【００４８】
距離差／速度差概略値算出部６２１，６２２は、初期状態量設定部６１によって設定された基準時刻における目標の状態ベクトルの初期値を用いて（若しくは、収束判定部６５からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。修正量算出部６３１，６３２は、距離差／速度差概略値算出部６２１，６２２により算出された概略値と距離差／速度差観測値検出部２１，２２から得られた観測値（センサ１１，１２とセンサ１２，１３による観測値）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。
【００４９】
修正状態ベクトル算出部６４は、修正量算出部６３１，６３２から修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。収束判定部６５では、修正状態ベクトル算出部６４から入力した修正量と所定の閾値との比較結果に応じて、距離差／速度差概略値算出部６２１，６２２に修正状態ベクトルをフィードバックしたり、非同期測位手段７の初期値として修正状態ベクトルを出力する。
【００５０】
非同期測位手段７は、時刻差算出部７１１〜７１Ｎ、距離差／速度差概略値算出部７２１〜７２Ｎ、修正量算出部７３１〜７３Ｎ、修正状態ベクトル算出部７４及び収束判定部７５を備え、仮測位／初期値算出手段６による測位結果を、目標の状態ベクトルの修正量を最小二乗法で算出する際に用いる初期値として用い、初期状態量設定部が省略されている以外は上記実施の形態１と同様に動作する。
【００５１】
図５は、時刻ｔ3においてセンサ１１及びセンサ１２、時刻ｔ4においてセンサ１２及びセンサ１３によって距離差及びドップラ周波数差が得られた場合を示す図である。ｘ−ｙの２次元直交座標系における目標の位置と速度を推定する場合、実施の形態３では、図５に示すような異なるセンサ組による検知結果の等時間差線（双曲線）の交点を測位し、交点における目標の位置及び速度を非同期測位手段７における目標の状態ベクトルの初期値として設定する。
【００５２】
例えば、図５中の破線で示す双曲線は、時刻ｔ3においてセンサ１１及びセンサ１２から目標へ放射されたレーダ電波（又は音波）が目標で反射してセンサ１１及びセンサ１２に到達する時刻差を用いて検出された２センサ間の目標との距離差及びドップラ周波数差による双曲線（目標とセンサ１１及びセンサ１２との距離差が一定な等距離差双曲線又は目標とセンサ１１及びセンサ１２とのドップラ周波数差が一定な等ドップラ周波数差双曲線）である。
【００５３】
また、図５中に実線で示す双曲線は、時刻ｔ4においてセンサ１２及びセンサ１３に反射波が到達する時刻差を用いて検出された２センサ間の目標との距離差及びドップラ周波数差による双曲線（センサ１２及びセンサ１３間の目標との距離差が一定な等距離差双曲線又はセンサ１２及びセンサ１３間の目標とのドップラ周波数差が一定な等ドップラ周波数差双曲線）である。
【００５４】
実施の形態３では、時刻ｔ3においてセンサ１１及びセンサ１２によって距離差及びドップラ周波数差を観測し、時刻ｔ4においてセンサ１２及びセンサ１３によって距離差及びドップラ周波数差を観測し、基準時刻として時刻ｔ4を設定して各センサ組の観測値から目標の状態ベクトルを算出することにより、これら双曲線の交点における目標の状態ベクトルを算出し、該状態ベクトルを非同期測位手段７による修正量の算出における初期値として使用する。
【００５５】
次に動作について説明する。
仮測位／初期値算出手段６では、初期状態量設定部６１により基準時刻及びこの基準時刻における目標の状態ベクトルの初期値が距離差／速度差概略値算出部６２１，６２２に設定される。距離差／速度差概略値算出部６２１，６２２では、初期状態量設定部６１から入力した目標の状態ベクトルの初期値から目標と各センサ組との距離差及び速度差の概略値を算出して修正量算出部６３１，６３２にそれぞれ出力する。
【００５６】
修正量算出部６３１，６３２では、距離差／速度差概略値算出部６２１，６２２からの概略値と距離差／速度差観測値検出部２１，２２からの観測値（２センサ間の目標との距離差及びドップラ周波数差より求めた速度差）を入力し、この概略値と観測値との誤差（距離差の誤差Δｈi,j、速度差の誤差Δｌi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部６４では、修正量算出部６３１，６３２からの修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【００５７】
収束判定部６５は、修正状態ベクトル算出部６４から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値の代わりに距離差／速度差概略値算出部６２１，６２２の入力へフィードバックし、上記処理を繰り返す。
【００５８】
修正量が閾値未満であると、収束判定部６５は、該修正量を反映させた目標の状態ベクトル（目標の位置、速度等）を修正状態ベクトル算出部６４から入力し、最新観測時刻（図５では時刻ｔ4）における目標の測位結果として非同期測位手段７へ出力する。具体的には、基準時刻として時刻ｔ4を非同期測位手段７の時間差算出部７１１〜７１Ｎへ出力し、目標の修正状態ベクトルを、非同期測位手段７において目標の状態ベクトルの修正量を最小二乗法で算出する際の初期値として距離差／速度差概略値算出部７２１〜７２Ｎへ出力する。
【００５９】
非同期測位手段７は、仮測位／初期値算出手段６から基準時刻及び目標の状態ベクトルの初期値が設定されると、上記実施の形態１と同様の処理により目標の測位結果を推定する。
【００６０】
以上のように、この実施の形態３によれば、仮測位／初期値算出手段６が異なる２つの時刻において異なるセンサ組により観測された観測値に基づいて算出した目標の測位結果を、非同期測位手段７による修正状態ベクトル算出処理の初期値として用いるので、異なる時刻の観測値をあたかも同時刻で観測された結果とみなすことができ、観測値が非同期であっても信頼性の高い目標の運動諸元の推定が可能である。
【００６１】
実施の形態４．
図６は、この発明の実施の形態４による運動諸元推定装置の構成を示す図である。図６に示すように、実施の形態４による運動諸元推定装置は、受信手段１、距離差／速度差観測値検出手段２、仮測位／初期値算出手段８及び非同期測位手段７を備える。なお、図６において、図１及び図４と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【００６２】
仮測位／初期値算出手段８は、初期状態量設定部８１、追尾フィルタ部８２１，８２２、距離差／速度差概略値算出部８３１，８３２、修正量算出部８４１，８４２、修正状態ベクトル算出部８５、及び収束判定部８６を備える。初期状態量設定部８１は、修正状態ベクトル算出部８５が目標の状態ベクトルの修正量を最小二乗法により算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。
【００６３】
追尾フィルタ部８２１，８２２は、受信手段１内のセンサの組み合わせ（図６の例では、センサ１１，１２とセンサ１２，１３）で得られる観測値（距離差及びドップラ周波数差から求めた速度差）を入力し、これより所定時刻におけるセンサ組のセンサ間の目標との距離差やドップラ周波数差から求めた速度差を推定する。距離差／速度差概略値算出部８３１，８３２は、初期状態量設定部８１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部８６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った目標の運動諸元の概略値を算出する。
【００６４】
修正量算出部８４１は、距離差／速度差概略値算出部８３１により算出された目標の運動諸元の概略値と追尾フィルタ部８２１により得られた観測値（センサ１１，１２間の目標との距離差及びドップラ周波数差により求まる速度差の推定値）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。修正量算出部８４２は、距離差／速度差概略値算出部８３２により算出された目標の運動諸元の概略値と追尾フィルタ部８２２により得られた観測値（センサ１２，１３間の目標との距離差及びドップラ周波数差により求まる速度差の推定値）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。
【００６５】
修正状態ベクトル算出部８５は、修正量算出部８４１，８４２からの修正量が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。収束判定部８６では、修正状態ベクトル算出部８５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差／速度差概略値算出部８３１，８３２にフィードバックしたり、収束計算を終了して修正状態ベクトルを非同期測位手段７による修正量算出の初期値として出力する。
【００６６】
図７は、時刻ｔ1，ｔ2においてセンサ１１とセンサ１２により距離差及びドップラ周波数差を観測し、時刻ｔ3においてセンサ１２とセンサ１３によって距離差及びドップラ周波数差が観測された場合を示す図である。ｘ−ｙの２次元直交座標系における目標の位置と速度を推定する場合、この実施の形態４では、いずれか一方のセンサ組み合わせで目標を追尾処理（目標との距離差、ドップラ周波数差による速度差を推定）し、他方のセンサ組み合わせにより観測値が得られた時刻に対する予測処理を実施することで時刻の同期を取る。
【００６７】
例えば、図７では、時刻ｔ3でセンサ１２とセンサ１３により距離差及び速度差に関する情報が得られている場合、センサ１１とセンサ１２の組み合わせにより得られる距離差及び速度差に関する情報について時刻ｔ3における予測処理を実施する。この結果、図７に示すように時刻ｔ3におけるセンサ１１，１２による観測値の双曲線とセンサ１２，１３による観測値の双曲線の交点が測位される。実施の形態４では、この交点での目標の状態ベクトルを、非同期測位手段７における修正量算出の初期値とする。
【００６８】
次に動作について説明する。
仮測位／初期値算出手段８において、初期状態量設定部８１が追尾フィルタ部８２１，８２２に基準時刻を設定し、基準時刻における目標の状態ベクトルの初期値を距離差／速度差概略値算出部８３１，８３２に設定する。
【００６９】
追尾フィルタ部８２１は、距離差／速度差観測値検出部２１から、センサ１１，１２の検知結果から得られる観測値（２センサ間の目標との距離差及びドップラ周波数差による速度差）及びその観測時刻を入力して、初期状態量設定部８１により設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差／速度差観測値検出部２１から入力した観測値とを用いてある時刻（図７の例では時刻ｔ1，ｔ2）における目標を追尾処理する（センサ１１，１２間の目標との距離差及び速度差を推定する）。
【００７０】
追尾フィルタ部８２２では、距離差／速度差観測値検出部２２から、センサ１２，１３の検知結果から得られる観測値及びその観測時刻（図７の例では時刻ｔ3）を入力して、初期状態量設定部８１に設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差／速度差観測値検出部２２から入力した観測値とを用いて時刻ｔ3における目標の追尾処理（２センサ間の目標との距離差及び速度差を推定）する。
【００７１】
このように、時刻ｔ3でセンサ１２，１３により目標との距離差及びドップラ周波数差による速度差が得られている場合、追尾フィルタ部８２１は、センサ１１，１２による目標との距離差及びドップラ周波数差による速度差について当該時刻ｔ3における予測処理を実行する。これにより、図７中の一点破線で示すセンサ１１，１２による観測値の等時間差線と図７中の実線で示すセンサ１２，１３による観測値の等時間差線との交点が測位される。つまり、センサ１２，１３により距離差及びドップラ周波数差が得られる、時刻ｔ3におけるセンサ１１，１２による観測の予測値を得ることで、同一時刻（時刻ｔ3）で目標との距離差及びドップラ周波数差より求まる速度差が得られたとみなして測位処理を実施する。但し、予測処理するセンサの組み合わせは１組以上とする。
【００７２】
距離差／速度差概略値算出部８３１，８３２は、初期状態量設定部８１から入力した目標の状態ベクトルの初期値を用いて各センサ組の２センサ間の目標との距離差及び速度差の概略値を算出して修正量算出部８４１，８４２にそれぞれ出力する。
【００７３】
修正量算出部８４１，８４２では、距離差／速度差概略値算出部８３１，８３２からの概略値と追尾フィルタ部８２１，８２２から入力した観測値（目標との距離差及びドップラ周波数差より求めた速度差の推定値）を入力し、概略値と観測値の誤差（距離差の誤差Δｈi,j、速度差の誤差Δｌi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部８５では、修正量算出部８４１，８４２からの修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【００７４】
収束判定部８６は、修正状態ベクトル算出部８５から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値の代わりに距離差／速度差概略値算出部８３１，８３２へフィードバックし、上記処理を繰り返す。
【００７５】
修正量が閾値未満であると、収束判定部８６は、該修正量を反映させた目標の状態ベクトル（目標の位置、速度等）を修正状態ベクトル算出部８５から入力し、図７に示す時刻ｔ3における目標の測位結果として非同期測位手段７へ出力する。具体的には、基準時刻として時刻ｔ3を非同期測位手段７の時間差算出部７１１〜７１Ｎへ出力し、目標の修正状態ベクトルを、非同期測位手段７において目標の状態ベクトルの修正量を最小二乗法で算出する際の初期値として距離差／速度差概略値算出部７２１〜７２Ｎへ出力する。
【００７６】
非同期測位手段７は、仮測位／初期値算出手段８から基準時刻及び目標の状態ベクトルの初期値が設定されると、上記実施の形態１と同様の処理により目標の測位結果を推定する。
【００７７】
以上のように、この実施の形態４によれば、仮測位／初期値算出手段８が、センサ１１，１２の検知結果から得られた目標との距離差及びドップラ周波数差より求まる速度差を追尾処理し、別の時刻において、異なるセンサ組（センサ１２，１３）の検知結果から目標との距離差及びドップラ周波数差より求まる速度差が得られる場合、上記追尾結果に基づいてそのセンサ組（センサ１２，１３）に対する時刻整合を行ってから、目標の位置、速度を算出し、最小二乗法による目標の状態ベクトル算出の初期値として設定するので、異なる時刻の観測値をあたかも同時刻で観測された結果とみなすことができ、観測値が非同期であっても信頼性の高い目標の運動諸元推定が可能である。
【００７８】
実施の形態５．
図８は、この発明の実施の形態５による運動諸元推定装置の構成を示す図である。図８に示すように、実施の形態５による運動諸元推定装置は、受信手段１、距離差／速度差観測値検出手段２及び非同期測位手段９を備える。なお、図８において、図１と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【００７９】
非同期測位手段９は、初期状態量設定部９１、追尾フィルタ部９２１〜９２Ｎ、距離差／速度差概略値算出部９３１〜９３Ｎ、修正量算出部９４１〜９４Ｎ、修正状態ベクトル算出部９５、及び収束判定部９６を備える。
【００８０】
初期状態量設定部９１は、修正状態ベクトル算出部９５により目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。追尾フィルタ部９２１〜９２Ｎは、受信手段１内のセンサの組み合わせで得られる検知結果に基づいて距離差／速度差観測値検出部２１〜２Ｎ距離差及びドップラ周波数差を入力し、追尾処理を実施する。
【００８１】
距離差／速度差概略値算出部９３１〜９３Ｎは、初期状態量設定部９１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部９６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。修正量算出部９４１〜９４Ｎは、距離差／速度差概略値算出部９３１〜９３Ｎにより算出された概略値と追尾フィルタ部９２１〜９２Ｎから得られた観測値との差分を用いて目標の状態ベクトルの修正量を算出する。
【００８２】
修正状態ベクトル算出部９５は、修正量算出部９４１〜９４Ｎから修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。収束判定部９６では、修正状態ベクトル算出部９５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差／速度差概略値算出部９３１〜９３Ｎにフィードバックしたり、収束計算を終了して修正状態ベクトルを測位結果として出力する。
【００８３】
次に動作について説明する。
初期状態量設定部９１は、追尾フィルタ部９２１〜９２Ｎに基準時刻を設定し、基準時刻における目標の状態ベクトルの初期値を距離差／速度差概略値算出部９３１〜９３Ｎに設定する。
【００８４】
追尾フィルタ部９２１〜９２Ｎは、距離差／速度差観測値検出部２１〜２Ｎから、センサ組の検知結果により得られる観測値（目標との距離差及びドップラ周波数差による速度差）及びその観測時刻をそれぞれ入力して、初期状態量設定部９１により設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差／速度差観測値検出部から入力した観測値とを用いて、目標を追尾処理する（各センサ組のセンサ間の目標との距離差及び速度差を推定する）。
【００８５】
追尾フィルタ部９２１〜９２Ｎのうち、ある追尾フィルタ部は、距離差／速度差観測値検出部から、ある時刻で他の追尾フィルタ部により異なるセンサ組で観測値が得られている場合、初期状態量設定部９１に設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差／速度差観測値検出部から入力した観測値とを用いて、当該時刻における目標との距離差及び速度差を予測処理（目標との距離差及び速度差を推定）する。
【００８６】
例えば、ある時刻で追尾フィルタ部９２２〜９２Ｎによってセンサ１２，１３〜１（Ｎ−１），１Ｎの検知結果で目標との距離差及び速度差が得られる場合、追尾フィルタ部９２１が該時刻における予測値を得ることで、同一時刻において目標との距離差及びドップラ周波数差により求まる速度差が得られたとみなして測位処理を実施する。但し、予測処理するセンサの組み合わせは１組以上とする。
【００８７】
距離差／速度差概略値算出部９３１〜９３Ｎは、初期状態量設定部９１から入力した目標の状態ベクトルの初期値を用いて目標と各センサ組との距離差及び速度差の概略値を算出して修正量算出部９４１〜９４Ｎにそれぞれ出力する。
【００８８】
修正量算出部９４１〜９４Ｎでは、距離差／速度差概略値算出部９３１〜９３Ｎからの概略値と追尾フィルタ部９２１〜９２Ｎから入力した観測値（目標との距離差及び速度差の推定値）を入力し、概略値と観測値の誤差（距離差の誤差Δｈi,j、速度差の誤差Δｌi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部９５では、修正量算出部９４１〜９４Ｎからの修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【００８９】
収束判定部９６は、修正状態ベクトル算出部９５から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値の代わりに距離差／速度差概略値算出部９３１〜９３Ｎの入力へフィードバックし、上記処理を繰り返す。修正量が閾値未満であると、収束判定部９６は、該修正量を反映させた目標の状態ベクトル（目標の位置、速度等）を修正状態ベクトル算出部９５から入力し、目標の測位結果として出力する。
【００９０】
以上のように、この実施の形態５によれば、非同期測位手段９が、センサ組の検知結果から得られた目標との距離差及びドップラ周波数差より求まる速度差を追尾処理し、別の時刻において、異なるセンサ組の検知結果から目標との距離差及び速度差が得られる場合、上記追尾結果に基づいてそのセンサ組に対する時刻整合を行ってから、目標の運動諸元を推定するので、異なる時刻の観測値をあたかも同時刻で観測された結果とみなすことができ、観測値が非同期であっても信頼性の高い目標の運動諸元推定が可能である。
【００９１】
実施の形態６．
図９は、この発明の実施の形態６による運動諸元推定装置の構成を示す図である。図９に示すように、実施の形態６による運動諸元推定装置は、受信手段１、距離差／速度差観測値検出手段２及び非同期測位手段１０を備える。なお、図９において、図１と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【００９２】
非同期測位手段１０は、初期状態量設定部１０１、時刻差算出部１０２１〜１０２Ｎ、距離差／速度差概略値算出部１０３１〜１０３Ｎ、修正量算出部１０４１〜１０４Ｎ、修正状態ベクトル算出部１０５、収束判定部１０６、及び収束演算カウント部１０７を備える。収束判定部１０６及び収束演算カウント部１０７以外の構成は、上記実施の形態１の図１で示した初期状態量設定部４１、時刻差算出部４２１〜４２Ｎ、距離差／速度差概略値算出部４３１〜４３Ｎ、修正量算出部４４１〜４４Ｎ、及び修正状態ベクトル算出部４５と同様に動作する。
【００９３】
収束判定部１０６は、収束演算カウント部１０７が計算回数限界値をカウントするまでの間、修正状態ベクトル算出部１０５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差／速度差概略値算出部１０３１〜１０３Ｎにフィードバックしたり、収束計算を終了して修正状態ベクトルを測位結果として出力する。
【００９４】
収束演算カウント部１０７は、収束判定部１０６により修正状態ベクトルが距離差／速度差概略値算出部１０３１〜１０３Ｎにフィードバックされる度に収束演算回数を、所定の計算回数限界値になるまでカウントする。ここで、計算回数限界値とは、実施の形態６による運動諸元推定装置を実現する計算機の演算性能や検知データ等のサンプリングレートに応じて決定した、最適な演算負荷と推定精度を与える収束演算回数である。
【００９５】
次に動作について説明する。
以降では、実施の形態６による運動諸元推定装置に特有な動作について主に説明する。
先ず、収束判定部１０６が収束演算を行うにあたり、収束演算カウント部１０７は、カウント値を０に初期化し、カウントの限界値として計算回数限界値を設定する。上記実施の形態１と同様にして、修正状態ベクトル算出部１０５から修正量が入力されると、収束判定部１０６は、所定の閾値と該修正量を比較する。このとき、修正量が閾値より大きく収束しなかった場合、収束演算カウント部１０７は、カウント値を１増やす。
【００９６】
計算回数限界値にカウント値が達しても修正量が収束しなかった場合、収束演算カウント部１０７は、計算終了の信号を収束判定部１０６に送信する。これにより、収束判定部１０６は、該計算終了時の修正量を反映させた修正状態ベクトル（修正量を加えた修正状態ベクトル）を目標の測位結果として出力する。なお、カウント値が計算回数限界値に達する前に修正量が収束した場合は、該収束した修正量を加えた修正状態ベクトルを目標の測位結果として出力し、収束演算カウント部１０７はカウントを停止する。
【００９７】
以上のように、この実施の形態６によれば、収束判定部１０６が、収束演算カウント部１０７によるカウントが予め定めた計算回数限界値まで達すると、収束演算を終了してその時点での運動諸元推定値を出力するので、修正状態ベクトル算出部１０５による収束計算の回数が限定されることから、本実施の形態６による運動諸元推定装置の性能を超える演算の実行が抑制され、かつ所定の時間で演算が終了することから収束計算の時間を短縮することができる。
【００９８】
なお、上記実施の形態６では、収束演算カウント部１０７を上記実施の形態１による構成に適用した場合を示したが、上記実施の形態２〜５の構成に適用してもよく、同様の効果を得ることができる。
【００９９】
実施の形態７．
図１０は、この発明の実施の形態７による運動諸元推定装置の構成を示す図である。図１０に示すように、実施の形態７による運動諸元推定装置は、受信手段１、距離差観測値検出手段２ａ及び非同期測位手段１２ａを備える。なお、図１０において、図１と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【０１００】
距離差観測値検出手段２ａは、距離差観測値検出部１１１〜１１Ｎ（Ｎは、Ｍ−１の整数）を備える。距離差観測値検出部１１１〜１１Ｎは、受信手段１を構成するセンサ１１〜１Ｍから２つのセンサごとに２センサ間の目標との距離差を検出し、該２センサ間の距離差とその観測時刻を出力する。
【０１０１】
実施の形態７によるセンサ１１〜１Ｍとしては、例えば目標へ放射したレーダ電波の反射電波が到達する時刻差から目標との距離差を算出するレーダセンサ等が考えられる。なお、上記式（３）に示すようにドップラ周波数差を算出する際にも目標の位置の初期値が必要であるが、目標の位置は、上記式（２）に示すように目標との距離差の観測値が必要である。そこで、本実施の形態７では、ドップラ周波数差を算出せず、任意の２センサ間の目標との距離差のみを用いて目標の状態ベクトルを推定することで演算処理の簡略化を図っている。
【０１０２】
非同期測位手段１２ａは、距離差観測値検出手段２ａから出力される、受信手段１における任意の２センサ間の目標との距離差を入力し、非同期を考慮して目標の状態ベクトルを推定する。また、非同期測位手段１２ａは、図１０に示すように初期状態量設定部１２１、時刻差算出部１２２１〜１２２Ｎ、距離差概略値算出部１２３１〜１２３Ｎ、修正量算出部１２４１〜１２４Ｎ、修正状態ベクトル算出部１２５、及び収束判定部１２６を備える。
【０１０３】
初期状態量設定部１２１は、修正状態ベクトル算出部１２５により目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。時刻差算出部１２２１〜１２２Ｎは、初期状態量設定部１２１により設定された基準時刻と観測時刻との時刻差を算出する。
【０１０４】
距離差概略値算出部１２３１〜１２３Ｎは、時刻差算出部１２２１〜１２２Ｎにより算出された時刻差及び初期状態量設定部１２１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部１２６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。
【０１０５】
修正量算出部１２４１〜１２４Ｎは、距離差概略値算出部１２３１〜１２３Ｎにより算出された概略値と距離差観測値検出手段２ａから得られた観測値との差分を算出し、この差分を用いて算出した修正量を出力する。修正状態ベクトル算出部１２５は、修正量算出部１２４１〜１２４Ｎから修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。
【０１０６】
収束判定部１２６では、修正状態ベクトル算出部１２５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差概略値算出部１２３１〜１２３Ｎにフィードバックしたり、収束計算を終了して修正状態ベクトルを測位結果として出力する。
【０１０７】
次に動作について説明する。
センサ１１〜１Ｍのうちの任意の２センサ間の目標との距離差の観測値は、受信手段１から距離差観測値検出手段２ａに出力される。距離差観測値検出手段２ａでは、受信手段１における任意の２センサによる観測値を距離差観測値検出部１１１〜１１Ｎが入力する。図１０の例では、受信手段１のセンサ１１〜１Ｍのうち、センサ１１及びセンサ１２のセンサ出力が距離差観測値検出部１１１に入力され、センサ１２及びセンサ１３のセンサ出力が距離差観測値検出部１１２に入力され、同様に２センサの出力が距離差観測値検出部に逐次入力される。
【０１０８】
距離差観測値検出部１１１〜１１Ｎでは、センサ１１〜１Ｍのうちの２センサからそれぞれ入力した検知結果に基づいて、２センサ間の目標との距離差を検出する。この後、距離差観測値検出部１１１〜１１Ｎは、２センサ間の目標との距離差を、非同期測位手段１２ａ内の修正量算出部１２３１〜１２３Ｎにそれぞれ出力し、各センサの観測時刻も非同期測位手段１２ａ内の時刻差算出部１２２１〜１２２Ｎにそれぞれ出力する。
【０１０９】
非同期測位手段１２ａ内の初期状態量設定部１２１は、基準時刻ｔを時刻差算出部１２２１〜１２２Ｎにそれぞれ出力し、目標の状態ベクトルの初期値ｘ（ｔ）を距離差概略値算出部１２３１〜１２３Ｎにそれぞれ出力する。時刻差算出部１２２１〜１２２Ｎは、基準時刻ｔと観測時刻とを用い、上記式（１７）により基準時刻ｔと観測時刻ｔkの時刻差Δｔkを算出し、距離差概略値算出部１２３１〜１２３Ｎにそれぞれ出力する。
【０１１０】
距離差概略値算出部１２３１〜１２３Ｎでは、初期状態量設定部１２１から入力した目標の状態ベクトルｘ（ｔ）の初期値と時刻差算出部１２２１〜１２２Ｎから入力した時刻差を用い、上記式（１２）に従ってそれぞれ異なる時刻における目標の状態ベクトルｘ（ｔ）（目標の位置、速度等）とセンサｉ，ｊとの距離差の概略値ｇi,jを算出して修正量算出部１２４１〜１２４Ｎにそれぞれ出力する（上記式（１６）のｇi,jに相当するもの）。
【０１１１】
修正量算出部１２４１〜１２４Ｎは、距離差概略値算出部１２３１〜１２３Ｎからの概略値ｇi,jと、距離差観測値検出部１１１〜１１Ｎからの観測値ｚi,j（距離差の観測値）とを入力し、概略値ｇi,jと観測値ｚi,jとの誤差（距離差の誤差Δｈi,j）を算出し、この誤差を用いて算出した修正量を出力する。修正状態ベクトル算出部１２５では、修正量算出部１２４１〜１２４Ｎから修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトル（該修正量を加えた状態ベクトル）を出力する。
【０１１２】
収束判定部１２６では、修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値ｘ（ｔ）の代わりに距離差概略値算出部１２３１〜１２３Ｎの入力へフィードバックし、上記処理を繰り返す。修正量が閾値未満になれば、修正状態ベクトルを測位結果として出力する。
【０１１３】
以上のように、この実施の形態７によれば、センサ１１〜１Ｍから任意の２センサ間の目標との距離差の観測値のみを入力し、該距離差に基づいて目標の状態ベクトルを推定するので、ドップラ周波数差が得られない場合であっても目標の運動諸元を推定することができる。これにより、ドップラ周波数差を得るための演算処理も省略することができ、簡易な構成で上記実施の形態１と同様の効果を得ることができる。
【０１１４】
実施の形態８．
図１１は、この発明の実施の形態８による運動諸元推定装置の構成を示す図である。図１１に示すように、実施の形態８による運動諸元推定装置の基本的な構成は、上記実施の形態７の図１０に示したものと同様であるが、非同期測位手段１３ａが距離差観測値検出部１１１〜１１Ｎから観測精度を入力する点で異なる。
【０１１５】
非同期測位手段１３ａ内の修正状態ベクトル算出部１３５は、距離差観測値検出部１１１〜１１Ｎからの観測精度を考慮して修正量算出部１３４１〜１３４Ｎからの修正量に対して最小二乗法を用いて目標の運動諸元を推定する。つまり、修正状態ベクトル算出部１３５が、状態ベクトルの修正量を算出するにあたり、上記式（１１）について距離差の観測精度を重み付けの項として加える。
【０１１６】
初期状態量設定部１３１は、修正状態ベクトル算出部１３５により目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。時刻差算出部１３２１〜１３２Ｎは、初期状態量設定部１３１により設定された基準時刻と観測時刻との時刻差を算出する。
【０１１７】
距離差概略値算出部１３３１〜１３３Ｎは、時刻差算出部１３２１〜１３２Ｎにより算出された時刻差及び初期状態量設定部１３１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部１３６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。
【０１１８】
収束判定部１３６では、修正状態ベクトル算出部１３５による修正量が閾値より大きい場合、修正状態ベクトルを距離差概略値算出部１３３１〜１３３Ｎに出力し、修正量が閾値より小さい場合、収束計算を終了して修正状態ベクトルを測位結果として出力する。なお、他の構成は、図１０と同一又はこれに相当する構成であるので同一符号を付して説明を省略する。
【０１１９】
次に動作について説明する。
上記実施の形態７と異なる動作について主に説明する。
修正状態ベクトル算出部１３５は、修正量算出部１３４１〜１３４Ｎから入力した状態ベクトルの修正量と、距離差観測値検出部１３３１〜１３３Ｎから入力した距離差の観測精度を用い、該距離差の観測精度を重み付けした最小二乗法により目標の状態ベクトルの修正量とこれを状態ベクトルに反映させた修正状態ベクトルを出力する。例えば、上記式（１８）のように観測精度Ｓを重み付けした式に従って目標の状態ベクトルの修正量と修正状態ベクトルを算出する。
【０１２０】
収束判定部１３６は、修正状態ベクトル算出部１３５から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標状態ベクトルの初期値ｘ（ｔ）の代わりに距離差概略値算出部１３３１〜１３３Ｎの入力にフィードバックし、上記処理を繰り返す。修正量が閾値未満になれば、修正状態ベクトルを測位結果として出力する。
【０１２１】
以上のように、この実施の形態８によれば、観測値が非同期であっても目標の状態ベクトルを推定することができる。また、観測情報として任意の２センサ間の目標との距離差のみでよいことから、上記実施の形態７と同様にドップラ周波数差が得られないセンサであっても目標の運動諸元を推定することができる。これにより、ドップラ周波数差を得るための演算処理も省略することができ、簡易な構成で上記実施の形態１と同様の効果を得ることができる。また、距離差の観測精度を考慮することにより、より信頼性の高い推定を実施することが可能である。
【０１２２】
実施の形態９．
図１２は、この発明の実施の形態９による運動諸元推定装置の構成を示す図である。図１２に示すように、実施の形態９による運動諸元推定装置は、受信手段１、距離差観測値検出手段２ａ、仮測位／初期値算出手段１４ａ及び非同期測位手段１５ａを備える。なお、図１２において、図１０と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【０１２３】
仮測位／初期値算出手段１４ａは、初期状態量設定部１４１、距離差概略値算出部１４２１，１４２２、修正量算出部１４３１，１４３２、修正状態ベクトル算出部１４４、収束判定部１４５、メモリ１４６及び初期値算出部１４７を備える。初期状態量設定部１４１は、修正状態ベクトル算出部１４４が目標の状態ベクトルの修正量を最小二乗法により算出する際に用いる初期値として目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。
【０１２４】
距離差概略値算出部１４２１，１４２２は、初期状態量設定部１４１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部１４５からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。修正量算出部１４３１，１４３２は、距離差概略値算出部１４２１，１４２２により算出された概略値と距離差観測値検出部１１１，１１２から得られた観測値（センサ１１，１２とセンサ１２，１３による観測値）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。
【０１２５】
修正状態ベクトル算出部１４４は、修正量算出部１４３１，１４３２から修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。収束判定部１４５では、修正状態ベクトル算出部１４４による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差概略値算出部１４２１，１４２２にフィードバックしたり、収束計算を終了して修正状態ベクトルをメモリ１４６に出力する。
【０１２６】
メモリ１４６は、収束判定部１４５で収束したものと判定された目標の測位結果（目標の状態ベクトル）を記憶する。初期値算出部１４７は、下記式（１９）及び下記式（２０）で表される異なる２つの時刻ｔ1，ｔ2における測位結果である目標の位置ｘ（ｔ1）ハット，ｘ（ｔ2）ハット（出願書類の処理の関係上、読みで表記する）から下記式（２１）に従って初期速度ｘドット（出願書類の処理の関係上、読みで表記する）を得る。この最新時刻における目標の測位結果を非同期測位手段１５ａへの初期値として出力する。
【数１０】
【０１２７】
非同期測位手段１５ａは、時刻差算出部１５１１〜１５１Ｎ、距離差概略値算出部１５２１〜１５２Ｎ、修正量算出部１５３１〜１５３Ｎ、修正状態ベクトル算出部１５４、及び収束判定部１５５を備え、仮測位／初期値算出手段１４ａによる測位結果を、目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として用い、初期状態量設定部が省略されている以外は上記実施の形態８と同様に動作する。
【０１２８】
図１３は、時刻ｔ1においてセンサ１１及びセンサ１２、時刻ｔ2においてセンサ１２及びセンサ１３、時刻ｔ3においてセンサ１１及びセンサ１２、時刻ｔ4においてセンサ１２及びセンサ１３によって距離差が得られた場合を示す図である。ｘ−ｙの２次元直交座標系における目標の位置と速度を推定する場合、実施の形態９では、図１３に示すような異なるセンサ組による検知結果の等時間差線（双曲線）の交点を測位し、交点における目標の位置及び速度を非同期測位手段１５ａにおける目標の状態ベクトルの初期値として設定する。
【０１２９】
例えば、図１３中の破線で示す双曲線は、時刻ｔ1，ｔ3において目標へ放射されたレーダ電波が目標で反射してセンサ１１及びセンサ１２に到達する時刻差を用いて検出された２センサ間の目標との距離差による双曲線（目標とのセンサ１１及びセンサ１２間の距離差が一定な等距離差双曲線）である。また、図１３中に実線で示す双曲線は、時刻ｔ2，ｔ4においてセンサ１２及びセンサ１３に反射電波が到達する時刻差を用いて検出された２センサ間の目標との距離差による双曲線（目標とのセンサ１２及びセンサ１３間の距離差が一定な等距離差双曲線）である。
【０１３０】
実施の形態９では、時刻ｔ1，ｔ3においてセンサ１１及びセンサ１２によって距離差を観測し、時刻ｔ2，ｔ4においてセンサ１２及びセンサ１３によって距離差を観測し、基準時刻として時刻ｔ2，ｔ4を設定して各センサ組の観測値から目標の状態ベクトルを算出することにより、これら双曲線の交点における目標の状態ベクトルを算出し、該状態ベクトルを非同期測位手段１５ａによる修正量の算出における初期値として使用する。
【０１３１】
次に動作について説明する。
仮測位／初期値算出手段１４ａでは、初期状態量設定部１４１により基準時刻及びこの基準時刻における目標の状態ベクトルの初期値が距離差概略値算出部１４２１，１４２２に設定される。距離差概略値算出部１４２１，１４２２では、初期状態量設定部１４１から入力した目標の状態ベクトルの初期値から各センサ組の２センサ間の目標との距離差の概略値を算出して修正量算出部１４３１，１４３２にそれぞれ出力する。
【０１３２】
修正量算出部１４３１，１４３２では、距離差概略値算出部１４２１，１４２２からの概略値と距離差観測値検出部１１１，１１２からの観測値（２センサ間の目標との距離差）を入力し、この概略値と観測値との誤差（距離差の誤差Δｈi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部１４４では、修正量算出部１４３１，１４３２から修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【０１３３】
収束判定部１４５は、修正状態ベクトル算出部１４４から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値の代わりに距離差概略値算出部１４２１，１４２２の入力へフィードバックし、上記処理を繰り返す。
【０１３４】
修正量が閾値未満であると、収束判定部１４５は、該修正量を反映させた目標の状態ベクトル（目標の位置、速度等）を修正状態ベクトル算出部１４４から入力し、最新観測時刻（図１３では、時間的に近い時刻ｔ1，ｔ2のときは時刻ｔ2であり、時刻ｔ3，ｔ4のときは時刻ｔ4）における目標の測位結果としてメモリ１４６に逐次出力して値を保持させる。
【０１３５】
初期値算出部１４７は、上記式（１９），（２０）により２つの時刻ｔ2，ｔ4における測位結果である目標の位置ｘ（ｔ2）ハット，ｘ（ｔ4）ハットを定義し、これらを上記式（２１）に代入することで目標の初期速度ｘドットを得る。この最新時刻における目標の測位結果を非同期測位手段１５ａへの初期値として出力する。具体的には、基準時刻として時刻ｔ4を非同期測位手段１５ａの時間差算出部１５１１〜１５１Ｎへ出力し、目標の修正状態ベクトルを、非同期測位手段１５ａにおける目標の状態ベクトルの修正量を最小二乗法により算出する際の初期値として距離差概略値算出部１５２１〜１５２Ｎへ出力する。
【０１３６】
非同期測位手段１５ａは、仮測位／初期値算出手段１４ａから基準時刻及び目標の状態ベクトルの初期値が設定されると、上記実施の形態７と同様の処理により目標の測位結果を推定する。
【０１３７】
以上のように、この実施の形態９によれば、仮測位／初期値算出手段１４ａが異なる２つの時刻において異なるセンサ組により観測された該２センサ間の目標との距離差に基づいて算出した目標の測位結果を、非同期測位手段１５ａによる修正状態ベクトル算出処理の初期値として用いるので、異なる時刻の観測値をあたかも同時刻で観測された結果とみなすことができ、観測値が非同期であっても信頼性の高い目標の運動諸元の推定が可能である。また、観測情報として任意の２センサ間の目標との距離差のみでよいことから、上記実施の形態７と同様にドップラ周波数差が得られないセンサであっても目標の運動諸元を推定することができる。
【０１３８】
実施の形態１０．
図１４は、この発明の実施の形態１０による運動諸元推定装置の構成を示す図である。図１４に示すように、実施の形態１０による運動諸元推定装置は、受信手段１、距離差観測値検出手段２ａ、仮測位／初期値算出手段１６ａ及び非同期測位手段１５ａを備える。なお、図１４において、図１、図６及び図１０と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【０１３９】
仮測位／初期値算出手段１６ａは、初期状態量設定部１６１、追尾フィルタ部１６２１，１６２２、距離差概略値算出部１６３１，１６３２、修正量算出部１６４１，１６４２、修正状態ベクトル算出部１６５、収束判定部１６６、メモリ１６７、及び初期値算出部１６８を備える。初期状態量設定部１６１は、修正状態ベクトル算出部１６５が目標の状態ベクトルの修正量を最小二乗法により算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。
【０１４０】
追尾フィルタ部１６２１，１６２２は、受信手段１内のセンサの組み合わせ（図１４の例では、センサ１１，１２とセンサ１２，１３）で得られる観測値（距離差）を入力し、これより所定時刻におけるセンサ組の２センサ間の目標との距離差を推定する。距離差概略値算出部１６３１，１６３２は、初期状態量設定部１６１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部１６６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った目標の運動諸元の概略値を算出する。
【０１４１】
修正量算出部１６４１は、距離差概略値算出部１６３１により算出された目標の運動諸元の概略値と追尾フィルタ部１６２１により得られた推定値（センサ１１，１２間の目標との距離差）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。修正量算出部１６４２は、距離差概略値算出部１６３２により算出された目標の運動諸元の概略値と追尾フィルタ部１６２２によって得られた推定値（センサ１２，１３間の目標との距離差）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。
【０１４２】
修正状態ベクトル算出部１６５は、修正量算出部１６４１，１６４２から修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。収束判定部１６６では、修正状態ベクトル算出部１６５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差概略値算出部１６３１，１６３２にフィードバックしたり、収束計算を終了して修正状態ベクトルをメモリ１６７に出力する。
【０１４３】
メモリ１６７は、収束判定部１６６で収束したものと判定された目標の測位結果（目標の状態ベクトル）を記憶する。初期値算出部１６８は、上記式（１９）及び上記式（２０）で表される異なる２つの時刻における測位結果である目標の位置から上記式（２１）に従って目標の初期速度を得る。この最新時刻における目標の測位結果を非同期測位手段１５ａへの初期値として出力する。
【０１４４】
図１５は、時刻ｔ1，ｔ2においてセンサ１１とセンサ１２により距離差を観測し、時刻ｔ3，ｔ4においてセンサ１２とセンサ１３によって距離差が観測された場合を示す図である。ｘ−ｙの２次元直交座標系における目標の位置と速度を推定する場合、実施の形態１０では、いずれか一方のセンサ組み合わせで得られる目標を追尾処理（目標との距離差を推定）し、他方のセンサ組み合わせにより距離差が得られる時刻に対する予測処理を実施することで時刻の同期を取る。
【０１４５】
例えば、図１５では、時刻ｔ3でセンサ１２とセンサ１３により距離差が得られた場合、センサ１１とセンサ１２の組み合わせによる距離差を時刻ｔ3，ｔ4において予測処理を実施する。この結果、図１５に示すように時刻ｔ3におけるセンサ１１，１２による観測値の双曲線（実線）とセンサ１２，１３による観測値の双曲線（一点破線）の交点が測位される。また、時刻ｔ4におけるセンサ１１，１２による観測値の双曲線（実線）とセンサ１２，１３による観測値の双曲線（一点破線）の交点が測位される。実施の形態１０では、これらの交点での目標の状態ベクトルを、非同期測位手段１５ａにおける修正量算出の初期値とする。
【０１４６】
次に動作について説明する。
仮測位／初期値算出手段１６ａにおいて、初期状態量設定部１６１が追尾フィルタ部１６２１，１６２２に基準時刻を設定し、基準時刻における目標の状態ベクトルの初期値を距離差概略値算出部１６３１，１６３２に設定する。
【０１４７】
追尾フィルタ部１６２１は、距離差観測値検出部１１１から、センサ１１，１２の検知結果によって得られる観測値（目標との距離差）及びその観測時刻を入力して、初期状態量設定部１６１により設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差観測値検出部１１１から入力した観測値を用いて、ある時刻（図１５では時刻ｔ1，ｔ2）における目標を追尾処理する（センサ１１，１２間の目標との距離差を推定する）。
【０１４８】
追尾フィルタ部１６２２では、距離差観測値検出部１１２から、センサ１２，１３の検知結果から得られる観測値及びその観測時刻を入力して、初期状態量設定部１６１に設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差観測値検出部１１２から入力した観測値とを用いて、目標の追尾処理（２センサ間の目標との距離差を推定）する。
【０１４９】
このように、時刻ｔ3でセンサ１２，１３により目標との距離差が得られている場合、追尾フィルタ部１６２１は、センサ１１，１２による目標との距離差について時刻ｔ3における予測処理を実行する。これにより、図１５中の一点破線で示すセンサ１１，１２による観測値の等時間差線と図１５中の実線で示すセンサ１２，１３による観測値の等時間差線との交点が測位される。つまり、センサ１２，１３により距離差が得られる時刻ｔ3，ｔ4におけるセンサ１１，１２による観測の予測値を得ることで、同一時刻（時刻ｔ3，ｔ4）で目標との距離差が得られたとみなして測位処理を実施する。但し、予測処理するセンサの組み合わせは１組以上とする。
【０１５０】
距離差概略値算出部１６３１，１６３２は、初期状態量設定部１６１から入力した目標の状態ベクトルの初期値を用いて目標との各センサ組の２センサ間の距離差の概略値を算出して修正量算出部１６４１，１６４２にそれぞれ出力する。
【０１５１】
修正量算出部１６４１，１６４２では、距離差概略値算出部１６３１，１６３２からの概略値と追尾フィルタ部１６２１，１６２２から入力した推定値（２センサ間の目標との距離差）を入力し、概略値と観測値の誤差（距離差の誤差Δｈi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部１６５では、修正量算出部１６４１，１６４２から修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【０１５２】
収束判定部１６６は、修正状態ベクトル算出部１６５から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値の代わりに距離差概略値算出部１６３１，１６３２の入力へフィードバックして上記処理を繰り返す。
【０１５３】
修正量が閾値未満であると、収束判定部１６６は、該修正量を反映させた目標の状態ベクトル（目標の位置、速度等）を修正状態ベクトル算出部１６５から入力し、図１５に示す時刻ｔ3，ｔ4における目標の測位結果としてメモリ１６７に逐次出力して値を保持させる。
【０１５４】
初期値算出部１６８は、上記式（１９），（２０）により２つの時刻ｔ3，ｔ4における測位結果である目標の位置ｘ（ｔ3）ハット，ｘ（ｔ4）ハットを定義し、これらを上記式（２１）に代入することで目標の初期速度ｘドットを得る。この最新時刻における目標の測位結果を非同期測位手段１５ａへの初期値として出力する。具体的には、基準時刻として時刻ｔ4を非同期測位手段１５ａの時間差算出部１５１１〜１５１Ｎへ出力し、目標の修正状態ベクトルを、非同期測位手段１５ａにおける目標の状態ベクトルの修正量を最小二乗法により算出する際の初期値として距離差概略値算出部１５２１〜１５２Ｎへ出力する。
【０１５５】
非同期測位手段１５ａは、仮測位／初期値算出手段１６ａから基準時刻及び目標の状態ベクトルの初期値が設定されると、上記実施の形態７と同様の処理により目標の測位結果を推定する。
【０１５６】
以上のように、この実施の形態１０によれば、仮測位／初期値算出手段１６ａが、センサ１１，１２の検知結果から得られた目標との距離差を追尾処理し、別の時刻において、異なるセンサ組（センサ１２，１３）の検知結果から目標との距離差が得られる場合、上記追尾結果に基づいてそのセンサ組（センサ１２，１３）に対する時刻整合を行ってから、目標の位置、速度を算出し、最小二乗法による目標の状態ベクトル算出の初期値として設定するので、異なる時刻の観測値をあたかも同時刻で観測された結果とみなすことができ、観測値が非同期であっても信頼性の高い目標の運動諸元推定が可能である。また、観測情報として任意の２センサ間の目標との距離差のみでよいことから、上記実施の形態７と同様にドップラ周波数差が得られないセンサであっても目標の運動諸元を推定することができる。
【０１５７】
実施の形態１１．
図１６は、この発明の実施の形態１１による運動諸元推定装置の構成を示す図である。図１６に示すように、実施の形態１１による運動諸元推定装置は、受信手段１、距離差観測値検出手段２ａ及び非同期測位手段１７ａを備える。なお、図１６において、図１、図８及び図１０と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【０１５８】
非同期測位手段１７ａは、初期状態量設定部１７１、追尾フィルタ部１７２１〜１７２Ｎ、距離差概略値算出部１７３１〜１７３Ｎ、修正量算出部１７４１〜１７４Ｎ、修正状態ベクトル算出部１７５、及び収束判定部１７６を備える。
【０１５９】
初期状態量設定部１７１は、修正状態ベクトル算出部１７５により目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。追尾フィルタ部１７２１〜１７２Ｎは、受信手段１内のセンサの組み合わせで得られる距離差を入力し、追尾処理を実施する。
【０１６０】
距離差概略値算出部１７３１〜１７３Ｎは、初期状態量設定部１７１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部１７６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。
【０１６１】
修正量算出部１７４１は、距離差概略値算出部１７３１により算出された概略値と追尾フィルタ部１７２から得られた観測値（センサ１１，１２による観測値）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。修正量算出部１７４２〜１７４Ｎは、距離差概略値算出部１７３２〜１７３Ｎにより算出された概略値と距離差観測値検出部１１２〜１１Ｎから得られた観測値との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。
【０１６２】
修正状態ベクトル算出部１７５は、修正量算出部１７４１〜１７４Ｎから修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。収束判定部１７６では、修正状態ベクトル算出部１７５による修正量が閾値より大きい場合、修正状態ベクトルを距離差概略値算出部１７３１〜１７３Ｎに出力し、修正量が閾値より小さい場合、収束計算を終了して修正状態ベクトルを測位結果として出力する。
【０１６３】
次に動作について説明する。
初期状態量設定部１７１は、追尾フィルタ部１７２１〜１７２Ｎに基準時刻を設定し、基準時刻における目標の状態ベクトルの初期値を距離差概略値算出部１７３１〜１７３Ｎに設定する。
【０１６４】
追尾フィルタ部１７２１〜１７２Ｎは、距離差観測値検出部１１１〜１１Ｎから、センサ組の検知結果により得られる観測値（２センサ間の目標との距離差）及びその観測時刻をそれぞれ入力して、初期状態量設定部１７１により設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差観測値検出部から入力した観測値とを用いて、目標を追尾処理する（各センサ組の２センサ間の目標との距離差を推定する）。
【０１６５】
追尾フィルタ部１７２１〜１７２Ｎのうち、ある追尾フィルタ部は、距離差観測値検出部から、ある時刻で他の追尾フィルタ部により異なるセンサ組で観測値が得られている場合、初期状態量設定部１７１に設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差観測値検出部から入力した観測値とを用いて、当該時刻における目標との距離差を予測処理（２センサ間の目標との距離差を推定）する。
【０１６６】
例えば、ある時刻で追尾フィルタ部１７２２〜１７２Ｎによってセンサ１２，１３〜１（Ｎ−１），１Ｎの検知結果で目標との距離差が得られている場合、追尾フィルタ部１７２１が該時刻における予測値を得ることで、同一時刻において目標との距離差が得られたとみなして測位処理を実施する。但し、予測処理するセンサの組み合わせは１組以上とする。
【０１６７】
距離差概略値算出部１７３１〜１７３Ｎは、初期状態量設定部１７１から入力した目標の状態ベクトルの初期値を用いて目標と各センサ組との距離差の概略値を算出して修正量算出部１７４１〜１７４Ｎにそれぞれ出力する。
【０１６８】
修正量算出部１７４１〜１７４Ｎでは、距離差概略値算出部１７３１〜１７３Ｎからの概略値と追尾フィルタ部１７２１〜１７２Ｎから入力した推定値（目標との距離差）を入力し、概略値と観測値の誤差（距離差の誤差Δｈi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部１７５では、修正量算出部１７４１〜１７４Ｎから修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【０１６９】
収束判定部１７６は、修正状態ベクトル算出部１７５から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値の代わりに距離差概略値算出部１７３１〜１７３Ｎの入力へフィードバックし、上記処理を繰り返す。修正量が閾値未満であると、収束判定部１７６は、該修正量を反映させた目標の状態ベクトル（目標の位置、速度等）を修正状態ベクトル算出部１７５から入力して目標の測位結果として出力する。
【０１７０】
以上のように、この実施の形態１１によれば、非同期測位手段１７ａが、センサ組の検知結果から得られた目標との距離差を追尾処理し、別の時刻において、異なるセンサ組の検知結果から目標との距離差が得られる場合、上記追尾結果に基づいてそのセンサ組に対する時刻整合を行ってから、目標の運動諸元を推定するので、異なる時刻の観測値をあたかも同時刻で観測された結果とみなすことができ、観測値が非同期であっても信頼性の高い目標の運動諸元の推定が可能である。また、観測情報として任意の２センサ間の目標との距離差のみでよいことから、上記実施の形態７と同様にドップラ周波数差が得られないセンサであっても目標の運動諸元を推定することができる。
【０１７１】
実施の形態１２．
図１７は、この発明の実施の形態１２による運動諸元推定装置の構成を示す図である。図１７に示すように、実施の形態１２による運動諸元推定装置は、受信手段１、距離差観測値検出手段２ａ及び非同期測位手段１８ａを備える。なお、図１７において、図１、図９及び図１０と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【０１７２】
非同期測位手段１８ａは、初期状態量設定部１８１、時刻差算出部１８２１〜１８２Ｎ、距離差概略値算出部１８３１〜１８３Ｎ、修正量算出部１８４１〜１８４Ｎ、修正状態ベクトル算出部１８５、収束判定部１８６、及び収束演算カウント部１８７を備える。収束判定部１８６及び収束演算カウント部１８７以外の構成は、上記実施の形態７の図１０で示した初期状態量設定部１２１、時刻差算出部１２２１〜１２２Ｎ、距離差概略値算出部１２３１〜１２３Ｎ、修正量算出部１２４１〜１２４Ｎ、及び修正状態ベクトル算出部１２５と同様に動作する。
【０１７３】
収束判定部１８６は、収束演算カウント部１８７が計算回数限界値をカウントするまでの間、修正状態ベクトル算出部１８５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差概略値算出部１８３１〜１８３Ｎにフィードバックしたり、収束計算を終了して修正状態ベクトルを測位結果として出力する。
【０１７４】
収束演算カウント部１８７は、収束判定部１８６により修正状態ベクトルが距離差概略値算出部１８３１〜１８３Ｎにフィードバックされる度に収束演算回数を、所定の計算回数限界値になるまで計数する。ここで、計算回数限界値とは、実施の形態１２による運動諸元推定装置を実現する計算機の演算性能や検知データ等のサンプリングレートに応じて決定した、最適な演算負荷と推定精度を与える収束演算回数である。
【０１７５】
次に動作について説明する。
以降では、本実施の形態１２による運動諸元推定装置に特有な動作を主に説明する。
先ず、収束判定部１８６が収束演算を行うにあたり、収束演算カウント部１８７は、カウント値を０に初期化し、カウントの限界値として計算回数限界値を設定する。上記実施の形態１と同様にして、修正状態ベクトル算出部１８５から修正量が入力されると、収束判定部１８６は、所定の閾値と該修正量を比較する。このとき、修正量が閾値より大きく収束しなかった場合、収束演算カウント部１８７は、カウント値を１増やす。
【０１７６】
計算回数限界値にカウント値が達しても修正量が収束しなかった場合、収束演算カウント部１８７は、計算終了の信号を収束判定部１８６に送信する。これにより、収束判定部１８６は、該計算終了時の修正量を反映させた修正状態ベクトル（修正量を加えた修正状態ベクトル）を目標の測位結果として出力する。なお、カウント値が計算回数限界値に達する前に修正量が収束した場合は、該収束した修正量を加えた修正状態ベクトルを目標の測位結果として出力し、収束演算カウント部１８７はカウントを停止する。
【０１７７】
以上のように、この実施の形態１２によれば、収束判定部１８６が、収束演算カウント部１８７が収束演算の回数をカウントし、予め定めた計算回数限界値に達しても修正量が収束しない場合、その時点の修正量を反映させた修正状態ベクトルを目標の測位結果とするので、修正状態ベクトル算出部１８５による収束計算の回数が限定されることから、本実施の形態１２による運動諸元推定装置の性能を超える演算の実行が抑制され、かつ所定の時間で演算が終了することから収束計算の時間を短縮することができる。また、観測情報として任意の２センサ間の目標との距離差のみでよいことから、上記実施の形態７と同様にドップラ周波数差が得られないセンサであっても目標の運動諸元を推定できる。
【０１７８】
なお、上記実施の形態１２では、収束演算カウント部１８７を上記実施の形態７による構成に適用した場合を示したが、上記実施の形態８〜１１の構成に適用してもよく、同様の効果を得ることができる。
【図面の簡単な説明】
【０１７９】
【図１】この発明の実施の形態１による運動諸元推定装置の構成を示す図である。
【図２】航空機等の目標について基準時刻ｔ〜ｔ3までに４回の観測値が得られた場合を示す図である。
【図３】この発明の実施の形態２による運動諸元推定装置の構成を示す図である。
【図４】この発明の実施の形態３による運動諸元推定装置の構成を示す図である。
【図５】時刻ｔ3にセンサ１１，１２、時刻ｔ4にセンサ１２，１３により距離差及びドップラ周波数差が得られた場合を示す図である。
【図６】この発明の実施の形態４による運動諸元推定装置の構成を示す図である。
【図７】時刻ｔ1，ｔ2にセンサ１１，１２、時刻ｔ3にセンサ１２，１３により距離差及びドップラ周波数差が得られた場合を示す図である。
【図８】この発明の実施の形態５による運動諸元推定装置の構成を示す図である。
【図９】この発明の実施の形態６による運動諸元推定装置の構成を示す図である。
【図１０】この発明の実施の形態７による運動諸元推定装置の構成を示す図である。
【図１１】この発明の実施の形態８による運動諸元推定装置の構成を示す図である。
【図１２】この発明の実施の形態９による運動諸元推定装置の構成を示す図である。
【図１３】時刻ｔ1にセンサ１１，１２、時刻ｔ2にセンサ１２，１３、時刻ｔ3にセンサ１１，１２、時刻ｔ4にセンサ１２，１３により距離差が得られた場合を示す図である。
【図１４】この発明の実施の形態１０による運動諸元推定装置の構成を示す図である。
【図１５】時刻ｔ1，ｔ2にセンサ１１，１２、時刻ｔ3，ｔ4にセンサ１２，１３により距離差が得られた場合を示す図である。
【図１６】この発明の実施の形態１１による運動諸元推定装置の構成を示す図である。
【図１７】この発明の実施の形態１２による運動諸元推定装置の構成を示す図である。
【符号の説明】
【０１８０】
１ 受信手段、２ 距離差／速度差観測値検出手段、２ａ 距離差観測値検出手段（検出手段）、４，５，７，９，１０，１２ａ，１３ａ，１５ａ，１７ａ，１８ａ 非同期測位手段、８，１４ａ，１６ａ 仮測位／初期値算出手段、１１〜１Ｍ センサ、２１〜２Ｎ 距離差／速度差観測値検出部、４１，６１，８１，９１，１４１，１６１，１８１ 初期状態量設定部、４２１〜４２Ｎ，５２１〜５２Ｎ，７１１〜７１Ｎ，１０２１〜１０２Ｎ，１２２１〜１２２Ｎ，１３２１〜１３２Ｎ，１５１１〜１５１Ｎ，１８２１〜１８２Ｎ 時刻差算出部、４３１〜４３Ｎ，５３１〜５３Ｎ，７２１〜７２Ｎ，８３１，８３２，９３１〜９３Ｎ，１０３１〜１０３Ｎ，１２３１〜１２３Ｎ 距離差／速度差概算値算出部、１３３１〜１３３Ｎ，１４２１，１４２２，１５２１〜１５２Ｎ，１６３１，１６３２，１７３１〜１７３Ｎ，１８３１〜１８３Ｎ 距離差概算値算出部、４４１〜４４Ｎ，５４１〜５４Ｎ，７３１〜７３Ｎ，８４１，８４２，９４１〜９４Ｎ，１０４１〜１０４Ｎ，１２４１〜１２４Ｎ，１３４１〜１３４Ｎ，１４３１，１４３２，１５３１〜１５３Ｎ，１６４１，１６４２，１７４１〜１７４Ｎ，１８４１〜１８４Ｎ 修正量算出部、４５，５５，６４，７４，８５，９５，１０６，１２５，１３５，１４４，１５４，１６５，１７５，１８５ 修正状態ベクトル算出部、４６，５６，６５，７５，８６，９６，１２６，１３６，１４５，１５５，１６６，１７６，１８６ 収束判定部、８２１，８２２，９２１〜９２Ｎ，１６２１，１６２２，１７２１〜１７２Ｎ 追尾フィルタ部、１０７，１８７ 収束演算カウント部、１１１〜１１Ｎ 距離差観測値検出部、１４６，１６７ メモリ、１４７，１６８，１７１ 初期値算出部。
【技術分野】
【０００１】
この発明は、レーダセンサ等の複数のセンサを用いて、航空機、船舶、車両などの移動体と複数のセンサのうちの任意の２つのセンサの距離差やドップラ周波数差等の観測情報を入力し、これら観測情報に基づいて移動体の真の位置や速度等の運動諸元を推定する運動諸元推定装置に関するものである。
【背景技術】
【０００２】
レーダセンサ等を含む複数のセンサによる目標との距離差やドップラ周波数差の観測結果によって目標の運動諸元を推定する従来の運動諸元推定装置では、一般的に複数のセンサ間で同期して得られた観測情報を用いていた。ここで、同期とは、複数センサ間で目標の観測が同一時刻に行われることを表す。また、非同期とは複数のセンサ間で目標の観測を異なる時刻に行うことを意味する。
【０００３】
非特許文献１に開示される従来の運動諸元推定装置について説明する。
ここでは、説明の簡単のため、複数のセンサで構成される観測装置の観測モデルを下記式（１）のように仮定する。
【数１】
ここで、ｚi,j（ｔ）は時刻ｔにおけるセンサｉ及びセンサｊ間の目標との距離差（又は時刻ｔにおけるセンサｉ及びセンサｊ間の目標とのドップラ周波数差）の観測値である。ｈi,j（ｘ（ｔ））は時刻ｔにおけるセンサｉ及びセンサｊ間の目標との距離差の真値を表す観測式であり、ｌi,j（ｘ（ｔ））は時刻ｔにおけるセンサｉ及びセンサｊ間の目標とのドップラ周波数差の真値を表す観測式である。ｖi,j（ｔ）は、時刻ｔにおけるセンサｉ及びセンサｊ間の目標との距離差の観測誤差（又は時刻ｔにおけるセンサｉ及びセンサｊ間の目標とのドップラ周波数差の観測誤差）である。
【０００４】
例えば、観測装置によってｘ−ｙの２次元直交座標系におけるセンサｉ及びセンサｊ間の目標位置との距離差の観測結果が得られたとする。このとき、観測値の真値ｈi,j（ｘ（ｔ））を下記式（２）及び下記式（４）のように定義する。また、観測装置によってセンサｉ及びセンサｊ間の目標位置とのドップラ周波数差の観測結果が得られた場合は、観測値の真値ｌi,j（ｘ（ｔ））は、下記式（３）、下記式（２）及び下記式（５）で定義される。
【数２】
【０００５】
なお、ｓi（ｔ），ｓj（ｔ）は、それぞれ時刻ｔにおけるセンサｉ，ｊの位置ベクトルを表し、ｓi（ｔ）ドット及びｓj（ｔ）ドット（以下、出願書類の処理の関係上、読みで表記する）は、それぞれ時刻ｔにおけるセンサｉ，ｊの速度を示している。また、ｘ（ｔ）は、上記式（６）で表される目標の状態ベクトルであり、運動諸元推定装置によって求める運動諸元を表している。
【０００６】
上記のような観測モデルの仮定の下、従来の運動諸元推定装置では、以下に示すアルゴリズムで、状態ベクトルの推定を行う。
センサが３つ以上の場合、上記式（２）（又は上記式（３））の方程式が２式以上得られるため、この連立方程式を解くことで状態ベクトルを求めることができる。しかしながら、観測誤差等の影響により、実際には上記式（１）となるため、一意に解が求まらない。そこで、下記のような方法を採用する。
【０００７】
先ず、上記式（２）（又は上記式（３））の目標の状態ベクトルに対して適当な初期値を定め、上記式（２）（又は上記式（３））の左辺の初期値を与える。次に、複数のセンサによって実際の距離差（又はドップラ周波数差）の観測値が得られると、上記初期値との誤差信号が得られる。
【０００８】
ここで、複数のセンサにより実際の距離差の観測値が得られた場合について説明する。上記式（２）より下記式（７）〜（１０）の関係を導き出すことができる。これらより、距離差の真値及びその誤差信号と目標の状態ベクトルの修正量との関係が得られるので、上記式（２）を目標の状態ベクトルの初期値で近似して（下記式（７）参照）、真値への修正量δｘアンダーバー（以下、出願書類の処理の関係上、読みで表記する）が決まる。
【数３】
【０００９】
上記式（７）を最小二乗法を用いて解くと、修正量δｘアンダーバーは下記式（１１）で与えられる。これにより、上記式（１０）を解く。
【数４】
【００１０】
以上の処理を修正量δｘアンダーバーが収束するまで繰り返すと、状態ベクトルが推定される。つまり、収束判定と判断された場合、現在の目標の状態ベクトルに修正量を加えたものが目標の状態ベクトルとなる。一方、収束判定とみなされない場合においては、状態ベクトルに修正量を付加し、上述したアルゴリズに従った処理を繰り返す。なお、ドップラ周波数差の観測値が得られた場合についても上述と同様なアルゴリズムを適用することができる。
【００１１】
【非特許文献１】K. C. HO: "An Accurate Algebraic Solution for Moving Source Location Using TDOA and FDOA Measurements" IEEE Trans. On Signal Processing, Vol.52, No.9, Sep. 2004
【発明の開示】
【発明が解決しようとする課題】
【００１２】
従来の運動諸元推定装置では、複数のセンサが同じ時刻に観測した場合を前提として目標の運動諸元を推定している。しかし、実際には観測タイミングのずれや伝送遅延等により複数のセンサ間で観測時刻が異なる場合が多くなることが想定される。従って、従来の運動諸元推定装置では目標の運動諸元の推定が困難となり、後段処理である目標追尾等の処理に移行できないという課題がある。このため、異なる時刻に距離差やドップラ周波数差が得られた場合においても目標の運動諸元を推定できることが望まれていた。
【００１３】
この発明は、上記のような課題を解決するためになされたものであり、複数のセンサが非同期に観測した場合においても目標の運動諸元を推定することができる運動諸元推定装置を得ることを目的とする。
【課題を解決するための手段】
【００１４】
この発明に係る運動諸元推定装置は、目標の運動を観測する複数のセンサを有する受信手段と、受信手段の複数のセンサのうち任意のセンサ対による同一時刻での観測結果から、該センサ対のセンサ間の目標との距離差及び速度差の観測値を検出する検出手段と、複数のセンサ対による異なる各時刻での観測結果から検出された各センサ対のセンサ間の目標との距離差及び速度差の観測値と、予め仮定した目標の運動方程式に従い基準時刻における目標の運動諸元の初期値に対し該基準時刻との時刻差を反映させて算出した各時刻での各センサ対のセンサ間の目標との距離差及び速度差の概略推定値と、の差分より目標の運動諸元の修正量を算出し、収束演算によって差分が収束するまで運動諸元を修正することで基準時刻における目標の運動諸元を推定する非同期測位手段とを備えるものである。
【発明の効果】
【００１５】
この発明によれば、複数のセンサ対による異なる各時刻での観測結果から検出された各センサ対のセンサ間の目標との距離差及び速度差の観測値と、予め仮定した目標の運動方程式に従い基準時刻における目標の運動諸元の初期推定値に対し該基準時刻との時刻差を反映させて算出した各時刻での各センサ対のセンサ間の目標との距離差及び速度差の概略推定値と、の差分により目標の運動諸元の修正量を算出し、収束演算によって差分が収束するまで運動諸元を修正することで基準時刻における目標の運動諸元を推定するので、複数のセンサが非同期、すなわち異なる時刻で観測した結果からでも目標の運動諸元を推定することができ、該運動諸元を用いて目標を早期に追尾処理等することができるという効果がある。
【発明を実施するための最良の形態】
【００１６】
実施の形態１．
図１は、この発明の実施の形態１による運動諸元推定装置の構成を示す図である。実施の形態１による運動諸元推定装置は、受信手段１、距離差／速度差観測値検出手段（検出手段）２、非同期測位手段４を備える。受信手段１は、センサ１１〜１Ｍ（Ｍは、１より大きい整数）を備え、各センサ１１〜１Ｍは、目標とセンサ１１〜１Ｍのうちの任意の２センサ間（図１の例では、センサ１１とセンサ１２，・・・，センサ１（Ｍ−１）とセンサ１Ｍ）との距離差やドップラ周波数差を観測する。
【００１７】
距離差／速度差観測値検出手段２は、距離差／速度差観測値検出部２１〜２Ｎ（Ｎは、Ｍ−１の整数）を備える。距離差／速度差観測値検出部２１〜２Ｎは、受信手段１を構成するセンサ１１〜１Ｍから任意の２つのセンサごとにセンサ間の目標との距離差及びドップラ周波数差の観測結果を検出し、該２センサ間の距離差及びドップラ周波数差から求めた速度差とその観測時刻を出力する。
【００１８】
非同期測位手段４は、距離差／速度差観測値検出手段２から出力される、受信手段１における任意の２センサ間の目標との距離差及びドップラ周波数差から得られる速度差を入力して、非同期を考慮して目標の状態ベクトルを推定する。また、非同期測位手段４は、図１に示すように、初期状態量設定部４１、時刻差算出部４２１〜４２Ｎ、距離差／速度差概略値算出部４３１〜４３Ｎ、修正量算出部４４１〜４４Ｎ、修正状態ベクトル算出部４５、及び収束判定部４６を備える。
【００１９】
初期状態量設定部４１は、修正状態ベクトル算出部４５により目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。時刻差算出部４２１〜４２Ｎは、初期状態量設定部４１により設定された基準時刻と観測時刻との時刻差を算出する。
【００２０】
距離差／速度差概略値算出部４３１〜４３Ｎは、時刻差算出部４２１〜４２Ｎにより算出された時刻差及び初期状態量設定部４１により設定された基準時刻における目標の初期値（若しくは収束判定部４６からの修正量を初期値として）を用いて予め仮定した運動方程式に沿った概略値を算出する。
【００２１】
修正量算出部４４１〜４４Ｎは、距離差／速度差概略値算出部４３１〜４３Ｎにより算出された概略値と距離差／速度差観測値検出手段２から得られた観測値との差分を算出し、この差分を用いて目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部４５は、修正量算出部４４１〜４４Ｎから修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。
【００２２】
収束判定部４６では、修正状態ベクトル算出部４５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差／速度差概略値算出部４３１〜４３Ｎにフィードバックしたり、収束計算を終了して修正状態ベクトルを測位結果として出力する。
【００２３】
本発明による運動諸元推定装置では、非同期に得られた距離差や速度差の観測情報を、予め仮定した運動方程式（例えば、等速直線運動）に当てはめて、基準時刻（目標の運動諸元を推定したい指定時刻）における目標の運動諸元として目標の状態ベクトル（位置、速度等）を推定する。なお、この推定には、例えば最小二乗法を用いる。
【００２４】
また、以降の説明では目標の運動モデルを下記式（１２）のように仮定する。
但し、ｔは基準時刻であり、ｘ（ｔ）は基準時刻ｔにおける目標の運動諸元の真値を表す状態ベクトルである。また、ΔＴは、基準時刻ｔからの時刻差である。
【数５】
【００２５】
例えば、目標の基本的な運動を等速直線運動と仮定し、ｘ−ｙの２次元直交座標系における目標の位置と速度を推定する場合、目標の状態ベクトルｘ（ｔ）と運動モデルは下記式（１３）及び下記式（１４）のように定義される。但し、Ｔはベクトルや行列の転置を示しており、ドットは時間微分を示している。また、Ｉn×nはｎ行ｎ列の単位行列であり、Ｏn×nは、ｎ行ｎ列の零行列である。
【数６】
【００２６】
次に、上記式（１）と上記式（１２）より、２センサ間の目標との距離差について下記式（１５）が定義される。
【数７】
【００２７】
例えば、図２に示すように目標が航空機である場合、当該目標についてそれぞれ異なる時刻ｔ1〜ｔ3で基準時刻ｔを含めて計４回の観測で観測値が得られたとすると、上記式（１５）より下記式（１６）が導出される。なお、時刻ｔk（ｋ＝１〜３）は、下記式（１７）より基準時刻ｔからΔｔk前の時刻である。
【数８】
【００２８】
上記式（１６）より最小二乗法による収束計算を用いて、基準時刻ｔにおける状態ベクトル（図２中、時刻ｔにおいて太線の四角と線で表したもの）の推定値を算出する。なお、最小二乗法による収束計算は、例えば非特許文献１に開示される従来の運動諸元推定装置と同様の処理を実施する。
【００２９】
次に動作について説明する。
センサ１１〜１Ｍのうちの任意の２センサ間の目標との距離差及びドップラ周波数差の観測値は、受信手段１から距離差／速度差観測値検出手段２に出力される。距離差／速度差観測値検出手段２では、受信手段１における任意の２センサによる観測値を距離差／速度差観測値検出部２１〜２Ｎが入力する。図１の例では、受信手段１のセンサ１１〜１Ｍのうち、センサ１１及びセンサ１２のセンサ出力が距離差／速度差観測値検出部２１に入力され、センサ１２及びセンサ１３のセンサ出力が距離差／速度差観測値検出部２２に入力され、同様に２センサの出力が距離差／速度差観測値検出部に逐次入力される。
【００３０】
距離差／速度差観測値検出部２１〜２Ｎでは、センサ１１〜１Ｍのうちの２センサからそれぞれ入力した検知結果に基づいて、２センサ間の目標との距離差を検出し、２センサ間の目標とのドップラ周波数差から速度差を求める。この後、距離差／速度差観測値検出部２１〜２Ｎは、２センサ間の目標との距離差及び速度差を、非同期測位手段４内の修正量算出部４４１〜４４Ｎにそれぞれ出力し、各センサの観測時刻も非同期測位手段４内の時刻差算出部４２１〜４２Ｎにそれぞれ出力する。
【００３１】
非同期測位手段４内の初期状態量設定部４１は、基準時刻ｔを時刻差算出部４２１〜４２Ｎにそれぞれ出力し、目標の状態ベクトルの初期値ｘ（ｔ）を距離差／速度差概略値算出部４３１〜４３Ｎにそれぞれ出力する。時刻差算出部４２１〜４２Ｎは、基準時刻ｔと観測時刻とを用い、上記式（１７）により基準時刻ｔと観測時刻ｔkの時刻差Δｔkを算出し、距離差／速度差概略値算出部４３１〜４３Ｎにそれぞれ出力する。
【００３２】
距離差／速度差概略値算出部４３１〜４３Ｎでは、初期状態量設定部４１から入力した目標の状態ベクトルｘ（ｔ）の初期値と時刻差算出部４２１〜４２Ｎから入力した時刻差を用い、上記式（１２）に従ってそれぞれ異なる時刻における目標の状態ベクトルｘ（ｔ）とセンサｉ，ｊ間の目標との距離差又は速度差の概略値（上記式（１６）のｇi,jに相当するもの）とを算出して修正量算出部４４１〜４４Ｎにそれぞれ出力する。
【００３３】
このように、距離差／速度差概略値算出部４３１〜４３Ｎは、予め仮定した目標の運動方程式である上記式（１２）〜（１７）に従って、基準時刻ｔにおける目標の運動諸元の初期値ｘ（ｔ）に対し基準時刻ｔとの時刻差ΔＴを反映させて各時刻での各センサ対のセンサ間の目標との距離差及び速度差の概略値（概略推定値）を算出する。
【００３４】
修正量算出部４４１〜４４Ｎでは、距離差／速度差概略値算出部４３１〜４３Ｎからの概略値ｇi,jと、距離差／速度差観測値検出部２１〜２Ｎからの観測値ｚi,j（距離差、速度差の観測値）とを入力し、概略値ｇi,jと観測値ｚi,jとの誤差（距離差の誤差Δｈi,j、速度差の誤差Δｌi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部４５では、修正量算出部４４１〜４４Ｎから異なる時刻における修正量をそれぞれ入力し、最小二乗法による収束計算を施して、目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【００３５】
収束判定部４６では、修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値ｘ（ｔ）の代わりに距離差／速度差概略値算出部４３１〜４３Ｎの入力へフィードバックし、上記処理を繰り返す。修正量が閾値未満になれば、修正状態ベクトルを測位結果として出力する。
【００３６】
以上のように、この実施の形態１によれば、目標の運動を観測する複数のセンサ１１〜１Ｍを有する受信手段１と、受信手段１の複数のセンサ１１〜１Ｍのうち任意のセンサ対による同一時刻での観測結果から該センサ対のセンサ間の目標との距離差及び速度差の観測値を検出する距離差／速度差観測値検出手段２と、複数のセンサ対による異なる各時刻での観測結果から検出された各センサ対のセンサ間の目標との距離差及び速度差の観測値と、予め仮定した目標の運動方程式に従い基準時刻における目標の運動諸元の初期値に対し該基準時刻との時刻差を反映させて算出した各時刻での各センサ対のセンサ間の目標との距離差及び速度差の概略値と、の差分により目標の運動諸元の修正量を算出し、収束演算によって差分が収束するまで運動諸元を修正することで基準時刻における目標の運動諸元を推定する非同期測位手段４とを備えるので、観測タイミングのずれや伝送遅延等により複数のセンサ間で観測時刻が異なり、観測値が非同期になっても、信頼性の高い目標の運動諸元推定が可能である。
【００３７】
また、上記実施の形態１では、ｘ−ｙの２次元直交座標系における目標の位置と速度を推定する場合を示したが、未知数を増やすことで３次元での目標の位置と速度を推定することもできる。この場合は、観測値の数を増やすことで、目標の状態ベクトルを推定できる。
【００３８】
実施の形態２．
図３は、この発明の実施の形態２による運動諸元推定装置の構成を示す図である。図３において、実施の形態２による運動諸元推定装置の基本的な構成は、上記実施の形態１の図１で示したものと同様であるが、非同期測位手段５が距離差／速度差観測値検出部２１〜２Ｎから観測精度を入力する点で異なる。
【００３９】
非同期測位手段５の修正状態ベクトル算出部５５は、距離差／速度差観測値検出部２１〜２Ｎから入力した観測精度を考慮して、修正量算出部５４１〜５４Ｎからの修正量に対し最小二乗法を用いて目標の運動諸元を推定する。つまり、修正状態ベクトル算出部５５が、状態ベクトルの修正量を算出するにあたり、上記式（１１）について距離差及びドップラ周波数差の観測精度を重み付けの項として加える。
【００４０】
初期状態量設定部５１は、修正状態ベクトル算出部５５により目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。時刻差算出部５２１〜５２Ｎは、初期状態量設定部５１により設定された基準時刻と観測時刻との時刻差を算出する。
【００４１】
距離差／速度差概略値算出部５３１〜５３Ｎは、時刻差算出部５２１〜５２Ｎにより算出された時刻差及び初期状態量設定部５１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部５６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。
【００４２】
収束判定部５６は、修正状態ベクトル算出部５５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差／速度差概略値算出部５３１〜５３Ｎにフィードバックしたり、収束計算を終了して修正状態ベクトルを測位結果として出力する。
なお、他の構成は、図１と同一又はこれに相当する構成であるので同一符号を付して説明を省略する。
【００４３】
次に動作について説明する。
上記実施の形態１と異なる動作について主に説明する。
修正状態ベクトル算出部５５は、修正量算出部５４１〜５４Ｎから入力した状態ベクトルの修正量と、距離差／速度差観測値検出部２１〜２Ｎから入力した距離差及びドップラ周波数差の観測精度を用い、該観測精度を重み付けした最小二乗法により目標の状態ベクトルの修正量とこれを状態ベクトルに反映させた修正状態ベクトルを出力する。例えば、下記式（１８）のように観測精度Ｓを重み付けした式に従って目標の状態ベクトルの修正量と修正状態ベクトルを算出する。
【数９】
【００４４】
収束判定部５６は、修正状態ベクトル算出部５５から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標状態ベクトルの初期値ｘ（ｔ）の代わりに距離差／速度差概略値算出部５３１〜５３Ｎの入力にフィードバックし、上記処理を繰り返す。修正量が閾値未満になれば、修正状態ベクトルを測位結果として出力する。
【００４５】
以上のように、この実施の形態２によれば、非同期測位手段５が、観測値と概略値（概略推定値）との誤差（差分）に対してセンサ１１〜１Ｍの観測精度を重み付けして目標の運動諸元の修正量を算出するので、観測値が非同期であっても目標の状態ベクトルを推定することができる。また、観測精度を考慮することにより、より信頼性の高い推定を実施することが可能である。
【００４６】
実施の形態３．
図４は、この発明の実施の形態３による運動諸元推定装置の構成を示す図である。図４に示すように、実施の形態３による運動諸元推定装置は、受信手段１、距離差／速度差観測値検出手段２、仮測位／初期値算出手段６及び非同期測位手段７を備える。なお、図４において、図１と同一又はこれに相当する構成要素に同一符号を付して説明を省略する。
【００４７】
仮測位／初期値算出手段６は、初期状態量設定部６１、距離差／速度差概略値算出部６２１，６２２、修正量算出部６３１，６３２、修正状態ベクトル算出部６４、及び収束判定部６５を備える。初期状態量設定部６１は、修正状態ベクトル算出部６４が目標の状態ベクトルの修正量を最小二乗法により算出する際に用いる初期値として目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。
【００４８】
距離差／速度差概略値算出部６２１，６２２は、初期状態量設定部６１によって設定された基準時刻における目標の状態ベクトルの初期値を用いて（若しくは、収束判定部６５からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。修正量算出部６３１，６３２は、距離差／速度差概略値算出部６２１，６２２により算出された概略値と距離差／速度差観測値検出部２１，２２から得られた観測値（センサ１１，１２とセンサ１２，１３による観測値）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。
【００４９】
修正状態ベクトル算出部６４は、修正量算出部６３１，６３２から修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。収束判定部６５では、修正状態ベクトル算出部６４から入力した修正量と所定の閾値との比較結果に応じて、距離差／速度差概略値算出部６２１，６２２に修正状態ベクトルをフィードバックしたり、非同期測位手段７の初期値として修正状態ベクトルを出力する。
【００５０】
非同期測位手段７は、時刻差算出部７１１〜７１Ｎ、距離差／速度差概略値算出部７２１〜７２Ｎ、修正量算出部７３１〜７３Ｎ、修正状態ベクトル算出部７４及び収束判定部７５を備え、仮測位／初期値算出手段６による測位結果を、目標の状態ベクトルの修正量を最小二乗法で算出する際に用いる初期値として用い、初期状態量設定部が省略されている以外は上記実施の形態１と同様に動作する。
【００５１】
図５は、時刻ｔ3においてセンサ１１及びセンサ１２、時刻ｔ4においてセンサ１２及びセンサ１３によって距離差及びドップラ周波数差が得られた場合を示す図である。ｘ−ｙの２次元直交座標系における目標の位置と速度を推定する場合、実施の形態３では、図５に示すような異なるセンサ組による検知結果の等時間差線（双曲線）の交点を測位し、交点における目標の位置及び速度を非同期測位手段７における目標の状態ベクトルの初期値として設定する。
【００５２】
例えば、図５中の破線で示す双曲線は、時刻ｔ3においてセンサ１１及びセンサ１２から目標へ放射されたレーダ電波（又は音波）が目標で反射してセンサ１１及びセンサ１２に到達する時刻差を用いて検出された２センサ間の目標との距離差及びドップラ周波数差による双曲線（目標とセンサ１１及びセンサ１２との距離差が一定な等距離差双曲線又は目標とセンサ１１及びセンサ１２とのドップラ周波数差が一定な等ドップラ周波数差双曲線）である。
【００５３】
また、図５中に実線で示す双曲線は、時刻ｔ4においてセンサ１２及びセンサ１３に反射波が到達する時刻差を用いて検出された２センサ間の目標との距離差及びドップラ周波数差による双曲線（センサ１２及びセンサ１３間の目標との距離差が一定な等距離差双曲線又はセンサ１２及びセンサ１３間の目標とのドップラ周波数差が一定な等ドップラ周波数差双曲線）である。
【００５４】
実施の形態３では、時刻ｔ3においてセンサ１１及びセンサ１２によって距離差及びドップラ周波数差を観測し、時刻ｔ4においてセンサ１２及びセンサ１３によって距離差及びドップラ周波数差を観測し、基準時刻として時刻ｔ4を設定して各センサ組の観測値から目標の状態ベクトルを算出することにより、これら双曲線の交点における目標の状態ベクトルを算出し、該状態ベクトルを非同期測位手段７による修正量の算出における初期値として使用する。
【００５５】
次に動作について説明する。
仮測位／初期値算出手段６では、初期状態量設定部６１により基準時刻及びこの基準時刻における目標の状態ベクトルの初期値が距離差／速度差概略値算出部６２１，６２２に設定される。距離差／速度差概略値算出部６２１，６２２では、初期状態量設定部６１から入力した目標の状態ベクトルの初期値から目標と各センサ組との距離差及び速度差の概略値を算出して修正量算出部６３１，６３２にそれぞれ出力する。
【００５６】
修正量算出部６３１，６３２では、距離差／速度差概略値算出部６２１，６２２からの概略値と距離差／速度差観測値検出部２１，２２からの観測値（２センサ間の目標との距離差及びドップラ周波数差より求めた速度差）を入力し、この概略値と観測値との誤差（距離差の誤差Δｈi,j、速度差の誤差Δｌi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部６４では、修正量算出部６３１，６３２からの修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【００５７】
収束判定部６５は、修正状態ベクトル算出部６４から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値の代わりに距離差／速度差概略値算出部６２１，６２２の入力へフィードバックし、上記処理を繰り返す。
【００５８】
修正量が閾値未満であると、収束判定部６５は、該修正量を反映させた目標の状態ベクトル（目標の位置、速度等）を修正状態ベクトル算出部６４から入力し、最新観測時刻（図５では時刻ｔ4）における目標の測位結果として非同期測位手段７へ出力する。具体的には、基準時刻として時刻ｔ4を非同期測位手段７の時間差算出部７１１〜７１Ｎへ出力し、目標の修正状態ベクトルを、非同期測位手段７において目標の状態ベクトルの修正量を最小二乗法で算出する際の初期値として距離差／速度差概略値算出部７２１〜７２Ｎへ出力する。
【００５９】
非同期測位手段７は、仮測位／初期値算出手段６から基準時刻及び目標の状態ベクトルの初期値が設定されると、上記実施の形態１と同様の処理により目標の測位結果を推定する。
【００６０】
以上のように、この実施の形態３によれば、仮測位／初期値算出手段６が異なる２つの時刻において異なるセンサ組により観測された観測値に基づいて算出した目標の測位結果を、非同期測位手段７による修正状態ベクトル算出処理の初期値として用いるので、異なる時刻の観測値をあたかも同時刻で観測された結果とみなすことができ、観測値が非同期であっても信頼性の高い目標の運動諸元の推定が可能である。
【００６１】
実施の形態４．
図６は、この発明の実施の形態４による運動諸元推定装置の構成を示す図である。図６に示すように、実施の形態４による運動諸元推定装置は、受信手段１、距離差／速度差観測値検出手段２、仮測位／初期値算出手段８及び非同期測位手段７を備える。なお、図６において、図１及び図４と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【００６２】
仮測位／初期値算出手段８は、初期状態量設定部８１、追尾フィルタ部８２１，８２２、距離差／速度差概略値算出部８３１，８３２、修正量算出部８４１，８４２、修正状態ベクトル算出部８５、及び収束判定部８６を備える。初期状態量設定部８１は、修正状態ベクトル算出部８５が目標の状態ベクトルの修正量を最小二乗法により算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。
【００６３】
追尾フィルタ部８２１，８２２は、受信手段１内のセンサの組み合わせ（図６の例では、センサ１１，１２とセンサ１２，１３）で得られる観測値（距離差及びドップラ周波数差から求めた速度差）を入力し、これより所定時刻におけるセンサ組のセンサ間の目標との距離差やドップラ周波数差から求めた速度差を推定する。距離差／速度差概略値算出部８３１，８３２は、初期状態量設定部８１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部８６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った目標の運動諸元の概略値を算出する。
【００６４】
修正量算出部８４１は、距離差／速度差概略値算出部８３１により算出された目標の運動諸元の概略値と追尾フィルタ部８２１により得られた観測値（センサ１１，１２間の目標との距離差及びドップラ周波数差により求まる速度差の推定値）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。修正量算出部８４２は、距離差／速度差概略値算出部８３２により算出された目標の運動諸元の概略値と追尾フィルタ部８２２により得られた観測値（センサ１２，１３間の目標との距離差及びドップラ周波数差により求まる速度差の推定値）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。
【００６５】
修正状態ベクトル算出部８５は、修正量算出部８４１，８４２からの修正量が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。収束判定部８６では、修正状態ベクトル算出部８５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差／速度差概略値算出部８３１，８３２にフィードバックしたり、収束計算を終了して修正状態ベクトルを非同期測位手段７による修正量算出の初期値として出力する。
【００６６】
図７は、時刻ｔ1，ｔ2においてセンサ１１とセンサ１２により距離差及びドップラ周波数差を観測し、時刻ｔ3においてセンサ１２とセンサ１３によって距離差及びドップラ周波数差が観測された場合を示す図である。ｘ−ｙの２次元直交座標系における目標の位置と速度を推定する場合、この実施の形態４では、いずれか一方のセンサ組み合わせで目標を追尾処理（目標との距離差、ドップラ周波数差による速度差を推定）し、他方のセンサ組み合わせにより観測値が得られた時刻に対する予測処理を実施することで時刻の同期を取る。
【００６７】
例えば、図７では、時刻ｔ3でセンサ１２とセンサ１３により距離差及び速度差に関する情報が得られている場合、センサ１１とセンサ１２の組み合わせにより得られる距離差及び速度差に関する情報について時刻ｔ3における予測処理を実施する。この結果、図７に示すように時刻ｔ3におけるセンサ１１，１２による観測値の双曲線とセンサ１２，１３による観測値の双曲線の交点が測位される。実施の形態４では、この交点での目標の状態ベクトルを、非同期測位手段７における修正量算出の初期値とする。
【００６８】
次に動作について説明する。
仮測位／初期値算出手段８において、初期状態量設定部８１が追尾フィルタ部８２１，８２２に基準時刻を設定し、基準時刻における目標の状態ベクトルの初期値を距離差／速度差概略値算出部８３１，８３２に設定する。
【００６９】
追尾フィルタ部８２１は、距離差／速度差観測値検出部２１から、センサ１１，１２の検知結果から得られる観測値（２センサ間の目標との距離差及びドップラ周波数差による速度差）及びその観測時刻を入力して、初期状態量設定部８１により設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差／速度差観測値検出部２１から入力した観測値とを用いてある時刻（図７の例では時刻ｔ1，ｔ2）における目標を追尾処理する（センサ１１，１２間の目標との距離差及び速度差を推定する）。
【００７０】
追尾フィルタ部８２２では、距離差／速度差観測値検出部２２から、センサ１２，１３の検知結果から得られる観測値及びその観測時刻（図７の例では時刻ｔ3）を入力して、初期状態量設定部８１に設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差／速度差観測値検出部２２から入力した観測値とを用いて時刻ｔ3における目標の追尾処理（２センサ間の目標との距離差及び速度差を推定）する。
【００７１】
このように、時刻ｔ3でセンサ１２，１３により目標との距離差及びドップラ周波数差による速度差が得られている場合、追尾フィルタ部８２１は、センサ１１，１２による目標との距離差及びドップラ周波数差による速度差について当該時刻ｔ3における予測処理を実行する。これにより、図７中の一点破線で示すセンサ１１，１２による観測値の等時間差線と図７中の実線で示すセンサ１２，１３による観測値の等時間差線との交点が測位される。つまり、センサ１２，１３により距離差及びドップラ周波数差が得られる、時刻ｔ3におけるセンサ１１，１２による観測の予測値を得ることで、同一時刻（時刻ｔ3）で目標との距離差及びドップラ周波数差より求まる速度差が得られたとみなして測位処理を実施する。但し、予測処理するセンサの組み合わせは１組以上とする。
【００７２】
距離差／速度差概略値算出部８３１，８３２は、初期状態量設定部８１から入力した目標の状態ベクトルの初期値を用いて各センサ組の２センサ間の目標との距離差及び速度差の概略値を算出して修正量算出部８４１，８４２にそれぞれ出力する。
【００７３】
修正量算出部８４１，８４２では、距離差／速度差概略値算出部８３１，８３２からの概略値と追尾フィルタ部８２１，８２２から入力した観測値（目標との距離差及びドップラ周波数差より求めた速度差の推定値）を入力し、概略値と観測値の誤差（距離差の誤差Δｈi,j、速度差の誤差Δｌi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部８５では、修正量算出部８４１，８４２からの修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【００７４】
収束判定部８６は、修正状態ベクトル算出部８５から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値の代わりに距離差／速度差概略値算出部８３１，８３２へフィードバックし、上記処理を繰り返す。
【００７５】
修正量が閾値未満であると、収束判定部８６は、該修正量を反映させた目標の状態ベクトル（目標の位置、速度等）を修正状態ベクトル算出部８５から入力し、図７に示す時刻ｔ3における目標の測位結果として非同期測位手段７へ出力する。具体的には、基準時刻として時刻ｔ3を非同期測位手段７の時間差算出部７１１〜７１Ｎへ出力し、目標の修正状態ベクトルを、非同期測位手段７において目標の状態ベクトルの修正量を最小二乗法で算出する際の初期値として距離差／速度差概略値算出部７２１〜７２Ｎへ出力する。
【００７６】
非同期測位手段７は、仮測位／初期値算出手段８から基準時刻及び目標の状態ベクトルの初期値が設定されると、上記実施の形態１と同様の処理により目標の測位結果を推定する。
【００７７】
以上のように、この実施の形態４によれば、仮測位／初期値算出手段８が、センサ１１，１２の検知結果から得られた目標との距離差及びドップラ周波数差より求まる速度差を追尾処理し、別の時刻において、異なるセンサ組（センサ１２，１３）の検知結果から目標との距離差及びドップラ周波数差より求まる速度差が得られる場合、上記追尾結果に基づいてそのセンサ組（センサ１２，１３）に対する時刻整合を行ってから、目標の位置、速度を算出し、最小二乗法による目標の状態ベクトル算出の初期値として設定するので、異なる時刻の観測値をあたかも同時刻で観測された結果とみなすことができ、観測値が非同期であっても信頼性の高い目標の運動諸元推定が可能である。
【００７８】
実施の形態５．
図８は、この発明の実施の形態５による運動諸元推定装置の構成を示す図である。図８に示すように、実施の形態５による運動諸元推定装置は、受信手段１、距離差／速度差観測値検出手段２及び非同期測位手段９を備える。なお、図８において、図１と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【００７９】
非同期測位手段９は、初期状態量設定部９１、追尾フィルタ部９２１〜９２Ｎ、距離差／速度差概略値算出部９３１〜９３Ｎ、修正量算出部９４１〜９４Ｎ、修正状態ベクトル算出部９５、及び収束判定部９６を備える。
【００８０】
初期状態量設定部９１は、修正状態ベクトル算出部９５により目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。追尾フィルタ部９２１〜９２Ｎは、受信手段１内のセンサの組み合わせで得られる検知結果に基づいて距離差／速度差観測値検出部２１〜２Ｎ距離差及びドップラ周波数差を入力し、追尾処理を実施する。
【００８１】
距離差／速度差概略値算出部９３１〜９３Ｎは、初期状態量設定部９１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部９６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。修正量算出部９４１〜９４Ｎは、距離差／速度差概略値算出部９３１〜９３Ｎにより算出された概略値と追尾フィルタ部９２１〜９２Ｎから得られた観測値との差分を用いて目標の状態ベクトルの修正量を算出する。
【００８２】
修正状態ベクトル算出部９５は、修正量算出部９４１〜９４Ｎから修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。収束判定部９６では、修正状態ベクトル算出部９５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差／速度差概略値算出部９３１〜９３Ｎにフィードバックしたり、収束計算を終了して修正状態ベクトルを測位結果として出力する。
【００８３】
次に動作について説明する。
初期状態量設定部９１は、追尾フィルタ部９２１〜９２Ｎに基準時刻を設定し、基準時刻における目標の状態ベクトルの初期値を距離差／速度差概略値算出部９３１〜９３Ｎに設定する。
【００８４】
追尾フィルタ部９２１〜９２Ｎは、距離差／速度差観測値検出部２１〜２Ｎから、センサ組の検知結果により得られる観測値（目標との距離差及びドップラ周波数差による速度差）及びその観測時刻をそれぞれ入力して、初期状態量設定部９１により設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差／速度差観測値検出部から入力した観測値とを用いて、目標を追尾処理する（各センサ組のセンサ間の目標との距離差及び速度差を推定する）。
【００８５】
追尾フィルタ部９２１〜９２Ｎのうち、ある追尾フィルタ部は、距離差／速度差観測値検出部から、ある時刻で他の追尾フィルタ部により異なるセンサ組で観測値が得られている場合、初期状態量設定部９１に設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差／速度差観測値検出部から入力した観測値とを用いて、当該時刻における目標との距離差及び速度差を予測処理（目標との距離差及び速度差を推定）する。
【００８６】
例えば、ある時刻で追尾フィルタ部９２２〜９２Ｎによってセンサ１２，１３〜１（Ｎ−１），１Ｎの検知結果で目標との距離差及び速度差が得られる場合、追尾フィルタ部９２１が該時刻における予測値を得ることで、同一時刻において目標との距離差及びドップラ周波数差により求まる速度差が得られたとみなして測位処理を実施する。但し、予測処理するセンサの組み合わせは１組以上とする。
【００８７】
距離差／速度差概略値算出部９３１〜９３Ｎは、初期状態量設定部９１から入力した目標の状態ベクトルの初期値を用いて目標と各センサ組との距離差及び速度差の概略値を算出して修正量算出部９４１〜９４Ｎにそれぞれ出力する。
【００８８】
修正量算出部９４１〜９４Ｎでは、距離差／速度差概略値算出部９３１〜９３Ｎからの概略値と追尾フィルタ部９２１〜９２Ｎから入力した観測値（目標との距離差及び速度差の推定値）を入力し、概略値と観測値の誤差（距離差の誤差Δｈi,j、速度差の誤差Δｌi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部９５では、修正量算出部９４１〜９４Ｎからの修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【００８９】
収束判定部９６は、修正状態ベクトル算出部９５から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値の代わりに距離差／速度差概略値算出部９３１〜９３Ｎの入力へフィードバックし、上記処理を繰り返す。修正量が閾値未満であると、収束判定部９６は、該修正量を反映させた目標の状態ベクトル（目標の位置、速度等）を修正状態ベクトル算出部９５から入力し、目標の測位結果として出力する。
【００９０】
以上のように、この実施の形態５によれば、非同期測位手段９が、センサ組の検知結果から得られた目標との距離差及びドップラ周波数差より求まる速度差を追尾処理し、別の時刻において、異なるセンサ組の検知結果から目標との距離差及び速度差が得られる場合、上記追尾結果に基づいてそのセンサ組に対する時刻整合を行ってから、目標の運動諸元を推定するので、異なる時刻の観測値をあたかも同時刻で観測された結果とみなすことができ、観測値が非同期であっても信頼性の高い目標の運動諸元推定が可能である。
【００９１】
実施の形態６．
図９は、この発明の実施の形態６による運動諸元推定装置の構成を示す図である。図９に示すように、実施の形態６による運動諸元推定装置は、受信手段１、距離差／速度差観測値検出手段２及び非同期測位手段１０を備える。なお、図９において、図１と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【００９２】
非同期測位手段１０は、初期状態量設定部１０１、時刻差算出部１０２１〜１０２Ｎ、距離差／速度差概略値算出部１０３１〜１０３Ｎ、修正量算出部１０４１〜１０４Ｎ、修正状態ベクトル算出部１０５、収束判定部１０６、及び収束演算カウント部１０７を備える。収束判定部１０６及び収束演算カウント部１０７以外の構成は、上記実施の形態１の図１で示した初期状態量設定部４１、時刻差算出部４２１〜４２Ｎ、距離差／速度差概略値算出部４３１〜４３Ｎ、修正量算出部４４１〜４４Ｎ、及び修正状態ベクトル算出部４５と同様に動作する。
【００９３】
収束判定部１０６は、収束演算カウント部１０７が計算回数限界値をカウントするまでの間、修正状態ベクトル算出部１０５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差／速度差概略値算出部１０３１〜１０３Ｎにフィードバックしたり、収束計算を終了して修正状態ベクトルを測位結果として出力する。
【００９４】
収束演算カウント部１０７は、収束判定部１０６により修正状態ベクトルが距離差／速度差概略値算出部１０３１〜１０３Ｎにフィードバックされる度に収束演算回数を、所定の計算回数限界値になるまでカウントする。ここで、計算回数限界値とは、実施の形態６による運動諸元推定装置を実現する計算機の演算性能や検知データ等のサンプリングレートに応じて決定した、最適な演算負荷と推定精度を与える収束演算回数である。
【００９５】
次に動作について説明する。
以降では、実施の形態６による運動諸元推定装置に特有な動作について主に説明する。
先ず、収束判定部１０６が収束演算を行うにあたり、収束演算カウント部１０７は、カウント値を０に初期化し、カウントの限界値として計算回数限界値を設定する。上記実施の形態１と同様にして、修正状態ベクトル算出部１０５から修正量が入力されると、収束判定部１０６は、所定の閾値と該修正量を比較する。このとき、修正量が閾値より大きく収束しなかった場合、収束演算カウント部１０７は、カウント値を１増やす。
【００９６】
計算回数限界値にカウント値が達しても修正量が収束しなかった場合、収束演算カウント部１０７は、計算終了の信号を収束判定部１０６に送信する。これにより、収束判定部１０６は、該計算終了時の修正量を反映させた修正状態ベクトル（修正量を加えた修正状態ベクトル）を目標の測位結果として出力する。なお、カウント値が計算回数限界値に達する前に修正量が収束した場合は、該収束した修正量を加えた修正状態ベクトルを目標の測位結果として出力し、収束演算カウント部１０７はカウントを停止する。
【００９７】
以上のように、この実施の形態６によれば、収束判定部１０６が、収束演算カウント部１０７によるカウントが予め定めた計算回数限界値まで達すると、収束演算を終了してその時点での運動諸元推定値を出力するので、修正状態ベクトル算出部１０５による収束計算の回数が限定されることから、本実施の形態６による運動諸元推定装置の性能を超える演算の実行が抑制され、かつ所定の時間で演算が終了することから収束計算の時間を短縮することができる。
【００９８】
なお、上記実施の形態６では、収束演算カウント部１０７を上記実施の形態１による構成に適用した場合を示したが、上記実施の形態２〜５の構成に適用してもよく、同様の効果を得ることができる。
【００９９】
実施の形態７．
図１０は、この発明の実施の形態７による運動諸元推定装置の構成を示す図である。図１０に示すように、実施の形態７による運動諸元推定装置は、受信手段１、距離差観測値検出手段２ａ及び非同期測位手段１２ａを備える。なお、図１０において、図１と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【０１００】
距離差観測値検出手段２ａは、距離差観測値検出部１１１〜１１Ｎ（Ｎは、Ｍ−１の整数）を備える。距離差観測値検出部１１１〜１１Ｎは、受信手段１を構成するセンサ１１〜１Ｍから２つのセンサごとに２センサ間の目標との距離差を検出し、該２センサ間の距離差とその観測時刻を出力する。
【０１０１】
実施の形態７によるセンサ１１〜１Ｍとしては、例えば目標へ放射したレーダ電波の反射電波が到達する時刻差から目標との距離差を算出するレーダセンサ等が考えられる。なお、上記式（３）に示すようにドップラ周波数差を算出する際にも目標の位置の初期値が必要であるが、目標の位置は、上記式（２）に示すように目標との距離差の観測値が必要である。そこで、本実施の形態７では、ドップラ周波数差を算出せず、任意の２センサ間の目標との距離差のみを用いて目標の状態ベクトルを推定することで演算処理の簡略化を図っている。
【０１０２】
非同期測位手段１２ａは、距離差観測値検出手段２ａから出力される、受信手段１における任意の２センサ間の目標との距離差を入力し、非同期を考慮して目標の状態ベクトルを推定する。また、非同期測位手段１２ａは、図１０に示すように初期状態量設定部１２１、時刻差算出部１２２１〜１２２Ｎ、距離差概略値算出部１２３１〜１２３Ｎ、修正量算出部１２４１〜１２４Ｎ、修正状態ベクトル算出部１２５、及び収束判定部１２６を備える。
【０１０３】
初期状態量設定部１２１は、修正状態ベクトル算出部１２５により目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。時刻差算出部１２２１〜１２２Ｎは、初期状態量設定部１２１により設定された基準時刻と観測時刻との時刻差を算出する。
【０１０４】
距離差概略値算出部１２３１〜１２３Ｎは、時刻差算出部１２２１〜１２２Ｎにより算出された時刻差及び初期状態量設定部１２１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部１２６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。
【０１０５】
修正量算出部１２４１〜１２４Ｎは、距離差概略値算出部１２３１〜１２３Ｎにより算出された概略値と距離差観測値検出手段２ａから得られた観測値との差分を算出し、この差分を用いて算出した修正量を出力する。修正状態ベクトル算出部１２５は、修正量算出部１２４１〜１２４Ｎから修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。
【０１０６】
収束判定部１２６では、修正状態ベクトル算出部１２５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差概略値算出部１２３１〜１２３Ｎにフィードバックしたり、収束計算を終了して修正状態ベクトルを測位結果として出力する。
【０１０７】
次に動作について説明する。
センサ１１〜１Ｍのうちの任意の２センサ間の目標との距離差の観測値は、受信手段１から距離差観測値検出手段２ａに出力される。距離差観測値検出手段２ａでは、受信手段１における任意の２センサによる観測値を距離差観測値検出部１１１〜１１Ｎが入力する。図１０の例では、受信手段１のセンサ１１〜１Ｍのうち、センサ１１及びセンサ１２のセンサ出力が距離差観測値検出部１１１に入力され、センサ１２及びセンサ１３のセンサ出力が距離差観測値検出部１１２に入力され、同様に２センサの出力が距離差観測値検出部に逐次入力される。
【０１０８】
距離差観測値検出部１１１〜１１Ｎでは、センサ１１〜１Ｍのうちの２センサからそれぞれ入力した検知結果に基づいて、２センサ間の目標との距離差を検出する。この後、距離差観測値検出部１１１〜１１Ｎは、２センサ間の目標との距離差を、非同期測位手段１２ａ内の修正量算出部１２３１〜１２３Ｎにそれぞれ出力し、各センサの観測時刻も非同期測位手段１２ａ内の時刻差算出部１２２１〜１２２Ｎにそれぞれ出力する。
【０１０９】
非同期測位手段１２ａ内の初期状態量設定部１２１は、基準時刻ｔを時刻差算出部１２２１〜１２２Ｎにそれぞれ出力し、目標の状態ベクトルの初期値ｘ（ｔ）を距離差概略値算出部１２３１〜１２３Ｎにそれぞれ出力する。時刻差算出部１２２１〜１２２Ｎは、基準時刻ｔと観測時刻とを用い、上記式（１７）により基準時刻ｔと観測時刻ｔkの時刻差Δｔkを算出し、距離差概略値算出部１２３１〜１２３Ｎにそれぞれ出力する。
【０１１０】
距離差概略値算出部１２３１〜１２３Ｎでは、初期状態量設定部１２１から入力した目標の状態ベクトルｘ（ｔ）の初期値と時刻差算出部１２２１〜１２２Ｎから入力した時刻差を用い、上記式（１２）に従ってそれぞれ異なる時刻における目標の状態ベクトルｘ（ｔ）（目標の位置、速度等）とセンサｉ，ｊとの距離差の概略値ｇi,jを算出して修正量算出部１２４１〜１２４Ｎにそれぞれ出力する（上記式（１６）のｇi,jに相当するもの）。
【０１１１】
修正量算出部１２４１〜１２４Ｎは、距離差概略値算出部１２３１〜１２３Ｎからの概略値ｇi,jと、距離差観測値検出部１１１〜１１Ｎからの観測値ｚi,j（距離差の観測値）とを入力し、概略値ｇi,jと観測値ｚi,jとの誤差（距離差の誤差Δｈi,j）を算出し、この誤差を用いて算出した修正量を出力する。修正状態ベクトル算出部１２５では、修正量算出部１２４１〜１２４Ｎから修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトル（該修正量を加えた状態ベクトル）を出力する。
【０１１２】
収束判定部１２６では、修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値ｘ（ｔ）の代わりに距離差概略値算出部１２３１〜１２３Ｎの入力へフィードバックし、上記処理を繰り返す。修正量が閾値未満になれば、修正状態ベクトルを測位結果として出力する。
【０１１３】
以上のように、この実施の形態７によれば、センサ１１〜１Ｍから任意の２センサ間の目標との距離差の観測値のみを入力し、該距離差に基づいて目標の状態ベクトルを推定するので、ドップラ周波数差が得られない場合であっても目標の運動諸元を推定することができる。これにより、ドップラ周波数差を得るための演算処理も省略することができ、簡易な構成で上記実施の形態１と同様の効果を得ることができる。
【０１１４】
実施の形態８．
図１１は、この発明の実施の形態８による運動諸元推定装置の構成を示す図である。図１１に示すように、実施の形態８による運動諸元推定装置の基本的な構成は、上記実施の形態７の図１０に示したものと同様であるが、非同期測位手段１３ａが距離差観測値検出部１１１〜１１Ｎから観測精度を入力する点で異なる。
【０１１５】
非同期測位手段１３ａ内の修正状態ベクトル算出部１３５は、距離差観測値検出部１１１〜１１Ｎからの観測精度を考慮して修正量算出部１３４１〜１３４Ｎからの修正量に対して最小二乗法を用いて目標の運動諸元を推定する。つまり、修正状態ベクトル算出部１３５が、状態ベクトルの修正量を算出するにあたり、上記式（１１）について距離差の観測精度を重み付けの項として加える。
【０１１６】
初期状態量設定部１３１は、修正状態ベクトル算出部１３５により目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。時刻差算出部１３２１〜１３２Ｎは、初期状態量設定部１３１により設定された基準時刻と観測時刻との時刻差を算出する。
【０１１７】
距離差概略値算出部１３３１〜１３３Ｎは、時刻差算出部１３２１〜１３２Ｎにより算出された時刻差及び初期状態量設定部１３１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部１３６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。
【０１１８】
収束判定部１３６では、修正状態ベクトル算出部１３５による修正量が閾値より大きい場合、修正状態ベクトルを距離差概略値算出部１３３１〜１３３Ｎに出力し、修正量が閾値より小さい場合、収束計算を終了して修正状態ベクトルを測位結果として出力する。なお、他の構成は、図１０と同一又はこれに相当する構成であるので同一符号を付して説明を省略する。
【０１１９】
次に動作について説明する。
上記実施の形態７と異なる動作について主に説明する。
修正状態ベクトル算出部１３５は、修正量算出部１３４１〜１３４Ｎから入力した状態ベクトルの修正量と、距離差観測値検出部１３３１〜１３３Ｎから入力した距離差の観測精度を用い、該距離差の観測精度を重み付けした最小二乗法により目標の状態ベクトルの修正量とこれを状態ベクトルに反映させた修正状態ベクトルを出力する。例えば、上記式（１８）のように観測精度Ｓを重み付けした式に従って目標の状態ベクトルの修正量と修正状態ベクトルを算出する。
【０１２０】
収束判定部１３６は、修正状態ベクトル算出部１３５から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標状態ベクトルの初期値ｘ（ｔ）の代わりに距離差概略値算出部１３３１〜１３３Ｎの入力にフィードバックし、上記処理を繰り返す。修正量が閾値未満になれば、修正状態ベクトルを測位結果として出力する。
【０１２１】
以上のように、この実施の形態８によれば、観測値が非同期であっても目標の状態ベクトルを推定することができる。また、観測情報として任意の２センサ間の目標との距離差のみでよいことから、上記実施の形態７と同様にドップラ周波数差が得られないセンサであっても目標の運動諸元を推定することができる。これにより、ドップラ周波数差を得るための演算処理も省略することができ、簡易な構成で上記実施の形態１と同様の効果を得ることができる。また、距離差の観測精度を考慮することにより、より信頼性の高い推定を実施することが可能である。
【０１２２】
実施の形態９．
図１２は、この発明の実施の形態９による運動諸元推定装置の構成を示す図である。図１２に示すように、実施の形態９による運動諸元推定装置は、受信手段１、距離差観測値検出手段２ａ、仮測位／初期値算出手段１４ａ及び非同期測位手段１５ａを備える。なお、図１２において、図１０と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【０１２３】
仮測位／初期値算出手段１４ａは、初期状態量設定部１４１、距離差概略値算出部１４２１，１４２２、修正量算出部１４３１，１４３２、修正状態ベクトル算出部１４４、収束判定部１４５、メモリ１４６及び初期値算出部１４７を備える。初期状態量設定部１４１は、修正状態ベクトル算出部１４４が目標の状態ベクトルの修正量を最小二乗法により算出する際に用いる初期値として目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。
【０１２４】
距離差概略値算出部１４２１，１４２２は、初期状態量設定部１４１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部１４５からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。修正量算出部１４３１，１４３２は、距離差概略値算出部１４２１，１４２２により算出された概略値と距離差観測値検出部１１１，１１２から得られた観測値（センサ１１，１２とセンサ１２，１３による観測値）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。
【０１２５】
修正状態ベクトル算出部１４４は、修正量算出部１４３１，１４３２から修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。収束判定部１４５では、修正状態ベクトル算出部１４４による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差概略値算出部１４２１，１４２２にフィードバックしたり、収束計算を終了して修正状態ベクトルをメモリ１４６に出力する。
【０１２６】
メモリ１４６は、収束判定部１４５で収束したものと判定された目標の測位結果（目標の状態ベクトル）を記憶する。初期値算出部１４７は、下記式（１９）及び下記式（２０）で表される異なる２つの時刻ｔ1，ｔ2における測位結果である目標の位置ｘ（ｔ1）ハット，ｘ（ｔ2）ハット（出願書類の処理の関係上、読みで表記する）から下記式（２１）に従って初期速度ｘドット（出願書類の処理の関係上、読みで表記する）を得る。この最新時刻における目標の測位結果を非同期測位手段１５ａへの初期値として出力する。
【数１０】
【０１２７】
非同期測位手段１５ａは、時刻差算出部１５１１〜１５１Ｎ、距離差概略値算出部１５２１〜１５２Ｎ、修正量算出部１５３１〜１５３Ｎ、修正状態ベクトル算出部１５４、及び収束判定部１５５を備え、仮測位／初期値算出手段１４ａによる測位結果を、目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として用い、初期状態量設定部が省略されている以外は上記実施の形態８と同様に動作する。
【０１２８】
図１３は、時刻ｔ1においてセンサ１１及びセンサ１２、時刻ｔ2においてセンサ１２及びセンサ１３、時刻ｔ3においてセンサ１１及びセンサ１２、時刻ｔ4においてセンサ１２及びセンサ１３によって距離差が得られた場合を示す図である。ｘ−ｙの２次元直交座標系における目標の位置と速度を推定する場合、実施の形態９では、図１３に示すような異なるセンサ組による検知結果の等時間差線（双曲線）の交点を測位し、交点における目標の位置及び速度を非同期測位手段１５ａにおける目標の状態ベクトルの初期値として設定する。
【０１２９】
例えば、図１３中の破線で示す双曲線は、時刻ｔ1，ｔ3において目標へ放射されたレーダ電波が目標で反射してセンサ１１及びセンサ１２に到達する時刻差を用いて検出された２センサ間の目標との距離差による双曲線（目標とのセンサ１１及びセンサ１２間の距離差が一定な等距離差双曲線）である。また、図１３中に実線で示す双曲線は、時刻ｔ2，ｔ4においてセンサ１２及びセンサ１３に反射電波が到達する時刻差を用いて検出された２センサ間の目標との距離差による双曲線（目標とのセンサ１２及びセンサ１３間の距離差が一定な等距離差双曲線）である。
【０１３０】
実施の形態９では、時刻ｔ1，ｔ3においてセンサ１１及びセンサ１２によって距離差を観測し、時刻ｔ2，ｔ4においてセンサ１２及びセンサ１３によって距離差を観測し、基準時刻として時刻ｔ2，ｔ4を設定して各センサ組の観測値から目標の状態ベクトルを算出することにより、これら双曲線の交点における目標の状態ベクトルを算出し、該状態ベクトルを非同期測位手段１５ａによる修正量の算出における初期値として使用する。
【０１３１】
次に動作について説明する。
仮測位／初期値算出手段１４ａでは、初期状態量設定部１４１により基準時刻及びこの基準時刻における目標の状態ベクトルの初期値が距離差概略値算出部１４２１，１４２２に設定される。距離差概略値算出部１４２１，１４２２では、初期状態量設定部１４１から入力した目標の状態ベクトルの初期値から各センサ組の２センサ間の目標との距離差の概略値を算出して修正量算出部１４３１，１４３２にそれぞれ出力する。
【０１３２】
修正量算出部１４３１，１４３２では、距離差概略値算出部１４２１，１４２２からの概略値と距離差観測値検出部１１１，１１２からの観測値（２センサ間の目標との距離差）を入力し、この概略値と観測値との誤差（距離差の誤差Δｈi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部１４４では、修正量算出部１４３１，１４３２から修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【０１３３】
収束判定部１４５は、修正状態ベクトル算出部１４４から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値の代わりに距離差概略値算出部１４２１，１４２２の入力へフィードバックし、上記処理を繰り返す。
【０１３４】
修正量が閾値未満であると、収束判定部１４５は、該修正量を反映させた目標の状態ベクトル（目標の位置、速度等）を修正状態ベクトル算出部１４４から入力し、最新観測時刻（図１３では、時間的に近い時刻ｔ1，ｔ2のときは時刻ｔ2であり、時刻ｔ3，ｔ4のときは時刻ｔ4）における目標の測位結果としてメモリ１４６に逐次出力して値を保持させる。
【０１３５】
初期値算出部１４７は、上記式（１９），（２０）により２つの時刻ｔ2，ｔ4における測位結果である目標の位置ｘ（ｔ2）ハット，ｘ（ｔ4）ハットを定義し、これらを上記式（２１）に代入することで目標の初期速度ｘドットを得る。この最新時刻における目標の測位結果を非同期測位手段１５ａへの初期値として出力する。具体的には、基準時刻として時刻ｔ4を非同期測位手段１５ａの時間差算出部１５１１〜１５１Ｎへ出力し、目標の修正状態ベクトルを、非同期測位手段１５ａにおける目標の状態ベクトルの修正量を最小二乗法により算出する際の初期値として距離差概略値算出部１５２１〜１５２Ｎへ出力する。
【０１３６】
非同期測位手段１５ａは、仮測位／初期値算出手段１４ａから基準時刻及び目標の状態ベクトルの初期値が設定されると、上記実施の形態７と同様の処理により目標の測位結果を推定する。
【０１３７】
以上のように、この実施の形態９によれば、仮測位／初期値算出手段１４ａが異なる２つの時刻において異なるセンサ組により観測された該２センサ間の目標との距離差に基づいて算出した目標の測位結果を、非同期測位手段１５ａによる修正状態ベクトル算出処理の初期値として用いるので、異なる時刻の観測値をあたかも同時刻で観測された結果とみなすことができ、観測値が非同期であっても信頼性の高い目標の運動諸元の推定が可能である。また、観測情報として任意の２センサ間の目標との距離差のみでよいことから、上記実施の形態７と同様にドップラ周波数差が得られないセンサであっても目標の運動諸元を推定することができる。
【０１３８】
実施の形態１０．
図１４は、この発明の実施の形態１０による運動諸元推定装置の構成を示す図である。図１４に示すように、実施の形態１０による運動諸元推定装置は、受信手段１、距離差観測値検出手段２ａ、仮測位／初期値算出手段１６ａ及び非同期測位手段１５ａを備える。なお、図１４において、図１、図６及び図１０と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【０１３９】
仮測位／初期値算出手段１６ａは、初期状態量設定部１６１、追尾フィルタ部１６２１，１６２２、距離差概略値算出部１６３１，１６３２、修正量算出部１６４１，１６４２、修正状態ベクトル算出部１６５、収束判定部１６６、メモリ１６７、及び初期値算出部１６８を備える。初期状態量設定部１６１は、修正状態ベクトル算出部１６５が目標の状態ベクトルの修正量を最小二乗法により算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。
【０１４０】
追尾フィルタ部１６２１，１６２２は、受信手段１内のセンサの組み合わせ（図１４の例では、センサ１１，１２とセンサ１２，１３）で得られる観測値（距離差）を入力し、これより所定時刻におけるセンサ組の２センサ間の目標との距離差を推定する。距離差概略値算出部１６３１，１６３２は、初期状態量設定部１６１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部１６６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った目標の運動諸元の概略値を算出する。
【０１４１】
修正量算出部１６４１は、距離差概略値算出部１６３１により算出された目標の運動諸元の概略値と追尾フィルタ部１６２１により得られた推定値（センサ１１，１２間の目標との距離差）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。修正量算出部１６４２は、距離差概略値算出部１６３２により算出された目標の運動諸元の概略値と追尾フィルタ部１６２２によって得られた推定値（センサ１２，１３間の目標との距離差）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。
【０１４２】
修正状態ベクトル算出部１６５は、修正量算出部１６４１，１６４２から修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。収束判定部１６６では、修正状態ベクトル算出部１６５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差概略値算出部１６３１，１６３２にフィードバックしたり、収束計算を終了して修正状態ベクトルをメモリ１６７に出力する。
【０１４３】
メモリ１６７は、収束判定部１６６で収束したものと判定された目標の測位結果（目標の状態ベクトル）を記憶する。初期値算出部１６８は、上記式（１９）及び上記式（２０）で表される異なる２つの時刻における測位結果である目標の位置から上記式（２１）に従って目標の初期速度を得る。この最新時刻における目標の測位結果を非同期測位手段１５ａへの初期値として出力する。
【０１４４】
図１５は、時刻ｔ1，ｔ2においてセンサ１１とセンサ１２により距離差を観測し、時刻ｔ3，ｔ4においてセンサ１２とセンサ１３によって距離差が観測された場合を示す図である。ｘ−ｙの２次元直交座標系における目標の位置と速度を推定する場合、実施の形態１０では、いずれか一方のセンサ組み合わせで得られる目標を追尾処理（目標との距離差を推定）し、他方のセンサ組み合わせにより距離差が得られる時刻に対する予測処理を実施することで時刻の同期を取る。
【０１４５】
例えば、図１５では、時刻ｔ3でセンサ１２とセンサ１３により距離差が得られた場合、センサ１１とセンサ１２の組み合わせによる距離差を時刻ｔ3，ｔ4において予測処理を実施する。この結果、図１５に示すように時刻ｔ3におけるセンサ１１，１２による観測値の双曲線（実線）とセンサ１２，１３による観測値の双曲線（一点破線）の交点が測位される。また、時刻ｔ4におけるセンサ１１，１２による観測値の双曲線（実線）とセンサ１２，１３による観測値の双曲線（一点破線）の交点が測位される。実施の形態１０では、これらの交点での目標の状態ベクトルを、非同期測位手段１５ａにおける修正量算出の初期値とする。
【０１４６】
次に動作について説明する。
仮測位／初期値算出手段１６ａにおいて、初期状態量設定部１６１が追尾フィルタ部１６２１，１６２２に基準時刻を設定し、基準時刻における目標の状態ベクトルの初期値を距離差概略値算出部１６３１，１６３２に設定する。
【０１４７】
追尾フィルタ部１６２１は、距離差観測値検出部１１１から、センサ１１，１２の検知結果によって得られる観測値（目標との距離差）及びその観測時刻を入力して、初期状態量設定部１６１により設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差観測値検出部１１１から入力した観測値を用いて、ある時刻（図１５では時刻ｔ1，ｔ2）における目標を追尾処理する（センサ１１，１２間の目標との距離差を推定する）。
【０１４８】
追尾フィルタ部１６２２では、距離差観測値検出部１１２から、センサ１２，１３の検知結果から得られる観測値及びその観測時刻を入力して、初期状態量設定部１６１に設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差観測値検出部１１２から入力した観測値とを用いて、目標の追尾処理（２センサ間の目標との距離差を推定）する。
【０１４９】
このように、時刻ｔ3でセンサ１２，１３により目標との距離差が得られている場合、追尾フィルタ部１６２１は、センサ１１，１２による目標との距離差について時刻ｔ3における予測処理を実行する。これにより、図１５中の一点破線で示すセンサ１１，１２による観測値の等時間差線と図１５中の実線で示すセンサ１２，１３による観測値の等時間差線との交点が測位される。つまり、センサ１２，１３により距離差が得られる時刻ｔ3，ｔ4におけるセンサ１１，１２による観測の予測値を得ることで、同一時刻（時刻ｔ3，ｔ4）で目標との距離差が得られたとみなして測位処理を実施する。但し、予測処理するセンサの組み合わせは１組以上とする。
【０１５０】
距離差概略値算出部１６３１，１６３２は、初期状態量設定部１６１から入力した目標の状態ベクトルの初期値を用いて目標との各センサ組の２センサ間の距離差の概略値を算出して修正量算出部１６４１，１６４２にそれぞれ出力する。
【０１５１】
修正量算出部１６４１，１６４２では、距離差概略値算出部１６３１，１６３２からの概略値と追尾フィルタ部１６２１，１６２２から入力した推定値（２センサ間の目標との距離差）を入力し、概略値と観測値の誤差（距離差の誤差Δｈi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部１６５では、修正量算出部１６４１，１６４２から修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【０１５２】
収束判定部１６６は、修正状態ベクトル算出部１６５から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値の代わりに距離差概略値算出部１６３１，１６３２の入力へフィードバックして上記処理を繰り返す。
【０１５３】
修正量が閾値未満であると、収束判定部１６６は、該修正量を反映させた目標の状態ベクトル（目標の位置、速度等）を修正状態ベクトル算出部１６５から入力し、図１５に示す時刻ｔ3，ｔ4における目標の測位結果としてメモリ１６７に逐次出力して値を保持させる。
【０１５４】
初期値算出部１６８は、上記式（１９），（２０）により２つの時刻ｔ3，ｔ4における測位結果である目標の位置ｘ（ｔ3）ハット，ｘ（ｔ4）ハットを定義し、これらを上記式（２１）に代入することで目標の初期速度ｘドットを得る。この最新時刻における目標の測位結果を非同期測位手段１５ａへの初期値として出力する。具体的には、基準時刻として時刻ｔ4を非同期測位手段１５ａの時間差算出部１５１１〜１５１Ｎへ出力し、目標の修正状態ベクトルを、非同期測位手段１５ａにおける目標の状態ベクトルの修正量を最小二乗法により算出する際の初期値として距離差概略値算出部１５２１〜１５２Ｎへ出力する。
【０１５５】
非同期測位手段１５ａは、仮測位／初期値算出手段１６ａから基準時刻及び目標の状態ベクトルの初期値が設定されると、上記実施の形態７と同様の処理により目標の測位結果を推定する。
【０１５６】
以上のように、この実施の形態１０によれば、仮測位／初期値算出手段１６ａが、センサ１１，１２の検知結果から得られた目標との距離差を追尾処理し、別の時刻において、異なるセンサ組（センサ１２，１３）の検知結果から目標との距離差が得られる場合、上記追尾結果に基づいてそのセンサ組（センサ１２，１３）に対する時刻整合を行ってから、目標の位置、速度を算出し、最小二乗法による目標の状態ベクトル算出の初期値として設定するので、異なる時刻の観測値をあたかも同時刻で観測された結果とみなすことができ、観測値が非同期であっても信頼性の高い目標の運動諸元推定が可能である。また、観測情報として任意の２センサ間の目標との距離差のみでよいことから、上記実施の形態７と同様にドップラ周波数差が得られないセンサであっても目標の運動諸元を推定することができる。
【０１５７】
実施の形態１１．
図１６は、この発明の実施の形態１１による運動諸元推定装置の構成を示す図である。図１６に示すように、実施の形態１１による運動諸元推定装置は、受信手段１、距離差観測値検出手段２ａ及び非同期測位手段１７ａを備える。なお、図１６において、図１、図８及び図１０と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【０１５８】
非同期測位手段１７ａは、初期状態量設定部１７１、追尾フィルタ部１７２１〜１７２Ｎ、距離差概略値算出部１７３１〜１７３Ｎ、修正量算出部１７４１〜１７４Ｎ、修正状態ベクトル算出部１７５、及び収束判定部１７６を備える。
【０１５９】
初期状態量設定部１７１は、修正状態ベクトル算出部１７５により目標の状態ベクトル修正量を最小二乗法で算出する際に用いる初期値として、目標の状態ベクトルの適当な初期値及び目標の運動諸元を推定すべき基準時刻を設定する。追尾フィルタ部１７２１〜１７２Ｎは、受信手段１内のセンサの組み合わせで得られる距離差を入力し、追尾処理を実施する。
【０１６０】
距離差概略値算出部１７３１〜１７３Ｎは、初期状態量設定部１７１により設定された基準時刻における目標の初期値を用いて（若しくは、収束判定部１７６からの修正量を初期値として用いて）、予め仮定した運動方程式に沿った概略値を算出する。
【０１６１】
修正量算出部１７４１は、距離差概略値算出部１７３１により算出された概略値と追尾フィルタ部１７２から得られた観測値（センサ１１，１２による観測値）との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。修正量算出部１７４２〜１７４Ｎは、距離差概略値算出部１７３２〜１７３Ｎにより算出された概略値と距離差観測値検出部１１２〜１１Ｎから得られた観測値との差分を算出し、この差分を用いて算出した目標の状態ベクトルの修正量を出力する。
【０１６２】
修正状態ベクトル算出部１７５は、修正量算出部１７４１〜１７４Ｎから修正量を入力し、上記差分が収束するまで最小二乗法を用いて目標の状態ベクトルを修正する。収束判定部１７６では、修正状態ベクトル算出部１７５による修正量が閾値より大きい場合、修正状態ベクトルを距離差概略値算出部１７３１〜１７３Ｎに出力し、修正量が閾値より小さい場合、収束計算を終了して修正状態ベクトルを測位結果として出力する。
【０１６３】
次に動作について説明する。
初期状態量設定部１７１は、追尾フィルタ部１７２１〜１７２Ｎに基準時刻を設定し、基準時刻における目標の状態ベクトルの初期値を距離差概略値算出部１７３１〜１７３Ｎに設定する。
【０１６４】
追尾フィルタ部１７２１〜１７２Ｎは、距離差観測値検出部１１１〜１１Ｎから、センサ組の検知結果により得られる観測値（２センサ間の目標との距離差）及びその観測時刻をそれぞれ入力して、初期状態量設定部１７１により設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差観測値検出部から入力した観測値とを用いて、目標を追尾処理する（各センサ組の２センサ間の目標との距離差を推定する）。
【０１６５】
追尾フィルタ部１７２１〜１７２Ｎのうち、ある追尾フィルタ部は、距離差観測値検出部から、ある時刻で他の追尾フィルタ部により異なるセンサ組で観測値が得られている場合、初期状態量設定部１７１に設定された基準時刻と観測時刻との時刻差を算出し、この時刻差と距離差観測値検出部から入力した観測値とを用いて、当該時刻における目標との距離差を予測処理（２センサ間の目標との距離差を推定）する。
【０１６６】
例えば、ある時刻で追尾フィルタ部１７２２〜１７２Ｎによってセンサ１２，１３〜１（Ｎ−１），１Ｎの検知結果で目標との距離差が得られている場合、追尾フィルタ部１７２１が該時刻における予測値を得ることで、同一時刻において目標との距離差が得られたとみなして測位処理を実施する。但し、予測処理するセンサの組み合わせは１組以上とする。
【０１６７】
距離差概略値算出部１７３１〜１７３Ｎは、初期状態量設定部１７１から入力した目標の状態ベクトルの初期値を用いて目標と各センサ組との距離差の概略値を算出して修正量算出部１７４１〜１７４Ｎにそれぞれ出力する。
【０１６８】
修正量算出部１７４１〜１７４Ｎでは、距離差概略値算出部１７３１〜１７３Ｎからの概略値と追尾フィルタ部１７２１〜１７２Ｎから入力した推定値（目標との距離差）を入力し、概略値と観測値の誤差（距離差の誤差Δｈi,j）を算出し、この誤差を用いて算出した目標の状態ベクトルの修正量を出力する。修正状態ベクトル算出部１７５では、修正量算出部１７４１〜１７４Ｎから修正量を入力し、最小二乗法による収束計算を施して目標の状態ベクトルの修正量と該修正量を状態ベクトルに反映させた修正状態ベクトルを出力する。
【０１６９】
収束判定部１７６は、修正状態ベクトル算出部１７５から修正状態ベクトルとその修正量を入力し、修正量が閾値以上であれば、修正状態ベクトルを目標の状態ベクトルの初期値の代わりに距離差概略値算出部１７３１〜１７３Ｎの入力へフィードバックし、上記処理を繰り返す。修正量が閾値未満であると、収束判定部１７６は、該修正量を反映させた目標の状態ベクトル（目標の位置、速度等）を修正状態ベクトル算出部１７５から入力して目標の測位結果として出力する。
【０１７０】
以上のように、この実施の形態１１によれば、非同期測位手段１７ａが、センサ組の検知結果から得られた目標との距離差を追尾処理し、別の時刻において、異なるセンサ組の検知結果から目標との距離差が得られる場合、上記追尾結果に基づいてそのセンサ組に対する時刻整合を行ってから、目標の運動諸元を推定するので、異なる時刻の観測値をあたかも同時刻で観測された結果とみなすことができ、観測値が非同期であっても信頼性の高い目標の運動諸元の推定が可能である。また、観測情報として任意の２センサ間の目標との距離差のみでよいことから、上記実施の形態７と同様にドップラ周波数差が得られないセンサであっても目標の運動諸元を推定することができる。
【０１７１】
実施の形態１２．
図１７は、この発明の実施の形態１２による運動諸元推定装置の構成を示す図である。図１７に示すように、実施の形態１２による運動諸元推定装置は、受信手段１、距離差観測値検出手段２ａ及び非同期測位手段１８ａを備える。なお、図１７において、図１、図９及び図１０と同一又はこれに相当する構成要素には同一符号を付して説明を省略する。
【０１７２】
非同期測位手段１８ａは、初期状態量設定部１８１、時刻差算出部１８２１〜１８２Ｎ、距離差概略値算出部１８３１〜１８３Ｎ、修正量算出部１８４１〜１８４Ｎ、修正状態ベクトル算出部１８５、収束判定部１８６、及び収束演算カウント部１８７を備える。収束判定部１８６及び収束演算カウント部１８７以外の構成は、上記実施の形態７の図１０で示した初期状態量設定部１２１、時刻差算出部１２２１〜１２２Ｎ、距離差概略値算出部１２３１〜１２３Ｎ、修正量算出部１２４１〜１２４Ｎ、及び修正状態ベクトル算出部１２５と同様に動作する。
【０１７３】
収束判定部１８６は、収束演算カウント部１８７が計算回数限界値をカウントするまでの間、修正状態ベクトル算出部１８５による修正量と所定の閾値との比較結果に応じて、修正状態ベクトルを距離差概略値算出部１８３１〜１８３Ｎにフィードバックしたり、収束計算を終了して修正状態ベクトルを測位結果として出力する。
【０１７４】
収束演算カウント部１８７は、収束判定部１８６により修正状態ベクトルが距離差概略値算出部１８３１〜１８３Ｎにフィードバックされる度に収束演算回数を、所定の計算回数限界値になるまで計数する。ここで、計算回数限界値とは、実施の形態１２による運動諸元推定装置を実現する計算機の演算性能や検知データ等のサンプリングレートに応じて決定した、最適な演算負荷と推定精度を与える収束演算回数である。
【０１７５】
次に動作について説明する。
以降では、本実施の形態１２による運動諸元推定装置に特有な動作を主に説明する。
先ず、収束判定部１８６が収束演算を行うにあたり、収束演算カウント部１８７は、カウント値を０に初期化し、カウントの限界値として計算回数限界値を設定する。上記実施の形態１と同様にして、修正状態ベクトル算出部１８５から修正量が入力されると、収束判定部１８６は、所定の閾値と該修正量を比較する。このとき、修正量が閾値より大きく収束しなかった場合、収束演算カウント部１８７は、カウント値を１増やす。
【０１７６】
計算回数限界値にカウント値が達しても修正量が収束しなかった場合、収束演算カウント部１８７は、計算終了の信号を収束判定部１８６に送信する。これにより、収束判定部１８６は、該計算終了時の修正量を反映させた修正状態ベクトル（修正量を加えた修正状態ベクトル）を目標の測位結果として出力する。なお、カウント値が計算回数限界値に達する前に修正量が収束した場合は、該収束した修正量を加えた修正状態ベクトルを目標の測位結果として出力し、収束演算カウント部１８７はカウントを停止する。
【０１７７】
以上のように、この実施の形態１２によれば、収束判定部１８６が、収束演算カウント部１８７が収束演算の回数をカウントし、予め定めた計算回数限界値に達しても修正量が収束しない場合、その時点の修正量を反映させた修正状態ベクトルを目標の測位結果とするので、修正状態ベクトル算出部１８５による収束計算の回数が限定されることから、本実施の形態１２による運動諸元推定装置の性能を超える演算の実行が抑制され、かつ所定の時間で演算が終了することから収束計算の時間を短縮することができる。また、観測情報として任意の２センサ間の目標との距離差のみでよいことから、上記実施の形態７と同様にドップラ周波数差が得られないセンサであっても目標の運動諸元を推定できる。
【０１７８】
なお、上記実施の形態１２では、収束演算カウント部１８７を上記実施の形態７による構成に適用した場合を示したが、上記実施の形態８〜１１の構成に適用してもよく、同様の効果を得ることができる。
【図面の簡単な説明】
【０１７９】
【図１】この発明の実施の形態１による運動諸元推定装置の構成を示す図である。
【図２】航空機等の目標について基準時刻ｔ〜ｔ3までに４回の観測値が得られた場合を示す図である。
【図３】この発明の実施の形態２による運動諸元推定装置の構成を示す図である。
【図４】この発明の実施の形態３による運動諸元推定装置の構成を示す図である。
【図５】時刻ｔ3にセンサ１１，１２、時刻ｔ4にセンサ１２，１３により距離差及びドップラ周波数差が得られた場合を示す図である。
【図６】この発明の実施の形態４による運動諸元推定装置の構成を示す図である。
【図７】時刻ｔ1，ｔ2にセンサ１１，１２、時刻ｔ3にセンサ１２，１３により距離差及びドップラ周波数差が得られた場合を示す図である。
【図８】この発明の実施の形態５による運動諸元推定装置の構成を示す図である。
【図９】この発明の実施の形態６による運動諸元推定装置の構成を示す図である。
【図１０】この発明の実施の形態７による運動諸元推定装置の構成を示す図である。
【図１１】この発明の実施の形態８による運動諸元推定装置の構成を示す図である。
【図１２】この発明の実施の形態９による運動諸元推定装置の構成を示す図である。
【図１３】時刻ｔ1にセンサ１１，１２、時刻ｔ2にセンサ１２，１３、時刻ｔ3にセンサ１１，１２、時刻ｔ4にセンサ１２，１３により距離差が得られた場合を示す図である。
【図１４】この発明の実施の形態１０による運動諸元推定装置の構成を示す図である。
【図１５】時刻ｔ1，ｔ2にセンサ１１，１２、時刻ｔ3，ｔ4にセンサ１２，１３により距離差が得られた場合を示す図である。
【図１６】この発明の実施の形態１１による運動諸元推定装置の構成を示す図である。
【図１７】この発明の実施の形態１２による運動諸元推定装置の構成を示す図である。
【符号の説明】
【０１８０】
１ 受信手段、２ 距離差／速度差観測値検出手段、２ａ 距離差観測値検出手段（検出手段）、４，５，７，９，１０，１２ａ，１３ａ，１５ａ，１７ａ，１８ａ 非同期測位手段、８，１４ａ，１６ａ 仮測位／初期値算出手段、１１〜１Ｍ センサ、２１〜２Ｎ 距離差／速度差観測値検出部、４１，６１，８１，９１，１４１，１６１，１８１ 初期状態量設定部、４２１〜４２Ｎ，５２１〜５２Ｎ，７１１〜７１Ｎ，１０２１〜１０２Ｎ，１２２１〜１２２Ｎ，１３２１〜１３２Ｎ，１５１１〜１５１Ｎ，１８２１〜１８２Ｎ 時刻差算出部、４３１〜４３Ｎ，５３１〜５３Ｎ，７２１〜７２Ｎ，８３１，８３２，９３１〜９３Ｎ，１０３１〜１０３Ｎ，１２３１〜１２３Ｎ 距離差／速度差概算値算出部、１３３１〜１３３Ｎ，１４２１，１４２２，１５２１〜１５２Ｎ，１６３１，１６３２，１７３１〜１７３Ｎ，１８３１〜１８３Ｎ 距離差概算値算出部、４４１〜４４Ｎ，５４１〜５４Ｎ，７３１〜７３Ｎ，８４１，８４２，９４１〜９４Ｎ，１０４１〜１０４Ｎ，１２４１〜１２４Ｎ，１３４１〜１３４Ｎ，１４３１，１４３２，１５３１〜１５３Ｎ，１６４１，１６４２，１７４１〜１７４Ｎ，１８４１〜１８４Ｎ 修正量算出部、４５，５５，６４，７４，８５，９５，１０６，１２５，１３５，１４４，１５４，１６５，１７５，１８５ 修正状態ベクトル算出部、４６，５６，６５，７５，８６，９６，１２６，１３６，１４５，１５５，１６６，１７６，１８６ 収束判定部、８２１，８２２，９２１〜９２Ｎ，１６２１，１６２２，１７２１〜１７２Ｎ 追尾フィルタ部、１０７，１８７ 収束演算カウント部、１１１〜１１Ｎ 距離差観測値検出部、１４６，１６７ メモリ、１４７，１６８，１７１ 初期値算出部。
【特許請求の範囲】
【請求項１】
目標の運動を観測する複数のセンサを有する受信手段と、
前記受信手段の複数のセンサのうち任意のセンサ対による同一時刻での観測結果から、該センサ対のセンサ間の目標との距離差及び速度差の観測値を検出する検出手段と、
複数のセンサ対による異なる各時刻での観測結果から検出された各センサ対のセンサ間の目標との距離差及び速度差の観測値と、予め仮定した目標の運動方程式に従い基準時刻における目標の運動諸元の初期値に対し該基準時刻との時刻差を反映させて算出した前記各時刻での各センサ対のセンサ間の目標との距離差及び速度差の概略推定値と、の差分より目標の運動諸元の修正量を算出し、収束演算によって差分が収束するまで運動諸元を修正することで前記基準時刻における目標の運動諸元を推定する非同期測位手段とを備えた運動諸元推定装置。
【請求項２】
非同期測位手段は、観測値と概略推定値との差分より目標の運動諸元の修正量を最小二乗法によって算出し、収束演算によって差分が収束するまで目標の運動諸元を修正することで基準時刻における目標の運動諸元を推定することを特徴とする請求項１記載の運動諸元推定装置。
【請求項３】
非同期測位手段は、観測値と概略推定値との差分に対してセンサの観測精度を重み付けして目標の運動諸元の修正量を算出することを特徴とする請求項１又は請求項２記載の運動諸元推定装置。
【請求項４】
複数のセンサのうち第１のセンサ対による観測結果から検出される該第１のセンサ対のセンサ間の目標との距離差及び速度差を追尾処理すると共に、前記第１のセンサ対とは異なる第２のセンサ対による同一時刻での観測結果から該第２のセンサ対のセンサ間の目標との距離差及び速度差が検出された場合、前記追尾処理結果から前記第２のセンサ対による観測時刻での前記第１のセンサ対のセンサ間の目標との距離差及び速度差を予測処理する追尾フィルタ部を備え、
非同期測位手段は、前記追尾フィルタ部の予測処理により同一時刻の観測に模擬された前記第１及び前記第２のセンサ対におけるセンサ間の目標との距離差及び速度差を観測値として収束演算を行い、基準時刻における目標の運動諸元を推定することを特徴とする請求項１記載の運動諸元推定装置。
【請求項５】
複数のセンサ対の異なる各時刻での観測結果から得られた各センサ対のセンサ間の目標との距離差及び速度差の観測値を同一時刻で観測したものと仮定して測位を行い、前記時刻における目標の運動諸元の値を、非同期測位手段の収束演算で使用する基準時刻における目標の運動諸元の初期値として設定する仮測位／初期値算出手段を備えたことを特徴とする請求項１記載の運動諸元推定装置。
【請求項６】
複数のセンサのうち第１のセンサ対による観測結果から検出される該第１のセンサ対のセンサ間の目標との距離差及び速度差を追尾処理すると共に、前記第１のセンサ対とは異なる第２のセンサ対による同一時刻での観測結果から該第２のセンサ対のセンサ間の目標との距離差及び速度差が検出された場合、前記追尾処理結果から前記第２のセンサ対による観測時刻での前記第１のセンサ対のセンサ間の目標との距離差及び速度差を予測処理する追尾フィルタ部を備え、
仮測位／初期値算出手段は、前記追尾フィルタ部の予測処理により同一時刻の観測に模擬された前記第１及び前記第２のセンサ対におけるセンサ間の目標との距離差及び速度差を観測値として収束演算を行い、推定した基準時刻における目標の運動諸元を、非同期測位手段の収束演算で使用する基準時刻における目標の運動諸元の初期値として設定することを特徴とする請求項５記載の運動諸元推定装置。
【請求項７】
検出手段は、受信手段の複数のセンサのうち任意のセンサ対による同一時刻での観測結果から該センサ対のセンサ間の目標との距離差の観測値を検出し、
非同期測位手段は、複数のセンサ対による異なる各時刻での観測結果から検出された各センサ対のセンサ間の目標との距離差の観測値と、予め仮定した目標の運動方程式に従い基準時刻における目標の運動諸元の初期値に対し該基準時刻との時刻差を反映させて算出した前記各時刻での各センサ対のセンサ間の目標との距離差の概略推定値と、の差分について収束演算を行い、前記基準時刻における目標の運動諸元を推定することを特徴とする請求項１から請求項３のうちのいずれか１項記載の運動諸元推定装置。
【請求項８】
検出手段は、受信手段の複数のセンサのうち任意のセンサ対による同一時刻での観測結果から該センサ対のセンサ間の目標との距離差の観測値を検出し、
追尾フィルタ部は、前記複数のセンサのうち第１のセンサ対による観測結果から検出される該第１のセンサ対のセンサ間の目標との距離差を追尾処理すると共に、前記第１のセンサ対とは異なる第２のセンサ対による同一時刻での観測結果から該第２のセンサ対のセンサ間の目標との距離差が検出された場合、前記追尾処理結果から前記第２のセンサ対による観測時刻での前記第１のセンサ対のセンサ間の目標との距離差を予測処理し、
非同期測位手段は、前記追尾フィルタ部の予測処理により同一時刻の観測に模擬された前記第１及び前記第２のセンサ対におけるセンサ間の目標との距離差を観測値として収束演算を行い、基準時刻における目標の運動諸元を推定することを特徴とする請求項４記載の運動諸元推定装置。
【請求項９】
仮測位／初期値算出手段は、複数のセンサ対の異なる各時刻での観測結果から得られた各センサ対のセンサ間の目標との距離差の観測値を同一時刻で観測したものと仮定して測位を行い、前記時刻における目標の運動諸元の値を、非同期測位手段の収束演算に使用する基準時刻における目標の運動諸元の初期値として設定することを特徴とする請求項５記載の運動諸元推定装置。
【請求項１０】
検出手段は、受信手段の複数のセンサのうち任意のセンサ対による同一時刻での観測結果から該センサ対のセンサ間の目標との距離差の観測値を検出し、
追尾フィルタ部は、前記複数のセンサのうち第１のセンサ対による観測結果から検出される該第１のセンサ対のセンサ間の目標との距離差を追尾処理すると共に、前記第１のセンサ対とは異なる第２のセンサ対による同一時刻での観測結果から該第２のセンサ対のセンサ間の目標との距離差が検出された場合、前記追尾処理結果から前記第２のセンサ対による観測時刻での前記第１のセンサ対のセンサ間の目標との距離差を予測処理し、
仮測位／初期値算出手段は、前記追尾フィルタ部の予測処理により同一時刻の観測に模擬された前記第１及び前記第２のセンサ対におけるセンサ間の目標との距離差を観測値として収束演算を行い、推定した基準時刻における目標の運動諸元を、非同期測位手段の収束演算に使用する基準時刻における目標の運動諸元の初期値として設定することを特徴とする請求項９記載の運動諸元推定装置。
【請求項１１】
収束演算の回数を所定の限度回数までカウントする収束演算カウント部を備え、
非同期測位手段は、前記収束演算カウント部によるカウントが限度回数まで達すると、収束演算を終了してその時点での運動諸元推定値を出力することを特徴とする請求項１から請求項１０のうちのいずれか１項記載の運動諸元推定装置。
【請求項１】
目標の運動を観測する複数のセンサを有する受信手段と、
前記受信手段の複数のセンサのうち任意のセンサ対による同一時刻での観測結果から、該センサ対のセンサ間の目標との距離差及び速度差の観測値を検出する検出手段と、
複数のセンサ対による異なる各時刻での観測結果から検出された各センサ対のセンサ間の目標との距離差及び速度差の観測値と、予め仮定した目標の運動方程式に従い基準時刻における目標の運動諸元の初期値に対し該基準時刻との時刻差を反映させて算出した前記各時刻での各センサ対のセンサ間の目標との距離差及び速度差の概略推定値と、の差分より目標の運動諸元の修正量を算出し、収束演算によって差分が収束するまで運動諸元を修正することで前記基準時刻における目標の運動諸元を推定する非同期測位手段とを備えた運動諸元推定装置。
【請求項２】
非同期測位手段は、観測値と概略推定値との差分より目標の運動諸元の修正量を最小二乗法によって算出し、収束演算によって差分が収束するまで目標の運動諸元を修正することで基準時刻における目標の運動諸元を推定することを特徴とする請求項１記載の運動諸元推定装置。
【請求項３】
非同期測位手段は、観測値と概略推定値との差分に対してセンサの観測精度を重み付けして目標の運動諸元の修正量を算出することを特徴とする請求項１又は請求項２記載の運動諸元推定装置。
【請求項４】
複数のセンサのうち第１のセンサ対による観測結果から検出される該第１のセンサ対のセンサ間の目標との距離差及び速度差を追尾処理すると共に、前記第１のセンサ対とは異なる第２のセンサ対による同一時刻での観測結果から該第２のセンサ対のセンサ間の目標との距離差及び速度差が検出された場合、前記追尾処理結果から前記第２のセンサ対による観測時刻での前記第１のセンサ対のセンサ間の目標との距離差及び速度差を予測処理する追尾フィルタ部を備え、
非同期測位手段は、前記追尾フィルタ部の予測処理により同一時刻の観測に模擬された前記第１及び前記第２のセンサ対におけるセンサ間の目標との距離差及び速度差を観測値として収束演算を行い、基準時刻における目標の運動諸元を推定することを特徴とする請求項１記載の運動諸元推定装置。
【請求項５】
複数のセンサ対の異なる各時刻での観測結果から得られた各センサ対のセンサ間の目標との距離差及び速度差の観測値を同一時刻で観測したものと仮定して測位を行い、前記時刻における目標の運動諸元の値を、非同期測位手段の収束演算で使用する基準時刻における目標の運動諸元の初期値として設定する仮測位／初期値算出手段を備えたことを特徴とする請求項１記載の運動諸元推定装置。
【請求項６】
複数のセンサのうち第１のセンサ対による観測結果から検出される該第１のセンサ対のセンサ間の目標との距離差及び速度差を追尾処理すると共に、前記第１のセンサ対とは異なる第２のセンサ対による同一時刻での観測結果から該第２のセンサ対のセンサ間の目標との距離差及び速度差が検出された場合、前記追尾処理結果から前記第２のセンサ対による観測時刻での前記第１のセンサ対のセンサ間の目標との距離差及び速度差を予測処理する追尾フィルタ部を備え、
仮測位／初期値算出手段は、前記追尾フィルタ部の予測処理により同一時刻の観測に模擬された前記第１及び前記第２のセンサ対におけるセンサ間の目標との距離差及び速度差を観測値として収束演算を行い、推定した基準時刻における目標の運動諸元を、非同期測位手段の収束演算で使用する基準時刻における目標の運動諸元の初期値として設定することを特徴とする請求項５記載の運動諸元推定装置。
【請求項７】
検出手段は、受信手段の複数のセンサのうち任意のセンサ対による同一時刻での観測結果から該センサ対のセンサ間の目標との距離差の観測値を検出し、
非同期測位手段は、複数のセンサ対による異なる各時刻での観測結果から検出された各センサ対のセンサ間の目標との距離差の観測値と、予め仮定した目標の運動方程式に従い基準時刻における目標の運動諸元の初期値に対し該基準時刻との時刻差を反映させて算出した前記各時刻での各センサ対のセンサ間の目標との距離差の概略推定値と、の差分について収束演算を行い、前記基準時刻における目標の運動諸元を推定することを特徴とする請求項１から請求項３のうちのいずれか１項記載の運動諸元推定装置。
【請求項８】
検出手段は、受信手段の複数のセンサのうち任意のセンサ対による同一時刻での観測結果から該センサ対のセンサ間の目標との距離差の観測値を検出し、
追尾フィルタ部は、前記複数のセンサのうち第１のセンサ対による観測結果から検出される該第１のセンサ対のセンサ間の目標との距離差を追尾処理すると共に、前記第１のセンサ対とは異なる第２のセンサ対による同一時刻での観測結果から該第２のセンサ対のセンサ間の目標との距離差が検出された場合、前記追尾処理結果から前記第２のセンサ対による観測時刻での前記第１のセンサ対のセンサ間の目標との距離差を予測処理し、
非同期測位手段は、前記追尾フィルタ部の予測処理により同一時刻の観測に模擬された前記第１及び前記第２のセンサ対におけるセンサ間の目標との距離差を観測値として収束演算を行い、基準時刻における目標の運動諸元を推定することを特徴とする請求項４記載の運動諸元推定装置。
【請求項９】
仮測位／初期値算出手段は、複数のセンサ対の異なる各時刻での観測結果から得られた各センサ対のセンサ間の目標との距離差の観測値を同一時刻で観測したものと仮定して測位を行い、前記時刻における目標の運動諸元の値を、非同期測位手段の収束演算に使用する基準時刻における目標の運動諸元の初期値として設定することを特徴とする請求項５記載の運動諸元推定装置。
【請求項１０】
検出手段は、受信手段の複数のセンサのうち任意のセンサ対による同一時刻での観測結果から該センサ対のセンサ間の目標との距離差の観測値を検出し、
追尾フィルタ部は、前記複数のセンサのうち第１のセンサ対による観測結果から検出される該第１のセンサ対のセンサ間の目標との距離差を追尾処理すると共に、前記第１のセンサ対とは異なる第２のセンサ対による同一時刻での観測結果から該第２のセンサ対のセンサ間の目標との距離差が検出された場合、前記追尾処理結果から前記第２のセンサ対による観測時刻での前記第１のセンサ対のセンサ間の目標との距離差を予測処理し、
仮測位／初期値算出手段は、前記追尾フィルタ部の予測処理により同一時刻の観測に模擬された前記第１及び前記第２のセンサ対におけるセンサ間の目標との距離差を観測値として収束演算を行い、推定した基準時刻における目標の運動諸元を、非同期測位手段の収束演算に使用する基準時刻における目標の運動諸元の初期値として設定することを特徴とする請求項９記載の運動諸元推定装置。
【請求項１１】
収束演算の回数を所定の限度回数までカウントする収束演算カウント部を備え、
非同期測位手段は、前記収束演算カウント部によるカウントが限度回数まで達すると、収束演算を終了してその時点での運動諸元推定値を出力することを特徴とする請求項１から請求項１０のうちのいずれか１項記載の運動諸元推定装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【公開番号】特開２００８−２０３０９５（Ｐ２００８−２０３０９５Ａ）
【公開日】平成２０年９月４日（２００８．９．４）
【国際特許分類】
【出願番号】特願２００７−３９７１３（Ｐ２００７−３９７１３）
【出願日】平成１９年２月２０日（２００７．２．２０）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２０年９月４日（２００８．９．４）
【国際特許分類】
【出願日】平成１９年２月２０日（２００７．２．２０）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]