JP2005308428A - 位置推定装置 - Google Patents

Abstract

【課題】 マルチパスによる影響を受ける環境下で移動体が送信する電波を受信してこの移動体の位置を精度よく推定する。
【解決手段】 対象物からの電波を複数の受信器により受信して得た受信波間の位相差が整数値バイアスの異なる複数の受信波位相差から、追尾処理によって真の位相差を選択し対象物の位置を推定するために、複数の受信波位相差から第１及び第２の位相差を選択しこれらの基づく航跡を維持して平滑値を算出する追尾処理器２２−１、２２−２と、追尾処理器２２−１、２２−２が算出した平滑値から位相差とは異なる物理量であって対象物の運動状態を表す物理量を算出する物理量算出器２３と、物理量算出器２３により算出された物理量の連続性に基づいて第１及び第２位相差に基づく航跡とを入れ替える航跡入れ替え器２４と、を備えた。
【選択図】 図１

Description

この発明は、電波を用いて対象物の位置を推定する装置に係るものであり、特に対象物が発する電波を複数の受信器で受信し、これら受信器間の受信波位相差を用いて対象物の位置を推定する技術に関する。

電波を用いて対象物の位置を計測する従来の方法としては、対象物から発信される電波を複数の受信器で受信し、これらの受信器の位置と受信器間の位相差、対象物の位置が満たす関係から対象物の位置を算出する、という方法がある（例えば、特許文献１）。

ここで、位相差はラジアン値で表されるので、２πのアンビギュイティが起こりうる。すなわち正しい位相差をΔφとした場合に、Δφ＋２π×ｎ（ｎは整数）となる位相差も同時に算出されうる。このように複数の位相差が算出される中で、対象物の位置に対応する位相差は一つだけである。複数の位相差から正しい位相差を見つけるために追尾技術を用いる方法が提案されている（例えば、非特許文献１）。

追尾技術は、過去に得られた値から予測値と平滑値とを外装法によって求め、現実の観測値との相関を評価して観測値の取捨選択を行う技術である。このような追尾技術によれば、各時刻（サンプル時刻と呼ぶ）において、複数の位相差を採りうる場合に、対象物に対応する最尤の位相差を選択することができる。

特開２００１−２７２４４８ 「」公報。 "Applied Optimal Estimation", The M.I.T Press, Cambridge, 1974, pp.107-119。

しかし、従来の追尾技術による位相差の選択方法は、受信器が対象物により送信される電波を直接波として受信する理想的な電波伝播環境を前提にしている。したがって、屋内や、屋外であっても壁面に囲まれた通路がある場合、位置測定の対象となる対象物が移動体である場合にこの対象物以外に併走している物体がある場合、そして地面や海面などによる乱反射によってマルチパスが発生する状況においては、上述のような選択方法を用いることは困難である。より具体的にいえば、マルチパス波の干渉によって位相差に誤差が生じ、正しい位相差と適切な相関が得られない状況が起こって、本来の位相差とは異なる位相差を選択してしまうおそれが強い。このような場合、対象物の位置に対応する位相差でない位相差に基づいて対象物の位置を計算することとなるため、位置の推定精度は著しく劣化することとなる。

この発明はこのような課題を解決するためになされたもので、マルチパス波等の干渉が生じる状況においても、その影響を軽減し、精度よく対象物の位置を推定することを目的とする。

対象物からの電波を複数の受信器により受信して得た受信波間の位相差が整数値バイアスの異なる複数の受信波位相差を含む場合に、これらの位相差に基づく追尾処理を行って上記対象物の位置推定に用いる平滑値を算出し、この平滑値から上記対象物の位置を推定する位置推定装置において、
上記複数の受信波位相差から第１位相差を選択しこの第１位相差に基づく航跡を維持して平滑値を算出する第１追尾処理器と、
上記第１位相差に隣接する第２位相差を選択してその第２位相差に基づく航跡を維持する第２追尾処理器と、
上記第１追尾処理器が算出した平滑値から位相差とは異なる物理量であって上記対象物の運動状態を表す物理量を算出する物理量算出器と、
上記物理量算出器により算出された物理量の連続性に基づいて上記第１位相差に基づく航跡と上記第２位相差に基づく航跡とを入れ替える航跡入れ替え器と、
を備えたものである。

ここで、追尾処理とは一般に、所定のサンプリング間隔で得られる観測値と前回サンプル時以前（前回を含む）の観測値及び平滑値から得られた今回サンプル用の予測値との相関を評価し、相関が認められた観測値に基づいて今回サンプル時の平滑値と次回サンプル時の予測値を算出する演算をサンプル毎に繰り返す処理をいう。そして、追尾処理においては、これまでのサンプル時に得られた観測値、予測値と平滑値は次回以降の予測値及び平滑値の算出に影響を及ぼすことが多いので、このような観測値、予測値と平滑値の時系列を、一般に航跡と呼んでいる。

また上述の位置推定装置において、位相差とは異なる物理量であって対象物の運動状態を表す物理量とは、例えば時間、位置、速度、角速度、加速度及び角加速度など、対象物の運動状態を表現する上で用いることのできる何れかの状態変数を指す。

このように、この発明に係る位置推定装置によれば、位相差にアンビギュイティが生じる状況において、位相差に基づく追尾処理のみでは対象物の位置に対応しない誤った位相差を選択してしまう状況であっても、位相差とは異なる対象物の物理量を算出しその物理量の整合性を評価することで、誤った位相差を選択することを抑制し、精度の高い位置推定処理を実現することができるのである。

さらにこの発明に係る位置推定装置においては、位相差にアンビギュイティがあることに基づいて上述のような構成としている。しかしながら、方位角を算出することのできるアレーアンテナを用いて構成した場合においては、位相差に替えて方位角のアンビギュイティに対しても同一の技術的思想を適用することができるのである。すなわち、かかる位置推定装置は、対象物からの電波を複数の受信器により受信して得た受信波から整数値バイアスの異なる複数の方位角が得られる場合に、これらの方位角に基づく追尾処理を行って上記対象物の位置推定に用いる主方位角を算出し、この主方位角から上記対象物の位置を推定する位置推定装置において、
上記複数の方位角から第１方位角を選択しこの第１方位角に基づく航跡を維持して上記主方位角を算出する第１追尾処理器と、
上記第１方位角に隣接する第２方位角を選択してこの第２方位角に基づく航跡を維持する第２追尾処理器と、
上記主方位角から方位角とは異なる物理量であって上記対象物の運動状態を表す物理量を算出する物理量算出器と、
上記物理量算出器により算出された物理量の連続性に基づいて上記第１方位角に基づく航跡と上記第２方位角に基づく航跡とを入れ替える航跡入れ替え器と、
を備えたもの、とすることもできる。

このように、この発明に係る位置推定装置によれば、方位角にアンビギュイティが生じる状況において、方位角に基づく追尾処理のみでは対象物の位置に対応しない誤った方位角を選択してしまう状況であっても、方位角とは異なる対象物の物理量を算出しその物理量の整合性を評価することで、誤った方位角を選択することを抑制し、精度の高い位置推定処理を実現することができるのである。

以下、この発明の実施の形態を図を用いて説明する。
実施の形態１．
図１は、この発明の実施の形態１による位置推定装置の構成を示すブロック図であある。図において、対象物１には電波送信機が装着されている。位置推定装置２はこの発明の対象となる位置推定装置であって、Ｍ個の受信器２１−１〜Ｍ、追尾処理器２２−１〜３、物理量算出器２３、航跡入れ替え器２４を備えている。

受信器２１−ｉと受信器２１−ｊはＭ個（Ｍは２以上の自然数）ある受信器２１−１〜Ｍの中から選択された２個の受信器であって（ｉ，ｊ＝１，２，…，Ｍ）、例えば対象物１に装着された電波送信機が送信する電波を受信する受信アンテナ、局部発信機、ダウンコンバータ、Ａ／Ｄ変換器などを用いて構成され、最終的に受信した電波信号を所定のサンプリング間隔でデジタル信号として出力するようになっている。ここで、受信器２１−ｉと２１−ｊによって出力されるデジタル信号ｒ_ｉとｒ_ｊとの位相差Δφ_ｉｊ、対象物１の送信する電波信号の波長をλ、対象物１の未知なる位置ｐをｐ＝(ｘ ｙ ｚ)、そして受信器２１−ｉと２１−ｊの位置を(ｘ_ｉ ｙ_ｉ ｚ_ｉ)、(ｘ_ｊ ｙ_ｊ ｚ_ｊ)とするならば、受信器２１−ｉ、２１−ｊと対象物１の位置ｐ、位相差Δφ_ｉｊは式（１）を満たす。

このようにすることで、対象物１が移動しても、一定のサンプリング間隔ごとに対象物１の位置が算出されるのである。なお、式（１）において、ｌ_ｉｊは位相差Δφ_ｉｊのアンビギュイティを表す整数バイアス値である。すなわち、位相差は角度値として表されるため、角度値としてはｌ_ｉｊによらずΔφ_ｉｊ＋２πｌ_ｉｊは同じ角を表すこととなる。ところが対象物１の位置は１つしかないので、式（１）によれば、対象物１の位置に対応するｌ_ｉｊはたった一つしか存在しないはずである。このことから、誤ったｌ_ｉｊを選択して式（１）に代入してしまうと、誤った位置を算出する結果を招くこととなる。

この問題を解決するための方法として、この発明の実施の形態１の位置推定装置は追尾処理により複数の位相差の中から正しい位相差を選択する。いま受信器２１−ｉと２１−ｊとの受信信号の位相差として得られる複数の位相差の集合を式（２）で示すＵ_ｉで表すこととする。

ただし、集合Ｕ_ｉの要素間には式（３）の関係が成立するものとする。

追尾処理器２２−１〜３は位相差をそれぞれ追尾処理する回路又は素子である。ここで追尾処理器２２−１は追尾処理によって正しい位相差であると推定された位相差（第１位相差）の航跡を維持する追尾処理回路または素子であって、最終的に位相差を平滑した値である位相差平滑値を算出するようになっている。また追尾処理器２２−２及び２２−３は追尾処理器２２−１が選択する第１位相差に隣接する第２位相差の航跡を維持する追尾処理回路または素子である。

図２は、追尾処理器２２−１を、例えばカルマンフィルタによる追尾処理を行うものとして構成した場合の詳細な構成を示したブロック図である。図に示すように、追尾処理器２２−１は位相差検出器２５、相関判定器２６、平滑器２７、相関予測器２８、位相差ゲイン算出器２９、範囲算出器３０を備えている。

このうち、位相差検出器２５は、受信器２１−ｉ、２１−ｊの出力するデジタル信号から位相差を検出する回路または素子である。また相関判定器２６は、位相差検出器２５が検出した位相差が後述する相関範囲算出器３０によって設定される相関範囲に含まれるか否かを判定し、含まれると判断した場合は新たな観測値として追尾処理に採用し、含まれないと判断した場合はこの位相差を棄却して、代わりに予測器２８が算出した予測値を出力する回路又は素子である。平滑器２７は相関判定器２６が出力するサンプル毎の位相差又は予測値に基づいてそのサンプルに対する位相差平滑値を算出する回路又は素子である。ここで算出される位相差平滑値は、追尾処理器２２−ｎの出力値として外部に供給される。予測器２８は、前回サンプル時の位相差平滑値に基づいて今回サンプルにおいて観測される位相差の予測値を算出する回路又は素子である。ただし前回サンプル時の位相差平滑値を直接の入力値とするのではなく、追尾処理器２２−ｎの外部にある航跡入れ替え器２４（後述）から取得した航跡の位相差平滑値を用いるようになっている。位相差ゲイン算出器２９は、平滑器２７が位相差平滑値の算出に用いる位相差ゲインを算出する回路又は素子である。相関範囲算出器３０は、予測器２８が算出した予測値から許容される今回サンプルの位相差の観測値の相関範囲を算出する回路又は素子である。

なお追尾処理器２２−２及び２２−３も追尾処理器２２−１と同等の構成要素を備えているのでその構成については特に示さないこととする。

引き続き、図１に示した位置推定装置２の構成について説明する。物理量算出器２３は、追尾処理器２２−１〜３が算出した各位相差平滑値に基づいて、対象物１に関する物理量を算出する回路又は素子である。ただしここで算出される物理量とは位相差とは異なる物理量であって、対象物１の運動状態を示す状態量である。

図３は、物理量算出器２３の例として対象物１の速度を算出する構成とした場合の詳細な構成を示すブロック図である。図において、測位演算器３１は追尾処理器２２−１〜３が算出した位相差平滑値に基づいて対象物１の位置を算出する回路又は素子である。なおこの発明の実施の形態１による位置推定装置２の出力結果である対象物の位置は、この部位の演算結果として求められる。速度算出器３２は、測位演算器３１が算出した対象物１の位置の時間変位から速度を算出する回路又は素子である。速度判定器３３は、速度算出器３２によって算出された対象物１の速度が所定の値域に含まれるかどうかを判断する部位である。

航跡入れ替え器２４は、追尾処理器２２−１〜３が使用する予測値を調整する回路又は素子である。すなわち、これらの予測値を調整することによって追尾処理器２２−１〜３の航跡の入れ替えを行うようになっている。この部位は、前回サンプルまでに算出された予測値を１サンプルの間記憶しておき、さらに物理量算出器２３の速度判定器３３の判定結果に基づいて追尾処理器２２−１〜３に従来の航跡をそのまま指定したり、入れ替えて指定する。

次に、位置推定装置２の動作について説明する。まず初めに位置推定装置２の動作原理について説明し、続いて具体的な動作の説明を行うこととする。対象物１から発信される電波は、位置推定装置２の受信器２１−１〜２１−Ｍによって受信され、検波処理を経てＡ／Ｄ変換されてデジタル信号として出力される。デジタル信号はサンプル毎に出力されるので、このデジタル信号から位相差を検出し、追尾処理に供するのである。ここで、ある受信器２１−ｉと２１−ｊに着目し、この受信器２１−ｉおよび２１−ｊの受信デジタル信号ｒ_ｉとｒ_ｊの位相差をΔΦ_ｉｊとし、この位相差の追尾処理について説明する。

図４は、追尾処理に基づいて複数の位相差から対象物１の位置に対応する位相差を選択する状況を説明する図である。図において、＊（アスタリスク）は対象物１の位置に対応する本来の位相差を表す点である。また×で表される点は、本来の位相差と整数バイアス値分だけ異なる位相差である。さらに矩形で囲まれた位相差は、追尾処理に基づいて算出した位相差平滑値を表している。追尾処理によれば、一時的に対象物１の位置に対応する本来の位相差を選択しうるが、常に本来の位相差を選択することは保証されない。つまり、例えば位相差として、前回の位相差とは近い位相差を選択するため、本来の位相差ではなく、実は誤った位相差を選択してしまうことが生じうる。図４の例で言えば、区間１０１において本来の位相差と離れて隣接する整数値バイアスを有する位相差に航跡が遷ってしまう。このような結果は、位相差のみ相関処理で位相差を選択していたことに起因するものである。

そこで、位相差のみの相関処理ではなく、位相差から得られた対象物１の運動状態、例えば速度の相関を考慮して位相差を選択するようにする。図５は、図４の位相差平滑値から算出される対象物１の速度の時間変位を表すグラフである。図が示すように航跡が誤った位相差に乗り移る時間帯１０１において、速度が大きく変化している。仮に対象物１の速度が取りうる値の範囲を定めることができる場合、選択した位相差から算出した速度がこの範囲を逸脱するような場合には、不適切な位相差を選択したことが判断できるのである。

例えば、対象物１が競走馬の場合、速度の上限値はおよそ時速６０ｋｍ程度であり、さらに下限値はおよそ時速４０ｋｍ程度であることが知られている。したがって例えば対象物１の速度が時速４０ｋｍ〜６０ｋｍを逸脱するような位相差は取り得ないはずであり、そのような条件に基づく位相差の相関処理を行うことで、適切な位相差を選択する可能性を大きく向上し、結果として位置の推測精度を高めることにつながるのである。

以上の原理を詳細に説明する。追尾処理器２２−ｎは位相差の追尾処理を行うが、ここでの追尾処理の運動モデルは式（４）で表されるものとする。

ここで、ｘ _ｋはサンプリング時刻ｔ_ｋにおける位相差の真値θ_ｋとその時間微分の真値θ'_ｋ（θ'_ｋはθ_ｋの時間による一回微分）を表す状態ベクトルであり、式（５）で表される。

また、サンプリング時刻ｔ_ｋ（ｋ＝１，２，…，ｎ）は、位相差を検出するタイミングを表す離散時刻であり、以降、時刻ｔ_ｋと呼ぶ。

式（４）において、Φ_ｋ−１は時刻ｔ_ｋ−１よりｔ_ｋへの状態ベクトルの推移行列であり、式（６）で表される。

ｗ_ｋは時刻ｔ_ｋにおける駆動雑音であり、Γ_１(ｋ)は時刻ｔ_ｋにおける駆動雑音の変換行列である。例えば、位相差の運動モデルを等速直線運動と仮定したことによる打ち切り誤差項をΓ_１(ｋ−１)ｗ_ｋ−１とみれば、ｗ_ｋは位相差の加速度ベクトルに相当する項であり、またΓ_１(ｋ−１)は式（７）で表される。なお、Ｔはサンプリング間隔である。平均を表す記号としてＥを用いると、ｗ_ｋは平均０、分散Ｑ _ｋの正規分布の白色雑音とする。

一方、位相差の観測モデルは式（８）で表されるものとする。

ここで、ｕ_ｋは位相差の観測値、Ｈは観測行列で、式（９）で表される。

なお、式（８）において、ν _ｋは時刻ｔ_ｋにおいて検出された位相差の観測雑音である。観測雑音は平均０、分散Ｒ_ｋの正規分布白色雑音に従うものと仮定する。また、時刻ｔ_ｋまでの間に位相差の追尾に用いた位相差の全体をＵ^ｋとする。

以上の運動モデルに基づき、カルマンフィルタを用いた場合を例に追尾フィルタを構成する。１サンプリング前の位相差航跡の平滑ベクトルをｘ^_ｋ−１(＋)（ｘ^は数式において文字ｘの上に＾を付した変数を表すものとする）、平滑誤差共分散行列をＰ_ｋ−１(＋)とすると、時刻ｔｋにおける位相差の状態ベクトルｘ _ｋの予測ベクトルｘ^(−)及び予測誤差共分散行列Ｐ_k(−)はそれぞれ条件付平均ベクトル、条件付共分散行列で定義され、式（１０）及び式（１１）で表される。

次に、時刻ｔ_ｋに計測された複数の位相差Ｕ_ｋから対象物１の位置に対応する位相差を選択する方法について述べる。偽の位相差の含まれる複数の位相差から正しい位相差を判定するため、式（１２）で表される位相差の予測値ｕ _ｋ(−)を利用する。

過去の位相差情報Ｕ^ｋ−１より得られる位相差の観測値の予測確率分布Ｐ[ｕ_ｋ｜Ｕ^ｋ−１]は、式（１３）に示す条件付確率密度関数で表される。

すなわち、位相差の観測値は、式（１１）で与えられる位相差の予測値ｕ_ｋ(−)を平均とし、式（１４）で与えられるＳ_ｋを分散とする１次元正規分布ｇ(ｕ_ｋ；ｕ_ｋ(−)，Ｓ_ｋ)に従うものとする。

ここで、式（１５）を満たす位相差を考える。

そして、式（１５）の左辺値ｆ_ｉが最小となるような位相差ｕⁿ _ｋを正しい位相差（第１位相差）と判断して選択するのである。さらにここでは、選択された位相差の前後の整数値バイアス値に対応する位相差ｕⁿ⁺¹ _ｋのいずれかを第２位相差とする。またｕ^n-1 _ｋを第３位相差としてもよい。なお、式（１５）において、ｄは目標との相関範囲を決めるパラメータであり、自由度１のχ自乗分布により算出する値である。

続いて、第１位相差の航跡と第２位相差及び第３位相差の航跡に対応する平滑ベクトルの算出方法について述べる。ゲイン行列Ｋ_ｋ、位相差航跡の平滑ベクトルｘ^_ｋ(＋)及び平滑誤差共分散行列Ｐ_ｋ(＋)は、カルマンフィルタの理論により式（１６）〜式（１８）で与えられる。

同様に、正しい位相差の前後の整数値バイアス値に対応する位相差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋをそれぞれ用いた位相差裏航跡の平滑ベクトルをｘ^^a _ｋ(＋)及びｘ^^b _ｋ(＋)とすると、これらは式（１９）及び式（２０）で与えられる。

次に、位相差航跡の位置または速度から、追尾が偽の位相差に乗り移った事象を判定し、位相差航跡を軌道修正する方法を、ここでは速度を利用した場合を例にとって説明する。第１位相差の航跡として選択された位相差ｕⁿ _ｋを用いて算出した移動体の速度をＶⁿ _ｋ、第２位相差、第３位相差の航跡として選択された位相差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋを用いて算出した移動体の速度をそれぞれ、Ｖ^n-1 _ｋ及びＶⁿ⁺¹ _ｋとする。

移動体の取りうる速度の下限をＶmin、上限をＶmaxとすると、位相差航跡の速度Ｖⁿ _ｋが誤差σを考慮して式（２１）または式（２２）を満たす場合に、位相差航跡が偽の位相差に乗り移ったと判定する。

乗り移りがあったと判定された場合、Ｖⁿ _ｋの代わりにＶ^n-1 _ｋ及びＶⁿ⁺¹ _ｋについても式（２１）及び式（２２）の判定を行い、これらの条件を満たさない速度に対応する位相差で算出した航跡の平滑ベクトルを、位相差航跡として採用する。例えばＶ^n-1 _ｋが式（２１）及び式（２２）の条件を満たさない場合は、式（２３）としてｘ^^a _ｋ(＋)を新たな位相差航跡とする。

そして、この時点でこれまでの第２位相差及び第３位相差の航跡を削除して、次サンプル時刻からは新たな位相差航跡を基準に、これまで同様、前後の整数値バイアスに対する位相差を用いて第２位相差及び第３位相差の航跡を生成するのである。

以上の処理方法に基づいて追尾処理器２２−ｎと追尾処理器２２−ｎ＋１、さらには物理量算出器２３と航跡入れ替え器２４の具体的な動作を示すと次のようになる。

追尾処理器２２−１において、相関判定器２６は、位相差検出器２５から位相差を受け取ると、整数値バイアスを考慮した複数の位相差（偽の位相差を含む）に対して、相関範囲算出器３０で算出された位相差の観測値の予測確率分布の平均値ｕ_ｋ(−)と分散Ｓ_ｋ、予め設定されたパラメータｄを用いて、式（１５）を満たす位相差のうち、ｆ_ｉが最小となる位相差ｕⁿ _ｋを正しい位相差と判断して選択する。また追尾処理器２２−２及び２２−３は、追尾処理器２２−１が選択した位相差ｕⁿ _ｋの前後の整数値バイアス値に対応する位相差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋをそれぞれの位相差の航跡を生成するためのデータとして選択する。

追尾処理器２２−１において、位相差ゲイン算出器２９は、予測器２８から取得した予測誤差共分散行列Ｐ_ｋ(−)と予め設定された受信器２１−１〜２１−Ｍの観測雑音の分散Ｒ_ｋとから、式（１６）に従い位相差のゲイン行列Ｋ_ｋを算出する。そして平滑器２７は、位相差ゲイン算出器２９から取得した位相差のゲイン行列Ｋ_ｋ、予測ベクトルｘ^_ｋ(−)、予測誤差共分散行列Ｐ_ｋ(−)と、相関判定器２６から取得した位相差航跡に対応する位相差ｕⁿ _ｋと、位相差の予測値ｕ_ｋ(−)とを用いて、式（１７）と式（１８）に従い、位相差航跡の平滑ベクトルｘ^_ｋ(＋)及び平滑誤差共分散行列Ｐ_ｋ(＋)を算出する。

同様に、追尾処理器２２−２及び２２−３においても、相関判定器２６から取得した位相差の航跡に対応する位相差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋ、位相差の予測値ｕ_ｋ(−)と、位相差ゲイン算出器２９から取得した位相差のゲイン行列Ｋ_ｋ、予測ベクトルｘ^(−)とを用いて、式（１９）と式（２０）に従い、それぞれの位相差の航跡の平滑ベクトルｘ^^a _ｋ(＋)及びｘ^^b _ｋ(＋)を算出する。

以上のようにして位相差検出単位毎に選択された第１位相差の航跡と第２及び第３位相差の航跡に対応する位相差ｕⁿ _ｋ、ｕ^n-1 _ｋ、ｕⁿ⁺¹ _ｋは、物理量算出器２３に出力される。物理量算出器２３の測位演算器３１は、各位相差毎に式（１）の連立方程式を解き、測位解（位置）を算出する。

続いて速度算出器３２は、測位演算器３１で算出した各位相差に対応する位置と過去の位置情報から、各位相差に対応する速度Ｖⁿ _ｋ、Ｖ^n-1 _ｋ、Ｖⁿ⁺¹ _ｋを算出する。そして速度判定器３３は、予め設定された移動体の速度の上限Ｖmax、下限Ｖmin及びその誤差σとから、速度算出器３２から取得した各位相差に対応する速度Ｖⁿ _ｋ、Ｖ^n-1 _ｋ、Ｖⁿ⁺¹ _ｋが式（２１）及び式（２２）を満たすか否かを判定する。

航跡入れ替え器２４は、速度判定器３３において、位相差航跡の位相差ｕⁿ _ｋに対応する速度Ｖⁿ _ｋが式（２１）及び式（２２）を満たした場合には、位相差航跡の位相差ｕⁿ _ｋに対応する測位解を現サンプリング時刻における測位結果とし、満たさない場合は、式（２１）及び式（２２）を満たす位相差裏航跡の位相差が正しい位相差であると判定し、これに対応する測位解を現サンプリング時刻における測位解とする。航跡入れ替え器２４は、速度判定器３３が第２及び第３位相差の航跡の位相差が正しい位相差であると判定された場合にのみ、式（２３）により位相差航跡の平滑ベクトルｘ^_ｋ(＋)を位相差裏航跡の平滑ベクトルに置き換えるとともに、これまで算出してきた位相差裏航跡の平滑ベクトルを初期化する。

予測器２８は、航跡入れ替え器２４から取得した位相差航跡の平滑ベクトルｘ^_ｋ(＋)、平滑誤差共分散行列Ｐ_ｋ(＋)と、予め設定された駆動雑音の誤差共分散行列Ｑ_ｋとから、式（１０）及び式（１１）に従い、次サンプリング時刻における位相差の予測ベクトルｘ^_ｋ＋１(−)及び予測誤差共分散行列Ｐ^_ｋ＋１(−)を算出する。相関範囲算出器３０は、予測器２８で算出された予測誤差共分散行列Ｐ_ｋ＋１(−)と、予め設定された受信器２１−１〜Ｍの観測雑音の分散Ｒｋとから、式（１４）に従い位相差の観測値の予測確率分布の分散Ｓ_ｋを算出する。

以上から明らかなように、この発明の実施の形態１の位置推定装置によれば、追尾処理器２２−１だけでなく、追尾処理器２２−２あるいは２２−３を設け、第１位相差の航跡と、この位相差の前後の整数値バイアスに対応する第２の位相差とを航跡として維持して平滑することとした。そして、速度判定器２６において、位相差航跡として選択された位相差により算出された測位解より算出された対象物１の速度が、移動体の取りうる速度範囲内であるかを判定して、範囲を超えている場合に、航跡入れ替え器２４において、速度が正常な位相差を用いて算出した測位解を最終的な測位結果とすることとした。かかる構成を採用した結果、マルチパス波の干渉により位相差の誤差が発生し、位相差の追尾が偽の位相差に乗り移っても、追尾を真の位相差に修正するため、測位精度の劣化を防ぐことができる。

上述の処理では、最良の実施の形態として速度の上限と下限が定められる構成について説明した。しかし速度の値域については上限と下限のいずれか一方しか定めることができない場合であっても式（２１）と式（２２）のいずれか一方のみを選択して、速度を評価することで、不適切な位相差を検出することができるので、上限と下限の双方が定められる構成に限定されるものではない。

また上述の処理では、同じく最良の実施の形態として、第１位相差の前後の位相差の航跡として第２位相差及び第３位相差を２つとも維持する構成について説明した。しかし対象物１の位置測定環境における電波の伝播特性から、マルチパスによる電波の干渉が位相差に及ぼす影響が予測可能となって、第１位相差よりも大きな位相差あるいは第１位相差よりも小さな位相差に限定できる場合もある。このような場合においては、第２位相差と第３位相差のいずれか一方だけを追尾すれば足りるのである。したがってこのような場合には、追尾処理器２２−１及び２２−２のみで十分である。

実施の形態２．
実施の形態１による位置推定装置では、正しいと判定された位相差ｕ^n-1 _ｋで算出した速度に矛盾があった場合に、位相差ｕⁿ _ｋの前後の整数値バイアス値に対応する位相差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋのうち、速度が正常となる位相差を正しい位相差として選択し直す構成とした。しかしながら、道路やトラックを周回する移動体のように、移動体の移動経路が限定されている場合は、対象物１がある基準点を通過する時間を予測して、この予測時間の整合性に基づいて位置推定結果の矛盾を判定するようにしてもよい。この発明の実施の形態２による位置推定装置は、かかる特徴を有するものである。

この発明の実施の形態２による位置推定装置の構成は実施の形態１と同様に図１によって表される。ただし実施の形態２による位置推定装置は実施の形態１と対比した場合に、物理量算出器２３の構成が異なっている。図６は、実施の形態２による位置推定装置２の物理量算出器２３の詳細な構成を示すブロック図である。図において、通過判定器３４は、測位演算器３１の出力する対象物１の位置から対象物１が基準点を通過したかどうかを判定する回路又は装置である。通過時刻予測器３５は、測位演算器３１の出力する対象物１の位置の時間変位から、基準位置の通過時刻を算出する回路又は素子である。通過時刻判定器３６は、通過時刻算出器３４が算出した通過時刻を評価し、測位演算器３１が位置の算出に用いた位相差平滑値の妥当性を判定する回路又は素子である。

ここで、対象物１の基準点通過が判定された時刻をｔ^~とし、その直前の追尾処理において、位相差航跡として選択された第１位相差ｕⁿ _ｋを用いて算出した対象物１の位置及び速度をそれぞれｐⁿ _ｋ及びＶⁿ _ｋ、第２位相差ｕ^n-1 _ｋに対応する位置及び速度をそれぞれｐ^n-1 _ｋ及びＶ^n-1 _ｋ、第３位相差ｕⁿ⁺¹ _ｋに対応する位置及び速度をそれぞれｐⁿ⁺¹ _ｋ及びＶⁿ⁺¹ _ｋ、基準点位置をＰ_refとすると、時刻ｔ^~における第１〜第３位相差の航跡に対応する移動体の予測位置は式（２４）〜式（２６）で表される。

このとき、予測位置と基準点位置の差は式（２７）〜式（２９）で表されるので、この差が最も小さいものが、対象物１の真の位相差航跡と判定できる。

以上のような原理に基づくこの発明の実施の形態２による位置推定装置２の具体的な動作を以下に説明する。実施の形態１と同様に、位置推定装置２においても追尾処理器２２−１〜３において第１位相差〜第３位相差の追尾処理を行う。その結果、各位相差平滑値ｕⁿ _ｋ、ｕ^n-1 _ｋ、ｕⁿ⁺¹ _ｋがそれぞれの追尾処理器により算出され、物理量算出器２３に出力される。物理量演算器２３の測位演算器３１は、取得した位相差平滑値のそれぞれについて式（１）の連立方程式を解き、測位解（位置）を算出する。

通過判定器３４は、移動体が起点を通過した時刻を記録し、その時刻を通過時刻予測手段に出力する。そして通過時刻予測器３５は、通過判定器３４から取得した移動体が基準点を通過した時刻ｔ^~と、測位演算器３１から取得した対象物１の位置ｐⁿ _ｋ及び速度Ｖⁿ _ｋ、位置ｐ^n-1 _ｋ及び速度Ｖ^n-1 _ｋ、位置ｐⁿ⁺¹ _ｋ及び速度Ｖⁿ⁺¹ _ｋとを用いて、式（２４）〜（２６）に従って、時刻ｔ^~における位相差航跡及び位相差裏航跡に対応する移動体の予測位置を算出する。通過時刻判定器３６は、通過時刻予測器３５から取得した時刻ｔ^~における第１〜第３位相差の航跡に対応する対象物１の予測位置と、予め設定された基準点位置ｐ_refを用いて、式（２７）〜式（２９）に従い予測位置と基準点位置の差を算出し、この中で最も小さい値を持つ予測位置に対応する位相差航跡または位相差裏航跡を、対象物１の真の位相差航跡と判定する。航跡入れ替え手段２４は、通過時刻判定器３６によって対象物１の真の位相差航跡と判定された予測位置が第２及び第３の位相差の航跡である場合には式（２３）により航跡の平滑ベクトルｘ^_ｋ(＋)を位相差裏航跡の平滑ベクトルに置き換えるとともに、これまで算出してきた第２及び第３の位相差の航跡の平滑ベクトルを初期化する。

予測器２８は、航跡入れ替え器２４から取得した位相差航跡の平滑ベクトルｘ^_ｋ(＋)、平滑誤差共分散行列Ｐ_ｋ(＋)と、予め設定された駆動雑音の誤差共分散行列Ｑｋとから、式（１０）及び式（１１）に従い、次サンプリング時刻における位相差の予測ベクトルｘ^_ｋ＋１(−)及び予測誤差共分散行列Ｐ_ｋ＋１(−)を算出する。相関範囲算出器３０は、予測器２８で算出された予測誤差共分散行列Ｐ_ｋ＋１(−)と、予め設定された受信器２１−１〜Ｍの観測雑音の分散Ｒ_ｋとから、式（１４）に従い位相差の観測値の予測確率分布の分散Ｓ_ｋを算出する。

以上から明らかなように、この発明の実施の形態２の位置推定装置によれば、通過時刻予測器３５において算出された第１〜第３の位相差航跡の基準点通過時刻における予測位置と基準点位置の差から、対象物１の真の位相差を判定し、航跡を入れ替えることとしたので、マルチパス波の干渉により位相差の誤差が発生し、位相差の追尾が偽の位相差に乗り移っても、追尾を真の位相差に修正するため、測位精度の劣化を防ぐことができる。

なお、この発明の実施の形態２においても、電波の伝播条件を仮定できる場合においては、第１位相差の前後の位相差の航跡の一方のみを維持すれば足りる点で、実施の形態１と同様である。

また、実施の形態１と２では速度に上限値や下限値を設定したり、基準位置の通過時間に上限値や下限値を設定して、位置推定の対象となる対象物の位相差の航跡の乗り移りを検出しているが、速度や通過時間といった条件は一例に過ぎない。すなわち対象物の運動状態を示すある物理量が位相差に基づいて導出可能であり、さらにある対象物についてその物理量の時間変化が何らかの整合性や連続性を仮定できるのであれば、如何なる物理量であってもこの発明による技術的思想に包含されるのである。

実施の形態３．
実施の形態１及び２による位置推定装置では、位相差について追尾処理を行い、平滑値と予測値とを算出している。しかしながら、位相差に替えて対象物の直交座標による位置を追尾するような構成を採用してもよい。この実施の形態３による位置推定装置はかかる特徴を有するものである。

この発明の実施の形態３による位置推定装置の構成は実施の形態１及び２と同様に図１によって表される。ただし実施の形態３による位置推定装置は追尾処理器２２−１〜３及び物理量算出器２３の詳細な構成が異なっている。図７は、実施の形態３による位置推定装置２の追尾処理器２２−１の詳細な構成を示すブロック図である。図において、直交座標ゲイン算出器３７は直交座標のゲインを算出する回路又は素子である。他の部位については実施の形態１及び２と同様であるが、後述するように、この追尾処理器２２−１は対象物１との距離差を直交座標によって追尾するように構成されているので、処理に供されるデータが異なっている。

なお、追尾処理器２１−２及び２２−３の構成については、追尾処理器２１−１と同様であるので説明を省略する。

図８は、この発明の実施の形態３における位置推定装置の物理量算出器２３の詳細な構成を示すブロック図である。図において、速度判定器３８は追尾処理器２２−１〜２２−３が算出した直交座標の平滑値の時間変位から速度を求めて判定する回路又は素子である。

ここで、この発明の実施の形態３における位置推定装置の追尾処理における運動モデルは式（３０）で表される。

ここで、ｘ _ｋはサンプリング時刻ｔ_ｋにおける直交座標の位置の真値とその時間微分の真値を表す状態ベクトルであり、式（３１）で表される。

またサンプリング時刻ｔ_ｋ（ｋ＝１，２，…，ｎ）は、位相差を検出するタイミングを表す離散時刻であり、以降、時刻ｔ_ｋと呼ぶ。Φ_ｋ−１は時刻ｔ_ｋ−１よりｔ_ｋへの状態ベクトルの推移行列であり、式（３２）で表される。

式（３０）において、ｗ_ｋは時刻ｔ_ｋにおける駆動雑音であり、Γ_１(ｋ)は時刻ｔ_ｋにおける駆動雑音の変換行列である。例えば、移動体の運動モデルを等速直線運動と仮定したことによる打ち切り誤差項をΓ_１(ｋ−１)ｗ_ｋ−１とみれば、ｗ_ｋは移動体の加速度ベクトルに相当するものである。また、Γ_１(ｋ−１)は式(３３)で表される。

なお、Ｔはサンプリング間隔、Ｉは３行３列の単位行列である。平均を表す記号としてＥを用いると、ｗ_ｋは平均０、共分散Ｑ_ｋの正規分布の白色雑音とする。

また、対象物１の観測モデルは式（３４）で表される。

ここで、ｕ_ｋは距離差の観測値、ｈ(ｘ _ｋ)は観測行列で、式（３５）で表される。

ただし、ν _ｋは時刻ｔ_ｋにおいて検出された距離差の観測雑音である。観測雑音は平均０、分散Ｒ_ｋの正規分布白色雑音に従うものと仮定する。また、時刻ｔ_ｋまでの間に移動体の追尾に用いた距離差の全体をＵ^ｋとする。

以上の運動モデルに基づき、拡張カルマンフィルタを用いて追尾フィルタを構成する。１サンプリング前の直交座標航跡の平滑ベクトルをｘ^_ｋ−１(＋)、平滑誤差共分散行列をＰ_ｋ−１(＋)とすると、時刻ｔ_ｋにおける移動体の状態ベクトルｘｋの予測ベクトルｘ^_ｋ(−)及び予測誤差共分散行列Ｐ_ｋ(−)はそれぞれ条件付平均ベクトル、条件付共分散行列で定義され、式（３６）及び式（３７）で表される。

次に、時刻ｔ_ｋに計測された複数の距離差Ｕ_ｋの選択方法について述べる。偽の距離差の含まれる複数の距離差から正しい距離差を判定するため、式（３８）で表される距離差の予測値ｕ _ｋ(−)を利用する。

過去の距離差情報Ｕ^ｋ-1より得られる距離差の観測値の予測確率分布Ｐ[ｕ_ｋ｜Ｕ^ｋ-1]は、式（３９）に示す条件付確率密度関数で表される。

すなわち、距離差の観測値は、式（３８）で与えられる距離差の予測値ｕ_ｋ(−)を平均とし、式（４０）で与えられるＳ_ｋを分散とする３次元正規分布ｇ(ｕ_ｋ；ｕ_ｋ(−)，Ｓ_ｋ)に従うのである。

そして、式（４１）を満たす距離差のうち、式(41)が最小となる距離差ｕⁿ _ｋを正しい距離差と判断し、選択する。

さらに、このようにして選択された距離差ｕⁿ _ｋを算出した第１位相差の前後の整数値バイアス値をとる第２及び第３位相差から算出される距離差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋについても直交座標を航跡を生成するためのデータとして選択するのである。なお、以後の説明においては、距離差ｕⁿ _ｋを第１距離差と呼び、距離差ｕ^n-1 _ｋを第２距離差、距離差ｕⁿ⁺¹ _ｋを第３距離差と呼ぶこととする。式（４１）において、ｄは目標との相関範囲を決めるパラメータであり、自由度３のχ自乗分布により算出される。

また、予測ベクトル値ｘ^_ｋ(−)は、式（４２）と式（４３）とを満たす。

また、予測ベクトル値ｘ^_ｋ(−)は、式（４２）と式（４３）とを満たす。 続いて、第１〜第３位相差に対応する直交座標の航跡における平滑ベクトルの算出方法について述べる。第１距離差のゲイン行列Ｋ_k、直交座標航跡の平滑ベクトルｘ^_ｋ(＋)及び平滑誤差共分散行列Ｐ_ｋ(＋)は拡張カルマンフィルタの理論により、式（４４）〜式（４６）で与えられる。

同様に、第２距離差ｕ^n-1 _ｋ及び第３距離差ｕⁿ⁺¹ _ｋをそれぞれ用いた直交座標航跡の平滑ベクトルをｘ^^a _ｋ(＋)及びｘ^^b _ｋ(＋)とすると、これらは式（４７）及び式（４８）で与えられる。

次に、直交座標航跡の位置または速度から、追尾が偽の位相差に乗り移った事象を判定し、直交座標航跡を軌道修正する方法を、ここでは速度を利用した場合を例にとって説明する。第１位相差の直交座標航跡の速度をＶⁿ _ｋ、第２及び第３位相差の直交座標航跡に対応する速度をそれぞれＶ^n-1 _ｋ及びＶⁿ⁺¹ _ｋとする。

その後、第２及び第３位相差についての直交座標航跡を削除して、次サンプル時刻からは新たな直交座標航跡を基準に、これまで同様、前後の整数値バイアスに対する距離差を用いて直交座標航跡を生成する。

以上の処理方法に基づいて、この発明の実施の形態３における追尾処理器２２−ｎと追尾処理器２２−ｎ＋１、さらには物理量算出器２３と航跡入れ替え器２４の具体的な動作を示すと次のようになる。

追尾処理器２２−１において、相関判定器２６は、位相差検出器２５から位相差を受け取ると、整数値バイアスを考慮した複数の距離差（偽の距離差を含む）に対して、相関範囲算出器３０で算出された距離差の観測値の予測確率分布の平均値ｕ_ｋ(−)と分散Ｓ_ｋ、予め設定されたパラメータｄとを用いて、式（４１）を満たす距離差のうち、ｆ_ｉが最小となる距離差ｕⁿ _ｋを正しい距離差と判断して選択する。また追尾処理器２２−２及び２２−３は、その位相差の前後の整数値バイアス値をとる位相差に対応する距離差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋを直交座標航跡を生成するためのデータとして選択する。

直交座標ゲイン算出器３７は、予測手段３１より入力した予測誤差共分散行列Ｐ_ｋ(−)と予め設定された受信器２１−１〜２１−Mの観測雑音の分散Ｒ_ｋとから、式（４４）に従い直交座標のゲイン行列Ｋ_ｋを算出する。平滑器２７は、直交座標ゲイン算出器３７から取得した直交座標のゲイン行列Ｋｋ、予測ベクトルｘ^_ｋ(−)、予測誤差共分散行列Ｐ_ｋ(−)と、相関判定器２６から取得した直交座標航跡に対応する距離差ｕⁿ _ｋと、距離差の予測値ｕ_ｋ(−)とを用いて、式（４５）〜（４６）に従い、直交座標航跡の平滑ベクトルｘ^_ｋ(＋)及び平滑誤差共分散行列Ｐ_ｋ(＋)を算出する。

同様に、追尾処理器２２−２及び２２−３においても、平滑器２７は、相関判定器２６から取得した直交座標航跡に対応する距離差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋ、距離差の予測値ｕ_ｋ(−)と、直交座標ゲイン算出器３７から取得した直交座標のゲイン行列Ｋ_ｋ、予測ベクトルｘ^_ｋ(−)とを用いて、式（４７）〜（４８）に従い、それぞれの直交座標の航跡の平滑ベクトルｘ^^a _ｋ(＋)及びｘ^^b _ｋ(＋)を算出する。

以上のようにして各位相差検出単位毎に選択された直交座標航跡に対応する距離差ｕⁿ _ｋ、ｕ^n-1 _ｋ、ｕⁿ⁺¹ _ｋは、物理量算出器２３の速度判定器３８に出力される。速度判定器３８は、予め設定された移動体の速度の上限Ｖmax、下限Ｖmin及びその誤差σとから、追尾処理器２２−１〜２２−３の平滑器２７から取得した各距離差に対応する速度Ｖⁿ _ｋ、Ｖ^n-1 _ｋ、Ｖⁿ⁺¹ _ｋが式（２１）及び式（２２）を満たすか否かを判定する。航跡入れ替え器２４は、速度判定器３８において、直交座標航跡の距離差ｕⁿ _ｋに対応する速度Ｖⁿ _ｋが式（２１）及び式（２２）を満たす場合は、直交座標航跡を現サンプリング時刻における移動体の状態とし、満たさない場合は、式式（２１）及び式（２２）を満たす直交座標裏航跡を現サンプリング時刻における移動体の状態とする。航跡入れ替え器２４は、速度判定器３８において直交座標裏航跡の距離差が正しい距離差であると判定された場合にのみ、式（２３）により直交座標航跡の平滑ベクトルｘ^_ｋ(＋)を直交座標裏航跡の平滑ベクトルに置き換えるとともに、これまで算出してきた直交座標裏航跡の平滑ベクトルを初期化する。

予測器２８は、航跡入れ替え器２４から取得した直交座標航跡の平滑ベクトルｘ^_ｋ(＋)、平滑誤差共分散行列Ｐ_ｋ(＋)と、予め設定された駆動雑音の誤差共分散行列Ｑ_ｋとから、式（３６）及び式（３７）に従い、次サンプリング時刻における直交座標の予測ベクトルｘ^_ｋ＋１(−)及び予測誤差共分散行列Ｐ_ｋ＋１(−)を算出する。相関範囲算出器３０は、予測器２８が算出した予測誤差共分散行列Ｐ_ｋ＋１(−)と、予め設定された受信機２１の観測雑音の分散Ｒ_ｋとから、式（４０）を用いて距離差の観測値の予測確率分布の分散Ｓ_ｋを算出する。

以上のように、この発明の実施の形態３の位置推定装置によれば、距離差に基づいて直交座標の航跡を維持し、直交座標の平滑値から直接的に対象物１の速度を算出することとした。この結果、実施の形態１の効果に加えて、実施の形態１及び２のように測位演算を別途行わなくてもよい、という効果が生じる。

また、これにより、追尾処理器２２−１〜２２−３における位相差検出器２５の処理が同期していなくてもよいとの利点がある。

なお、この発明の実施の形態３による位置推定装置は、航跡間の乗り移りの発生を判断基準として、対象物の速度が所定の値域（式（２１）及び式（２２）で示される）に含まれるかどうかを調べた。しかしながら実施の形態２で説明したように運動状態を表す物理量としては、速度に限定されないのであり、例えば位置、通過時間、加速度、角速度等を用いてもよいことはいうまでもない。

また、この発明の実施の形態３から、位相差の乗り移りを判定するために必要となる追尾処理は、位相差自体の追尾処理に限られるものではなく、位相差に基づく直交座標の追尾処理でもよく、また他の物理量に関する追尾処理であってもよいことが理解されるはずである。さらに追尾処理のアルゴリズムとしてはカルマンフィルタに限定されるものではなく、例えば上述したように拡張カルマンフィルタを用いてもよく、またα−βフィルタその他の外挿法を採用してもこの発明の特徴が失われることはないのである。

実施の形態４．
実施の形態１〜３で示した位置推定装置は、マルチパス環境下における位相差のアンビギュイティを解決するために、対象物の運動状態を表す物理量であって、位相差から導出可能な物理量の時間的な整合性、連続性に着目し、この物理量が所定の条件を満たす範囲で位相差を選択することを特徴としていた。

しかしこの原理は、位相差のアンビギュイティを解決するためだけでなく、方位角のアンビギュイティを解決する上でも役立つ。この発明の実施の形態４では、かかる特徴を有する位置推定装置を示す。

図９は、この発明の実施の形態４における位置推定装置の構成を示すブロック図である。図において、図１と同一の符号を付した構成要素については、実施の形態１と同様であるので説明を省略する。ただしこの位置推定装置２においては、受信器２１−１〜２１−Ｍはアレーアンテナを構成するアレー素子であるものとする。方位角算出器４０は、アレーアンテナ２１−１〜２１−Ｍから得られた受信信号から送信波を送出した対象物１の方位角を算出する回路又は素子である。また、図９における追尾処理器２２−１〜２２−３は方位角に関する追尾処理を行うように構成されている。

図１０は、位置推定装置２の追尾処理器２２−１の詳細な構成を示すブロック図である。図において、方位角ゲイン算出器４１は方位角のゲインを算出する回路又は素子である。他の部位については実施の形態１及び２と同様であるが、後述するように、この追尾処理器２２−１は対象物１の方位角を追尾するように構成されているので、処理に供されるデータが異なっている。

なお、追尾処理器２１−２及び２２−３の構成については、追尾処理器２１−１と同様であるので説明を省略する。またこの発明の実施４では、物理量算出器２３として実施の形態１と同様に図３に示す構成を有するものとする。

ここで、対象物１の方位角の運動モデルは式（４）によって示されるものとする。なお、ｘ _ｋはサンプリング時刻ｔ_ｋにおける方位角の真値θ_ｋとその時間微分の真値θ'_ｋを表す状態ベクトルであり、式（５）で表される。ここで、サンプリング時刻ｔ_ｋ（ｋ＝１，２，…，ｎ）は、方位角を検出するタイミングを表す離散時刻であり、以降、時刻ｔ_ｋと呼ぶ。また、Φ_ｋ−１は時刻ｔ_ｋ−１よりｔ_ｋへの状態ベクトルの推移行列であり、式（６）で表される。また、ｗ_ｋは時刻ｔ_ｋにおける駆動雑音であり、Γ_１(ｋ)は時刻ｔ_ｋにおける駆動雑音の変換行列である。例えば、位相差の運動モデルを等速直線運動と仮定したことによる打ち切り誤差項をΓ_１(ｋ−１)ｗ_ｋ−１とみれば、ｗ_ｋは位相差の加速度ベクトル相当であり、Γ_１(ｋ−１)は式（７）で表される。なお、Ｔはサンプリング間隔である。平均を表す記号としてＥを用いると、ｗ_ｋは平均０、分散Ｑ_ｋの正規分布の白色雑音とする。

方位角の観測モデルは式（８）で表される。ここで、ｕ_ｋは方位角の観測値、Ｈは観測行列で、式（９）で表される。ν _ｋは時刻ｔ_ｋにおいて検出された方位角の観測雑音である。観測雑音は平均０、分散Ｒ_ｋの正規分布白色雑音に従うものと仮定する。また、時刻ｔ_ｋまでの間に方位角の追尾に用いた方位角の全体をＵ^kとする。

以上の運動モデルに基づき、カルマンフィルタを用いた場合を例に追尾フィルタを構成する。１サンプリング前の方位角航跡の平滑ベクトルをｘ^_ｋ−１(＋)、平滑誤差共分散行列をＰ_ｋ−１(＋)とすると、時刻ｔ_ｋにおける方位角の状態ベクトルｘ _ｋの予測ベクトルｘ^_ｋ(−)及び予測誤差共分散行列Ｐ_ｋ(−)はそれぞれ条件付平均ベクトル、条件付共分散行列で定義され、式（１０）及び式（１１）で表される。
次に、時刻ｔ_ｋに計測された複数の方位角Ｕ_ｋの選択方法について述べる。偽の方位角の含まれる複数の方位角から正しい方位角を判定するため、式(１２)で表される方位角の予測値ｕ _ｋ(−)を利用する。

過去の方位角情報Ｕ^ｋ-1より得られる方位角の観測値の予測確率分布Ｐ[ｕ_ｋ｜Ｕ^ｋ-1]は、式（１３）に示す条件付確率密度関数で表される。すなわち、方位角の観測値は、式（１１）で与えられる方位角の予測値ｕ_ｋ(−)を平均とし、式（１４）で与えられるＳ_ｋを分散とする１次元正規分布ｇ(ｕ_ｋ；ｕ_ｋ(−)，Ｓ_ｋ)に従うものとする。そして、式（１５）を満たす方位角のうち、ｆ_ｉが最小となる方位角ｕⁿ _ｋを正しい方位角と判断し、選択する。また、選択された方位角の前後の整数値バイアス値に対応する方位角ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋを方位角裏航跡を生成するためのデータとする。なお、ｄは目標との相関範囲を決めるパラメータであり、自由度１のχ自乗分布により算出する。

次に、方位角航跡と方位角裏航跡に対応する平滑ベクトルの算出方法について述べる。ゲイン行列Ｋ_ｋ、方位角航跡の平滑ベクトルｘ^_ｋ(＋)及び平滑誤差共分散行列Ｐ_ｋ(＋)はカルマンフィルタの理論により、式（１６）〜式（１８）で与えられる。同様に、正しい方位角の前後の整数値バイアス値に対応する方位角ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋをそれぞれ用いた方位角裏航跡の平滑ベクトルをｘ^^a _ｋ(＋)及びｘ^^b _ｋ(＋)とすると、これらは式（１９）及び式（２０）で与えられる。

以上のように、方位角についても実施の形態１〜３で扱った位相差と全く同じ追尾処理を適用できることが分かる。そしてここで求めた方位角平滑値を複数組み合わせれば、対象物１の位置を推定できる。またその時間変位を算出すれば対象物１の速度を求めることもできる。

このことから各整数値バイアスをとる方位角について追尾処理を行い、その平滑値から対象物１の運動状態を示す物理量を算出して、式（２１）及び式（２２）、あるいは式（２７）〜式（２９）を最小にする方位角を算出することで、方位角のアンビギュイティを抑制し、対象物１の位置を精度よく推定することができるのである。

なお、アレーアンテナを用いて方位角を算出する方位角算出器４０の処理については公知の方位角算出方法を用いて構成すればよい。このような方位角の算出方法としては、例えば、隣接するビーム間の受信信号の振幅や位相などの差(Δ値)と和(Σ値)の比(Δ／Σ値)を求め、このΔ／Σ値から反射波の入射方向を求める方法などがある。

以上から明らかなように、位相差に替えて方位角から対象物の位置を測定する機器においても、この発明の原理を適用することが可能である。

以上のように、この発明に係る位置推定装置は、例えば、マルチパスによる干渉波が発生する環境下において対象物から送信される電波を受信して、その対象物の位置を推定するシステムに適用することが可能である。

この発明の実施の形態１による位置推定装置の構成を示すブロック図である。 この発明の実施の形態１による位置推定装置の詳細な構成を示すブロック図である。 この発明の実施の形態１による位置推定装置の詳細な構成を示す別のブロック図である。 整数バイアス値の異なる位相差の航跡が乗り移る様子を示す図である。 整数バイアス値の異なる位相差の航跡が乗り移る場合の対象物の速度変位を示す図である。 この発明の実施の形態２による位置推定装置の詳細な構成を示すブロック図である。 この発明の実施の形態３による位置推定装置の詳細な構成を示すブロック図である。 この発明の実施の形態３による位置推定装置の詳細な構成を示す別のブロック図である。 この発明の実施の形態４による位置推定装置の構成を示すブロック図である。 この発明の実施の形態４による位置推定装置の詳細な構成を示すブロック図である。

符号の説明

２２−１〜２２−３ 追尾処理器、
２３ 物理量演算器、
２４ 航跡入れ替え器。

Claims (8)

1. 対象物からの電波を複数の受信器により受信して得た受信波間の位相差が整数値バイアスの異なる複数の受信波位相差を含む場合に、これらの位相差に基づく追尾処理を行って上記対象物の位置推定に用いる平滑値を算出し、この平滑値から上記対象物の位置を推定する位置推定装置において、
   上記複数の受信波位相差から第１位相差を選択しこの第１位相差に基づく航跡を維持して平滑値を算出する第１追尾処理器と、
   上記第１位相差に隣接する第２位相差を選択してその第２位相差に基づく航跡を維持する第２追尾処理器と、
   上記第１追尾処理器が算出した平滑値から位相差とは異なる物理量であって上記対象物の運動状態を表す物理量を算出する物理量算出器と、
   上記物理量算出器により算出された物理量の連続性に基づいて上記第１位相差に基づく航跡と上記第２位相差に基づく航跡とを入れ替える航跡入れ替え器と、
   を備えたことを特徴とする位置推定装置。
2. 上記物理量算出器は、上記平滑値から上記対象物の位置を推定するとともに、この位置の時間変位から上記対象物の速度を算出し、
   上記航跡入れ替え器は、上記物理量算出器により算出された速度が所定の値域外にある場合に上記第１位相差に基づく航跡と上記第２位相差に基づく航跡とを入れ替えることを特徴とする請求項１に記載の位置推定装置。
3. 上記物理量算出器は、上記平滑値から上記対象物の位置を推定するとともに、この位置の時間変位から上記対象物が基準地点を通過する予測時刻を算出し、
   上記航跡入れ替え器は、上記物理量算出器により算出された予測時刻が所定の地域外にある場合に上記第１位相差に基づく航跡と上記第２位相差に基づく航跡とを入れ替えることを特徴とする請求項１に記載の位置推定装置。
4. 上記第１及び第２追尾処理器は、上記第１及び第２位相差に基づく航跡としてそれらの位相差に基づいて算出される上記対象物の直交座標の航跡をそれぞれ維持することを特徴とする請求項１に記載の位置推定装置。
5. 対象物からの電波を複数の受信器により受信して得た受信波から整数値バイアスの異なる複数の方位角が得られる場合に、これらの方位角に基づく追尾処理を行って上記対象物の位置推定に用いる平滑値を算出し、この平滑値から上記対象物の位置を推定する位置推定装置において、
   上記複数の方位角から第１方位角を選択しこの第１方位角に基づく航跡を維持して上記平滑値を算出する第１追尾処理器と、
   上記第１方位角に隣接する第２方位角を選択してその第２方位角に基づく航跡を維持する第２追尾処理器と、
   上記平滑値から方位角とは異なる物理量であって上記対象物の運動状態を表す物理量を算出する物理量算出器と、
   上記物理量算出器により算出された物理量の連続性に基づいて上記第１方位角に基づく航跡と上記第２方位角に基づく航跡とを入れ替える航跡入れ替え器と、
   を備えたことを特徴とする位置推定装置。
6. 上記物理量算出器は、上記平滑値から上記対象物の位置を推定するとともに、この位置の時間変位から上記対象物の速度を算出し、
   上記航跡入れ替え器は、上記物理量算出器により算出された速度が所定の値域外にある場合に上記第１方位角に基づく航跡と上記第２方位角に基づく航跡とを入れ替えることを特徴とする請求項５に記載の位置推定装置。
7. 上記物理量算出器は、上記平滑値から上記対象物の位置を推定するとともに、この位置の時間変位から上記対象物が基準地点を通過する予測時刻を算出し、
   上記航跡入れ替え器は、上記物理量算出器により算出された予測時刻が所定の地域外にある場合に上記第１方位角に基づく航跡と上記第２方位角に基づく航跡とを入れ替えることを特徴とする請求項５に記載の位置推定装置。
8. 上記第１及び第２追尾処理器は、上記第１及び第２方位角に基づく航跡としてそれらの方位角に基づいて算出される上記対象物の直交座標の航跡をそれぞれ維持することを特徴とする請求項５に記載の位置推定装置。

Images