JP2012002734A - Position detecting device, method, and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a position detecting device which requires less computational complexity and which can obtain a reliable measurement result.SOLUTION: A device comprises: distance abnormality determining means for determining whether or not a distance to each landmark device newly measured is abnormal on the basis of a history of the distance to each landmark device; and position determining means for obtaining and outputting a current position by using a Kalman filter on the basis of position information about each landmark device and the distance to each landmark device. In the device, the position determining means calculates a weighting factor, which decreases with an increase in a value obtained by subtracting a predicted value by the Kalman filter from a distance to a first landmark device newly measured, if the distance to the first landmark device newly measured is abnormal, adjusts a Kalman gain by using the calculated weighting factor, and obtains the current position by using the adjusted Kalman gain.

Description

  The present invention relates to a position detection technique for detecting a current position indoors.

  A TDOA (Time Difference Of Arrival) system or the like is known as a method of measuring the current position in an indoor area where a GPS (Global Positioning System) signal cannot be received.

  In the TDOA system, a plurality of landmark devices installed at appropriate intervals periodically transmit a signal for distance measurement, for example, a TDOA signal including a sound wave signal and a radio signal. Each landmark device is controlled to transmit the TDOA signal almost simultaneously, and the radio signal included in the TDOA signal includes information on the installation position of the landmark device that transmitted the radio signal. ing.

  Assuming that the position detection device receives a radio signal from a certain landmark device at time t1 and a sound wave signal from the landmark device at time t2, the distance between the position detection device and the landmark device is the speed of sound × It can be calculated by (t2-t1). Since the position information of the landmark is included in the radio signal from the landmark apparatus, the position detection apparatus calculates its current position by measuring the distance from three or more landmark apparatuses. be able to.

  However, when measuring the position while carrying the position detection device, the measured value is not used as it is, but the current position is usually estimated using a Kalman filter, etc., and corrected using the estimated value. The measured values are output (for example, see Non-Patent Documents 1 to 6).

A. H. Mohamed, et al. , “Adaptive Kalman Filtering for INS / GPS,” Journal of Geodesy, 1999. A. Smith, et al. , “Tracking Moving Devices with the Cricket Location System,” Mobisys, 2004 S. C. Chan, et al. , “A New Robust Kalman Filter Algorithm Under Operators and System Uncertaines,” IEEE International Symposium on Circuits and Systems, 5th year. T.A. Perala, et al, “Robust Extended Kalman Filtering in Hybrid Positioning Applications,” 4th Workshop on Positioning, Navigation and Communication, 2007 J. et al. Ting, et al. , “A Kalman Filter for Robot Operator Detection,” IEEE / RSJ International Conferencing on Intelligent Robots and Systems, 2007 M.M. Shuai, et al. , “A Kalman Filter Based Approach for Outlier Detection in Sensor Networks,” International Conference on Computer Engineering, 200th year. R. Mehra, “On the identification of variants and adaptive Kalman filtering,” Automatic Control, IEEE Transactions on, Vol. 15, no. 2. (1970), pp. 175-184

  However, during movement, the TDOA signal from the landmark apparatus becomes unstable due to the influence of obstacles, multipaths, etc., which causes the measurement accuracy to deteriorate.

  For this reason, for example, Non-Patent Document 2 discloses the use of the LSQ (Least Squares Minimization) algorithm, but the method described in Cited Document 2 is based on a certain assumption condition, and greatly deviates from the assumption condition. In such a case, there is a problem that the accuracy is greatly deteriorated.

  Non-Patent Documents 1 and 3 to 5 disclose that integration processing and iterative calculation processing are performed based on special statistical assumptions, but they are not suitable for mounting on a portable device with low calculation capability. There is no problem.

  Therefore, an object of the present invention is to provide a position detection apparatus, method, and program that can obtain a highly reliable measurement result with a small amount of calculation.

According to the position detection device of the present invention,
Each newly measured land based on the distance history between the receiving device for receiving the position information of the landmark device from the landmark device, the positioning device for measuring the distance from the landmark device, and the distance from each landmark device. Based on the distance anomaly judgment means that determines whether the distance to the mark device is abnormal, the position information of each landmark device, and the distance to each landmark device, the current position is obtained and output by the Kalman filter And a position determining means that, when the distance from the newly measured first landmark device is abnormal, the position determining means determines the Kalman from the distance from the newly measured first landmark device.・ The larger the value obtained by subtracting the predicted value by the filter, the smaller the weighting factor is calculated, the Kalman gain is adjusted by the calculated weighting factor, and the adjusted And it obtains the current position by down gain.

According to another embodiment of the position detection device according to the present invention,
Means for measuring acceleration, and means for determining whether or not the newly measured acceleration is abnormal based on the history of acceleration, wherein the position determining means includes each newly measured landmark device and If any one of the distances is abnormal and the newly measured acceleration is abnormal, it is also preferable to determine that the current position is not measurable.

Further, according to another embodiment of the position detection device according to the present invention,
The distance abnormality determining means determines a first value that is an average value of the difference between the distance from each landmark device measured by the positioning means and the predicted value by the Kalman filter when it is determined that there is no abnormality. A first landmark device that is held and updated based on a second value that is the difference between the distance from each newly measured landmark device and its predicted value, and is newly measured. And a third value that is the difference between the updated average value and the updated average value, a threshold value is determined for the value based on the third value, and the newly measured distance to the first landmark device is It is also preferable to determine whether or not there is an abnormality.

Furthermore, according to another embodiment of the position detecting device according to the present invention,
The distance anomaly judgment means sets the fourth value, which is the standard deviation value of the difference between the distance from each landmark device measured by the positioning means and the predicted value, to the first value and the second value. A first value calculated based on the third value is obtained by dividing the third value by the fourth value to obtain a fifth value, and a threshold value is determined for the value based on the fifth value. It is also preferable to determine whether or not the distance from the mark device is abnormal.

Furthermore, according to another embodiment of the position detecting device according to the present invention,
In the normal distribution in which the average abnormality and the standard deviation are the updated average value and the fourth value, the distance abnormality determination unit has an integral value centered on the average value that is 2 to 1 from the fifth value. A probability density at a position equal to the subtracted value is obtained, a sixth value obtained by multiplying the obtained probability density by the number of landmark devices and the number of times the distance abnormality determining means determines that it is not abnormal is obtained. It is also preferable to determine whether or not the distance from the newly measured first landmark apparatus is abnormal.

According to the program of the present invention,
A computer is made to function as the position detection device.

According to the position detection method of the present invention,
A position detection method in a position detection device that receives signals from a plurality of landmark devices, the step of receiving position information of each landmark device from each landmark device and measuring a distance from each landmark device And determining whether or not the distance to each newly measured landmark device is abnormal based on the history of the distance to each landmark device, position information of each landmark device, and each landmark And a step of obtaining and outputting the current position by a Kalman filter based on the distance to the apparatus, and when the distance from the newly measured first landmark apparatus is abnormal, the newly measured first From the distance to the landmark device, calculate a weighting factor that decreases as the value obtained by subtracting the predicted value by the Kalman filter increases. Adjust the Kalman gain by number, by the Kalman gain after adjustment and obtains the current position.

According to another embodiment of the position detection method according to the present invention,
A step of measuring acceleration, and a step of determining whether or not the newly measured acceleration is abnormal based on a history of acceleration, and any one of the distances to each newly measured landmark device Is abnormal and it is also preferable to determine that the current position is not measurable when the newly measured acceleration is abnormal.

  Even if the distance measurement with each landmark device becomes unstable due to the movement, the position can be estimated without degrading the measurement accuracy. The calculation processing required for position detection according to the present invention is small, and the present invention is suitable for mounting on a portable device.

It is a block diagram of the position detection apparatus by this invention. It is a block diagram of the position detection system by this invention. It is a flowchart of the position detection method by this invention. It is a figure explaining abnormality determination processing.

  EMBODIMENT OF THE INVENTION The form for implementing this invention is demonstrated in detail below using drawing.

  As shown in FIG. 2, the position detection system according to the present invention includes a plurality of landmark devices 20-1 to 20-n and a position detection device 10, and each of the landmark devices 20-1 to 20-n. Periodically transmits a sound wave signal and a radio signal simultaneously. That is, a TDOA signal for distance measurement is transmitted. Preferably, the transmission of the TDOA signal from each of the landmark devices 20-1 to 20-n is controlled to occur synchronously, that is, at approximately the same time. Further, the radio signal included in each TDOA signal includes information on the installed position of the landmark apparatus. Note that when the position detection device 10 is stationary, n must be 3 or more in order to determine the position of the position detection device 10, but in the present invention, the user carries the position detection device 10. Since the past history is used to determine the current position, n may be 2 or more.

  FIG. 1 is a configuration diagram of a position detection apparatus according to the present invention. According to FIG. 1, the position detection device 10 includes a reception unit 1, a distance measurement unit 2, a distance abnormality determination unit 3, an acceleration abnormality determination unit 4, an acceleration measurement unit 5, and a position determination unit 6. Yes. The receiving unit 1 receives a TDOA signal from each landmark device, acquires installation position information of each landmark device from a radio signal included in the TDOA signal, and distances the acquired installation position information of each landmark device. Notify the abnormality determination unit 3 and the position determination unit 6 (not shown).

  Each time the distance measuring unit 2 receives a TDOA signal from each landmark device, the distance measuring unit 2 measures the distance to each landmark device based on the received TDOA signal, and the measured distance is determined as a distance abnormality determining unit 3 and a position determining unit. 6 is output. The acceleration measuring unit 5 includes a biaxial or triaxial acceleration sensor, and outputs the periodically measured acceleration to the acceleration abnormality determining unit 4.

  The distance abnormality determination unit 3 determines whether or not the distance input from the distance measurement unit 2 in each cycle is abnormal from the distance history with each landmark device, and outputs the determination result to the position determination unit 6. To do. Similarly, the acceleration abnormality determination unit 4 determines whether or not the acceleration input from the acceleration measurement unit 5 is abnormal in each cycle from the acceleration history, and outputs the determination result to the position determination unit 6.

  FIG. 3 is a processing flowchart in the position determination unit 6. In the following description, a position that satisfies a distance from each landmark device measured by the TDOA signal at a certain time is referred to as a “measurement position”, and a position predicted by the position detection device 10 at that time is referred to as a “predicted position”. The position obtained from the measured position and the predicted position at the time point is referred to as an “optimal position”. The position determination unit 6 outputs the optimum position to the outside as the position at the time point. The processing flow of FIG. 3 will be described below.

  The position determination unit 6 periodically receives the distances from the distance measurement unit 2 to each landmark device and the determination results from the distance abnormality determination unit 3 and the acceleration abnormality determination unit 4 (S31). When the determination result from the distance abnormality determination unit 3 is normal (S32), the optimal position is obtained by the first position determination process (S34), and the obtained optimal position is output (S35). On the other hand, when the determination result from the distance abnormality determination unit 3 is abnormal (S32), it is confirmed whether the determination result from the acceleration abnormality determination unit 4 is abnormal (S33). If the determination result from the acceleration abnormality determination unit 4 is normal, the optimum position is obtained by the second position decision process (S36), and the obtained optimum position is output (S37). On the other hand, when the determination result from the acceleration abnormality determination unit 4 is abnormal, measurement impossible is output (S38). The position determination unit 6 periodically repeats the processes from S31 to S35, S37, or S38 until an instruction to end the position detection process is received from the outside.

The first position determination process is a process when it is determined that the distance from each landmark device at a certain processing time point is not abnormal from the past history. That is, the process is performed when the received TDOA signal is reliable, and the optimum position is calculated by the Kalman filter. Before describing the first position determination process, each parameter necessary for describing the present invention will be described. First, the position of the position detection device 10 at time k is (p _x (k), p _y (k)), the speed of the position detection device 10 is (v _x (k), v _y (k)),

とすると、Ｘ（ｋ）は、以下の式で表されることになる。なお、Δｔは測定周期である。
Ｘ（ｋ）＝ＡＸ（ｋ−１）＋μ（ｋ）

Then, X (k) is expressed by the following equation. Note that Δt is a measurement cycle.
X (k) = AX (k−1) + μ (k)

  Note that μ (k) in the above equation is noise related to time transition, and follows a covariance matrix Q and a normal distribution. The covariance matrix Q may be a fixed value or may be changed according to time according to the method described in Non-Patent Document 7.

At time k, the position detector 10 receives the TDOA signal from the n landmark devices, the position of the i-th landmark device is (p _xi , p _yi ), and the i-th landmark device calculated from the TDOA signal. the distance between the position detecting device 10 and a d _i,

とすると、Ｚ（ｋ）は、以下の式で表されることになる。
Ｚ（ｋ）＝Ｂ（ｋ）＋ｖ（ｋ）

Then, Z (k) is expressed by the following equation.
Z (k) = B (k) + v (k)

  In the above equation, v (k) is an observation noise and follows a multivariate normal distribution with covariance matrix R and zero mean. The covariance matrix R may be a fixed value or may be changed with time according to the method described in Non-Patent Document 7. In the present invention, the observation model H (k) is defined as the following matrix obtained by partial differentiation of each element of B (k).

(First Position Determination Process) Next, the first position determination process will be described. The position determination unit 6 obtains the prediction matrix X _p (k) at time k from the optimum matrix X _o (k−1) at time k−1 by the following expression.
_{X p (k) = AX o} (k-1)
Note that the prediction matrix X _p and the optimal matrix X _o are 4 rows and 1 column, and the elements of the first row and the third row indicate the x coordinate and the y coordinate of the predicted position and the optimal position, respectively. The elements in the rows indicate velocity components in the x direction and the y direction, respectively. Further, the initial value of the optimal matrix _Xo is arbitrary because it eventually converges. For example, all elements are set to 0.

Subsequently, a prediction error matrix P (k) and a Kalman gain K (k) at time k are obtained by the following equations.
P (k) = AC (k−1) A ^T + Q
K (k) = P (k) H (k) ^T (H (k) P (k) H (k) ^T + R) ⁻¹
Here, C (k−1) is obtained by the following equation.
C (k-1) = (1-K (k-1) H (k-1)) P (k-1)
Note that the initial value of C (k) is arbitrary because it eventually converges, and is, for example, a unit matrix of 4 rows and 4 columns.

Finally, the position determination unit 6 obtains the optimum matrix X _o (k) at time k by the following equation, extracts the first and third rows of the obtained X _o (k), and outputs them as optimum positions.
_{X o (k) = X p} (k) + K (k) (Z (k) -H (k) X p (k))
Also, for processing at time k + 1,
C (k) = (1−K (k) H (k)) P (k) is obtained in advance.

  When the position detection device 10 can always stably receive signals from the respective landmark devices, it is only necessary to repeat the first position determination process described above. However, in reality, the reception state is affected by the influence of an obstacle or the like. It becomes unstable. If the abnormal value detected in this unstable state is used as it is in the first position determination process, its accuracy deteriorates. In the present invention, when the measurement distance is abnormal (S32 in FIG. 3) The optimum position is not determined by the first position determination process, and the optimum position is determined by the second position determination process or the measurement is not possible depending on whether the acceleration is abnormal (S33 in FIG. 3). I do.

(Distance abnormality determining process) The distance abnormality determining process by the distance abnormality determining unit 3 will be described below. The position determination unit 6 calculates the prediction matrix X _p (k) at time k after the process at time k−1, and the positions represented by the elements in the first and third rows, that is, The predicted position at time k is output to the distance abnormality determination unit 3.

The distance abnormality determination unit 3 _obtains a distance d _pi (k) from the i-th landmark device based on the predicted position at time k (i is a natural number equal to or less than n), and each element d _i (Z (k)) Difference γ _i (k) = d _i (k) −d _pi (k) from k) is obtained.

  Further, the distance abnormality determination unit 3 records the number of times it is determined that there is no abnormality in the distance abnormality determination processing at each time up to time k−1, and each γ determined in the processing when there is no abnormality. Average of all

と、時刻ｋにおけるγ_ｉ（ｋ）から、時刻ｋにおけるγの平均値

And the average value of γ at time k from γ _i (k) at time k

を、以下の式により求める。

Is obtained by the following equation.

なお、時刻ｋ−１までの各時刻における距離異常判定処理にて、異常ではないと判断した回数をｐ−１とし、平均値の初期値は、以下の式で表される。

さらに、距離異常判定部３は、時刻ｋにおけるγの分散σ^２（ｋ）を以下の式により求める。

Note that the number of times it is determined that there is no abnormality in the distance abnormality determination processing at each time up to time k−1 is p−1, and the initial value of the average value is expressed by the following equation.

Further, the distance abnormality determination unit 3 obtains the variance σ ² (k) of γ at time k by the following equation.

For each γ _i (k), the distance abnormality determination unit 3 obtains the value of Φ by the following formula, and then obtains the value of 2Φ−1.

続いて、距離異常判定部３は、平均及び分散がそれぞれ

Subsequently, the distance abnormality determination unit 3 determines that the average and the variance are respectively

である正規分布において、その平均値を中心とした積分値が２Φ−１となる位置における確率密度を求める。例えば、Φの値が０．９５であり、図４が求めた正規分布であり、符号６１から６２に示す範囲の積分値が０．９であるものとすると、２Φ−１の値は０．９であるから、図４に示すαに対応する値を求める。

In the normal distribution, the probability density at a position where the integrated value centering on the average value is 2Φ−1 is obtained. For example, assuming that the value of Φ is 0.95, the normal distribution obtained in FIG. 4, and the integrated value in the range indicated by reference numerals 61 to 62 is 0.9, the value of 2Φ−1 is 0. Since it is 9, a value corresponding to α shown in FIG.

  Finally, the distance abnormality determination unit 3 multiplies the obtained probability density value by np, compares this value with a predetermined threshold, for example, 0.5, and if the np multiplied value is smaller than the predetermined threshold, It determines with it being abnormal, and when that is not right, it determines with it being normal. That is, when the positions indicated by reference numerals 61 and 62 in FIG. 4 are separated from the average by a predetermined distance, it is determined to be abnormal.

(Acceleration abnormality determination process) Next, the acceleration abnormality determination process in the acceleration abnormality determination unit 4 will be described. First, the acceleration abnormality determination unit 4 determines the acceleration value a (k) at time k from the acceleration values a _x (k) and a _y (k) in two orthogonal directions at time k acquired from the acceleration measurement unit 5. Is obtained by the following equation.

Note that a _x (k) and a _y (k) may be instantaneous values at the time k acquired by the acceleration measuring unit 5 from the acceleration sensor, but the acceleration measuring unit 5 may perform from the time k−1 to the time k. It is preferable to integrate the values output by the acceleration sensor in a certain period until the acceleration abnormality determination unit 4 outputs the integrated values as a _x (k) and a _y (k).

Subsequently, the acceleration abnormality determining unit 4 obtains the predicted acceleration value a _p (k) at the time k from the acceleration value a (k−1) at the time k by the following expression.
a _p (k) = a (k−1) + Δτ
Δτ is a value represented by the following equation.
Δτ = sgn (a (k−1) −a (k−2)) * Δτ ′
Here, Δτ ′ is an average value of changes in acceleration from time k−M to time k−1 as shown in the following equation.

また、ｓｇｎ（）は、引数の符号を戻り値とする関数であり、具体的には、引数が正の値のときは１、引数が負の値のときは−１、引数が零のときは零がそれぞれ戻り値となる。

Sgn () is a function whose return value is the sign of the argument. Specifically, the argument is 1 when the argument is positive, -1 when the argument is negative, and 0 when the argument is zero. The return value for each is zero.

Subsequently, the acceleration abnormality determination unit 4 obtains an acceleration prediction error diff (k) at time k by the following equation.
diff (k) = a (k) −a _p (k)

  The acceleration abnormality determination unit 4 records the number of times it is determined that there is no abnormality in the acceleration abnormality determination processing at each time up to time k−1, and each prediction error obtained in the processing when there is no abnormality. average of all diffs

と、時刻ｋにおけるｄｉｆｆ（ｋ）から、時刻ｋにおけるｄｉｆｆの平均値

And the average value of diff at time k from diff (k) at time k

を、以下の式により求める。

Is obtained by the following equation.

なお、時刻ｋ−１までの各時刻における加速度異常判定処理にて、異常ではないと判断された回数はｑ−１とする。さらに、加速度異常判定部４は、時刻ｋにおける予測誤差の分散δ^２（ｋ）を以下の式により求める。

Note that the number of times that it is determined that there is no abnormality in the acceleration abnormality determination processing at each time up to time k−1 is q−1. Further, the acceleration abnormality determination unit 4 obtains the prediction error variance δ ² (k) at time k by the following equation.

以下、距離異常判定処理と同じく、加速度異常判定部４は、以下の式によりΨの値を求め、続いて、２Ψ−１の値を求める。

Hereinafter, similarly to the distance abnormality determination process, the acceleration abnormality determination unit 4 calculates the value of Ψ by the following formula, and then calculates the value of 2Ψ−1.

加速度異常判定部４は、平均及び分散がそれぞれ

The acceleration abnormality determination unit 4 has an average and a variance of

である正規分布において、その平均値を中心とした積分値が２Ψ−１となる位置における確率密度を求め、求めた確率密度の値をｑ倍し、この値と所定の閾値とを比較し、ｑ倍した値が所定の閾値より小さい場合、異常であると判定する。

In the normal distribution, the probability density at a position where the integral value centered on the average value is 2Ψ−1 is obtained, the value of the obtained probability density is multiplied by q, and this value is compared with a predetermined threshold value. When the value multiplied by q is smaller than a predetermined threshold, it is determined that there is an abnormality.

  As described above, in the present invention, when a measurement position abnormality is detected in the distance abnormality determination process, it is determined whether an acceleration abnormality has occurred in the acceleration abnormality determination process, and an acceleration abnormality has occurred. Otherwise, the second position determination process described below is performed on the assumption that no significant change has occurred in the operation of the user carrying the position detection device. On the other hand, when an abnormality in acceleration occurs, it is determined that the position cannot be determined, and measurement impossible is output.

(Second Position Determination Process) Next, the second position determination process will be described. In the second position determination process, the position determination unit 6 acquires each parameter used for the determination by the distance abnormality determination unit 3 from the distance abnormality determination unit 3, and for each γ _i (k) at time k, the Mahalanobis distance M _i (k) is obtained by the following equation.

  Subsequently, the position determination unit 6 calculates the weight matrix W (k) as follows.

なお、重み係数ｗ_ｉ（ｋ）は、以下の式で表される。

The weighting factor w _i (k) is expressed by the following equation.

The position determination unit 6 adjusts Z (k), B (k), H (k), and R using a weight matrix as follows.
Z ′ (k) = W (k) Z (k)
B ′ (k) = W (k) B (k)
H ′ (k) = W (k) H (k)
R ′ = cov [W (k) v (k)]

Further, the position determination unit 6 obtains the Kalman gain K ′ (k) adjusted with the weight matrix by the following equation.
K ′ (k) = P (k) H ′ (k) ^T (H ′ (k) P (k) H ′ (k) ^T + R ′) ⁻¹

Finally, the position determination unit 6 obtains the optimum matrix X _o (k) at time k by the following equation, extracts the first and third rows of the obtained X _o (k), and outputs them as optimum positions.
X _o (k) = X _p (k) + K ′ (k) (Z ′ (k) −B ′ (k))
Also, for processing at time k + 1,
C (k) = (1−K ′ (k) H ′ (k)) P (k) is obtained in advance.

  As described above, when the position detection apparatus 10 detects an abnormality in the distance obtained from the TDOA signal, the position detection device 10 obtains a weighting coefficient based on the difference between the predicted position of the measurement position and the past average of the difference, and the obtained weighting coefficient. Thus, by adjusting the Kalman gain and calculating the optimum position, the position can be estimated without degrading the measurement accuracy even when an abnormal value is detected. The calculation processing load required for calculating the optimum position according to the present invention is small, and the present invention is suitable for mounting on a portable device.

  In the above-described embodiment, the position detection device 10 measures the distance from each landmark device using a TDOA signal including a sound wave signal and a radio signal. However, the position detection device 10 is a distance to the landmark device. Various known methods for measuring can be used.

  The position detection apparatus 10 according to the present invention can be realized by a program that causes a computer to function as each unit in FIG. These computer programs can be stored in a computer-readable storage medium or distributed via a network. Furthermore, the present invention can be realized by a combination of hardware and software.

DESCRIPTION OF SYMBOLS 1 Reception part 2 Distance measurement part 3 Distance abnormality determination part 4 Acceleration abnormality determination part 5 Acceleration measurement part 6 Position determination part 10 Position detection apparatus 20-1 to 20-n Landmark apparatus

Claims (8)

Receiving means for receiving position information of the landmark device from the landmark device;
Positioning means for measuring the distance from the landmark device;
Distance abnormality determination means for determining whether or not the distance to each newly measured landmark device is abnormal based on the history of the distance to each landmark device;
Position determining means for obtaining and outputting the current position by a Kalman filter based on the position information of each landmark device and the distance from each landmark device;
With
The position determination means subtracts the predicted value by the Kalman filter from the distance from the newly measured first landmark device when the distance from the newly measured first landmark device is abnormal. The larger the value, the smaller the weighting factor is calculated, the Kalman gain is adjusted by the calculated weighting factor, and the current position is obtained by the adjusted Kalman gain.
Position detection device. Means for measuring acceleration;
Means for determining whether the newly measured acceleration is abnormal based on the history of acceleration;
Further comprising
The position determination means determines that the current position is not measurable if any of the distances to each newly measured landmark device is abnormal and the newly measured acceleration is abnormal.
The position detection device according to claim 1. The distance abnormality determining means determines a first value that is an average value of the difference between the distance from each landmark device measured by the positioning means and the predicted value by the Kalman filter when it is determined that there is no abnormality. A first landmark device that is held and updated based on a second value that is the difference between the distance from each newly measured landmark device and its predicted value, and is newly measured. And a third value that is the difference between the updated average value and the updated average value, a threshold value is determined for the value based on the third value, and the newly measured distance to the first landmark device is Determine whether it is abnormal,
The position detection device according to claim 1 or 2. The distance anomaly judgment means sets the fourth value, which is the standard deviation value of the difference between the distance from each landmark device measured by the positioning means and the predicted value, to the first value and the second value. A first value calculated based on the third value is obtained by dividing the third value by the fourth value to obtain a fifth value, and a threshold value is determined for the value based on the fifth value. Determine whether the distance from the marking device is abnormal,
The position detection device according to claim 3. In the normal distribution in which the average abnormality and the standard deviation are the updated average value and the fourth value, the distance abnormality determination unit has an integral value centered on the average value that is 2 to 1 from the fifth value. A probability density at a position equal to the subtracted value is obtained, a sixth value obtained by multiplying the obtained probability density by the number of landmark devices and the number of times the distance abnormality determining means determines that it is not abnormal is obtained. To determine whether or not the distance from the newly measured first landmark device is abnormal,
The position detection device according to claim 4.   A program that causes a computer to function as the position detection device according to any one of claims 1 to 5. A position detection method in a position detection device for receiving signals from a plurality of landmark devices,
Receiving the position information of each landmark device from each landmark device and measuring the distance to each landmark device;
Determining whether the distance to each newly measured landmark device is abnormal based on the history of distance to each landmark device; and
Based on the position information of each landmark device and the distance to each landmark device, obtaining and outputting the current position by a Kalman filter;
With
When the distance from the newly measured first landmark device is abnormal, the larger the value obtained by subtracting the predicted value by the Kalman filter from the distance from the newly measured first landmark device, Calculate a smaller weighting factor, adjust the Kalman gain with the calculated weighting factor, and obtain the current position with the adjusted Kalman gain.
Position detection method. Measuring the acceleration;
Determining whether the newly measured acceleration is abnormal based on the acceleration history;
Further comprising
If any of the distances to each newly measured landmark device is abnormal and the newly measured acceleration is abnormal, it is determined that the current position is not measurable,
The position detection method according to claim 7.

Images