JP2005308428A - Position estimation system - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To accurately estimate the position of a moving body by receiving a wave that the moving body transmits under an environment affected by a multi-path. <P>SOLUTION: The system comprises tracking processors 22-1 and 22-2 for selecting a first and a second phase difference from a plurality of phase differences of reception waves, maintaining the trace based on them and calculating a smoothed value for selecting a true phase difference by a tracking process from a plurality of reception wave phase differences with difference integer bias in the phase difference between reception waves obtained by receiving the wave from the object with a plurality of receivers, and estimating the position of the object. The system also comprises a physical quantity calculator 23 calculating physical quantity which is different from the phase difference and indicating motion state of the object from the smoothed value calculated by the tracking processors 22-1 and 22-2, and a trace exchanger 24 for exchanging traces based on the first and the second phase differences on the basis of continuity of the physical quantity calculated by the physical quantity calculator 23. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

この発明は、電波を用いて対象物の位置を推定する装置に係るものであり、特に対象物が発する電波を複数の受信器で受信し、これら受信器間の受信波位相差を用いて対象物の位置を推定する技術に関する。   The present invention relates to an apparatus for estimating the position of an object using radio waves, and in particular, receives radio waves emitted from an object by a plurality of receivers, and uses the received wave phase difference between these receivers as a target. The present invention relates to a technique for estimating the position of an object.

電波を用いて対象物の位置を計測する従来の方法としては、対象物から発信される電波を複数の受信器で受信し、これらの受信器の位置と受信器間の位相差、対象物の位置が満たす関係から対象物の位置を算出する、という方法がある（例えば、特許文献１）。   As a conventional method of measuring the position of an object using radio waves, radio waves transmitted from the object are received by a plurality of receivers, the phase difference between these receivers and the receiver, There is a method of calculating the position of an object from the relationship that the position satisfies (for example, Patent Document 1).

ここで、位相差はラジアン値で表されるので、２πのアンビギュイティが起こりうる。すなわち正しい位相差をΔφとした場合に、Δφ＋２π×ｎ（ｎは整数）となる位相差も同時に算出されうる。このように複数の位相差が算出される中で、対象物の位置に対応する位相差は一つだけである。複数の位相差から正しい位相差を見つけるために追尾技術を用いる方法が提案されている（例えば、非特許文献１）。   Here, since the phase difference is expressed in radians, an ambiguity of 2π can occur. That is, when the correct phase difference is Δφ, a phase difference of Δφ + 2π × n (n is an integer) can be calculated at the same time. Thus, among the plurality of phase differences calculated, there is only one phase difference corresponding to the position of the object. A method using a tracking technique for finding a correct phase difference from a plurality of phase differences has been proposed (for example, Non-Patent Document 1).

追尾技術は、過去に得られた値から予測値と平滑値とを外装法によって求め、現実の観測値との相関を評価して観測値の取捨選択を行う技術である。このような追尾技術によれば、各時刻（サンプル時刻と呼ぶ）において、複数の位相差を採りうる場合に、対象物に対応する最尤の位相差を選択することができる。   The tracking technique is a technique that obtains a predicted value and a smooth value from values obtained in the past by an exterior method, evaluates a correlation with an actual observation value, and selects an observation value. According to such a tracking technique, when a plurality of phase differences can be taken at each time (referred to as sample time), the maximum likelihood phase difference corresponding to the object can be selected.

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

しかし、従来の追尾技術による位相差の選択方法は、受信器が対象物により送信される電波を直接波として受信する理想的な電波伝播環境を前提にしている。したがって、屋内や、屋外であっても壁面に囲まれた通路がある場合、位置測定の対象となる対象物が移動体である場合にこの対象物以外に併走している物体がある場合、そして地面や海面などによる乱反射によってマルチパスが発生する状況においては、上述のような選択方法を用いることは困難である。より具体的にいえば、マルチパス波の干渉によって位相差に誤差が生じ、正しい位相差と適切な相関が得られない状況が起こって、本来の位相差とは異なる位相差を選択してしまうおそれが強い。このような場合、対象物の位置に対応する位相差でない位相差に基づいて対象物の位置を計算することとなるため、位置の推定精度は著しく劣化することとなる。   However, the phase difference selection method according to the conventional tracking technique assumes an ideal radio wave propagation environment in which a receiver receives a radio wave transmitted by an object as a direct wave. Therefore, if there is a passage surrounded by walls even indoors or outdoors, if the target object for position measurement is a moving object, there is an object running alongside this target object, and In a situation where multipath occurs due to irregular reflection by the ground or the sea surface, it is difficult to use the selection method as described above. More specifically, an error occurs in the phase difference due to the interference of the multipath wave, and a situation in which a correct phase difference and an appropriate correlation cannot be obtained occurs, and a phase difference different from the original phase difference is selected. There is a strong fear. In such a case, since the position of the object is calculated based on a phase difference that is not a phase difference corresponding to the position of the object, the position estimation accuracy is significantly deteriorated.

この発明はこのような課題を解決するためになされたもので、マルチパス波等の干渉が生じる状況においても、その影響を軽減し、精度よく対象物の位置を推定することを目的とする。   The present invention has been made to solve such a problem, and an object of the present invention is to reduce the influence and accurately estimate the position of an object even in a situation where interference such as a multipath wave occurs.

対象物からの電波を複数の受信器により受信して得た受信波間の位相差が整数値バイアスの異なる複数の受信波位相差を含む場合に、これらの位相差に基づく追尾処理を行って上記対象物の位置推定に用いる平滑値を算出し、この平滑値から上記対象物の位置を推定する位置推定装置において、
上記複数の受信波位相差から第１位相差を選択しこの第１位相差に基づく航跡を維持して平滑値を算出する第１追尾処理器と、
上記第１位相差に隣接する第２位相差を選択してその第２位相差に基づく航跡を維持する第２追尾処理器と、
上記第１追尾処理器が算出した平滑値から位相差とは異なる物理量であって上記対象物の運動状態を表す物理量を算出する物理量算出器と、
上記物理量算出器により算出された物理量の連続性に基づいて上記第１位相差に基づく航跡と上記第２位相差に基づく航跡とを入れ替える航跡入れ替え器と、
を備えたものである。 When the phase difference between received waves obtained by receiving radio waves from an object by a plurality of receivers includes a plurality of received wave phase differences having different integer value biases, tracking processing based on these phase differences is performed and In a position estimation device that calculates a smooth value used for position estimation of an object and estimates the position of the object from the smooth value,
A first tracking processor that selects a first phase difference from the plurality of received wave phase differences and calculates a smooth value while maintaining a wake based on the first phase difference;
A second tracking processor for selecting a second phase difference adjacent to the first phase difference and maintaining a track based on the second phase difference;
A physical quantity calculator that calculates a physical quantity that is different from a phase difference and represents a motion state of the object from the smooth value calculated by the first tracking processor;
A wake changer that replaces the wake based on the first phase difference and the wake based on the second phase difference based on the continuity of the physical quantity calculated by the physical quantity calculator;
It is equipped with.

ここで、追尾処理とは一般に、所定のサンプリング間隔で得られる観測値と前回サンプル時以前（前回を含む）の観測値及び平滑値から得られた今回サンプル用の予測値との相関を評価し、相関が認められた観測値に基づいて今回サンプル時の平滑値と次回サンプル時の予測値を算出する演算をサンプル毎に繰り返す処理をいう。そして、追尾処理においては、これまでのサンプル時に得られた観測値、予測値と平滑値は次回以降の予測値及び平滑値の算出に影響を及ぼすことが多いので、このような観測値、予測値と平滑値の時系列を、一般に航跡と呼んでいる。   Here, the tracking process generally evaluates the correlation between the observed value obtained at a predetermined sampling interval, the observed value before the previous sample (including the previous time), and the predicted value for the current sample obtained from the smooth value. This is a process for repeating the calculation for calculating the smooth value at the current sample and the predicted value at the next sample on the basis of the observed value for which the correlation is recognized. In the tracking process, the observed value, predicted value and smooth value obtained at the time of the previous sampling often affect the calculation of the predicted value and smooth value from the next time onward. A time series of values and smooth values is generally called a wake.

また上述の位置推定装置において、位相差とは異なる物理量であって対象物の運動状態を表す物理量とは、例えば時間、位置、速度、角速度、加速度及び角加速度など、対象物の運動状態を表現する上で用いることのできる何れかの状態変数を指す。   In the position estimation device described above, the physical quantity that is different from the phase difference and represents the motion state of the object represents the motion state of the object such as time, position, speed, angular velocity, acceleration, and angular acceleration. Any state variable that can be used in

このように、この発明に係る位置推定装置によれば、位相差にアンビギュイティが生じる状況において、位相差に基づく追尾処理のみでは対象物の位置に対応しない誤った位相差を選択してしまう状況であっても、位相差とは異なる対象物の物理量を算出しその物理量の整合性を評価することで、誤った位相差を選択することを抑制し、精度の高い位置推定処理を実現することができるのである。   As described above, according to the position estimation device according to the present invention, in a situation where ambiguity occurs in the phase difference, an erroneous phase difference that does not correspond to the position of the object is selected only by the tracking process based on the phase difference. Even in the situation, calculating the physical quantity of the object different from the phase difference and evaluating the consistency of the physical quantity suppresses selection of the wrong phase difference and realizes highly accurate position estimation processing. It can be done.

さらにこの発明に係る位置推定装置においては、位相差にアンビギュイティがあることに基づいて上述のような構成としている。しかしながら、方位角を算出することのできるアレーアンテナを用いて構成した場合においては、位相差に替えて方位角のアンビギュイティに対しても同一の技術的思想を適用することができるのである。すなわち、かかる位置推定装置は、対象物からの電波を複数の受信器により受信して得た受信波から整数値バイアスの異なる複数の方位角が得られる場合に、これらの方位角に基づく追尾処理を行って上記対象物の位置推定に用いる主方位角を算出し、この主方位角から上記対象物の位置を推定する位置推定装置において、
上記複数の方位角から第１方位角を選択しこの第１方位角に基づく航跡を維持して上記主方位角を算出する第１追尾処理器と、
上記第１方位角に隣接する第２方位角を選択してこの第２方位角に基づく航跡を維持する第２追尾処理器と、
上記主方位角から方位角とは異なる物理量であって上記対象物の運動状態を表す物理量を算出する物理量算出器と、
上記物理量算出器により算出された物理量の連続性に基づいて上記第１方位角に基づく航跡と上記第２方位角に基づく航跡とを入れ替える航跡入れ替え器と、
を備えたもの、とすることもできる。 Furthermore, the position estimation apparatus according to the present invention has the above-described configuration based on the ambiguity in the phase difference. However, in the case of using an array antenna that can calculate the azimuth angle, the same technical idea can be applied to the ambiguity of the azimuth angle instead of the phase difference. That is, such a position estimation device, when a plurality of azimuth angles with different integer value biases are obtained from received waves obtained by receiving radio waves from an object by a plurality of receivers, tracking processing based on these azimuth angles In the position estimation device that calculates the main azimuth angle used for position estimation of the object by performing and estimates the position of the object from the main azimuth angle,
A first tracking processor for selecting the first azimuth angle from the plurality of azimuth angles and maintaining the track based on the first azimuth angle to calculate the main azimuth angle;
A second tracking processor for selecting a second azimuth angle adjacent to the first azimuth angle and maintaining a track based on the second azimuth angle;
A physical quantity calculator that calculates a physical quantity that is different from the main azimuth angle and that represents the motion state of the object;
A wake changer that replaces the wake based on the first azimuth and the wake based on the second azimuth based on the continuity of the physical quantity calculated by the physical quantity calculator;
Can also be provided.

このように、この発明に係る位置推定装置によれば、方位角にアンビギュイティが生じる状況において、方位角に基づく追尾処理のみでは対象物の位置に対応しない誤った方位角を選択してしまう状況であっても、方位角とは異なる対象物の物理量を算出しその物理量の整合性を評価することで、誤った方位角を選択することを抑制し、精度の高い位置推定処理を実現することができるのである。   As described above, according to the position estimation device according to the present invention, in a situation where ambiguity occurs in the azimuth angle, an incorrect azimuth angle that does not correspond to the position of the object is selected only by the tracking process based on the azimuth angle. Even in the situation, by calculating the physical quantity of the object that is different from the azimuth angle and evaluating the consistency of the physical quantity, it is possible to suppress the selection of the wrong azimuth angle and realize highly accurate position estimation processing. It can be done.

以下、この発明の実施の形態を図を用いて説明する。
実施の形態１．
図１は、この発明の実施の形態１による位置推定装置の構成を示すブロック図であある。図において、対象物１には電波送信機が装着されている。位置推定装置２はこの発明の対象となる位置推定装置であって、Ｍ個の受信器２１−１〜Ｍ、追尾処理器２２−１〜３、物理量算出器２３、航跡入れ替え器２４を備えている。 Embodiments of the present invention will be described below with reference to the drawings.
Embodiment 1 FIG.
FIG. 1 is a block diagram showing a configuration of a position estimation apparatus according to Embodiment 1 of the present invention. In the drawing, a radio wave transmitter is attached to the object 1. The position estimation apparatus 2 is a position estimation apparatus that is an object of the present invention, and includes M receivers 21-1 to 21 -M, tracking processors 2-1 to 2, a physical quantity calculator 23, and a wake changer 24. Yes.

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

このようにすることで、対象物１が移動しても、一定のサンプリング間隔ごとに対象物１の位置が算出されるのである。なお、式（１）において、ｌ_ｉｊは位相差Δφ_ｉｊのアンビギュイティを表す整数バイアス値である。すなわち、位相差は角度値として表されるため、角度値としてはｌ_ｉｊによらずΔφ_ｉｊ＋２πｌ_ｉｊは同じ角を表すこととなる。ところが対象物１の位置は１つしかないので、式（１）によれば、対象物１の位置に対応するｌ_ｉｊはたった一つしか存在しないはずである。このことから、誤ったｌ_ｉｊを選択して式（１）に代入してしまうと、誤った位置を算出する結果を招くこととなる。 The receiver 21-i and the receiver 21-j are two receivers selected from M receivers 21-1 to 21-M (M is a natural number of 2 or more) (i, j = 1). , 2,..., M), for example, using a receiving antenna, a local transmitter, a down converter, an A / D converter, etc. that receive a radio wave transmitted by a radio wave transmitter mounted on the object 1, and finally The received radio signal is output as a digital signal at a predetermined sampling interval. Here, the phase difference Δφ _ij between the digital signals r _i and r _j output by the receivers 21-i and 21-j, the wavelength of the radio signal transmitted from the object 1, and the unknown position of the object 1 If p is p = (x yz) and the positions of the receivers 21-i and 21-j are (x _i y _i z _i ) and (x _j y _j z _j ), then the receiver 21-i 21-j, the position p of the object 1, and the phase difference Δφ _ij satisfy Expression (1).

By doing in this way, even if the target object 1 moves, the position of the target object 1 is calculated for every fixed sampling interval. In equation (1), l _ij is an integer bias value representing the ambiguity of the phase difference Δφ _ij . That is, since the phase difference is expressed as an angle value, Δφ _ij + 2πl _ij represents the same angle regardless of l _ij as the angle value. However, since there is only one position of the target object 1, according to the formula (1), there should be only one l _ij corresponding to the position of the target object 1. For this reason, if an incorrect l _ij is selected and substituted into the expression (1), a result of calculating an incorrect position is caused.

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

As a method for solving this problem, the position estimation apparatus according to the first embodiment of the present invention selects a correct phase difference from a plurality of phase differences by tracking processing. Now, a set of a plurality of phase differences obtained as the phase difference between the received signals of the receivers 21-i and 21-j is represented by U _i represented by Expression (2).

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

However, the relationship of Expression (3) is established between the elements of the set U _i .

追尾処理器２２−１〜３は位相差をそれぞれ追尾処理する回路又は素子である。ここで追尾処理器２２−１は追尾処理によって正しい位相差であると推定された位相差（第１位相差）の航跡を維持する追尾処理回路または素子であって、最終的に位相差を平滑した値である位相差平滑値を算出するようになっている。また追尾処理器２２−２及び２２−３は追尾処理器２２−１が選択する第１位相差に隣接する第２位相差の航跡を維持する追尾処理回路または素子である。   The tracking processors 22-1 to 2-3 are circuits or elements that respectively track the phase difference. Here, the tracking processor 22-1 is a tracking processing circuit or element that maintains the track of the phase difference (first phase difference) estimated to be the correct phase difference by the tracking process, and finally smoothes the phase difference. The phase difference smoothing value which is the calculated value is calculated. The tracking processors 22-2 and 22-3 are tracking processing circuits or elements that maintain the track of the second phase difference adjacent to the first phase difference selected by the tracking processor 22-1.

図２は、追尾処理器２２−１を、例えばカルマンフィルタによる追尾処理を行うものとして構成した場合の詳細な構成を示したブロック図である。図に示すように、追尾処理器２２−１は位相差検出器２５、相関判定器２６、平滑器２７、相関予測器２８、位相差ゲイン算出器２９、範囲算出器３０を備えている。   FIG. 2 is a block diagram showing a detailed configuration when the tracking processor 22-1 is configured to perform a tracking process using, for example, a Kalman filter. As shown in the figure, the tracking processor 22-1 includes a phase difference detector 25, a correlation determiner 26, a smoother 27, a correlation predictor 28, a phase difference gain calculator 29, and a range calculator 30.

このうち、位相差検出器２５は、受信器２１−ｉ、２１−ｊの出力するデジタル信号から位相差を検出する回路または素子である。また相関判定器２６は、位相差検出器２５が検出した位相差が後述する相関範囲算出器３０によって設定される相関範囲に含まれるか否かを判定し、含まれると判断した場合は新たな観測値として追尾処理に採用し、含まれないと判断した場合はこの位相差を棄却して、代わりに予測器２８が算出した予測値を出力する回路又は素子である。平滑器２７は相関判定器２６が出力するサンプル毎の位相差又は予測値に基づいてそのサンプルに対する位相差平滑値を算出する回路又は素子である。ここで算出される位相差平滑値は、追尾処理器２２−ｎの出力値として外部に供給される。予測器２８は、前回サンプル時の位相差平滑値に基づいて今回サンプルにおいて観測される位相差の予測値を算出する回路又は素子である。ただし前回サンプル時の位相差平滑値を直接の入力値とするのではなく、追尾処理器２２−ｎの外部にある航跡入れ替え器２４（後述）から取得した航跡の位相差平滑値を用いるようになっている。位相差ゲイン算出器２９は、平滑器２７が位相差平滑値の算出に用いる位相差ゲインを算出する回路又は素子である。相関範囲算出器３０は、予測器２８が算出した予測値から許容される今回サンプルの位相差の観測値の相関範囲を算出する回路又は素子である。   Among these, the phase difference detector 25 is a circuit or element that detects a phase difference from digital signals output from the receivers 21-i and 21-j. The correlation determiner 26 determines whether or not the phase difference detected by the phase difference detector 25 is included in a correlation range set by a correlation range calculator 30 described later. This is a circuit or element that is employed in the tracking process as an observed value and rejects this phase difference when it is determined not to be included, and outputs the predicted value calculated by the predictor 28 instead. The smoother 27 is a circuit or element that calculates a phase difference smooth value for a sample based on the phase difference or predicted value for each sample output from the correlation determiner 26. The phase difference smooth value calculated here is supplied to the outside as an output value of the tracking processor 22-n. The predictor 28 is a circuit or element that calculates a predicted value of the phase difference observed in the current sample based on the phase difference smoothed value at the previous sample. However, instead of using the phase difference smoothing value at the time of the previous sample as a direct input value, the wake phase difference smoothing value acquired from the wake changer 24 (described later) outside the tracking processor 22-n is used. It has become. The phase difference gain calculator 29 is a circuit or element that calculates a phase difference gain that the smoother 27 uses to calculate a phase difference smooth value. The correlation range calculator 30 is a circuit or element that calculates the correlation range of the observed value of the phase difference of the current sample that is allowed from the predicted value calculated by the predictor 28.

なお追尾処理器２２−２及び２２−３も追尾処理器２２−１と同等の構成要素を備えているのでその構成については特に示さないこととする。   Since the tracking processors 22-2 and 22-3 also include the same components as the tracking processor 22-1, the configuration thereof is not particularly shown.

引き続き、図１に示した位置推定装置２の構成について説明する。物理量算出器２３は、追尾処理器２２−１〜３が算出した各位相差平滑値に基づいて、対象物１に関する物理量を算出する回路又は素子である。ただしここで算出される物理量とは位相差とは異なる物理量であって、対象物１の運動状態を示す状態量である。   Next, the configuration of the position estimation device 2 shown in FIG. 1 will be described. The physical quantity calculator 23 is a circuit or element that calculates a physical quantity related to the object 1 based on each phase difference smoothed value calculated by the tracking processors 22-1 to 23-1. However, the physical quantity calculated here is a physical quantity different from the phase difference, and is a state quantity indicating the motion state of the object 1.

図３は、物理量算出器２３の例として対象物１の速度を算出する構成とした場合の詳細な構成を示すブロック図である。図において、測位演算器３１は追尾処理器２２−１〜３が算出した位相差平滑値に基づいて対象物１の位置を算出する回路又は素子である。なおこの発明の実施の形態１による位置推定装置２の出力結果である対象物の位置は、この部位の演算結果として求められる。速度算出器３２は、測位演算器３１が算出した対象物１の位置の時間変位から速度を算出する回路又は素子である。速度判定器３３は、速度算出器３２によって算出された対象物１の速度が所定の値域に含まれるかどうかを判断する部位である。   FIG. 3 is a block diagram showing a detailed configuration when the physical quantity calculator 23 is configured to calculate the speed of the object 1 as an example. In the figure, a positioning calculator 31 is a circuit or element that calculates the position of the object 1 based on the phase difference smoothing value calculated by the tracking processors 22-1 to 22-1. Note that the position of the object, which is the output result of the position estimation apparatus 2 according to the first embodiment of the present invention, is obtained as the calculation result of this part. The speed calculator 32 is a circuit or element that calculates a speed from the time displacement of the position of the object 1 calculated by the positioning calculator 31. The speed determiner 33 is a part that determines whether or not the speed of the object 1 calculated by the speed calculator 32 is included in a predetermined value range.

航跡入れ替え器２４は、追尾処理器２２−１〜３が使用する予測値を調整する回路又は素子である。すなわち、これらの予測値を調整することによって追尾処理器２２−１〜３の航跡の入れ替えを行うようになっている。この部位は、前回サンプルまでに算出された予測値を１サンプルの間記憶しておき、さらに物理量算出器２３の速度判定器３３の判定結果に基づいて追尾処理器２２−１〜３に従来の航跡をそのまま指定したり、入れ替えて指定する。   The wake changer 24 is a circuit or an element that adjusts the predicted value used by the tracking processors 22-1 to 22-1. That is, the wakes of the tracking processors 22-1 to 2-3 are changed by adjusting these predicted values. This part stores the predicted value calculated up to the previous sample for one sample, and further adds the conventional values to the tracking processors 22-1 to 2-3 based on the determination result of the speed determiner 33 of the physical quantity calculator 23. Specify the track as it is or replace it.

次に、位置推定装置２の動作について説明する。まず初めに位置推定装置２の動作原理について説明し、続いて具体的な動作の説明を行うこととする。対象物１から発信される電波は、位置推定装置２の受信器２１−１〜２１−Ｍによって受信され、検波処理を経てＡ／Ｄ変換されてデジタル信号として出力される。デジタル信号はサンプル毎に出力されるので、このデジタル信号から位相差を検出し、追尾処理に供するのである。ここで、ある受信器２１−ｉと２１−ｊに着目し、この受信器２１−ｉおよび２１−ｊの受信デジタル信号ｒ_ｉとｒ_ｊの位相差をΔΦ_ｉｊとし、この位相差の追尾処理について説明する。 Next, the operation of the position estimation device 2 will be described. First, the operation principle of the position estimation apparatus 2 will be described, and then a specific operation will be described. The radio wave transmitted from the object 1 is received by the receivers 21-1 to 21-M of the position estimation device 2, is subjected to A / D conversion through a detection process, and is output as a digital signal. Since the digital signal is output for each sample, the phase difference is detected from the digital signal and used for the tracking process. Here, paying attention to a certain receiver 21-i and 21-j, the phase difference between the received digital signals r _i and r _j of the receivers 21-i and 21-j is set to ΔΦ _ij, and this phase difference tracking process Will be described.

図４は、追尾処理に基づいて複数の位相差から対象物１の位置に対応する位相差を選択する状況を説明する図である。図において、＊（アスタリスク）は対象物１の位置に対応する本来の位相差を表す点である。また×で表される点は、本来の位相差と整数バイアス値分だけ異なる位相差である。さらに矩形で囲まれた位相差は、追尾処理に基づいて算出した位相差平滑値を表している。追尾処理によれば、一時的に対象物１の位置に対応する本来の位相差を選択しうるが、常に本来の位相差を選択することは保証されない。つまり、例えば位相差として、前回の位相差とは近い位相差を選択するため、本来の位相差ではなく、実は誤った位相差を選択してしまうことが生じうる。図４の例で言えば、区間１０１において本来の位相差と離れて隣接する整数値バイアスを有する位相差に航跡が遷ってしまう。このような結果は、位相差のみ相関処理で位相差を選択していたことに起因するものである。   FIG. 4 is a diagram illustrating a situation in which a phase difference corresponding to the position of the object 1 is selected from a plurality of phase differences based on the tracking process. In the figure, * (asterisk) is a point representing the original phase difference corresponding to the position of the object 1. A point represented by x is a phase difference that differs from the original phase difference by an integer bias value. Further, the phase difference surrounded by a rectangle represents the phase difference smoothing value calculated based on the tracking process. According to the tracking process, the original phase difference corresponding to the position of the object 1 can be temporarily selected, but it is not guaranteed that the original phase difference is always selected. That is, for example, since a phase difference close to the previous phase difference is selected as the phase difference, it is possible to actually select an incorrect phase difference instead of the original phase difference. In the example of FIG. 4, the wake transitions to a phase difference having an integer value bias adjacent to and away from the original phase difference in the section 101. Such a result is due to the fact that only the phase difference is selected in the correlation process.

そこで、位相差のみの相関処理ではなく、位相差から得られた対象物１の運動状態、例えば速度の相関を考慮して位相差を選択するようにする。図５は、図４の位相差平滑値から算出される対象物１の速度の時間変位を表すグラフである。図が示すように航跡が誤った位相差に乗り移る時間帯１０１において、速度が大きく変化している。仮に対象物１の速度が取りうる値の範囲を定めることができる場合、選択した位相差から算出した速度がこの範囲を逸脱するような場合には、不適切な位相差を選択したことが判断できるのである。   Therefore, the phase difference is selected in consideration of the motion state of the object 1 obtained from the phase difference, for example, the correlation of the speed, instead of the correlation process using only the phase difference. FIG. 5 is a graph showing the temporal displacement of the speed of the object 1 calculated from the phase difference smooth value in FIG. As shown in the figure, the speed changes greatly in the time zone 101 in which the wake changes to the wrong phase difference. If it is possible to determine a range of values that can be taken by the speed of the object 1, if the speed calculated from the selected phase difference deviates from this range, it is determined that an inappropriate phase difference has been selected. It can be done.

例えば、対象物１が競走馬の場合、速度の上限値はおよそ時速６０ｋｍ程度であり、さらに下限値はおよそ時速４０ｋｍ程度であることが知られている。したがって例えば対象物１の速度が時速４０ｋｍ〜６０ｋｍを逸脱するような位相差は取り得ないはずであり、そのような条件に基づく位相差の相関処理を行うことで、適切な位相差を選択する可能性を大きく向上し、結果として位置の推測精度を高めることにつながるのである。   For example, when the object 1 is a racehorse, it is known that the upper limit value of the speed is about 60 km / h, and the lower limit value is about 40 km / h. Therefore, for example, a phase difference in which the speed of the target 1 deviates from 40 km to 60 km per hour should not be obtained, and an appropriate phase difference can be selected by performing a phase difference correlation process based on such conditions. This greatly improves the performance and as a result, increases the position estimation accuracy.

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

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

また、サンプリング時刻ｔ_ｋ（ｋ＝１，２，…，ｎ）は、位相差を検出するタイミングを表す離散時刻であり、以降、時刻ｔ_ｋと呼ぶ。 The above principle will be described in detail. The tracking processor 22-n performs a tracking process of the phase difference, and the motion model of the tracking process here is assumed to be expressed by Expression (4).

Here, x _k is the state vector representing the true value θ _{'k (θ'} true values theta _k and its time derivative of the phase difference _k is a differential one with time of theta _k) at the sampling time t _k, the formula It is represented by (5).

The sampling time _{t k (k = 1,2, ...} , n) is the discrete time representing a timing for detecting a phase difference, hereinafter referred to as time _{t k.}

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

In equation (4), Φ _k−1 is a transition matrix of the state vector from time t _k−1 to t _k and is represented by equation (6).

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

w _k is the drive noise at time t _k , and Γ ₁ (k) is the drive noise conversion matrix at time t _k . For example, if the truncation error term due to the assumption that the motion model of phase difference is constant linear motion is Γ ₁ (k−1) w _k−1 , w _k is a term corresponding to the acceleration vector of phase difference. Γ ₁ (k−1) is expressed by the following equation (7). T is a sampling interval. When E is used as a symbol representing an average, w _k is a white noise having a normal distribution with an average of 0 and a variance Q _k .

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

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

なお、式（８）において、ν _ｋは時刻ｔ_ｋにおいて検出された位相差の観測雑音である。観測雑音は平均０、分散Ｒ_ｋの正規分布白色雑音に従うものと仮定する。また、時刻ｔ_ｋまでの間に位相差の追尾に用いた位相差の全体をＵ^ｋとする。 On the other hand, it is assumed that the observation model of phase difference is expressed by Expression (8).

Here, u _k is the observed value of the phase difference, H is the observation matrix, represented by the formula (9).

In Expression (8), the [nu _k is the observation noise of the detected phase difference at time t _k. The average measurement noise is zero, it is assumed to follow a normal distribution white noise variance R _k. Further, the entire phase difference used for tracking the phase difference up to time t _k is U ^k .

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

Based on the above motion model, the tracking filter is configured using the Kalman filter as an example. The smooth vector of the phase difference wake before one sampling is _represented by x ^ _k-1 (+) (x ^ represents a variable with ^ attached to the letter x in the formula), and the smoothing error covariance matrix is represented by _{Pk. −1} (+), the prediction vector x ^ (−) and the prediction error covariance matrix P _k (−) of the phase difference state vector x _k at time tk are a conditional mean vector and a conditional covariance matrix, respectively. It is defined and represented by Formula (10) and Formula (11).

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

Next, a method for selecting a phase difference corresponding to the position of the object 1 from a plurality of phase differences U _k measured at time t _k will be described. In order to determine a correct phase difference from a plurality of phase differences including a false phase difference, a predicted value u _k (−) of the phase difference expressed by Expression (12) is used.

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

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

The predicted probability distribution P [u _k | U ^k−1 ] of the observed value of the phase difference obtained from the past phase difference information U ^k−1 is represented by a conditional probability density function shown in Expression (13).

That is, the observed value of the phase difference is a one-dimensional normal distribution g (u) in which the predicted value u _k (−) of the phase difference given by Expression (11) is averaged and S _k given by Expression (14) is variance. _k ; u _k (−), S _k ).

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

Here, a phase difference satisfying Expression (15) is considered.

そして、式（１５）の左辺値ｆ_ｉが最小となるような位相差ｕⁿ _ｋを正しい位相差（第１位相差）と判断して選択するのである。さらにここでは、選択された位相差の前後の整数値バイアス値に対応する位相差ｕⁿ⁺¹ _ｋのいずれかを第２位相差とする。またｕ^n-1 _ｋを第３位相差としてもよい。なお、式（１５）において、ｄは目標との相関範囲を決めるパラメータであり、自由度１のχ自乗分布により算出する値である。 Then, the phase difference u ⁿ _k that minimizes the left-side value f _i of Expression (15) is selected as a correct phase difference (first phase difference). Furthermore, here, any one of the phase differences u ^{n + 1} _k corresponding to the integer bias values before and after the selected phase difference is set as the second phase difference. Further, u ⁿ⁻¹ _k may be the third phase difference. In equation (15), d is a parameter that determines the range of correlation with the target, and is a value calculated by a chi-square distribution with one degree of freedom.

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

Next, a smoothing vector calculation method corresponding to the first phase difference track, the second phase difference track, and the third phase difference track will be described. The gain matrix K _k , the smoothing vector x ^ _k (+) of the phase difference wake, and the smoothing error covariance matrix P _k (+) are given by the equations (16) to (18) according to the Kalman filter theory.

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

Similarly, the smooth vectors of the phase difference back wake using the phase differences u ⁿ⁻¹ _k and u ^{n + 1} _k corresponding to the integer bias values before and after the correct phase difference are expressed as x ^ ^a _k (+) and x If ^ ^b _k (+), these are given by equations (19) and (20).

次に、位相差航跡の位置または速度から、追尾が偽の位相差に乗り移った事象を判定し、位相差航跡を軌道修正する方法を、ここでは速度を利用した場合を例にとって説明する。第１位相差の航跡として選択された位相差ｕⁿ _ｋを用いて算出した移動体の速度をＶⁿ _ｋ、第２位相差、第３位相差の航跡として選択された位相差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋを用いて算出した移動体の速度をそれぞれ、Ｖ^n-1 _ｋ及びＶⁿ⁺¹ _ｋとする。 Next, a method of determining an event in which tracking has shifted to a false phase difference from the position or speed of the phase difference wake and correcting the trajectory of the phase difference wake will be described by taking the case of using speed as an example. The speed of the moving body calculated using the phase difference u ⁿ _k selected as the wake of the first phase difference is the phase difference u ⁿ⁻¹ selected as the wake of the V ⁿ _k , the second phase difference, and the third phase difference. The speeds of the moving objects calculated using _k and u ^{n + 1} _k are V ⁿ⁻¹ _k and V ^{n + 1} _k , respectively.

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

The lower limit of the rate that can be taken of the mobile Vmin, when the upper limit and Vmax, when the velocity V ⁿ _k of the phase difference track is to satisfy the equation (21) or formula (22) in consideration of an error sigma, the phase difference track It is determined that a false phase difference has been transferred.

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

If Noriutsuri is determined that there is, for the V ^n-1 _k and V ^{n + 1} _k instead of V ⁿ _k a judgment of formula (21) and (22), the speed that does not meet these conditions The smooth vector of the wake calculated by the corresponding phase difference is adopted as the phase difference wake. For example, when V ⁿ⁻¹ _k does not satisfy the conditions of Expressions (21) and (22), x ^ ^a _k (+) is set as a new phase difference wake as Expression (23).

そして、この時点でこれまでの第２位相差及び第３位相差の航跡を削除して、次サンプル時刻からは新たな位相差航跡を基準に、これまで同様、前後の整数値バイアスに対する位相差を用いて第２位相差及び第３位相差の航跡を生成するのである。   At this point, the previous second phase difference and third phase difference tracks are deleted, and from the next sampling time, the new phase difference track is used as a reference, and the phase difference with respect to the preceding and following integer value biases is the same as before. Is used to generate the wake of the second phase difference and the third phase difference.

以上の処理方法に基づいて追尾処理器２２−ｎと追尾処理器２２−ｎ＋１、さらには物理量算出器２３と航跡入れ替え器２４の具体的な動作を示すと次のようになる。   Based on the above processing method, specific operations of the tracking processor 22-n and the tracking processor 22-n + 1, and further the physical quantity calculator 23 and the track switching unit 24 are as follows.

追尾処理器２２−１において、相関判定器２６は、位相差検出器２５から位相差を受け取ると、整数値バイアスを考慮した複数の位相差（偽の位相差を含む）に対して、相関範囲算出器３０で算出された位相差の観測値の予測確率分布の平均値ｕ_ｋ(−)と分散Ｓ_ｋ、予め設定されたパラメータｄを用いて、式（１５）を満たす位相差のうち、ｆ_ｉが最小となる位相差ｕⁿ _ｋを正しい位相差と判断して選択する。また追尾処理器２２−２及び２２−３は、追尾処理器２２−１が選択した位相差ｕⁿ _ｋの前後の整数値バイアス値に対応する位相差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋをそれぞれの位相差の航跡を生成するためのデータとして選択する。 In the tracking processor 22-1, when the correlation determining unit 26 receives the phase difference from the phase difference detector 25, the correlation determining unit 26 determines the correlation range for a plurality of phase differences (including false phase differences) in consideration of the integer value bias. Among the phase differences satisfying Expression (15) using the average value u _k (−) of the observed probability distribution of the phase difference calculated by the calculator 30 and the variance S _k and a preset parameter d, The phase difference u ⁿ _{k at} which f _i is minimized is selected as a correct phase difference. Further, the tracking processors 22-2 and 22-3 have phase differences u ⁿ⁻¹ _k and u ^{n + 1} _k corresponding to integer value bias values before and after the phase difference u ⁿ _k selected by the tracking processor 22-1. Are selected as data for generating the wake of each phase difference.

追尾処理器２２−１において、位相差ゲイン算出器２９は、予測器２８から取得した予測誤差共分散行列Ｐ_ｋ(−)と予め設定された受信器２１−１〜２１−Ｍの観測雑音の分散Ｒ_ｋとから、式（１６）に従い位相差のゲイン行列Ｋ_ｋを算出する。そして平滑器２７は、位相差ゲイン算出器２９から取得した位相差のゲイン行列Ｋ_ｋ、予測ベクトルｘ^_ｋ(−)、予測誤差共分散行列Ｐ_ｋ(−)と、相関判定器２６から取得した位相差航跡に対応する位相差ｕⁿ _ｋと、位相差の予測値ｕ_ｋ(−)とを用いて、式（１７）と式（１８）に従い、位相差航跡の平滑ベクトルｘ^_ｋ(＋)及び平滑誤差共分散行列Ｐ_ｋ(＋)を算出する。 In the tracking processor 22-1, the phase difference gain calculator 29 calculates the prediction error covariance matrix P _k (−) acquired from the predictor 28 and the observation noises of the receivers 21-1 to 21 -M set in advance. From the variance R _k , a phase difference gain matrix K _k is calculated according to the equation (16). The smoother 27 acquires the phase difference gain matrix K _k acquired from the phase difference gain calculator 29, the prediction vector x ^ _k (−), the prediction error covariance matrix P _k (−), and the correlation determiner 26. Using the phase difference u ⁿ _k corresponding to the phase difference wake and the predicted value u _k (−) of the phase difference, the smooth vector x ^ _k ( +) And a smoothing error covariance matrix P _k (+) are calculated.

同様に、追尾処理器２２−２及び２２−３においても、相関判定器２６から取得した位相差の航跡に対応する位相差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋ、位相差の予測値ｕ_ｋ(−)と、位相差ゲイン算出器２９から取得した位相差のゲイン行列Ｋ_ｋ、予測ベクトルｘ^(−)とを用いて、式（１９）と式（２０）に従い、それぞれの位相差の航跡の平滑ベクトルｘ^^a _ｋ(＋)及びｘ^^b _ｋ(＋)を算出する。 Similarly, in the tracking processors 22-2 and 22-3, the phase differences u ^n-1 _k and u ^{n + 1} _k corresponding to the phase difference tracks acquired from the correlation determiner 26, and the phase difference prediction value u are also obtained. _{Using k} (−), the phase difference gain matrix K _k acquired from the phase difference gain calculator 29, and the prediction vector x ^ (−), the respective phase differences according to the equations (19) and (20). The smooth vectors x ^ ^a _k (+) and x ^ ^b _k (+) are calculated.

以上のようにして位相差検出単位毎に選択された第１位相差の航跡と第２及び第３位相差の航跡に対応する位相差ｕⁿ _ｋ、ｕ^n-1 _ｋ、ｕⁿ⁺¹ _ｋは、物理量算出器２３に出力される。物理量算出器２３の測位演算器３１は、各位相差毎に式（１）の連立方程式を解き、測位解（位置）を算出する。 As described above, the phase differences u ⁿ _k , u ^n-1 _k , u ^{n + 1} _k corresponding to the first phase difference track and the second and third phase difference tracks selected for each phase difference detection unit. Is output to the physical quantity calculator 23. The positioning calculator 31 of the physical quantity calculator 23 solves the simultaneous equations of the equation (1) for each phase difference and calculates a positioning solution (position).

続いて速度算出器３２は、測位演算器３１で算出した各位相差に対応する位置と過去の位置情報から、各位相差に対応する速度Ｖⁿ _ｋ、Ｖ^n-1 _ｋ、Ｖⁿ⁺¹ _ｋを算出する。そして速度判定器３３は、予め設定された移動体の速度の上限Ｖmax、下限Ｖmin及びその誤差σとから、速度算出器３２から取得した各位相差に対応する速度Ｖⁿ _ｋ、Ｖ^n-1 _ｋ、Ｖⁿ⁺¹ _ｋが式（２１）及び式（２２）を満たすか否かを判定する。 Subsequently, the speed calculator 32 calculates the speeds V ⁿ _k , V ⁿ⁻¹ _k , and V ^{n + 1} _k corresponding to each phase difference from the position corresponding to each phase difference calculated by the positioning calculator 31 and the past position information. calculate. Then, the speed determination unit 33 calculates the speeds V ⁿ _k and V ⁿ⁻¹ _k corresponding to each phase difference acquired from the speed calculator 32 from the preset upper limit Vmax and lower limit Vmin of the moving body and the error σ. , V ^{n + 1} _k determines whether or not Expression (21) and Expression (22) are satisfied.

航跡入れ替え器２４は、速度判定器３３において、位相差航跡の位相差ｕⁿ _ｋに対応する速度Ｖⁿ _ｋが式（２１）及び式（２２）を満たした場合には、位相差航跡の位相差ｕⁿ _ｋに対応する測位解を現サンプリング時刻における測位結果とし、満たさない場合は、式（２１）及び式（２２）を満たす位相差裏航跡の位相差が正しい位相差であると判定し、これに対応する測位解を現サンプリング時刻における測位解とする。航跡入れ替え器２４は、速度判定器３３が第２及び第３位相差の航跡の位相差が正しい位相差であると判定された場合にのみ、式（２３）により位相差航跡の平滑ベクトルｘ^_ｋ(＋)を位相差裏航跡の平滑ベクトルに置き換えるとともに、これまで算出してきた位相差裏航跡の平滑ベクトルを初期化する。 The wake changer 24 determines the position of the phase difference wake when the speed determination unit 33 satisfies the expressions (21) and (22) when the speed V ⁿ _k corresponding to the phase difference u ⁿ _k of the phase difference wake satisfies the expressions (21) and (22). If the positioning solution corresponding to the phase difference u ⁿ _k is the positioning result at the current sampling time and does not satisfy the positioning result, it is determined that the phase difference of the phase difference back wake satisfying the equations (21) and (22) is the correct phase difference. The positioning solution corresponding to this is the positioning solution at the current sampling time. The wake exchanging device 24, when the speed determiner 33 determines that the phase difference between the wakes of the second and third phase differences is the correct phase difference, the smooth vector x ^ of the phase difference wake according to the equation (23). _k (+) is replaced with the smooth vector of the phase difference back wake, and the smooth vector of the phase difference back wake calculated so far is initialized.

予測器２８は、航跡入れ替え器２４から取得した位相差航跡の平滑ベクトルｘ^_ｋ(＋)、平滑誤差共分散行列Ｐ_ｋ(＋)と、予め設定された駆動雑音の誤差共分散行列Ｑ_ｋとから、式（１０）及び式（１１）に従い、次サンプリング時刻における位相差の予測ベクトルｘ^_ｋ＋１(−)及び予測誤差共分散行列Ｐ^_ｋ＋１(−)を算出する。相関範囲算出器３０は、予測器２８で算出された予測誤差共分散行列Ｐ_ｋ＋１(−)と、予め設定された受信器２１−１〜Ｍの観測雑音の分散Ｒｋとから、式（１４）に従い位相差の観測値の予測確率分布の分散Ｓ_ｋを算出する。 The predictor 28 includes a smooth vector x ^ _k (+) of the phase difference wake acquired from the wake changer 24, a smooth error covariance matrix P _k (+), and a preset error covariance matrix Q _{k of} the drive noise. From the above, the prediction vector x ^ _{k + 1} (-) and the prediction error covariance matrix P ^ _{k + 1} (-) of the phase difference at the next sampling time are calculated according to the equations (10) and (11). The correlation range calculator 30 uses the prediction error covariance matrix P _{k + 1} (−) calculated by the predictor 28 and the observation noise variance Rk of the receivers 21-1 to 21 -M set in advance in Expression (14). Then, the variance S _k of the predicted probability distribution of the observed value of the phase difference is calculated.

以上から明らかなように、この発明の実施の形態１の位置推定装置によれば、追尾処理器２２−１だけでなく、追尾処理器２２−２あるいは２２−３を設け、第１位相差の航跡と、この位相差の前後の整数値バイアスに対応する第２の位相差とを航跡として維持して平滑することとした。そして、速度判定器２６において、位相差航跡として選択された位相差により算出された測位解より算出された対象物１の速度が、移動体の取りうる速度範囲内であるかを判定して、範囲を超えている場合に、航跡入れ替え器２４において、速度が正常な位相差を用いて算出した測位解を最終的な測位結果とすることとした。かかる構成を採用した結果、マルチパス波の干渉により位相差の誤差が発生し、位相差の追尾が偽の位相差に乗り移っても、追尾を真の位相差に修正するため、測位精度の劣化を防ぐことができる。   As is apparent from the above, according to the position estimation apparatus of the first embodiment of the present invention, not only the tracking processor 22-1 but also the tracking processor 22-2 or 22-3 is provided, and the first phase difference The wake and the second phase difference corresponding to the integer bias before and after the phase difference are maintained as a wake and smoothed. Then, in the speed determiner 26, it is determined whether the speed of the target 1 calculated from the positioning solution calculated by the phase difference selected as the phase difference wake is within the speed range that the moving body can take, When the range is exceeded, the positioning solution calculated using the phase difference with the normal speed in the wake changer 24 is set as the final positioning result. As a result of adopting such a configuration, even if a phase difference error occurs due to interference of multipath waves, and tracking of the phase difference shifts to a false phase difference, the tracking is corrected to a true phase difference. Can be prevented.

上述の処理では、最良の実施の形態として速度の上限と下限が定められる構成について説明した。しかし速度の値域については上限と下限のいずれか一方しか定めることができない場合であっても式（２１）と式（２２）のいずれか一方のみを選択して、速度を評価することで、不適切な位相差を検出することができるので、上限と下限の双方が定められる構成に限定されるものではない。   In the above-described processing, the configuration in which the upper limit and the lower limit of the speed are determined as the best embodiment has been described. However, even if only one of the upper limit and the lower limit can be determined for the speed range, only one of the formula (21) and the formula (22) is selected and the speed is evaluated. Since an appropriate phase difference can be detected, the present invention is not limited to a configuration in which both an upper limit and a lower limit are determined.

また上述の処理では、同じく最良の実施の形態として、第１位相差の前後の位相差の航跡として第２位相差及び第３位相差を２つとも維持する構成について説明した。しかし対象物１の位置測定環境における電波の伝播特性から、マルチパスによる電波の干渉が位相差に及ぼす影響が予測可能となって、第１位相差よりも大きな位相差あるいは第１位相差よりも小さな位相差に限定できる場合もある。このような場合においては、第２位相差と第３位相差のいずれか一方だけを追尾すれば足りるのである。したがってこのような場合には、追尾処理器２２−１及び２２−２のみで十分である。   In the above-described processing, the configuration in which both the second phase difference and the third phase difference are maintained as the wake of the phase difference before and after the first phase difference has been described as the best embodiment. However, from the propagation characteristics of the radio wave in the position measurement environment of the object 1, it is possible to predict the effect of radio wave interference due to multipath on the phase difference, which is larger than the first phase difference or the first phase difference. In some cases, it can be limited to a small phase difference. In such a case, it is sufficient to track only one of the second phase difference and the third phase difference. Therefore, in such a case, only the tracking processors 22-1 and 22-2 are sufficient.

実施の形態２．
実施の形態１による位置推定装置では、正しいと判定された位相差ｕ^n-1 _ｋで算出した速度に矛盾があった場合に、位相差ｕⁿ _ｋの前後の整数値バイアス値に対応する位相差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋのうち、速度が正常となる位相差を正しい位相差として選択し直す構成とした。しかしながら、道路やトラックを周回する移動体のように、移動体の移動経路が限定されている場合は、対象物１がある基準点を通過する時間を予測して、この予測時間の整合性に基づいて位置推定結果の矛盾を判定するようにしてもよい。この発明の実施の形態２による位置推定装置は、かかる特徴を有するものである。 Embodiment 2. FIG.
In the position estimation apparatus according to the first embodiment, when there is a contradiction in the speed calculated with the phase difference u ⁿ⁻¹ _k determined to be correct, the position corresponding to the integer value bias values before and after the phase difference u ⁿ _k. Of the phase differences u ^n-1 _k and u ^{n + 1} _k , the phase difference at which the speed is normal is selected again as the correct phase difference. However, when the moving path of a moving body is limited, such as a moving body that circulates on a road or a truck, the time required for the object 1 to pass a certain reference point is predicted, and the consistency of the predicted time is determined. Based on this, a contradiction in the position estimation result may be determined. The position estimation apparatus according to Embodiment 2 of the present invention has such a feature.

この発明の実施の形態２による位置推定装置の構成は実施の形態１と同様に図１によって表される。ただし実施の形態２による位置推定装置は実施の形態１と対比した場合に、物理量算出器２３の構成が異なっている。図６は、実施の形態２による位置推定装置２の物理量算出器２３の詳細な構成を示すブロック図である。図において、通過判定器３４は、測位演算器３１の出力する対象物１の位置から対象物１が基準点を通過したかどうかを判定する回路又は装置である。通過時刻予測器３５は、測位演算器３１の出力する対象物１の位置の時間変位から、基準位置の通過時刻を算出する回路又は素子である。通過時刻判定器３６は、通過時刻算出器３４が算出した通過時刻を評価し、測位演算器３１が位置の算出に用いた位相差平滑値の妥当性を判定する回路又は素子である。   The configuration of the position estimation apparatus according to the second embodiment of the present invention is represented by FIG. 1 as in the first embodiment. However, the position estimation apparatus according to the second embodiment differs from the first embodiment in the configuration of the physical quantity calculator 23. FIG. 6 is a block diagram illustrating a detailed configuration of the physical quantity calculator 23 of the position estimation device 2 according to the second embodiment. In the figure, a passage determination unit 34 is a circuit or a device that determines whether or not the target object 1 has passed the reference point from the position of the target object 1 output from the positioning calculator 31. The passage time predictor 35 is a circuit or element that calculates the passage time of the reference position from the time displacement of the position of the object 1 output from the positioning calculator 31. The passage time determination unit 36 is a circuit or element that evaluates the passage time calculated by the passage time calculator 34 and determines the validity of the phase difference smoothing value used by the positioning calculator 31 for calculating the position.

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

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

Here, the time at which the reference point of the object 1 is determined to pass is set as t ^~, and the object 1 calculated using the first phase difference u ⁿ _k selected as the phase difference wake in the immediately preceding tracking process is used. The position and velocity are respectively p ⁿ _k and V ⁿ _k , and the position and velocity corresponding to the second phase difference u ⁿ⁻¹ _k are p ⁿ⁻¹ _k and V ⁿ⁻¹ _k , respectively, and the third phase difference u ^{n + 1.} _k corresponding position and velocity, respectively p ^{n + 1} _k and V ^{n + 1} _k, when the reference point position and P _ref, the prediction of the first to third movable body corresponding to the track of the phase difference at time t ^~ The position is expressed by Expression (24) to Expression (26).

At this time, since the difference between the predicted position and the reference point position is expressed by Expression (27) to Expression (29), the smallest difference can be determined as the true phase difference wake of the object 1.

以上のような原理に基づくこの発明の実施の形態２による位置推定装置２の具体的な動作を以下に説明する。実施の形態１と同様に、位置推定装置２においても追尾処理器２２−１〜３において第１位相差〜第３位相差の追尾処理を行う。その結果、各位相差平滑値ｕⁿ _ｋ、ｕ^n-1 _ｋ、ｕⁿ⁺¹ _ｋがそれぞれの追尾処理器により算出され、物理量算出器２３に出力される。物理量演算器２３の測位演算器３１は、取得した位相差平滑値のそれぞれについて式（１）の連立方程式を解き、測位解（位置）を算出する。 A specific operation of the position estimation apparatus 2 according to the second embodiment of the present invention based on the above principle will be described below. Similarly to the first embodiment, also in the position estimation apparatus 2, the tracking processors 22-1 to 2-3 perform tracking processing of the first phase difference to the third phase difference. As a result, the respective phase difference smooth values u ⁿ _k , u ⁿ⁻¹ _k , and u ^{n + 1} _k are calculated by the respective tracking processors and output to the physical quantity calculator 23. The positioning calculator 31 of the physical quantity calculator 23 solves the simultaneous equations of Expression (1) for each of the acquired phase difference smoothed values and calculates a positioning solution (position).

通過判定器３４は、移動体が起点を通過した時刻を記録し、その時刻を通過時刻予測手段に出力する。そして通過時刻予測器３５は、通過判定器３４から取得した移動体が基準点を通過した時刻ｔ^~と、測位演算器３１から取得した対象物１の位置ｐⁿ _ｋ及び速度Ｖⁿ _ｋ、位置ｐ^n-1 _ｋ及び速度Ｖ^n-1 _ｋ、位置ｐⁿ⁺¹ _ｋ及び速度Ｖⁿ⁺¹ _ｋとを用いて、式（２４）〜（２６）に従って、時刻ｔ^~における位相差航跡及び位相差裏航跡に対応する移動体の予測位置を算出する。通過時刻判定器３６は、通過時刻予測器３５から取得した時刻ｔ^~における第１〜第３位相差の航跡に対応する対象物１の予測位置と、予め設定された基準点位置ｐ_refを用いて、式（２７）〜式（２９）に従い予測位置と基準点位置の差を算出し、この中で最も小さい値を持つ予測位置に対応する位相差航跡または位相差裏航跡を、対象物１の真の位相差航跡と判定する。航跡入れ替え手段２４は、通過時刻判定器３６によって対象物１の真の位相差航跡と判定された予測位置が第２及び第３の位相差の航跡である場合には式（２３）により航跡の平滑ベクトルｘ^_ｋ(＋)を位相差裏航跡の平滑ベクトルに置き換えるとともに、これまで算出してきた第２及び第３の位相差の航跡の平滑ベクトルを初期化する。 The passage determination unit 34 records the time when the moving body passes through the starting point, and outputs the time to the passage time prediction unit. The passage time predictor 35 includes the time t ^~ when the moving body acquired from the passage determination unit 34 passes the reference point, the position ^pn _k and the speed V ⁿ _k , the position of the object 1 acquired from the positioning calculator 31. p _{^n-1 k} and the velocity V _{^n-1 k,} using the position p ^{n +} _{1 k} and the velocity V ^{n +} _{1 k,} equation (24) according to (26), the time t the phase difference track and positions in ^~ The predicted position of the moving object corresponding to the phase difference back wake is calculated. Passage time determiner 36 uses the predicted position of the object 1 corresponding to the track of the first to third phase difference at time t ^~ obtained from the passage time predictor 35, the preset reference point position p _ref Thus, the difference between the predicted position and the reference point position is calculated according to the equations (27) to (29), and the phase difference wake or the phase difference back wake corresponding to the predicted position having the smallest value among these is obtained as the object 1. The true phase difference wake is determined. If the predicted position determined as the true phase difference wake of the object 1 by the passage time determination unit 36 is the wake of the second and third phase differences, the wake replacement unit 24 calculates the wake of the wake by the equation (23). The smooth vector x ^ _k (+) is replaced with the smooth vector of the phase difference back wake, and the second and third phase difference smooth vectors calculated so far are initialized.

予測器２８は、航跡入れ替え器２４から取得した位相差航跡の平滑ベクトルｘ^_ｋ(＋)、平滑誤差共分散行列Ｐ_ｋ(＋)と、予め設定された駆動雑音の誤差共分散行列Ｑｋとから、式（１０）及び式（１１）に従い、次サンプリング時刻における位相差の予測ベクトルｘ^_ｋ＋１(−)及び予測誤差共分散行列Ｐ_ｋ＋１(−)を算出する。相関範囲算出器３０は、予測器２８で算出された予測誤差共分散行列Ｐ_ｋ＋１(−)と、予め設定された受信器２１−１〜Ｍの観測雑音の分散Ｒ_ｋとから、式（１４）に従い位相差の観測値の予測確率分布の分散Ｓ_ｋを算出する。 The predictor 28 includes a smooth vector x ^ _k (+) of the phase difference wake acquired from the wake changer 24, a smooth error covariance matrix P _k (+), and a preset error covariance matrix Qk of drive noise. Then, according to the equations (10) and (11), the prediction vector x ^ _{k + 1} (−) and the prediction error covariance matrix P _{k + 1} (−) of the phase difference at the next sampling time are calculated. The correlation range calculator 30 calculates the equation (14) from the prediction error covariance matrix P _{k + 1} (−) calculated by the predictor 28 and the variance R _k of the observation noise of the receivers 21-1 to 21 -M set in advance. ) To calculate the variance S _k of the predicted probability distribution of the observed value of the phase difference.

以上から明らかなように、この発明の実施の形態２の位置推定装置によれば、通過時刻予測器３５において算出された第１〜第３の位相差航跡の基準点通過時刻における予測位置と基準点位置の差から、対象物１の真の位相差を判定し、航跡を入れ替えることとしたので、マルチパス波の干渉により位相差の誤差が発生し、位相差の追尾が偽の位相差に乗り移っても、追尾を真の位相差に修正するため、測位精度の劣化を防ぐことができる。   As is apparent from the above, according to the position estimation apparatus of the second embodiment of the present invention, the predicted position and reference at the reference point passage time of the first to third phase difference wakes calculated by the passage time predictor 35 Since the true phase difference of the object 1 is determined from the difference of the point positions and the wakes are replaced, an error of the phase difference occurs due to the interference of the multipath wave, and the tracking of the phase difference becomes a false phase difference. Even if a change is made, since tracking is corrected to a true phase difference, it is possible to prevent deterioration in positioning accuracy.

なお、この発明の実施の形態２においても、電波の伝播条件を仮定できる場合においては、第１位相差の前後の位相差の航跡の一方のみを維持すれば足りる点で、実施の形態１と同様である。   In the second embodiment of the present invention, when the propagation condition of radio waves can be assumed, it is sufficient to maintain only one of the phase difference tracks before and after the first phase difference. It is the same.

また、実施の形態１と２では速度に上限値や下限値を設定したり、基準位置の通過時間に上限値や下限値を設定して、位置推定の対象となる対象物の位相差の航跡の乗り移りを検出しているが、速度や通過時間といった条件は一例に過ぎない。すなわち対象物の運動状態を示すある物理量が位相差に基づいて導出可能であり、さらにある対象物についてその物理量の時間変化が何らかの整合性や連続性を仮定できるのであれば、如何なる物理量であってもこの発明による技術的思想に包含されるのである。   In the first and second embodiments, an upper limit value and a lower limit value are set for the speed, and an upper limit value and a lower limit value are set for the passage time of the reference position, so that the phase difference track of the target object for position estimation is obtained. However, conditions such as speed and transit time are only examples. In other words, if a certain physical quantity indicating the motion state of an object can be derived based on the phase difference, and the temporal change of the physical quantity of a certain object can assume some consistency or continuity, any physical quantity can be used. Is also included in the technical idea of the present invention.

実施の形態３．
実施の形態１及び２による位置推定装置では、位相差について追尾処理を行い、平滑値と予測値とを算出している。しかしながら、位相差に替えて対象物の直交座標による位置を追尾するような構成を採用してもよい。この実施の形態３による位置推定装置はかかる特徴を有するものである。 Embodiment 3 FIG.
In the position estimation apparatus according to the first and second embodiments, the tracking process is performed on the phase difference, and the smooth value and the predicted value are calculated. However, a configuration in which the position of the object according to the orthogonal coordinates is tracked instead of the phase difference may be employed. The position estimation apparatus according to the third embodiment has such a feature.

この発明の実施の形態３による位置推定装置の構成は実施の形態１及び２と同様に図１によって表される。ただし実施の形態３による位置推定装置は追尾処理器２２−１〜３及び物理量算出器２３の詳細な構成が異なっている。図７は、実施の形態３による位置推定装置２の追尾処理器２２−１の詳細な構成を示すブロック図である。図において、直交座標ゲイン算出器３７は直交座標のゲインを算出する回路又は素子である。他の部位については実施の形態１及び２と同様であるが、後述するように、この追尾処理器２２−１は対象物１との距離差を直交座標によって追尾するように構成されているので、処理に供されるデータが異なっている。   The configuration of the position estimation apparatus according to the third embodiment of the present invention is represented by FIG. 1 as in the first and second embodiments. However, the position estimation apparatus according to Embodiment 3 differs in the detailed configurations of the tracking processors 22-1 to 2-3 and the physical quantity calculator 23. FIG. 7 is a block diagram illustrating a detailed configuration of the tracking processor 22-1 of the position estimation device 2 according to the third embodiment. In the figure, a Cartesian coordinate gain calculator 37 is a circuit or element that calculates a Cartesian coordinate gain. The other parts are the same as those in the first and second embodiments. However, as will be described later, the tracking processor 22-1 is configured to track the distance difference from the object 1 using orthogonal coordinates. The data provided for processing is different.

なお、追尾処理器２１−２及び２２−３の構成については、追尾処理器２１−１と同様であるので説明を省略する。   Note that the configuration of the tracking processors 21-2 and 22-3 is the same as that of the tracking processor 21-1, and a description thereof will be omitted.

図８は、この発明の実施の形態３における位置推定装置の物理量算出器２３の詳細な構成を示すブロック図である。図において、速度判定器３８は追尾処理器２２−１〜２２−３が算出した直交座標の平滑値の時間変位から速度を求めて判定する回路又は素子である。   FIG. 8 is a block diagram showing a detailed configuration of the physical quantity calculator 23 of the position estimation apparatus according to Embodiment 3 of the present invention. In the figure, a speed determiner 38 is a circuit or element that determines and determines the speed from the time displacement of smooth values of orthogonal coordinates calculated by the tracking processors 22-1 to 22-3.

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

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

Here, the motion model in the tracking process of the position estimation apparatus according to Embodiment 3 of the present invention is expressed by Expression (30).

Here, x _k is a state vector representing the true value of the position of the Cartesian coordinates at the sampling time t _k and the true value of the time derivative thereof, and is represented by Expression (31).

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

The sampling time _{t k (k = 1,2, ...} , n) is the discrete time representing a timing for detecting a phase difference, hereinafter referred to as time _{t k.} Φ _k−1 is a state vector transition matrix from time t _k−1 to t _k , and is expressed by Expression (32).

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

なお、Ｔはサンプリング間隔、Ｉは３行３列の単位行列である。平均を表す記号としてＥを用いると、ｗ_ｋは平均０、共分散Ｑ_ｋの正規分布の白色雑音とする。 In the formula (30), _{w k} is the process noise at time _{t k, Γ 1 (k)} is the transformation matrix driving noise at time _{t k.} For example, if the truncation error term due to the assumption that the motion model of the moving body is a constant linear motion is Γ ₁ (k−1) w _k−1 , w _k corresponds to the acceleration vector of the moving body. . Further, Γ ₁ (k−1) is expressed by Expression (33).

T is a sampling interval, and I is a 3 × 3 unit matrix. When E is used as a symbol representing an average, w _k is a white noise having a normal distribution with an average of 0 and a covariance Q _k .

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

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

ただし、ν _ｋは時刻ｔ_ｋにおいて検出された距離差の観測雑音である。観測雑音は平均０、分散Ｒ_ｋの正規分布白色雑音に従うものと仮定する。また、時刻ｔ_ｋまでの間に移動体の追尾に用いた距離差の全体をＵ^ｋとする。 In addition, the observation model of the object 1 is expressed by Expression (34).

Here, u _k is an observation value of the distance difference, and h ( x _k ) is an observation matrix, which is expressed by Expression (35).

However, [nu _k is the observation noise of the detected distance difference at time t _k. The average measurement noise is zero, it is assumed to follow a normal distribution white noise variance R _k. Further, the entire distance difference used for tracking the moving object until time t _k is U ^k .

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

Based on the above motion model, a tracking filter is configured using an extended Kalman filter. If the smooth vector of the Cartesian coordinate track before one sampling is x ^ _k-1 (+) and the smoothing error covariance matrix is P _k-1 (+), the prediction vector of the state vector x k of the moving object at time t _k x ^ _k (−) and the prediction error covariance matrix P _k (−) are defined by a conditional mean vector and a conditional covariance matrix, respectively, and are expressed by Expression (36) and Expression (37).

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

Next, a method for selecting a plurality of distance differences U _k measured at time t _k will be described. In order to determine a correct distance difference from a plurality of distance differences including a false distance difference, a predicted value u _k (−) of the distance difference represented by Expression (38) is used.

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

The predicted probability distribution P [u _k | U ^k−1 ] of the observation value of the distance difference obtained from the past distance difference information U ^k−1 is expressed by a conditional probability density function shown in Expression (39).

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

That is, the observed value of the distance difference is a three-dimensional normal distribution g (u) in which the distance difference predicted value u _k (−) given by Expression (38) is averaged and S _k given by Expression (40) is variance. _k ; u _k (−), S _k ).

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

Then, among the distance differences that satisfy Expression (41), the distance difference u ⁿ _k that minimizes Expression (41) is determined as the correct distance difference and is selected.

さらに、このようにして選択された距離差ｕⁿ _ｋを算出した第１位相差の前後の整数値バイアス値をとる第２及び第３位相差から算出される距離差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋについても直交座標を航跡を生成するためのデータとして選択するのである。なお、以後の説明においては、距離差ｕⁿ _ｋを第１距離差と呼び、距離差ｕ^n-1 _ｋを第２距離差、距離差ｕⁿ⁺¹ _ｋを第３距離差と呼ぶこととする。式（４１）において、ｄは目標との相関範囲を決めるパラメータであり、自由度３のχ自乗分布により算出される。 Further, the distance differences u ⁿ⁻¹ _k and u calculated from the second and third phase differences that take integer value bias values before and after the first phase difference for which the distance difference u ⁿ _k selected in this way is calculated. ^{For n + 1} _k , orthogonal coordinates are selected as data for generating a wake. In the following description, the distance difference u ⁿ _k is referred to as a first distance difference, the distance difference u ⁿ⁻¹ _{k is referred} to as a second distance difference, and the distance difference u ^{n + 1} _k is referred to as a third distance difference. To do. In equation (41), d is a parameter for determining the correlation range with the target, and is calculated by a χ square distribution with 3 degrees of freedom.

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

Further, the predicted vector value x ^ _k (−) satisfies Expression (42) and Expression (43).

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

Further, the predicted vector value x ^ _k (−) satisfies the expressions (42) and (43). Subsequently, a method for calculating a smooth vector in a wake of orthogonal coordinates corresponding to the first to third phase differences will be described. The gain matrix K _k of the first distance difference, the smooth vector x ^ _k (+) of the Cartesian coordinate wake, and the smooth error covariance matrix P _k (+) are expressed by Equations (44) to (46) according to the theory of the extended Kalman filter. Given.

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

Similarly, when the smoothed vector of the second distance difference u ^n-1 _k and the third distance difference u ^{n + 1} _k orthogonal coordinate track using respectively the x ^ ^a _k (+) and x ^ ^b _k (+) These are given by equations (47) and (48).

次に、直交座標航跡の位置または速度から、追尾が偽の位相差に乗り移った事象を判定し、直交座標航跡を軌道修正する方法を、ここでは速度を利用した場合を例にとって説明する。第１位相差の直交座標航跡の速度をＶⁿ _ｋ、第２及び第３位相差の直交座標航跡に対応する速度をそれぞれＶ^n-1 _ｋ及びＶⁿ⁺¹ _ｋとする。 Next, a method for determining an event in which tracking has shifted to a false phase difference from the position or speed of the Cartesian coordinate track and correcting the trajectory of the Cartesian coordinate track will be described here using an example of speed. It is assumed that the velocity of the orthogonal coordinate wake of the first phase difference is V ⁿ _k , and the velocity corresponding to the orthogonal coordinate wake of the second and third phase differences is V ⁿ⁻¹ _k and V ^{n + 1} _k , respectively.

その後、第２及び第３位相差についての直交座標航跡を削除して、次サンプル時刻からは新たな直交座標航跡を基準に、これまで同様、前後の整数値バイアスに対する距離差を用いて直交座標航跡を生成する。   After that, the Cartesian coordinate track for the second and third phase differences is deleted, and from the next sample time, the new Cartesian coordinate track is used as a reference, and the Cartesian coordinate is used using the distance difference with respect to the previous and next integer bias as before. Generate a wake.

以上の処理方法に基づいて、この発明の実施の形態３における追尾処理器２２−ｎと追尾処理器２２−ｎ＋１、さらには物理量算出器２３と航跡入れ替え器２４の具体的な動作を示すと次のようになる。   Based on the above processing method, specific operations of the tracking processor 22-n and the tracking processor 22-n + 1, and further the physical quantity calculator 23 and the track switching unit 24 according to the third embodiment of the present invention are as follows. become that way.

追尾処理器２２−１において、相関判定器２６は、位相差検出器２５から位相差を受け取ると、整数値バイアスを考慮した複数の距離差（偽の距離差を含む）に対して、相関範囲算出器３０で算出された距離差の観測値の予測確率分布の平均値ｕ_ｋ(−)と分散Ｓ_ｋ、予め設定されたパラメータｄとを用いて、式（４１）を満たす距離差のうち、ｆ_ｉが最小となる距離差ｕⁿ _ｋを正しい距離差と判断して選択する。また追尾処理器２２−２及び２２−３は、その位相差の前後の整数値バイアス値をとる位相差に対応する距離差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋを直交座標航跡を生成するためのデータとして選択する。 In the tracking processor 22-1, when the correlation determiner 26 receives the phase difference from the phase difference detector 25, the correlation range is calculated with respect to a plurality of distance differences (including false distance differences) in consideration of the integer value bias. Of the distance differences satisfying the equation (41), the average value u _k (−) of the observed probability distribution of the distance difference calculated by the calculator 30 and the variance S _k and a preset parameter d are used. , F _i at which the distance difference u ⁿ _k is minimized is selected as a correct distance difference. Further, the tracking processors 22-2 and 22-3 generate orthogonal coordinate tracks based on the distance differences u ⁿ⁻¹ _k and u ^{n + 1} _k corresponding to the phase differences that take integer bias values before and after the phase difference. Select as data for.

直交座標ゲイン算出器３７は、予測手段３１より入力した予測誤差共分散行列Ｐ_ｋ(−)と予め設定された受信器２１−１〜２１−Mの観測雑音の分散Ｒ_ｋとから、式（４４）に従い直交座標のゲイン行列Ｋ_ｋを算出する。平滑器２７は、直交座標ゲイン算出器３７から取得した直交座標のゲイン行列Ｋｋ、予測ベクトルｘ^_ｋ(−)、予測誤差共分散行列Ｐ_ｋ(−)と、相関判定器２６から取得した直交座標航跡に対応する距離差ｕⁿ _ｋと、距離差の予測値ｕ_ｋ(−)とを用いて、式（４５）〜（４６）に従い、直交座標航跡の平滑ベクトルｘ^_ｋ(＋)及び平滑誤差共分散行列Ｐ_ｋ(＋)を算出する。 The Cartesian coordinate gain calculator 37 uses the prediction error covariance matrix P _k (−) input from the prediction unit 31 and the variance R _k of the observation noise of the receivers 21-1 to 21 -M set in advance. 44), a gain matrix K _k of orthogonal coordinates is calculated. The smoother 27 includes the orthogonal coordinate gain matrix Kk acquired from the orthogonal coordinate gain calculator 37, the prediction vector x ^ _k (−), the prediction error covariance matrix P _k (−), and the orthogonality acquired from the correlation determiner 26. Using the distance difference u ⁿ _k corresponding to the coordinate track and the predicted value u _k (−) of the distance difference, according to the equations (45) to (46), the smooth vector x ^ _k (+) of the orthogonal coordinate track and A smooth error covariance matrix P _k (+) is calculated.

同様に、追尾処理器２２−２及び２２−３においても、平滑器２７は、相関判定器２６から取得した直交座標航跡に対応する距離差ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋ、距離差の予測値ｕ_ｋ(−)と、直交座標ゲイン算出器３７から取得した直交座標のゲイン行列Ｋ_ｋ、予測ベクトルｘ^_ｋ(−)とを用いて、式（４７）〜（４８）に従い、それぞれの直交座標の航跡の平滑ベクトルｘ^^a _ｋ(＋)及びｘ^^b _ｋ(＋)を算出する。 Similarly, in the tracking processors 22-2 and 22-3, the smoother 27 also includes distance differences u ⁿ⁻¹ _k and u ^{n + 1} _k and distance differences corresponding to the orthogonal coordinate tracks acquired from the correlation determiner 26. Using the predicted value u _k (−), the orthogonal coordinate gain matrix K _k acquired from the orthogonal coordinate gain calculator 37, and the predicted vector x ^ _k (−), according to the equations (47) to (48), The smooth vectors x ^ ^a _k (+) and x ^ ^b _k (+) of the wakes of the respective orthogonal coordinates are calculated.

以上のようにして各位相差検出単位毎に選択された直交座標航跡に対応する距離差ｕⁿ _ｋ、ｕ^n-1 _ｋ、ｕⁿ⁺¹ _ｋは、物理量算出器２３の速度判定器３８に出力される。速度判定器３８は、予め設定された移動体の速度の上限Ｖmax、下限Ｖmin及びその誤差σとから、追尾処理器２２−１〜２２−３の平滑器２７から取得した各距離差に対応する速度Ｖⁿ _ｋ、Ｖ^n-1 _ｋ、Ｖⁿ⁺¹ _ｋが式（２１）及び式（２２）を満たすか否かを判定する。航跡入れ替え器２４は、速度判定器３８において、直交座標航跡の距離差ｕⁿ _ｋに対応する速度Ｖⁿ _ｋが式（２１）及び式（２２）を満たす場合は、直交座標航跡を現サンプリング時刻における移動体の状態とし、満たさない場合は、式式（２１）及び式（２２）を満たす直交座標裏航跡を現サンプリング時刻における移動体の状態とする。航跡入れ替え器２４は、速度判定器３８において直交座標裏航跡の距離差が正しい距離差であると判定された場合にのみ、式（２３）により直交座標航跡の平滑ベクトルｘ^_ｋ(＋)を直交座標裏航跡の平滑ベクトルに置き換えるとともに、これまで算出してきた直交座標裏航跡の平滑ベクトルを初期化する。 As described above, the distance differences u ⁿ _k , u ^n-1 _k , and u ^{n + 1} _k corresponding to the orthogonal coordinate wake selected for each phase difference detection unit are output to the speed determiner 38 of the physical quantity calculator 23. Is done. The speed determination unit 38 corresponds to each distance difference acquired from the smoothing unit 27 of the tracking processing units 22-1 to 22-3 from the preset upper limit Vmax, lower limit Vmin and error σ of the moving body. It is determined whether or not the speeds V ⁿ _k , V ⁿ⁻¹ _k , and V ^{n + 1} _k satisfy the expressions (21) and (22). The wake changer 24, when the speed determination unit 38 determines that the speed V ⁿ _k corresponding to the distance difference u ⁿ _k of the Cartesian coordinate track satisfies Expressions (21) and (22), In the case of not satisfying the state, the orthogonal coordinate back wake satisfying the formulas (21) and (22) is set as the state of the mobile body at the current sampling time. The wake exchanging device 24 calculates the smooth vector x ^ _k (+) of the orthogonal coordinate wake according to the equation (23) only when the speed determiner 38 determines that the distance difference between the orthogonal coordinate back wake is the correct distance difference. Replace with the smooth vector of the orthogonal coordinate back wake, and initialize the smooth vector of the orthogonal coordinate back wake calculated so far.

予測器２８は、航跡入れ替え器２４から取得した直交座標航跡の平滑ベクトルｘ^_ｋ(＋)、平滑誤差共分散行列Ｐ_ｋ(＋)と、予め設定された駆動雑音の誤差共分散行列Ｑ_ｋとから、式（３６）及び式（３７）に従い、次サンプリング時刻における直交座標の予測ベクトルｘ^_ｋ＋１(−)及び予測誤差共分散行列Ｐ_ｋ＋１(−)を算出する。相関範囲算出器３０は、予測器２８が算出した予測誤差共分散行列Ｐ_ｋ＋１(−)と、予め設定された受信機２１の観測雑音の分散Ｒ_ｋとから、式（４０）を用いて距離差の観測値の予測確率分布の分散Ｓ_ｋを算出する。 The predictor 28 includes a smooth vector x ^ _k (+) of a rectangular coordinate wake acquired from the wake changer 24, a smooth error covariance matrix P _k (+), and a preset error covariance matrix Q _{k of} drive noise. Then, according to Equation (36) and Equation (37), the orthogonal coordinate prediction vector x ^ _{k + 1} (−) and the prediction error covariance matrix P _{k + 1} (−) at the next sampling time are calculated. The correlation range calculator 30 calculates the distance using the equation (40) from the prediction error covariance matrix P _{k + 1} (−) calculated by the predictor 28 and the observation noise variance R _{k of} the receiver 21 set in advance. The variance S _k of the predicted probability distribution of the observed difference value is calculated.

以上のように、この発明の実施の形態３の位置推定装置によれば、距離差に基づいて直交座標の航跡を維持し、直交座標の平滑値から直接的に対象物１の速度を算出することとした。この結果、実施の形態１の効果に加えて、実施の形態１及び２のように測位演算を別途行わなくてもよい、という効果が生じる。   As described above, according to the position estimation apparatus of the third embodiment of the present invention, the wake of the orthogonal coordinates is maintained based on the distance difference, and the speed of the object 1 is directly calculated from the smooth value of the orthogonal coordinates. It was decided. As a result, in addition to the effect of the first embodiment, there is an effect that the positioning calculation need not be performed separately as in the first and second embodiments.

また、これにより、追尾処理器２２−１〜２２−３における位相差検出器２５の処理が同期していなくてもよいとの利点がある。   This also has the advantage that the processing of the phase difference detector 25 in the tracking processors 22-1 to 22-3 does not have to be synchronized.

なお、この発明の実施の形態３による位置推定装置は、航跡間の乗り移りの発生を判断基準として、対象物の速度が所定の値域（式（２１）及び式（２２）で示される）に含まれるかどうかを調べた。しかしながら実施の形態２で説明したように運動状態を表す物理量としては、速度に限定されないのであり、例えば位置、通過時間、加速度、角速度等を用いてもよいことはいうまでもない。   In the position estimation device according to the third embodiment of the present invention, the speed of the object is included in a predetermined range (shown by Expression (21) and Expression (22)) with the occurrence of transfer between wakes as a criterion. I checked whether it was possible. However, as described in the second embodiment, the physical quantity representing the motion state is not limited to the speed, and needless to say, for example, position, passage time, acceleration, angular velocity, and the like may be used.

また、この発明の実施の形態３から、位相差の乗り移りを判定するために必要となる追尾処理は、位相差自体の追尾処理に限られるものではなく、位相差に基づく直交座標の追尾処理でもよく、また他の物理量に関する追尾処理であってもよいことが理解されるはずである。さらに追尾処理のアルゴリズムとしてはカルマンフィルタに限定されるものではなく、例えば上述したように拡張カルマンフィルタを用いてもよく、またα−βフィルタその他の外挿法を採用してもこの発明の特徴が失われることはないのである。   Further, from the third embodiment of the present invention, the tracking process necessary for determining the transition of the phase difference is not limited to the tracking process of the phase difference itself, and is also a tracking process of orthogonal coordinates based on the phase difference. It should be understood that the tracking process may be related to other physical quantities. Furthermore, the tracking algorithm is not limited to the Kalman filter. For example, as described above, an extended Kalman filter may be used, and even if an α-β filter or other extrapolation method is employed, the characteristics of the present invention are lost. It will not be deceived.

実施の形態４．
実施の形態１〜３で示した位置推定装置は、マルチパス環境下における位相差のアンビギュイティを解決するために、対象物の運動状態を表す物理量であって、位相差から導出可能な物理量の時間的な整合性、連続性に着目し、この物理量が所定の条件を満たす範囲で位相差を選択することを特徴としていた。 Embodiment 4 FIG.
In order to solve the ambiguity of the phase difference in the multipath environment, the position estimation device shown in the first to third embodiments is a physical quantity that represents the motion state of the object and can be derived from the phase difference. Focusing on the temporal consistency and continuity, the phase difference is selected in a range in which the physical quantity satisfies a predetermined condition.

しかしこの原理は、位相差のアンビギュイティを解決するためだけでなく、方位角のアンビギュイティを解決する上でも役立つ。この発明の実施の形態４では、かかる特徴を有する位置推定装置を示す。   However, this principle is useful not only for solving the ambiguity of the phase difference but also for solving the ambiguity of the azimuth angle. Embodiment 4 of the present invention shows a position estimation device having such a feature.

図９は、この発明の実施の形態４における位置推定装置の構成を示すブロック図である。図において、図１と同一の符号を付した構成要素については、実施の形態１と同様であるので説明を省略する。ただしこの位置推定装置２においては、受信器２１−１〜２１−Ｍはアレーアンテナを構成するアレー素子であるものとする。方位角算出器４０は、アレーアンテナ２１−１〜２１−Ｍから得られた受信信号から送信波を送出した対象物１の方位角を算出する回路又は素子である。また、図９における追尾処理器２２−１〜２２−３は方位角に関する追尾処理を行うように構成されている。   FIG. 9 is a block diagram showing the configuration of the position estimation apparatus according to Embodiment 4 of the present invention. In the figure, the components denoted by the same reference numerals as those in FIG. 1 are the same as those in the first embodiment, and thus the description thereof is omitted. However, in this position estimation apparatus 2, the receivers 21-1 to 21-M are assumed to be array elements that constitute an array antenna. The azimuth calculator 40 is a circuit or element that calculates the azimuth of the object 1 that has transmitted a transmission wave from the received signals obtained from the array antennas 21-1 to 21-M. Further, the tracking processors 22-1 to 22-3 in FIG. 9 are configured to perform tracking processing related to the azimuth.

図１０は、位置推定装置２の追尾処理器２２−１の詳細な構成を示すブロック図である。図において、方位角ゲイン算出器４１は方位角のゲインを算出する回路又は素子である。他の部位については実施の形態１及び２と同様であるが、後述するように、この追尾処理器２２−１は対象物１の方位角を追尾するように構成されているので、処理に供されるデータが異なっている。   FIG. 10 is a block diagram illustrating a detailed configuration of the tracking processor 22-1 of the position estimation apparatus 2. In the figure, an azimuth gain calculator 41 is a circuit or an element that calculates an azimuth gain. Other parts are the same as those in the first and second embodiments. However, as will be described later, the tracking processor 22-1 is configured to track the azimuth angle of the object 1, and thus is used for processing. The data to be processed is different.

なお、追尾処理器２１−２及び２２−３の構成については、追尾処理器２１−１と同様であるので説明を省略する。またこの発明の実施４では、物理量算出器２３として実施の形態１と同様に図３に示す構成を有するものとする。   Note that the configuration of the tracking processors 21-2 and 22-3 is the same as that of the tracking processor 21-1, and a description thereof will be omitted. In the fourth embodiment of the present invention, the physical quantity calculator 23 has the configuration shown in FIG. 3 as in the first embodiment.

ここで、対象物１の方位角の運動モデルは式（４）によって示されるものとする。なお、ｘ _ｋはサンプリング時刻ｔ_ｋにおける方位角の真値θ_ｋとその時間微分の真値θ'_ｋを表す状態ベクトルであり、式（５）で表される。ここで、サンプリング時刻ｔ_ｋ（ｋ＝１，２，…，ｎ）は、方位角を検出するタイミングを表す離散時刻であり、以降、時刻ｔ_ｋと呼ぶ。また、Φ_ｋ−１は時刻ｔ_ｋ−１よりｔ_ｋへの状態ベクトルの推移行列であり、式（６）で表される。また、ｗ_ｋは時刻ｔ_ｋにおける駆動雑音であり、Γ_１(ｋ)は時刻ｔ_ｋにおける駆動雑音の変換行列である。例えば、位相差の運動モデルを等速直線運動と仮定したことによる打ち切り誤差項をΓ_１(ｋ−１)ｗ_ｋ−１とみれば、ｗ_ｋは位相差の加速度ベクトル相当であり、Γ_１(ｋ−１)は式（７）で表される。なお、Ｔはサンプリング間隔である。平均を表す記号としてＥを用いると、ｗ_ｋは平均０、分散Ｑ_ｋの正規分布の白色雑音とする。 Here, it is assumed that the motion model of the azimuth angle of the object 1 is represented by Expression (4). Incidentally, x _k is the state vector representing the true value theta _'k of the true value theta _k azimuth at the sampling time t _k which time derivative, represented by the formula (5). Here, sampling time _{t k (k = 1,2, ...} , n) is the discrete time representing a timing of detecting the azimuth angle, hereinafter referred to as time _{t k.} Φ _k−1 is a state vector transition matrix from time t _k−1 to t _k , and is expressed by Expression (6). Further, w _k is the drive noise at time t _k , and Γ ₁ (k) is a drive noise conversion matrix at time t _k . For example, if the truncation error term due to the assumption that the motion model of the phase difference is constant linear motion is Γ ₁ (k−1) w _k−1 , w _k is equivalent to the acceleration vector of the phase difference, and Γ ₁ (k-1) is represented by Formula (7). T is a sampling interval. When E is used as a symbol representing an average, w _k is a white noise having a normal distribution with an average of 0 and a variance Q _k .

方位角の観測モデルは式（８）で表される。ここで、ｕ_ｋは方位角の観測値、Ｈは観測行列で、式（９）で表される。ν _ｋは時刻ｔ_ｋにおいて検出された方位角の観測雑音である。観測雑音は平均０、分散Ｒ_ｋの正規分布白色雑音に従うものと仮定する。また、時刻ｔ_ｋまでの間に方位角の追尾に用いた方位角の全体をＵ^kとする。 The azimuth angle observation model is expressed by equation (8). Here, u _k is the observed value of the azimuth, H is the observation matrix, represented by the formula (9). ν _k is the observation noise of the azimuth angle detected at time t _k . The average measurement noise is zero, it is assumed to follow a normal distribution white noise variance R _k. Further, the entire azimuth angle used for tracking the azimuth angle until time t _k is U ^k .

以上の運動モデルに基づき、カルマンフィルタを用いた場合を例に追尾フィルタを構成する。１サンプリング前の方位角航跡の平滑ベクトルをｘ^_ｋ−１(＋)、平滑誤差共分散行列をＰ_ｋ−１(＋)とすると、時刻ｔ_ｋにおける方位角の状態ベクトルｘ _ｋの予測ベクトルｘ^_ｋ(−)及び予測誤差共分散行列Ｐ_ｋ(−)はそれぞれ条件付平均ベクトル、条件付共分散行列で定義され、式（１０）及び式（１１）で表される。
次に、時刻ｔ_ｋに計測された複数の方位角Ｕ_ｋの選択方法について述べる。偽の方位角の含まれる複数の方位角から正しい方位角を判定するため、式(１２)で表される方位角の予測値ｕ _ｋ(−)を利用する。 Based on the above motion model, the tracking filter is configured using the Kalman filter as an example. If the smooth vector of the azimuth wake before one sampling is x ^ _k-1 (+) and the smoothing error covariance matrix is P _k-1 (+), the prediction vector of the state vector x _k of the azimuth at time t _k x ^ _k (−) and the prediction error covariance matrix P _k (−) are defined by a conditional mean vector and a conditional covariance matrix, respectively, and are expressed by Expression (10) and Expression (11).
Next, a method for selecting a plurality of azimuth angles U _k measured at time t _k will be described. In order to determine a correct azimuth angle from a plurality of azimuth angles including a false azimuth angle, the predicted azimuth value u _k (−) represented by the equation (12) is used.

過去の方位角情報Ｕ^ｋ-1より得られる方位角の観測値の予測確率分布Ｐ[ｕ_ｋ｜Ｕ^ｋ-1]は、式（１３）に示す条件付確率密度関数で表される。すなわち、方位角の観測値は、式（１１）で与えられる方位角の予測値ｕ_ｋ(−)を平均とし、式（１４）で与えられるＳ_ｋを分散とする１次元正規分布ｇ(ｕ_ｋ；ｕ_ｋ(−)，Ｓ_ｋ)に従うものとする。そして、式（１５）を満たす方位角のうち、ｆ_ｉが最小となる方位角ｕⁿ _ｋを正しい方位角と判断し、選択する。また、選択された方位角の前後の整数値バイアス値に対応する方位角ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋを方位角裏航跡を生成するためのデータとする。なお、ｄは目標との相関範囲を決めるパラメータであり、自由度１のχ自乗分布により算出する。 The predicted probability distribution P [u _k | U ^k−1 ] of the observed value of the azimuth obtained from the past azimuth angle information U ^k−1 is represented by a conditional probability density function shown in Expression (13). That is, the observed value of the azimuth angle, equation (11) the predicted value u _k of the azimuth angle given by _(-) as the average, one-dimensional normal distribution g to disperse the S _k given by Equation (14) (u _k ; u _k (−), S _k ). Then, among the azimuth angles satisfying Expression (15), the azimuth angle u ⁿ _k that minimizes f _i is determined as the correct azimuth angle and selected. Further, the azimuth angles u ⁿ⁻¹ _k and u ^{n + 1} _k corresponding to the integer bias values before and after the selected azimuth are used as data for generating the azimuth back track. Here, d is a parameter for determining the correlation range with the target, and is calculated by a chi-square distribution with one degree of freedom.

次に、方位角航跡と方位角裏航跡に対応する平滑ベクトルの算出方法について述べる。ゲイン行列Ｋ_ｋ、方位角航跡の平滑ベクトルｘ^_ｋ(＋)及び平滑誤差共分散行列Ｐ_ｋ(＋)はカルマンフィルタの理論により、式（１６）〜式（１８）で与えられる。同様に、正しい方位角の前後の整数値バイアス値に対応する方位角ｕ^n-1 _ｋ及びｕⁿ⁺¹ _ｋをそれぞれ用いた方位角裏航跡の平滑ベクトルをｘ^^a _ｋ(＋)及びｘ^^b _ｋ(＋)とすると、これらは式（１９）及び式（２０）で与えられる。 Next, a method for calculating a smooth vector corresponding to the azimuth wake and the azimuth back wake will be described. The gain matrix K _k , the azimuth wake smooth vector x ^ _k (+), and the smoothing error covariance matrix P _k (+) are given by the equations (16) to (18) according to the Kalman filter theory. Similarly, the smooth vector of the azimuth back wake using the azimuth angles u ⁿ⁻¹ _k and u ^{n + 1} _k corresponding to the integer bias values before and after the correct azimuth are expressed as x ^ ^a _k (+) and x If ^ ^b _k (+), these are given by equations (19) and (20).

以上のように、方位角についても実施の形態１〜３で扱った位相差と全く同じ追尾処理を適用できることが分かる。そしてここで求めた方位角平滑値を複数組み合わせれば、対象物１の位置を推定できる。またその時間変位を算出すれば対象物１の速度を求めることもできる。   As described above, it can be seen that the same tracking process as the phase difference handled in the first to third embodiments can be applied to the azimuth. And if the azimuth angle smooth value calculated | required here is combined together, the position of the target object 1 can be estimated. If the time displacement is calculated, the speed of the object 1 can be obtained.

このことから各整数値バイアスをとる方位角について追尾処理を行い、その平滑値から対象物１の運動状態を示す物理量を算出して、式（２１）及び式（２２）、あるいは式（２７）〜式（２９）を最小にする方位角を算出することで、方位角のアンビギュイティを抑制し、対象物１の位置を精度よく推定することができるのである。   From this, the tracking processing is performed for the azimuth angle at which each integer value bias is taken, and the physical quantity indicating the motion state of the object 1 is calculated from the smoothed value, and Expression (21) and Expression (22) or Expression (27) is calculated. By calculating the azimuth angle that minimizes the expression (29), it is possible to suppress the ambiguity of the azimuth angle and accurately estimate the position of the object 1.

なお、アレーアンテナを用いて方位角を算出する方位角算出器４０の処理については公知の方位角算出方法を用いて構成すればよい。このような方位角の算出方法としては、例えば、隣接するビーム間の受信信号の振幅や位相などの差(Δ値)と和(Σ値)の比(Δ／Σ値)を求め、このΔ／Σ値から反射波の入射方向を求める方法などがある。   In addition, what is necessary is just to comprise about the process of the azimuth angle calculator 40 which calculates an azimuth angle using an array antenna using the well-known azimuth angle calculation method. As a method for calculating such an azimuth angle, for example, a ratio (Δ / Σ value) between a difference (Δ value) and a sum (Σ value) of the amplitude or phase of a received signal between adjacent beams is obtained, and this Δ There is a method of obtaining the incident direction of the reflected wave from the / Σ value.

以上から明らかなように、位相差に替えて方位角から対象物の位置を測定する機器においても、この発明の原理を適用することが可能である。   As is clear from the above, the principle of the present invention can be applied to an apparatus that measures the position of an object from an azimuth angle instead of a phase difference.

以上のように、この発明に係る位置推定装置は、例えば、マルチパスによる干渉波が発生する環境下において対象物から送信される電波を受信して、その対象物の位置を推定するシステムに適用することが可能である。   As described above, the position estimation device according to the present invention is applied to, for example, a system that receives a radio wave transmitted from an object in an environment where multipath interference waves are generated and estimates the position of the object. Is possible.

この発明の実施の形態１による位置推定装置の構成を示すブロック図である。It is a block diagram which shows the structure of the position estimation apparatus by Embodiment 1 of this invention. この発明の実施の形態１による位置推定装置の詳細な構成を示すブロック図である。It is a block diagram which shows the detailed structure of the position estimation apparatus by Embodiment 1 of this invention. この発明の実施の形態１による位置推定装置の詳細な構成を示す別のブロック図である。It is another block diagram which shows the detailed structure of the position estimation apparatus by Embodiment 1 of this invention. 整数バイアス値の異なる位相差の航跡が乗り移る様子を示す図である。It is a figure which shows a mode that the track of a phase difference from which an integer bias value differs changes. 整数バイアス値の異なる位相差の航跡が乗り移る場合の対象物の速度変位を示す図である。It is a figure which shows the speed displacement of the target object when the track of a phase difference from which an integer bias value differs changes. この発明の実施の形態２による位置推定装置の詳細な構成を示すブロック図である。It is a block diagram which shows the detailed structure of the position estimation apparatus by Embodiment 2 of this invention. この発明の実施の形態３による位置推定装置の詳細な構成を示すブロック図である。It is a block diagram which shows the detailed structure of the position estimation apparatus by Embodiment 3 of this invention. この発明の実施の形態３による位置推定装置の詳細な構成を示す別のブロック図である。It is another block diagram which shows the detailed structure of the position estimation apparatus by Embodiment 3 of this invention. この発明の実施の形態４による位置推定装置の構成を示すブロック図である。It is a block diagram which shows the structure of the position estimation apparatus by Embodiment 4 of this invention. この発明の実施の形態４による位置推定装置の詳細な構成を示すブロック図である。It is a block diagram which shows the detailed structure of the position estimation apparatus by Embodiment 4 of this invention.

符号の説明Explanation of symbols

２２−１〜２２−３ 追尾処理器、
２３ 物理量演算器、
２４ 航跡入れ替え器。 22-1 to 22-3 tracking processor,
23 Physical quantity calculator,
24 Wake changer.

Claims (8)

対象物からの電波を複数の受信器により受信して得た受信波間の位相差が整数値バイアスの異なる複数の受信波位相差を含む場合に、これらの位相差に基づく追尾処理を行って上記対象物の位置推定に用いる平滑値を算出し、この平滑値から上記対象物の位置を推定する位置推定装置において、
上記複数の受信波位相差から第１位相差を選択しこの第１位相差に基づく航跡を維持して平滑値を算出する第１追尾処理器と、
上記第１位相差に隣接する第２位相差を選択してその第２位相差に基づく航跡を維持する第２追尾処理器と、
上記第１追尾処理器が算出した平滑値から位相差とは異なる物理量であって上記対象物の運動状態を表す物理量を算出する物理量算出器と、
上記物理量算出器により算出された物理量の連続性に基づいて上記第１位相差に基づく航跡と上記第２位相差に基づく航跡とを入れ替える航跡入れ替え器と、
を備えたことを特徴とする位置推定装置。 When the phase difference between received waves obtained by receiving radio waves from an object by a plurality of receivers includes a plurality of received wave phase differences having different integer value biases, tracking processing based on these phase differences is performed and In a position estimation device that calculates a smooth value used for position estimation of an object and estimates the position of the object from the smooth value,
A first tracking processor that selects a first phase difference from the plurality of received wave phase differences and calculates a smooth value while maintaining a wake based on the first phase difference;
A second tracking processor for selecting a second phase difference adjacent to the first phase difference and maintaining a track based on the second phase difference;
A physical quantity calculator that calculates a physical quantity that is different from a phase difference and represents a motion state of the object from the smooth value calculated by the first tracking processor;
A wake changer that replaces the wake based on the first phase difference and the wake based on the second phase difference based on the continuity of the physical quantity calculated by the physical quantity calculator;
A position estimation apparatus comprising: 上記物理量算出器は、上記平滑値から上記対象物の位置を推定するとともに、この位置の時間変位から上記対象物の速度を算出し、
上記航跡入れ替え器は、上記物理量算出器により算出された速度が所定の値域外にある場合に上記第１位相差に基づく航跡と上記第２位相差に基づく航跡とを入れ替えることを特徴とする請求項１に記載の位置推定装置。 The physical quantity calculator estimates the position of the object from the smooth value, calculates the speed of the object from the time displacement of the position,
The wake changer replaces the wake based on the first phase difference and the wake based on the second phase difference when the speed calculated by the physical quantity calculator is outside a predetermined value range. Item 2. The position estimation device according to Item 1. 上記物理量算出器は、上記平滑値から上記対象物の位置を推定するとともに、この位置の時間変位から上記対象物が基準地点を通過する予測時刻を算出し、
上記航跡入れ替え器は、上記物理量算出器により算出された予測時刻が所定の地域外にある場合に上記第１位相差に基づく航跡と上記第２位相差に基づく航跡とを入れ替えることを特徴とする請求項１に記載の位置推定装置。 The physical quantity calculator estimates the position of the object from the smooth value, calculates a predicted time at which the object passes through a reference point from the time displacement of the position,
The wake changer replaces the wake based on the first phase difference and the wake based on the second phase difference when the predicted time calculated by the physical quantity calculator is outside a predetermined region. The position estimation apparatus according to claim 1. 上記第１及び第２追尾処理器は、上記第１及び第２位相差に基づく航跡としてそれらの位相差に基づいて算出される上記対象物の直交座標の航跡をそれぞれ維持することを特徴とする請求項１に記載の位置推定装置。 The first tracking processor and the second tracking processor respectively maintain a track of orthogonal coordinates of the object calculated based on the phase difference as a track based on the first and second phase differences. The position estimation apparatus according to claim 1. 対象物からの電波を複数の受信器により受信して得た受信波から整数値バイアスの異なる複数の方位角が得られる場合に、これらの方位角に基づく追尾処理を行って上記対象物の位置推定に用いる平滑値を算出し、この平滑値から上記対象物の位置を推定する位置推定装置において、
上記複数の方位角から第１方位角を選択しこの第１方位角に基づく航跡を維持して上記平滑値を算出する第１追尾処理器と、
上記第１方位角に隣接する第２方位角を選択してその第２方位角に基づく航跡を維持する第２追尾処理器と、
上記平滑値から方位角とは異なる物理量であって上記対象物の運動状態を表す物理量を算出する物理量算出器と、
上記物理量算出器により算出された物理量の連続性に基づいて上記第１方位角に基づく航跡と上記第２方位角に基づく航跡とを入れ替える航跡入れ替え器と、
を備えたことを特徴とする位置推定装置。 When a plurality of azimuth angles with different integer value biases are obtained from received waves obtained by receiving radio waves from the target with a plurality of receivers, a tracking process based on these azimuth angles is performed to determine the position of the target object. In a position estimation device that calculates a smooth value used for estimation and estimates the position of the object from the smooth value,
A first tracking processor that selects a first azimuth angle from the plurality of azimuth angles, maintains a track based on the first azimuth angle, and calculates the smooth value;
A second tracking processor for selecting a second azimuth angle adjacent to the first azimuth angle and maintaining a track based on the second azimuth angle;
A physical quantity calculator that calculates a physical quantity that is a physical quantity different from the azimuth angle from the smooth value and represents a motion state of the object;
A wake changer that replaces the wake based on the first azimuth and the wake based on the second azimuth based on the continuity of the physical quantity calculated by the physical quantity calculator;
A position estimation apparatus comprising: 上記物理量算出器は、上記平滑値から上記対象物の位置を推定するとともに、この位置の時間変位から上記対象物の速度を算出し、
上記航跡入れ替え器は、上記物理量算出器により算出された速度が所定の値域外にある場合に上記第１方位角に基づく航跡と上記第２方位角に基づく航跡とを入れ替えることを特徴とする請求項５に記載の位置推定装置。 The physical quantity calculator estimates the position of the object from the smooth value, calculates the speed of the object from the time displacement of the position,
The wake changer replaces the wake based on the first azimuth and the wake based on the second azimuth when the speed calculated by the physical quantity calculator is outside a predetermined value range. Item 6. The position estimation device according to Item 5. 上記物理量算出器は、上記平滑値から上記対象物の位置を推定するとともに、この位置の時間変位から上記対象物が基準地点を通過する予測時刻を算出し、
上記航跡入れ替え器は、上記物理量算出器により算出された予測時刻が所定の地域外にある場合に上記第１方位角に基づく航跡と上記第２方位角に基づく航跡とを入れ替えることを特徴とする請求項５に記載の位置推定装置。 The physical quantity calculator estimates the position of the object from the smooth value, calculates a predicted time at which the object passes through a reference point from the time displacement of the position,
The wake changer replaces the wake based on the first azimuth and the wake based on the second azimuth when the predicted time calculated by the physical quantity calculator is outside a predetermined area. The position estimation apparatus according to claim 5. 上記第１及び第２追尾処理器は、上記第１及び第２方位角に基づく航跡としてそれらの方位角に基づいて算出される上記対象物の直交座標の航跡をそれぞれ維持することを特徴とする請求項５に記載の位置推定装置。 The first and second tracking processors respectively maintain wakes of Cartesian coordinates of the object calculated based on the azimuth angles as wakes based on the first and second azimuth angles. The position estimation apparatus according to claim 5.

Images