JP5419784B2 - Prediction device, prediction system, computer program, and prediction method - Google Patents

Description

  The present invention relates to a prediction apparatus that predicts a future position of a moving observation target.

In order to predict the future position of the observation target, it is necessary to accurately estimate the current position and speed of the observation target.
As a method for observing the position of an observation target, there is a method using a position observation device such as a radar or a global positioning system (GPS) receiver. Since either method has an observation error, there is a technique for estimating the current position or velocity of an observation target using a tracking filter such as a Kalman filter.
A tracking filter such as a Kalman filter may be based on the premise that the observation target is moving according to a predetermined motion model such as constant velocity linear motion. Since the estimation accuracy is low when the motion model does not match the actual motion of the observation target, there is a technique for switching the plurality of motion models and estimating the current position, velocity, etc. of the observation target.
In addition, as a method for observing the angular velocity of an observation target, there is a method using an angular velocity observation device such as a gyro sensor.

JP-A-11-120500 JP-A-2005-274300

When the motion state of the observation target changes, the tracking filter generally has a slow response, so the motion model cannot be switched immediately. An angular velocity observation device generally has a fast response, but has an observation error, and cannot be used for selecting a motion model as it is.
The present invention has been made, for example, in order to solve the above-described problems, and an object thereof is to predict the future position of an observation target with high accuracy.

The prediction device according to the present invention is a prediction device that predicts the future position of the observation target based on the position of the observation target observed by the position observation device and the angular velocity of the observation target observed by the angular velocity observation device.
The prediction device includes an angular velocity error estimation device, an angular velocity correction device, and a predicted position calculation device,
The angular velocity error estimation device estimates an angular velocity error observed by the angular velocity observation device based on the position observed by the position observation device,
The angular velocity correction device corrects the angular velocity observed by the angular velocity observation device based on the error estimated by the angular velocity error estimation device,
The predicted position calculation device predicts a future position of the observation target based on the angular velocity corrected by the angular velocity correction device.

  Since the future position of the observation target is predicted based on the highly accurate angular velocity corrected for the error of the angular velocity observed by the angular velocity observation device, the future position of the observation target can be accurately determined even when the observation target is turning. Can be predicted.

FIG. 2 is a block configuration diagram illustrating an example of a configuration of a tracking system 800 in the first embodiment. FIG. 3 is a detailed block diagram illustrating an example of a configuration of a bias correction unit 141 according to the first embodiment. FIG. 6 is a diagram illustrating an example of a relationship between a time at which the observation device 810 in the first embodiment observes an observation position and a time at which the gyro sensor 820 observes the gyro angular velocity. The figure which shows an example of the relationship of the prediction position 631,632 which the observation angular velocity position prediction part 143 and the estimated angular velocity position prediction part 144 in Embodiment 1 predict. FIG. 6 is a diagram illustrating an example of a relationship among a true angular velocity 606, a gyro angular velocity 616, and a tracking angular velocity 626 in the first embodiment. FIG. 6 is a diagram illustrating another example of the relationship among the true angular velocity 606, the gyro angular velocity 616, and the tracking angular velocity 626 in the first embodiment. FIG. 7 is a flowchart showing an example of a flow of bias correction processing S710 in the first embodiment. FIG. 3 is a detailed block diagram illustrating an example of a configuration of a turning determination unit 142 in the first embodiment. FIG. 6 is a diagram for explaining an operation of a turning determination unit 142 in the first embodiment. FIG. 3 is a detailed block diagram illustrating an example of a configuration of a model selection unit 132 in the first embodiment. FIG. 6 is a diagram for explaining an example of the operation of a transition probability update unit 212 in the first embodiment. The figure which shows the example of the transition probability which the transition probability update part 212 in Embodiment 1 updates. The figure for demonstrating operation | movement of the movement state extrapolation part 180 in Embodiment 1. FIG. FIG. 6 is a block configuration diagram illustrating an example of a configuration of a tracking system 800 according to a second embodiment. FIG. 10 is a block configuration diagram illustrating an example of a configuration of a tracking system 800 according to a third embodiment. FIG. 10 is a detailed block diagram illustrating an example of a configuration of a bias correction unit 141 according to Embodiment 3. The figure which shows an example of the relationship of the prediction position 631,633 which the observation angular velocity position prediction part 143 in Embodiment 3 and the linear position prediction part 148 predict. FIG. 10 is a system configuration diagram illustrating an example of a configuration of a prediction system 801 in a fourth embodiment.

Embodiment 1 FIG.
The first embodiment will be described with reference to FIGS.

  FIG. 1 is a block configuration diagram showing an example of the configuration of the tracking system 800 in this embodiment.

  The tracking system 800 (prediction system) has an observation device 810 (position observation device). The observation device 810 observes the position of a moving body (observation target) such as an aircraft, a ship, or a vehicle. The position of the moving body observed by the observation apparatus 810 is referred to as “observation position”. The observation device 810 is, for example, a sensor such as a radar or a GPS receiver. The observation device 810 may be a device that observes the moving body from the outside, for example, a radar, or may be a device mounted on the moving body, for example, a GPS receiver.

  The tracking system 800 includes a gyro sensor 820 (angular velocity observation device). The gyro sensor 820 is mounted on a moving object that is an observation target. The gyro sensor 820 observes the angular velocity of the mounted mobile body. The angular velocity observed by the gyro sensor 820 is referred to as “gyro angular velocity” (JYRO angular velocity).

  The tracking system 800 includes the target tracking device 100 (tracking unit 130, exercise state determination unit 140, bias correction value memory 150, exercise performance storage unit 160, exercise state extrapolation unit 180). The target tracking device 100 estimates the future position of the moving body with high accuracy based on the observation results of the observation device 810 and the gyro sensor 820.

The target tracking device 100 has a bias correction value memory 150 (angular velocity error storage device). The bias correction value memory 150 stores a bias error calculated by a bias correction unit 141 described later.
The target tracking device 100 includes an exercise performance storage unit 160. The athletic performance storage unit 160 stores the athletic performance of the moving object such as the maximum speed, the maximum acceleration, and the maximum angular velocity of the moving object. The athletic performance storage unit 160 stores, for example, a set of a moving object type and an athletic performance of a moving object belonging to the type in a database format (athletic performance database).

The target tracking device 100 (prediction device) has a tracking unit 130.
The tracking unit 130 includes a plurality of tracking filters 131 (tracking devices). Note that a plurality of tracking filters 131 may be distinguished by adding “lower tracking filter 131a”, “tracking filter 131b”, etc., with alphabetic lowercase letters added to the reference numerals.
The tracking filter 131 performs tracking processing based on a predetermined motion model. That is, the tracking filter 131 assumes the moving body is moving according to a predetermined movement model, and estimates the position and speed of the moving body based on the observation position observed by the observation device 810. Hereinafter, estimated values such as the position and speed of the moving body estimated by the tracking filter 131 are referred to as “tracking position”, “tracking speed”, and the like. The tracking filter 131 is, for example, a Kalman filter, an alpha-beta (α-β) filter, an alpha-beta-gamma (α-β-γ) filter, or another filter.
Each of the tracking filters 131a to 131c has a different motion model. Each of the tracking filters 131a to 131c estimates the position, speed, and the like of the moving body on the assumption that the moving body is moving according to the movement model of each tracking filter 131a to 131c. For example, the tracking filter 131a has a turning motion model as a motion model. That is, the tracking filter 131a estimates the position, velocity, angular velocity, and the like of the moving body on the assumption that the moving body is turning. For example, the tracking filter 131b has a constant velocity linear motion model as a motion model. That is, the tracking filter 131b (straight line position estimation device) estimates the tracking position / speed, etc., assuming that the moving body is moving in a uniform linear motion. For example, the tracking filter 131c has a constant acceleration motion model as a motion model. That is, the tracking filter 131c estimates the position / velocity / acceleration etc. of the moving body on the assumption that the moving body is moving at a constant acceleration.
The tracking filter 131 having the turning motion model may be configured (a predetermined angular velocity position estimating device) on the assumption that the moving body is making a turning motion at a predetermined angular velocity. In that case, the plurality of tracking filters 131 may have a turning motion model, and each may assume different angular velocities.
Similarly, the tracking filter 131 having the uniform acceleration motion model may be configured to assume that the moving body is performing uniform acceleration motion at a predetermined acceleration. In that case, the plurality of tracking filters 131 may have a constant acceleration motion model, and each may assume a different acceleration.

The tracking unit 130 includes a model selection unit 132 (predicted position calculation device). The model selection unit 132 selects the motion model that best matches the actual motion of the moving body based on the determination result of the turning determination unit 142 described later. The model selection unit 132 calculates estimated values such as the position / velocity / angular velocity / acceleration of the moving body based on the selected motion model. Hereinafter, the estimated values such as the position / angular velocity of the moving body calculated by the model selection unit 132 are referred to as “estimated position”, “estimated angular velocity”, and the like.
For example, the model selection unit 132 calculates the probability that the motion model possessed by each of the plurality of tracking filters 131 matches the actual motion of the moving object, and based on the calculated probability, each of the tracking filters 131a to 131a. The tracking position estimated by 131c is weighted and combined to calculate the estimated position.
Alternatively, the model selection unit 132 selects the tracking filter 131 having the highest probability that the motion model matches the actual motion of the moving object from the plurality of tracking filters 131, and the selected tracking filter 131 estimates the selected one. The tracking position is used as the estimated position.

Further, the tracking filter 131 estimates how much error the estimated tracking position and tracking speed include. For example, the tracking filter 131 calculates error variance and error covariance such as the tracking position and tracking speed.
The model selection unit 132 calculates an error variance and an error covariance such as an estimated position and an estimated speed based on the error variance and error covariance calculated by the tracking filter 131. For example, the model selection unit 132 combines error variances and error covariances calculated by the plurality of tracking filters 131, and calculates error variances and error covariances such as estimated positions and estimated speeds. Alternatively, the model selection unit 132 employs the error variance and error covariance calculated by the selected tracking filter 131 as the error variance and error covariance such as the estimated position and the estimated speed.

The target tracking device 100 includes an exercise state determination unit 140. The motion state determination unit 140 determines the motion state of the moving body based on the gyro angular velocity observed by the gyro sensor 820.
The motion state determination unit 140 includes a bias correction unit 141 (an angular velocity error estimation device and an angular velocity correction device). The bias correction unit 141 estimates the bias error of the gyro angular velocity observed by the gyro sensor 820. The bias correction unit 141 corrects the gyro angular velocity based on the estimated bias error, and outputs the corrected gyro angular velocity. For example, the bias correction unit 141 estimates the bias error of the gyro angular velocity observed by the gyro sensor 820 based on the tracking angular velocity (TRK angular velocity) estimated by the tracking filter 131, the estimated angular velocity calculated by the model selection unit 132, and the like.
The motion state determination unit 140 includes a turning determination unit 142 (a turning determination device). The turning determination unit 142 determines whether or not the moving body is turning based on the gyro angular velocity corrected by the bias correction unit 141, the movement performance of the moving body stored in the movement performance storage unit 160, and the like.

  The target tracking device 100 includes a motion state extrapolation unit 180 (predicted position calculation device). The motion state extrapolation unit 180 predicts the future position of the moving object. The position of the moving body predicted by the motion state extrapolation unit 180 is referred to as a “predicted position”. The motion state extrapolation unit 180 calculates a predicted position based on the estimated position / estimated speed calculated by the model selection unit 132, the determination result of the turning determination unit 142, and the like.

FIG. 2 is a detailed block diagram showing an example of the configuration of the bias correction unit 141 in this embodiment.
The bias correction unit 141 inputs the tracking angular velocity estimated by the tracking filter 131a having the turning motion model. The bias correction unit 141 receives the estimated position, estimated speed, error variance, and error covariance calculated by the model selection unit 132. The bias correction unit 141 inputs the gyro angular velocity observed by the gyro sensor 820. In addition, instead of the bias correction unit 141 directly inputting the gyro angular velocity observed by the gyro sensor 820, a smoothing processing unit (tracking filter) that smoothes the gyro angular velocity observed by the gyro sensor 820 is provided, and the smoothing processing unit performs smooth processing. The bias correction unit 141 may be configured to input the gyro angular velocity that has been performed.

  The bias correction unit 141 includes an observation angular velocity position prediction unit 143 (observation angular velocity position prediction device). The observation angular velocity position prediction unit 143 predicts the future position of the moving object based on the estimated position and estimated speed calculated by the model selection unit 132. However, the observed angular velocity position prediction unit 143 uses the gyro angular velocity input by the bias correction unit 141 instead of the estimated angular velocity calculated by the model selection unit 132 as the angular velocity of the moving object. The observation angular velocity position prediction unit 143 predicts the future position of the moving body based on the gyro angular velocity.

FIG. 3 is a diagram showing an example of the relationship between the time when the observation device 810 in this embodiment observes the observation position and the time when the gyro sensor 820 observes the gyro angular velocity.
In general, the observation interval of the gyro sensor 820 is shorter than the observation interval of the observation device 810. At some point, the time the gyro sensor 820 observes the newest gyro angular velocity and t _1, the time at which the observation device 810 has observed the most recent observation position and t _2. In general, time t ₁ and time t ₂ do not coincide with each other, and time t ₁ is later than time t ₂ .
Tracking filter 131, based on the observation position observation device 810 has observed, since estimates the like tracking position, etc. tracking position tracking filter 131 is estimated, but at time t _2. Well as the estimated position model selector 132 is calculated similarly, but at time t _2.
In order to predict the future position of the mobile object by combining the estimated position calculated by the model selection unit 132 and the gyro angular velocity observed by the gyro sensor 820, the time of the estimated position, the time of the gyro angular velocity, Must match.

ただし、山型付きのｘ（文中において「ｘ＾」と表記する。）は、推定値ベクトルである。推定値ベクトルｘ＾は、ｋ次の列ベクトルである。推定値ベクトルｘ＾は、時刻ｔの関数である。推定値ベクトルｘ＾の要素は、所定の座標系で表した時刻ｔにおける推定位置の座標などである。推定値ベクトルｘ＾の次数ｋは、推定値の数である。例えば、３次元座標で表された位置および速度を推定値とする場合、次数ｋは６である。
Ｆは、状態遷移行列である。状態遷移行列Ｆは、ｋ次の正方行列である。状態遷移行列Ｆは、時間Δｔの関数である。状態遷移行列Ｆは、所定の運動モデルにおいて、ある時刻における移動体の位置などを表わすベクトルを、時間Δｔが経過した後における移動体の位置などを表わすベクトルへ写す写像を表わす。
すなわち、この式は、時刻ｔ_２における推定値ベクトルｘ＾（ｔ_２）に、時間（ｔ_１−ｔ_２）の経過を表わす状態遷移行列Ｆ（ｔ_１−ｔ_２）を作用させて、時刻ｔ_１における推定値ベクトルｘ＾（ｔ_１）を算出することを意味している。
なお、状態遷移行列Ｆの運動モデルは、モデル選択部１３２が選択した追尾フィルタ１３１の運動モデルであってもよいし、等速直線運動モデルや旋回運動モデルなどあらかじめ定めた所定の運動モデルであってもよい。通常、時刻ｔ_１と時刻ｔ_２との差は十分小さいので、運動モデルの差による違いはほとんどない。 Therefore, the observed angular position prediction unit 143, for example, performs extrapolation processing on the basis of such estimated position at time t _2, the calculated and estimated position at time t _1.
For example, the observation angular velocity position prediction unit 143 calculates the estimated position at the time t ₁ by calculating the right side of the following equation.

However, x with a mountain shape (indicated as “x ^” in the sentence) is an estimated value vector. The estimated value vector x ^ is a k-th order column vector. The estimated value vector x ^ is a function of time t. The elements of the estimated value vector x ^ are the coordinates of the estimated position at time t expressed in a predetermined coordinate system. The order k of the estimated value vector x ^ is the number of estimated values. For example, when the position and velocity represented by three-dimensional coordinates are estimated values, the order k is 6.
F is a state transition matrix. The state transition matrix F is a k-th order square matrix. The state transition matrix F is a function of time Δt. The state transition matrix F represents a mapping in which a vector representing the position of the moving object at a certain time is mapped to a vector representing the position of the moving object after the time Δt has elapsed in a predetermined motion model.
That is, this equation, the estimated value vector x ^ _{(t 2)} at time _{t 2, the} by the action of time _(t 1 -t ₂₎ the state transition matrix representing the course of the _{F (t} 1 -t _2), the time it means that calculates an estimated value vector x ^ _{(t 1)} at t _1.
The motion model of the state transition matrix F may be a motion model of the tracking filter 131 selected by the model selection unit 132, or may be a predetermined motion model determined in advance, such as a constant velocity linear motion model or a turning motion model. May be. Usually, the difference between the time t ₁ and the time t ₂ is sufficiently small, so there is almost no difference due to the difference between the motion models.

ただし、ｘ＾_Ｊは、時刻ｔ_１におけるジャイロ角速度で推定角速度を置き換えた推定値ベクトルである。Ｆ_Ｊは、時刻ｔ_１におけるジャイロ角速度で移動体が旋回運動をしているという運動モデルに基づく状態遷移行列である。
すなわち、この式は、時刻ｔ_１における推定値ベクトルｘ＾_Ｊ（ｔ_１）に、移動体が時刻ｔ_１におけるジャイロ角速度で旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列Ｆ_Ｊを作用させて、時刻ｔ_３における推定値ベクトルｘ＾_Ｊ（ｔ_３）を算出することを意味している。 Based on the estimated position at the time t _{1 and} the like, the observation angular velocity position prediction unit 143 follows a motion model in which the moving body performs a turning motion at the gyro angular velocity at the time t ₁ , and performs a predetermined time from the time t _1. The position of the moving body at time t _{3 when} ΔT has elapsed (that is, t ₃ = t ₁ + ΔT) is predicted.
For example, the observation angular velocity position prediction unit 143 calculates the position of the moving object at time t ₃ by calculating the right side of the following expression.

However, x ^ _J is an estimated value vector obtained by replacing the estimated angular velocity gyro angular velocity at time t _1. F _J is a state transition matrix based on a motion model in which the moving body performs a turning motion at the gyro angular velocity at time t ₁ .
That is, the state this equation, that the estimated value vector x ^ J _{(t 1)} at time t _1, representing the elapsed time ΔT based on the motion model that the moving body is a turning motion by the gyro angular velocity at time t ₁ by the action of the transition matrix _{F J,} which means that calculates an estimated value vector _x ^ J _{(t 3)} at time _{t 3.}

  Further, the observation angular velocity position prediction unit 143 estimates the error variance and error covariance of the predicted future position of the mobile object based on the error variance and error covariance calculated by the model selection unit 132.

ただし、Ｐは、推定値ベクトルｘ＾の誤差の分散共分散行列である。分散共分散行列Ｐは、ｋ次の正方行列である。分散共分散行列Ｐは、時刻ｔの関数である。分散共分散行列Ｐの要素は、時刻ｔにおける推定値ベクトルの各要素の誤差の間の分散または共分散である。
上付きのＴは、行列の転置を表わす。
Ｑは、システム雑音の分散共分散行列である。分散共分散行列Ｑは、ｋ次の正方行列である。分散共分散行列Ｑは、時間Δｔの関数である。分散共分散行列Ｑの要素は、時間Δｔが経過する間に発生するシステム雑音の分散または共分散である。
すなわち、この式は、時刻ｔ_２における推定誤差の分散共分散行列Ｐ（ｔ_２）に、時間（ｔ_１−ｔ_２）の経過を表わす状態遷移行列Ｆ（ｔ_１−ｔ_２）の転置行列を左から作用させ、時間（ｔ_１−ｔ_２）の経過を表わす状態遷移行列Ｆ（ｔ_１−ｔ_２）を右から作用させたものに、時間（ｔ_１−ｔ_２）の経過により発生したシステム雑音の分散共分散行列Ｑ（ｔ_１−ｔ_２）を加えて、時刻ｔ_１における推定誤差の分散共分散行列Ｐ（ｔ_１）を算出することを意味している。 Similar to the estimated position and the like, the observation angular velocity position prediction unit 143 matches the error variance at time t ₁ based on the error variance at time t ₂ to match the time of error variance and the time of the gyro angular velocity, for example. Is calculated.
For example, the observation angular velocity position prediction unit 143 calculates the error variance and the like at time t ₁ by calculating the right side of the following equation.

Here, P is a variance covariance matrix of the error of the estimated value vector x ^. The variance-covariance matrix P is a k-th order square matrix. The variance-covariance matrix P is a function of time t. The elements of the variance-covariance matrix P are variances or covariances between errors of the elements of the estimated value vector at time t.
The superscript T represents the transpose of the matrix.
Q is a variance-covariance matrix of system noise. The variance-covariance matrix Q is a k-th order square matrix. The variance-covariance matrix Q is a function of time Δt. The element of the variance-covariance matrix Q is the variance or covariance of system noise that occurs during the time Δt.
That is, this equation is obtained by transposing the state transition matrix F (t ₁ -t ₂ ) representing the passage of time (t ₁ -t ₂ ) into the variance-covariance matrix P (t ₂ ) of the estimation error at time t ₂ . was applied from the left, those obtained by the action time _(t 1 -t ₂₎ the state transition matrix representing the course of the _{F (t} 1 -t ₂₎ from the right, caused by the passage of time _(t 1 -t ₂₎ This means that the variance / covariance matrix P (t ₁ ) of the estimation error at time t ₁ is calculated by adding the system noise variance / covariance matrix Q (t ₁ −t ₂ ).

ただし、Ｐ_Ｊは、時刻ｔ_１におけるジャイロ角速度で移動体が旋回運動をしているという運動モデルに基づく予測の誤差の分散共分散行列である。
この式は、時刻ｔ_１における推定誤差の分散共分散行列Ｐ（ｔ_１）に、移動体が時刻ｔ_１におけるジャイロ角速度で旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列Ｆ_Ｊ（ΔＴ）の転置行列を左から作用させ、移動体が時刻ｔ_１におけるジャイロ角速度で旋回運動をしているという運動モデルに基づく時間ΔＴの経過を表わす状態遷移行列Ｆ_Ｊ（ΔＴ）を右から作用させたものに、時間ΔＴの経過により発生したシステム雑音の分散共分散行列Ｑ（ΔＴ）を加えて、時刻ｔ_３についての予測誤差の分散共分散行列Ｐ_Ｊ（ｔ_３）を算出することを意味している。 Based on the calculated error variance at time t ₁ , the observation angular velocity position predicting unit 143 follows an exercise model in which the mobile body performs a turning motion at the gyro angular velocity at time t ₁ , and the error variance at time t ₃ . Is calculated.
For example, the observation angular velocity position prediction unit 143 calculates error variance and the like at time t ₃ by calculating the right side of the following expression.

Here, P _J is a variance-covariance matrix of a prediction error based on a motion model that the moving body is making a turning motion at the gyro angular velocity at time t ₁ .
Condition This equation, which the variance-covariance matrix P of the estimation error at time t ₁ (t _1), represents the elapsed time ΔT based on the motion model that the moving body is a turning motion by the gyro angular velocity at time t ₁ A transposition matrix of the transition matrix F _J (ΔT) is applied from the left, and a state transition matrix F _J (ΔT) representing the passage of time ΔT based on a motion model in which the moving body performs a turning motion at the gyro angular velocity at time t ₁ . ) From the right is added to the variance / covariance matrix Q (ΔT) of the system noise generated over time ΔT, and the variance / covariance matrix P _J (t ₃ ) of the prediction error at time t ₃ Is calculated.

Observation angular position prediction unit 143, for example, as described above, predicts the position and the error variance of the moving object at time t _3.
Note that the observation angular velocity position prediction unit 143 is configured to predict the position of the moving body, error variance, and the like based on the gyro angular velocity corrected by the angular velocity correction unit 147 described later, instead of the gyro angular velocity input by the bias correction unit 141. There may be.

The bias correction unit 141 includes an estimated angular velocity position prediction unit 144 (estimated angular velocity position prediction device). The estimated angular velocity position predicting unit 144 predicts the future position of the mobile object based on the estimated position and estimated speed calculated by the model selecting unit 132. However, the estimated angular velocity position prediction unit 144 uses the tracking angular velocity estimated by the tracking filter 131a as the angular velocity of the moving object, not the estimated angular velocity calculated by the model selection unit 132. The estimated angular velocity position prediction unit 144 predicts the future position of the moving object based on the tracking angular velocity.
Further, the estimated angular velocity position prediction unit 144 estimates the error variance and error covariance of the predicted future position of the moving body based on the error variance and error covariance calculated by the model selection unit 132.
That is, the observed angular velocity position prediction unit 143 uses the gyro angular velocity as the estimated angular velocity, whereas the estimated angular velocity position prediction unit 144 performs the same processing as the observation angular velocity position prediction unit 143 using the tracking angular velocity as the estimated angular velocity. Since the time of the tracking angular velocity estimated by the tracking filter 131a and the time such as the estimated position calculated by the model selection unit 132 are the same, a calculation for making the times coincide is unnecessary.

ただし、ｘ＾_Ｔは、時刻ｔ_２における追尾角速度で推定角速度を置き換えた推定値ベクトルである。Ｆ_Ｔは、時刻ｔ_２における追尾角速度で移動体が旋回運動をしているという運動モデルに基づく状態遷移行列である。Ｐ_Ｔは、時刻ｔ_２における追尾角速度で移動体が旋回運動をしているという運動モデルに基づく予測の誤差の分散共分散行列である。 Estimated angular position prediction section 144, for example, by calculating the right side of the following equation to calculate the like estimated position and error variance at time t _3.

However, x ^ _T is an estimated value vector obtained by replacing the estimated angular velocity by the tracking angular velocity at time t _2. F _T is a state transition matrix based on the motion model of the moving body tracking angular velocity at time t ₂ is the pivoting movement. P _T is a variance-covariance matrix of a prediction error based on a motion model that the moving body is making a turning motion at the tracking angular velocity at time t ₂ .

The estimated angular velocity position prediction unit 144 predicts the position of the moving body, error variance, and the like, for example, as described above.
The estimated angular velocity position predicting unit 144 is not based on the tracking angular velocity estimated by the tracking filter 131a but based on the estimated angular velocity calculated by the model selecting unit 132 (that is, using the estimated angular velocity without replacing it with the tracking angular velocity). It may be configured to predict the position of the moving body, error variance, and the like.

  The bias correction unit 141 includes a condition determination unit 145 (condition determination device). The condition determination unit 145 determines whether a condition (predetermined condition) for estimating an error in the gyro angular velocity observed by the gyro sensor 820 is satisfied.

FIG. 4 is a diagram showing an example of the relationship between the predicted positions 631 and 632 predicted by the observation angular velocity position prediction unit 143 and the estimated angular velocity position prediction unit 144 in this embodiment.
Observation position 611a~611d is at up to time _{t 1,} representing the observed position of the moving object 600 to the observation apparatus 810 has observed. Of these, the observation position 611d is a most new observation position was observed at time t _2.
Tracking unit 130, based on the observation position 611A～611d, calculates the estimated position 621 of the moving object 600 at time _{t 2.}
The observation angular velocity position prediction unit 143 calculates the estimated position 622 of the moving object 600 at time t ₁ based on the estimated position 621 estimated by the tracking unit 130.
The observation angular velocity position prediction unit 143 calculates the predicted position 631 of the moving body 600 at time t ₃ based on the calculated estimated position 622 and the gyro angular velocity.
Estimated angular position prediction section 144, and the like estimated position 621 the tracking unit 130 is estimated, based on the tracking angular velocity tracking filter 131a is estimated to calculate the predicted position 632 of the moving object 600 at time _{t 3.}

If there is no error in the observation position observed by the observation device 810 and the gyro angular velocity observed by the gyro sensor 820, the gyro angular velocity and the tracking angular velocity coincide. Therefore, the predicted position 631 of the moving body 600 predicted by the observation angular velocity position prediction unit 143 and the predicted position 632 of the moving body 600 predicted by the estimated angular velocity position prediction unit 144 should match.
However, actually, the predicted position 631 and the predicted position 632 do not match due to the influence of errors.

The gyro angular velocity observed by the gyro sensor 820 includes a random error and a substantially constant bias error regardless of time. The estimated position estimated by the model selection unit 132 also includes random errors.
Among these, the influence of random errors can be predicted to some extent by the error variance predicted by the observation angular velocity position prediction unit 143 and the estimated angular velocity position prediction unit 144.
Therefore, when the predicted position 631 and the predicted position 632 are predicted to exceed the range expected due to error variance or the like, it is considered to be due to the influence of the bias error of the gyro angular velocity observed by the gyro sensor 820.

ただし、εは、検定値である。
条件判定部１４５は、算出した検定値εを、所定の閾値ε_ｔｈと比較する。閾値ε_ｔｈは、所定の有意水準に基づいて、例えばカイ二乗分布表から求めたものである。検定値εが閾値ε_ｔｈ以下である場合、条件判定部１４５は、予測位置６３１と予測位置６３２との差が、予想される範囲内であると判定する。検定値εが閾値ε_ｔｈより大きい場合、条件判定部１４５は、予測位置６３１と予測位置６３２との差が、予想される範囲を超えていると判定する。 For example, the condition determination unit 145 determines whether or not the difference between the predicted position 631 predicted by the observation angular velocity position prediction unit 143 and the predicted position 632 predicted by the estimated angular velocity position prediction unit 144 is within an expected range. If it exceeds the expected range, it is determined that the condition for estimating the error of the gyro angular velocity is satisfied.
A statistical method can be used for this determination. The condition determination unit 145 determines whether or not the difference between the predicted position 631 and the predicted position 632 is within an expected range, for example, by chi-square test. For example, the condition determination unit 145 calculates a test value by calculating the right side of the following expression.

Here, ε is a test value.
The condition determination unit 145 compares the calculated test value ε with a predetermined threshold value ε _th . The threshold value ε _th is obtained from, for example, a chi-square distribution table based on a predetermined significance level. If test value epsilon is equal to or less than the threshold epsilon _th, condition determining unit 145 determines that the difference between the predicted position 631 and predicted position 632 is within the expected range. When the test value ε is greater than the threshold ε _th , the condition determination unit 145 determines that the difference between the predicted position 631 and the predicted position 632 exceeds the expected range.

FIG. 5 is a diagram showing an example of the relationship among the true angular velocity 606, the gyro angular velocity 616, and the tracking angular velocity 626 in this embodiment.
The horizontal axis represents time t. The vertical axis represents the angular velocity ω.
For example, it is assumed that the moving body 600 moves to a turning motion at a certain time after performing a rectilinear motion.
It is assumed that the true angular velocity 606 of the moving body 600 is 0 during the straight movement, and is constant at a certain value during the turning movement.
Even when the gyro sensor 820 has no bias error, the gyro angular velocity 616 observed by the gyro sensor 820 does not completely match the true angular velocity 606 due to other error factors. However, the gyro angular velocity 616 changes following the change of the true angular velocity 606 without delay.
On the other hand, the tracking angular velocity 626 estimated by the tracking filter 131a has a calculation interval longer than the observation interval of the gyro angular velocity 616, and is greatly affected by random errors. Also, the tracking angular velocity 626 does not immediately follow the change of the true angular velocity 606, but changes slowly. Therefore, immediately after the true angular velocity 606 changes, the difference between the true angular velocity 606 and the tracking angular velocity 626 becomes large due to the transient response.

  Thus, since a difference due to the transient response of the tracking angular velocity occurs during turning, the condition determination unit 145 may determine that the difference between the predicted position 631 and the predicted position 632 exceeds an expected range. For this reason, the condition determination unit 145 sets a bias correction interval. The bias correction section is a determination result for N consecutive times (N is a predetermined positive integer) including the latest determination result among a plurality of determination results arranged in time series. The condition determination unit 145 counts the number of determinations that the difference between the predicted position 631 and the predicted position 632 exceeds the expected range in the bias correction section. The condition determination unit 145 considers that a certain bias has occurred when the number of times is M or more (M is a predetermined positive integer less than or equal to N), and the condition for estimating the bias error is satisfied. It is determined that

  The bias correction unit 141 includes an angular velocity error estimation unit 146 (angular velocity error estimation device). The angular velocity error estimation unit 146 estimates an error of the gyro angular velocity observed by the gyro sensor 820 when the condition determination unit 145 determines that the condition for estimating the bias error is satisfied.

ただし、Δω_ｋは、時刻ｋにおけるバイアス誤差の真の値を表わす。ｚ_ｋは、時刻ｋにおけるバイアス誤差の観測値を表わす。ｖ_ｋは、時刻ｋにおけるバイアス誤差の観測誤差を表わす。ω_Ｊ，ｋは、時刻ｋにおいてジャイロセンサ８２０が観測したジャイロ角速度を表わす。ω_Ｔ，ｋは、時刻ｋについて追尾フィルタ１３１ａが推定した追尾角速度を表わす。ｖ_Ｊ，ｋは、時刻ｋにおいてジャイロセンサ８２０が観測したジャイロ角速度の観測誤差を表わす。ｖ_Ｔ，ｋは、時刻ｋについて追尾フィルタ１３１ａが推定した追尾角速度の推定誤差を表わす。 The angular velocity error estimating unit 146 estimates the bias error of the gyro sensor 820 using, for example, a Kalman filter. The angular velocity error estimation unit 146 uses, for example, an observation model defined by the following equation.

However, Δω _k represents the true value of the bias error at time k. z _k represents an observed value of the bias error at time k. v _k represents an observation error of the bias error at time k. ω _{J, k} represents the gyro angular velocity observed by the gyro sensor 820 at time k. ω _{T, k} represents the tracking angular velocity estimated by the tracking filter 131a at time k. v _{J, k} represents the observation error of the gyro angular velocity observed by the gyro sensor 820 at time k. v _{T, k} represents an estimation error of the tracking angular velocity estimated by the tracking filter 131a at time k.

ただし、ｗ_ｋは、時刻ｋ−１から時刻ｋまでの間に発生するシステム雑音を表わす実数である。 The angular velocity error estimation unit 146 uses a state equation defined by the following equation, for example.

Here, w _k is a real number representing system noise generated between time k-1 and time k.

FIG. 6 is a diagram showing another example of the relationship among the true angular velocity 606, the gyro angular velocity 616, and the tracking angular velocity 626 in this embodiment.
In this example, the gyro angular velocity 616 observed by the gyro sensor 820 includes a bias error Δω. The bias error Δω is basically unchanged regardless of the time.

The angular velocity error estimating unit 146 estimates the bias error Δω as described above, for example.
The bias correction value memory 150 stores the bias error Δω estimated by the angular velocity error estimation unit 146.

The angular velocity correction unit 147 corrects the gyro angular velocity ω _J input by the bias correction unit 141 using the bias error Δω stored in the bias correction value memory 150. For example, the angular velocity correction unit 147 obtains a corrected gyro angular velocity by calculating a difference (ω _J −Δω) obtained by subtracting the bias error Δω from the gyro angular velocity ω _J.
When the angular velocity error estimating unit 146 estimates the bias error Δω, the bias correction value memory 150 stores the bias error Δω estimated by the angular velocity error estimating unit 146. The angular velocity correction unit 147 corrects the error of the gyro angular velocity ω _J using the bias error Δω.
When the angular velocity error estimation unit 146 has not estimated the bias error Δω, the bias correction value memory 150 stores the bias error Δω previously estimated by the angular velocity error estimation unit 146. The angular velocity correction unit 147 corrects the error of the gyro angular velocity ω _J using the bias error Δω previously estimated by the angular velocity error estimation unit 146.
The angular velocity correction unit 147 outputs the corrected gyro angular velocity.

FIG. 7 is a flowchart showing an example of the flow of the bias correction processing S710 in this embodiment.
In the bias correction process S710, the bias correction unit 141 corrects the bias error of the gyro angular velocity.

The bias correction process S710 includes a predicted position calculation step S711. The predicted position calculation step S711 is executed at the beginning of the bias correction process S710.
In the prediction position calculation step S711, the observed angular position prediction unit 143, based on the gyro angular velocity omega _J, is calculated and predicted position and the prediction error variance. Further, the estimated angular velocity position prediction unit 144, based on the tracking angular velocity omega _T, is calculated and predicted position and the prediction error variance.

The bias correction process S710 includes a condition determination step S712. The condition determination step S712 is executed after the predicted position calculation step S711.
In the condition determination step S712, the condition determination unit 145 calculates the test value ε based on the predicted position and the prediction error variance calculated by the observation angular velocity position prediction unit 143 and the estimated angular velocity position prediction unit 144 in the predicted position calculation step S711. To do. The condition determination unit 145 compares the calculated test value ε with the threshold value ε _th to determine whether or not the determination formula is satisfied. The condition determination unit 145 stores the determination result. The condition determination unit 145 obtains the determination results for the past N times including the current determination result from the stored determination results, and does not satisfy the determination formula (that is, the test value ε is greater than the threshold ε _th ). Count the number of times judged. The condition determination unit 145 compares the counted number with the integer M. The condition determination unit 145 branches based on the comparison result.

The bias correction process S710 includes a bias error estimation process S713. The bias error estimation process S713 is executed when the condition determination unit 145 determines that the number of times that the determination formula is not satisfied is M or more in the condition determination step S712.
In the bias error estimation process S713, the angular velocity error estimating unit 146 estimates the bias error Δω of the gyro angular velocity ω _J.

The bias correction process S710 includes a bias error storage step S714. The bias error storage step S714 is executed after the bias error estimation process S713.
In the bias error storage step S714, the bias correction value memory 150 stores the bias error Δω estimated by the angular velocity error estimation unit 146 in the bias error estimation process S713.

The bias correction processing S710 includes a bias error reading step S715. The bias error reading step S715 is executed when the condition determination unit 145 determines that the number of times that the determination formula is not satisfied is less than M times in the condition determination step S712.
In the bias error reading step S715, the angular velocity correction unit 147 reads the bias error Δω stored in the bias correction value memory 150.

The bias correction processing S710 includes a gyro angular velocity correction step S716. The gyro angular velocity correcting step S716 is executed after the bias error storing step S714 and after the bias error reading step S715.
In the gyro angular velocity correction step S716, the angular velocity correction unit 147 determines the gyro angular velocity ω _J based on the bias error Δω estimated by the angular velocity error estimation unit 146 in the bias error estimation process S713 or the bias error Δω read by the angular velocity error estimation unit 146. Correct.

  The bias correction processing S710 ends with the gyro angular velocity correction step S716.

  FIG. 8 is a detailed block diagram showing an example of the configuration of the turning determination unit 142 in this embodiment.

The turning determination unit 142 includes a maximum angular velocity acquisition unit 191. The maximum angular velocity acquisition unit 191 acquires the maximum angular velocity ω _max that is the performance limit value of the moving body from the movement performance of the moving body stored in the movement performance storage unit 160.

The turning determination unit 142 includes an abnormal value determination unit 192. The abnormal value determination unit 192 determines whether or not the (corrected) gyro angular velocity ω _J output from the bias correction unit 141 is an abnormal value based on the angular velocity ω _max acquired by the maximum angular velocity acquisition unit 191. For example, the abnormal value determining unit 192 determines, when the absolute value of the gyro angular velocity omega _J is greater than the angular velocity omega _max, a gyro angular velocity omega _J is an abnormal value. The abnormal value determination unit 192 removes the gyro angular velocity ω _J determined to be an abnormal value, and outputs only the gyro angular velocity ω _J determined not to be an abnormal value.

The turning determination unit 142 includes a threshold value comparison unit 193. The threshold comparison unit 193 compares the gyro angular velocity ω _J output from the abnormal value determination unit 192 with a predetermined threshold ω _min to determine which is larger.
The threshold value ω _min may be a predetermined constant, or may be configured such that the standard deviation of the gyro angular velocity error is calculated and the calculated standard deviation (or a constant multiple thereof) is used as the threshold value ω _min .

The turning determination unit 142 includes a number determination unit 195. The number determination unit 195 calculates the number of times the threshold comparison unit 193 continuously determines that the gyro angular velocity ω _J is _{equal to} or greater than the threshold ω _min .
For example, the number determination unit 195 stores the number of continuous determinations. When the threshold comparison unit 193 determines that the gyro angular velocity ω _J is equal to or greater than the threshold ω _min , the number determination unit 195 increases the stored number of continuous determinations by one. When the threshold comparison unit 193 determines that the gyro angular velocity ω _J is less than the threshold ω _min , the number determination unit 195 initializes the stored number of continuous determinations to zero.
The number determination unit 195 compares the calculated continuous determination number with a predetermined integer M ′ to determine which is larger. When the number of continuous determinations is equal to or greater than the integer M ′, the turning determination unit 142 determines that the moving body is turning.

  The turning determination unit 142 includes a determination result output unit 196. When the number determination unit 195 determines that the continuous determination number is equal to or greater than the integer M ′, the determination result output unit 196 outputs a determination result that the moving body is turning. The determination result output unit 196 may be configured to represent a determination result that is determined that the moving body is turning by outputting the gyro angular velocity output from the abnormal value determining unit 192 as the angular velocity of the moving body. If it is determined that the moving body is not turning, the gyro angular velocity may not be output, and the determination result may be expressed. Alternatively, the moving body is not turning by outputting 0 as the angular velocity of the moving body. The structure showing the determination result determined as may be used.

FIG. 9 is a diagram for explaining the operation of the turning determination unit 142 in this embodiment.
When the gyro angular velocity ω _J is larger than the angular velocity ω _max , the abnormal value determination unit 192 removes the gyro angular velocity at that time and is treated as missing measurement. The threshold value comparison unit 193 compares the gyro angular velocity output from the abnormal value determination unit 192 with the threshold value ω _min . Based on the comparison result of the threshold comparison unit 193, the number determination unit 195 calculates the number of continuous determinations, and the determination result output by the determination result output unit 196 is determined. In addition, when the gyro angular velocity is missing including the case where the abnormal value determination unit 192 is removed, the threshold comparison unit 193 does not perform the comparison itself, so that the calculation of the number of continuous determinations by the number determination unit 195 is not affected.

  FIG. 10 is a detailed block diagram showing an example of the configuration of the model selection unit 132 in this embodiment.

  The model selection unit 132 includes a model probability storage unit 213. The model probability storage unit 213 stores, for each of the tracking filters 131a to 131c, the probability that the motion model possessed by the tracking filter 131 matches the actual motion of the moving object. The probability stored in the model probability storage unit 213 is referred to as “model probability”.

ただし、ｘ＾は、重付平均算出部２１５が算出する推定位置などを表わす推定値ベクトルである。推定値ベクトルｘ＾は、ｋ次の列ベクトルである。Ｘ＾は、追尾値行列である。追尾値行列Ｘ＾は、ｋ行ｊ列の行列である。追尾値行列Ｘ＾のｉ列目（ｉは１以上ｊ以下の整数。）は、ｉ番目の追尾フィルタ１３１の追尾値ベクトルｘ＾_ｉである。追尾値行列Ｘ＾の列数ｊは、運動モデルの数である。追尾値ベクトルｘ＾_ｉは、ｋ次の列ベクトルである。追尾値ベクトルの各要素は、その追尾フィルタ１３１が推定した推定位置の座標などである。ｐは、モデル確率ベクトルである。モデル確率ベクトルは、ｊ次の列ベクトルである。モデル確率ベクトルのｉ行目の要素は、ｉ番目の追尾フィルタ１３１が持っている運動モデルのモデル確率である。
この式は、各運動モデルの追尾値を、モデル確率により重み付けして合成して、推定値を算出することを意味する。 The model selection unit 132 includes a weighted average calculation unit 215. The weighted average calculation unit 215 synthesizes the tracking positions estimated by the tracking filters 131a to 131c based on the model probabilities stored in the model probability storage unit 213, and calculates an estimated position and the like. For example, the weighted average calculation unit 215 calculates an estimated position by calculating the right side of the following expression.

However, x ^ is an estimated value vector representing the estimated position calculated by the weighted average calculating unit 215. The estimated value vector x ^ is a k-th order column vector. X ^ is a tracking value matrix. The tracking value matrix X ^ is a matrix with k rows and j columns. The i-th column (i is an integer of 1 to j) of the tracking value matrix X ^ is a tracking value vector x ^ _i of the i-th tracking filter 131. The number of columns j of the tracking value matrix X ^ is the number of motion models. The tracking value vector x ^ _i is a k-th order column vector. Each element of the tracking value vector is the coordinates of the estimated position estimated by the tracking filter 131. p is a model probability vector. The model probability vector is a j-th order column vector. The i-th element of the model probability vector is the model probability of the motion model possessed by the i-th tracking filter 131.
This equation means that the estimated value is calculated by combining the tracking value of each motion model by weighting the model probability.

  The model selection unit 132 includes a transition probability storage unit 211. For each pair of the two tracking filters 131, the transition probability storage unit 211 starts from the state in which the actual motion of the moving object matches the motion model of the first tracking filter 131. 131 stores a probability of transition to a state that matches the motion model of 131 (a transition probability parameter between models). The probability stored in the transition probability storage unit 211 is referred to as “transition probability”.

ただし、Ｐ_Ｔ，ｋは、時刻ｋにおける推移確率行列である。推移確率行列Ｐは、ｊ次の正方行列である。推移確率行列Ｐ_Ｔ，ｋのａ行ｂ列の要素（ａ及びｂは、１以上ｊ以下の整数。）は、ｂ番目の追尾フィルタ１３１からａ番目の追尾フィルタ１３１への推移確率である。なお、ａとｂとが等しい場合は、その追尾フィルタ１３１の運動モデルが維持される確率を表わす。推移確率行列Ｐ_Ｔ，ｋの次数ｊは、運動モデルの数である。 The model selection unit 132 includes a model probability update unit 214 (weight calculation device). The model probability update unit 214 updates the model probability stored by the model probability storage unit 213 based on the transition probability stored by the transition probability storage unit 211. The model probability update unit 214 updates the model probability by calculating the right side of the following expression, for example.

Here, P _{T, k} is a transition probability matrix at time k. The transition probability matrix P is a j-th order square matrix. The elements of row a and column b of transition probability matrix _{PT, k} (a and b are integers of 1 to j) are transition probabilities from b-th tracking filter 131 to a-th tracking filter 131. When a and b are equal, it represents the probability that the motion model of the tracking filter 131 is maintained. The order j of the transition probability matrix P _{T, k} is the number of motion models.

  The model selection unit 132 includes a transition probability update unit 212. The transition probability update unit 212 updates the transition probability stored in the transition probability storage unit 211 based on the exercise state determined by the exercise state determination unit 140.

FIG. 11 is a diagram for explaining an example of the operation of the transition probability update unit 212 in this embodiment.
The transition probability update unit 212 inputs the gyro angular velocity output from the turning determination unit 142. The transition probability update unit 212 integrates the input gyro angular velocity in a predetermined accumulation interval.

ただし、ｐ_ｍｉｎは、推移確率の下限値である。ｐ_ｍａｘは、推移確率の上限値である。下限値ｐ_ｍｉｎ及び上限値ｐ_ｍａｘは、あらかじめ定めた０以上１以下の実数である。
この式によれば、累積区間におけるジャイロ角速度ω_Ｊの合計が、移動体の性能限界値である角速度の最大値ω_ｍａｘ以上の場合、推移確率更新部２１２は、推移確率ｐ_Ａ→Ｂを上限値ｐ_ｍａｘとする。また、累積区間におけるジャイロ角速度ω_Ｊの合計が０の場合、推移確率更新部２１２は、推移確率ｐ_Ａ→Ｂを下限値ｐ_ｍｉｎとする。累積区間におけるジャイロ角速度ω_Ｊの合計が０と最大値ω_ｍａｘとの間である場合、推移確率更新部２１２は、推移確率ｐ_Ａ→Ｂを、下限値ｐ_ｍｉｎと上限値ｐ_ｍａｘとの間の値とする。 FIG. 12 is a diagram showing an example of the transition probability updated by the transition probability update unit 212 in this embodiment.
For example, it is assumed that there are two motion models, the motion model A is a constant velocity linear motion model, and the motion model B is a turning motion model. The transition probability updating unit 212 changes the transition probability p _{A → B} from the motion model A to the motion model B stepwise using, for example, a value obtained by integrating the gyro angular velocity. By using the accumulated value in the time direction, when the gyro angular velocity data is missing, the transition probability is prevented from changing abruptly. The transition probability update unit 212 calculates the transition probability p _{A → B} by calculating the right side of the following expression, for example.

However, p _min is a lower limit value of the transition probability. p _max is an upper limit value of the transition probability. The lower limit value p _min and the upper limit value p _max are predetermined real numbers of 0 or more and 1 or less.
According to this equation, when the sum of the gyro angular velocities ω _J in the cumulative section is equal to or greater than the maximum angular velocity ω _max that is the performance limit value of the moving object, the transition probability updating unit 212 limits the transition probability p _{A → B} to the upper limit. Let the value p _max . When the total of the gyro angular velocities ω _J in the cumulative section is 0, the transition probability update unit 212 sets the transition probability p _{A → B} as the lower limit value p _min . When the sum of the gyro angular velocities ω _J in the cumulative interval is between 0 and the maximum value ω _max , the transition probability update unit 212 sets the transition probability p _{A → B} between the lower limit value p _min and the upper limit value p _max. The value of

Note that the model selection unit 132 does not perform weighted averaging of the tracking position estimated by the tracking filter 131 and calculate the estimated position, but selects one of the tracking filters 131 and selects the selected tracking filter. The tracking position estimated by 131 may be used as the estimated position.
For example, a filter selection unit is provided instead of the weighted average calculation unit 215. The filter selection unit selects the tracking filter 131 having the motion model with the highest model probability based on the model probability stored in the model probability storage unit 213. The filter selection unit outputs the tracking position estimated by the selected tracking filter 131 as the estimated position.
Further, the model selection unit 132 may be configured to switch models without using the transition probability parameter as a simplified configuration. For example, the model selection unit 132 calculates the ratio of the number of times that the mobile body is determined to be turning based on the determination result determined by the turning determination unit 142 during the cumulative interval. The model selection unit 132 selects the tracking filter 131 having the turning motion model when the calculated ratio exceeds the predetermined ratio, and the tracking filter 131 having the constant velocity linear motion model when the calculated ratio is less than the predetermined ratio. Select. The model selection unit 132 outputs the tracking position estimated by the selected tracking filter 131 as the estimated position.

ただし、Ｆ_０は、移動体が等速直線運動をしているという運動モデルにおける状態遷移行列である。Ｆ_Ｊは、移動体が角速度ω_Ｊで旋回運動をしているという運動モデルにおける状態遷移行列である。ｔ_２は、モデル選択部１３２が算出した最新の推定位置などの時刻である。
この式は、追尾部１３０が算出した最新の推定位置などに基づいて、移動体が旋回していないと旋回判定部１４２が判定した場合は、等速直線運動モデルを用いて移動体の運動状態を外挿し、移動体が旋回していると旋回判定部１４２が判定した場合は、旋回モデルを用いて移動体の運動状態を外挿することにより、移動体の位置を予測することを意味する。 The motion state extrapolation unit 180 predicts the future position of the moving object based on the estimated position calculated by the model selection unit 132 and the gyro angular velocity ω _J output from the turning determination unit 142. Motion state extrapolator 180, for example, by calculating the right side of the following equation to calculate the predicted values such as the position of the moving object at time t _p.

However, F ₀ is a state transition matrix in a motion model in which the moving body is in a constant velocity linear motion. F _J is a state transition matrix in a motion model in which the moving body performs a turning motion at an angular velocity ω _J. t ₂ is the time such as the latest estimated position calculated by the model selection unit 132.
When the turning determination unit 142 determines that the moving body is not turning based on the latest estimated position calculated by the tracking unit 130, the equation indicates that the moving state of the moving body is based on a constant velocity linear motion model. When the turning determination unit 142 determines that the moving body is turning, it means that the position of the moving body is predicted by extrapolating the motion state of the moving body using the turning model. .

  FIG. 13 is a diagram for explaining the operation of the motion state extrapolation unit 180 in this embodiment.

この式は、追尾部１３０が算出した時刻ｔ_２における推定位置などに基づいて、等速直線運動モデルを用いて時刻ｔ_１における推定位置などを算出し、算出した時刻ｔ_１の推定位置などに基づいて、旋回運動モデルを用いて時刻ｔ_ｐにおける推定することを意味している。 Since the turning determination unit 142 determines whether or not the moving body is turning every time the gyro sensor 820 observes the gyro angular velocity, the turning determination unit 142 determines whether or not the moving body is turning. Is generally shorter than the interval at which the model selection unit 132 calculates the estimated position and the like. Therefore, after time t _2, such as the most recent estimated position of the model selection unit 132 is calculated, which may vary the judgment by the turning decision unit 142. For example, as shown in this figure, at time t _2, the while moving entity and does not pivot turning decision unit 142 had been determined, the time t ₁ later, turning the mobile is turning determination section 142 Is determined. In that case, the motion state extrapolator 180, for example, by calculating the right side of the following equation, predicts the position or the like of the moving object at time t _p.

This equation, on the basis of such estimated position at time t ₂ the tracking unit 130 is calculated, to calculate the like estimated position at time t ₁ with a uniform linear motion model, such as the estimated position of the calculated time t ₁ based on, which means that the estimated at time t _p with a swirling motion model.

  In addition, in order to remove a random observation error of the gyro angular velocity, the motion state extrapolation unit 180 may be configured to predict the position of the moving body based on the gyro angular velocity smoothed by a Kalman filter or the like.

The target tracking device 100 calculates an estimated value from the observed value of the target (moving body) using the tracking filter 131 composed of a plurality of different motion models, discriminates the motion state from the angular velocity of the gyro, and enters the motion state. Based on this, predict the future position of the target.
The motion state determination unit 140 performs bias correction on the angular velocity of the gyro using the angular velocity calculated by the tracking unit 130, and determines the motion state using the angular velocity after the bias correction.
The tracking unit 130 selects an appropriate motion model from the motion state and calculates an estimated value.
The motion state extrapolation unit 180 extrapolates time based on the estimated value and the motion state.
Using the angular velocity calculated from the sensor observation position, the bias error that occurs in the angular velocity of the gyroscope is removed, and the corrected angular velocity is used to detect the change in the target's motion faster, and the appropriate motion according to the target's motion Precise prediction based on the model can be performed.

  The motion state determination unit 140 performs the chi-square determination using the prediction error covariance matrix on the difference between the prediction values calculated by extrapolating the gyro angular velocity and the tracking unit angular velocity from the estimated position obtained from the tracking unit 130. If they are not regarded as the same, a bias error included in the angular velocity of the gyro is estimated.

  The motion state determination unit 140 estimates a bias error included in the angular velocity of the gyro using a Kalman filter.

  The motion state determination unit 140 removes a value exceeding the performance limit value of the motion performance database in the angular velocity of the gyro.

  When the observation time of the gyro angular velocity and the estimation time of the tracking unit are different from each other, the motion state determination unit 140 extrapolates the difference between the observation time and the estimation time to set the time of the tracking estimation value to the observation time of the gyro angular velocity. Match.

  The tracking unit 130 changes the model transition probability using the angular velocity calculated by the motion state determination unit 140.

  The tracking unit 130 changes to a turning motion model or an acceleration motion model when the angular velocity calculated by the motion state determination unit 140 is large.

  The motion state extrapolation unit 180 outputs a position extrapolated in time using the angular velocity calculated by the motion state determination unit 140.

  The motion state extrapolation unit 180 outputs a position extrapolated in time using a value obtained by smoothing the angular velocity calculated by the motion state determination unit 140.

  The motion state extrapolation unit 180 performs time extrapolation based on the constant velocity linear motion model until the time when the motion state determination unit 140 determines to turn, and determines the motion state from the time when the turn is determined to the desired time. The position extrapolated in time using the angular velocity calculated by the unit 140 is output.

The tracking unit 130 calculates a target estimated position by performing tracking processing based on a plurality of motion models from observation positions obtained from sensors such as radar or GPS. The motion state determination unit 140 corrects the bias error of the angular velocity obtained from the gyro sensor from the angular velocity estimated by the tracking unit 130, and determines the target turning from the angular velocity. The motion state extrapolation unit 180 predicts the future position by time extrapolation based on the motion phase.
Thereby, compared with the case where a future position is calculated on the assumption that the target is a uniform linear motion, the position prediction accuracy can be increased even during the target turning. Further, even when there is a bias error in the angular velocity obtained from the gyro sensor, the position prediction accuracy can be increased.

The prediction device (target tracking device 100) includes the position of the observation target (moving body) observed by the position observation device (observation device 810) and the angular velocity (gyro angular velocity) of the observation target observed by the angular velocity observation device (gyro sensor 820). ) To predict the future position of the observation target.
The prediction device (100) includes an angular velocity error estimation device (angular velocity error estimation unit 146), an angular velocity correction device (angular velocity correction unit 147), and a predicted position calculation device (model selection unit 132, motion state extrapolation unit 180). Have
The angular velocity error estimation device (146) estimates an error in angular velocity observed by the angular velocity observation device (820) based on the position observed by the position observation device (810).
The angular velocity correction device (angular velocity correction unit 147) corrects the angular velocity observed by the angular velocity observation device (810) based on the error estimated by the angular velocity error estimation device (146).
The predicted position calculation device (132, 180) predicts the future position of the observation target based on the angular velocity corrected by the angular velocity correction device (147).
Since the future position of the observation target is predicted based on the highly accurate angular velocity corrected for the error of the angular velocity observed by the angular velocity observation device, the future position of the observation target can be accurately determined even when the observation target is turning. Can be predicted.

The prediction device (100) includes an angular velocity error storage device (bias correction value memory 150).
The angular velocity error estimation device (146) estimates an error in angular velocity observed by the angular velocity observation device (820) when a predetermined condition is satisfied.
The angular velocity error storage device (150) stores the error estimated by the angular velocity error estimation device (146).
The angular velocity correction device (147) corrects the angular velocity observed by the angular velocity observation device (820) based on the error stored in the angular velocity error storage device (150).
The error of the angular velocity observed by the angular velocity observation device is estimated only when a predetermined condition is satisfied. In other cases, the error of the angular velocity observed by the angular velocity observation device is corrected using the error stored in the angular velocity error storage device. Therefore, for example, when the accuracy of the observation position observed by the position observation device is low, it is possible to prevent the estimation accuracy of the angular velocity error from being lowered.

The prediction device (100) includes a tracking device (tracking unit 130) and a condition determining device (condition determining unit 145).
The tracking device (130) estimates the angular velocity of the observation target based on the position observed by the position observation device (810).
The condition determination device (145) determines whether or not a predetermined condition is satisfied based on the angular velocity observed by the angular velocity observation device (820) and the angular velocity estimated by the tracking device (130).
The angular velocity error estimation device (146) estimates an error in angular velocity observed by the angular velocity observation device (820) when the condition determination device (145) determines that the predetermined condition is satisfied.
Estimate the angular velocity of the observation target based on the position observed by the position observation device, and determine whether the condition for estimating the angular velocity error is satisfied.For example, if the estimation accuracy of the angular velocity of the observation target is low, the angular velocity error It is possible to prevent the estimation accuracy from being lowered.

The prediction device (100) includes an estimated angular velocity position prediction device (estimated angular velocity position prediction unit 144) and an observation angular velocity position prediction device (observation angular velocity position prediction unit 143).
The estimated angular velocity position predicting device (144) predicts the future position of the observation target based on the angular velocity estimated by the tracking device (130).
The observation angular velocity position prediction device (143) predicts the future position of the observation target based on the angular velocity observed by the angular velocity observation device (820).
The condition determination device (145) determines whether the predetermined condition is satisfied based on the position predicted by the estimated angular velocity position prediction device (144) and the position predicted by the observation angular velocity position prediction device (143). Determine whether.
Based on the position of the observation target predicted based on the estimated angular velocity of the observation target and the position of the observation target predicted based on the observed angular velocity, it is determined whether the condition for estimating the error in angular velocity is satisfied. When the prediction accuracy of the position to be observed is low, it is possible to prevent the estimation accuracy of the angular velocity error from being lowered.

The condition determining device (145) determines whether or not there is a significant difference between the position predicted by the estimated angular velocity position predicting device (144) and the position predicted by the observed angular velocity position predicting device (143). Test.
The angular velocity error estimator (146) determines that there is a significant difference between the position predicted by the estimated angular velocity position predictor (144) and the position predicted by the observed angular velocity position predictor (143). When the determination device (145) determines, an error in angular velocity observed by the angular velocity observation device (820) is estimated.
Since it is tested whether there is a significant difference between the two predicted positions to determine whether the condition for estimating the angular velocity error is satisfied, the angular velocity error can be calculated only when the angular velocity error needs to be estimated. Can be estimated, and the calculation load can be reduced.

The prediction device (100) includes a linear position estimation device (tracking filter 131).
The linear position estimation device (131) estimates the position of the observation target based on the position observed by the position observation device (810), assuming that the observation target is in a linear motion.
The predicted position calculation device (132, 180) is based on the position estimated by the linear position estimation device (131) when the turning determination device (142) determines that the observation target is not turning. Predict the future position of the observation target.
When it is determined that the observation target is not turning, the future position of the observation target is predicted based on the position estimated based on the linear motion model, so that the prediction accuracy can be improved.

The prediction device (100) includes a plurality of predetermined angular velocity position estimation devices (tracking filters 131).
The plurality of predetermined angular velocity position estimators (131) assume that the observation target is turning at different predetermined angular velocities, and based on the positions observed by the position observation device (810), Estimate the position.
The predicted position calculation device (132, 180) is configured to detect the position of the observation target based on the angular velocity corrected by the angular velocity correction device (147) and the positions estimated by the plurality of predetermined angular velocity position estimation devices (131). And the future position of the observation target is predicted based on the calculated position of the observation target.
Since the position of the observation target is estimated based on the turning motion model having a predetermined angular velocity, the calculation load can be reduced. In addition, since the position of the observation target is calculated based on the corrected angular velocity and the positions estimated by a plurality of predetermined angular velocity position estimation devices, the position of the observation target can be estimated with high accuracy, and the future of the observation target can be estimated. Can be predicted with high accuracy.

The prediction device (100) includes a weight calculation device (model probability update unit 214).
The weight calculation device (214) calculates a weight (model probability) for each of the plurality of predetermined angular velocity position estimation devices (131) based on the angular velocity corrected by the angular velocity correction device (147).
The predicted position calculation device (weighted average calculation unit 215) combines the positions estimated by the plurality of predetermined angular velocity position estimation devices (131) according to the weight calculated by the weight calculation device (214), The position of the observation target is calculated.
Based on the weighting calculated based on the corrected angular velocity, the positions estimated by a plurality of predetermined angular velocity position estimation devices are combined, so that the position of the observation target can be estimated with high accuracy, and the future position of the observation target can be determined. It can be predicted with high accuracy.

The prediction device (100) includes a turning determination device (turning determination unit 142).
The turning determination device (142) determines whether or not the observation target (moving body) is turning based on the angular velocity corrected by the angular velocity correction device (147).
The predicted position calculation device (132, 180) predicts the future position of the observation target based on the result determined by the turning determination device (142).
Since the future position of the observation target is predicted based on the determination result of the turning determination device, the future position of the observation target can be predicted with high accuracy.

Based on the angular velocity corrected by the angular velocity correction device (147), the predicted position calculation device (filter selection unit) selects a predetermined angular velocity position estimation device (from among the plurality of predetermined angular velocity position estimation devices (131)). 131) is selected, and the future position of the observation target is calculated based on the position estimated by the selected predetermined angular velocity position estimation device (131).
Based on the corrected angular velocity, the position of the observation target is selected from the positions estimated by a plurality of predetermined angular velocity position estimation devices, so that the position of the observation target can be estimated with high accuracy, and the future position of the observation target can be estimated. Can be predicted with high accuracy.

The prediction system (tracking system 800) includes a position observation device (810) that observes the position of the observation target, an angular velocity observation device (820) that observes the angular velocity of the observation target, and a prediction device (100).
Since the future position of the observation target is predicted based on the highly accurate angular velocity corrected for the error of the angular velocity observed by the angular velocity observation device, the future position of the observation target can be accurately determined even when the observation target is turning. Can be predicted.

The prediction device (100) predicts the future position of the observation target based on the position of the observation target observed by the position observation device (810) and the angular velocity of the observation target observed by the angular velocity observation device (820). The prediction method has the following steps.
The angular velocity error estimation device (146) estimates the error of the angular velocity observed by the angular velocity observation device (820) based on the position observed by the position observation device (810).
The angular velocity correction device (147) corrects the angular velocity observed by the angular velocity observation device (820) based on the error estimated by the angular velocity error estimation device (146).
The predicted position calculation device (132, 180) predicts the future position of the observation target based on the angular velocity corrected by the angular velocity correction device (147).
Since the future position of the observation target is predicted based on the highly accurate angular velocity corrected for the error of the angular velocity observed by the angular velocity observation device, the future position of the observation target can be accurately determined even when the observation target is turning. Can be predicted.

  Since an appropriate motion model is selected from a tracking device having a plurality of motion models, the current position of the observation target can be estimated with high accuracy, and the future position of the observation target can be predicted with high accuracy. Compared to selecting an appropriate motion model using the difference (residual) between the position observed by the sensor and the predicted tracking position, the number of samples is not required, and the motion model can be selected quickly.

Embodiment 2. FIG.
The second embodiment will be described with reference to FIG.
In addition, about the part which is common in Embodiment 1, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  FIG. 14 is a block configuration diagram showing an example of the configuration of the tracking system 800 in this embodiment.

  In addition to the structure demonstrated in Embodiment 1, the target tracking apparatus 100 has the map memory | storage part 170 further. The map storage unit 170 stores information (map data) that serves as a clue for estimating the motion state of the moving body, such as the shape of the region in which the moving body moves. For example, when the moving body is a vehicle, the map storage unit 170 stores the shape of the road. Alternatively, when the moving body is an aircraft, the map storage unit 170 stores a flight plan route. For example, the moving body is expected to move linearly on a straight road or on a straight section of a flight plan route.

  The condition determination unit 145 (straight line determination device) of the bias correction unit 141 collates the estimated position of the moving object calculated by the model selection unit 132 with the information stored in the map storage unit 170 so that the moving object moves linearly. Is present in the expected straight line section. The condition determination unit 145 determines to estimate the bias error of the gyro angular velocity when there is a moving body in the straight section.

ただし、Ｉは、直線区間内に移動体がある間に、ジャイロセンサ８２０がジャイロ角速度ω_Ｊを観測した数である。Ｊは、直線区間内に移動体がある間に、追尾フィルタ１３１が追尾角速度ω_Ｔを推定した数である。
すなわち、この式は、直線区間内に移動体がある間に、ジャイロセンサ８２０が観測したジャイロ角速度ω_Ｊを平均し、追尾フィルタ１３１が推定した追尾角速度ω_Ｔを平均して、２つの平均の差を取ることにより、ジャイロ角速度のバイアス誤差Δωを算出することを意味する。 The angular velocity error estimating unit 146 of the bias correcting unit 141 estimates the bias error of the gyro angular velocity observed by the gyro sensor 820 when the condition determining unit 145 determines that the bias error of the gyro angular velocity is estimated. The estimation method may be a method using the Kalman filter described in the first embodiment, but in this embodiment, a different method will be described.
The angular velocity error estimation unit 146 calculates the average value of the gyro angular velocity observed by the gyro sensor 820, calculates the average value of the tracking angular velocity estimated by the tracking filter 131, and calculates the difference between the calculated average values. Get the angular velocity bias error. The angular velocity error estimator 146 calculates the gyro angular velocity bias error Δω, for example, by calculating the right side of the following equation.

However, I is the number that the gyro sensor 820 observed the gyro angular velocity ω _J while there was a moving object in the straight section. J is the number of tracking angular velocities ω _T estimated by the tracking filter 131 while there is a moving object in the straight section.
That is, this equation is obtained by averaging the gyro angular velocity ω _J observed by the gyro sensor 820 while the moving object is in the straight section and averaging the tracking angular velocity ω _T estimated by the tracking filter 131. By taking the difference, it means calculating the bias error Δω of the gyro angular velocity.

  The transition probability update unit 212 of the model selection unit 132 compares the estimated position of the moving object with the information stored in the map storage unit 170 to determine a section where the moving object exists. When it is determined that there is a moving body in the straight section, the transition probability update unit 212 updates the transition probability stored in the transition probability storage unit 211 to increase the transition probability to the linear motion model. In addition, when the transition probability update unit 212 determines that there is a moving body in the curve section, the transition probability update unit 212 updates the transition probability stored in the transition probability storage unit 211 to change the transition probability to the turning model. To increase.

  Note that the model probability may be updated without using the transition probability. For example, when it is determined that there is a moving object in the straight section, the model probability update unit 214 of the model selection unit 132 sets the model probability of the linear motion model to 1 and sets the model probability of other motion models to 0. When it is determined that there is a moving body in the curve section, the model probability update unit 214 sets the model probability of the linear motion model to 0 and the model probability of the turning motion model to 1.

  The tracking unit 130 refers to the map data, and changes to a constant velocity linear motion model on a straight road and a turning motion model or an acceleration motion model on a curved road.

The prediction device (target tracking device 100) includes a straight line determination device (condition determination unit 145).
The straight line determination device (145) determines whether or not the observation target (moving body) is in a linear motion based on the position observed by the position observation device (observation device 810).
The angular velocity error estimation device (angular velocity error estimation unit 146) is observed by the angular velocity observation device (gyro sensor 820) when the linear determination device (145) determines that the observation target is in linear motion. Estimate the error of angular velocity.
When it is determined that the observation target is moving in a straight line, the error in angular velocity observed by the angular velocity observation device is estimated, so the error in angular velocity can be estimated with high accuracy and the future position of the observation target is high. Can be predicted with accuracy.

Embodiment 3 FIG.
The third embodiment will be described with reference to FIGS.
In addition, about the part which is common in Embodiment 1 and Embodiment 2, the same code | symbol is attached | subjected and description is abbreviate | omitted.

FIG. 15 is a block configuration diagram showing an example of the configuration of the tracking system 800 in this embodiment.
The tracking unit 130 has one tracking filter 131b. The tracking filter 131b estimates the position of the moving body based on the constant velocity linear motion model. The tracking filter 131b is, for example, an α-β filter, a Kalman filter, or another filter. By using an α-β filter as the tracking filter 131b, the calculation load can be reduced as compared with a Kalman filter or the like.

FIG. 16 is a detailed block diagram showing an example of the configuration of the bias correction unit 141 in this embodiment.
Based on the tracking position estimated by the tracking filter 131b and the gyro angular velocity observed by the gyro sensor 820, the observation angular velocity position predicting unit 143 follows a motion model in which the moving body performs a turning motion at the gyro angular velocity. Predict the future position of the mobile.
The bias correction unit 141 includes a linear position prediction unit 148 instead of the estimated angular velocity position prediction unit 144. The linear position predicting unit 148 predicts the future position of the moving body according to a motion model in which the moving body is performing a uniform linear motion based on the tracking position estimated by the tracking filter 131b.

FIG. 17 is a diagram illustrating an example of the relationship between the predicted positions 631 and 633 predicted by the observation angular velocity position predicting unit 143 and the linear position predicting unit 148 in this embodiment.
Observation position 611a~611d is at up to time _{t 1,} representing the observed position of the moving object 600 to the observation apparatus 810 has observed. Of these, the observation position 611d is a most new observation position was observed at time t _2.
Tracking unit 130, based on the observation position 611A～611d, calculates the estimated position 621 of the moving object 600 at time _{t 2.}
The observation angular velocity position prediction unit 143 calculates the estimated position 622 of the moving object 600 at time t ₁ based on the estimated position 621 estimated by the tracking unit 130.
The observation angular velocity position prediction unit 143 calculates the predicted position 631 of the moving body 600 at time t ₃ based on the calculated estimated position 622 and the gyro angular velocity.
The linear position prediction unit 148 calculates the predicted position 633 of the moving body 600 at time t ₃ based on the estimated position 621 estimated by the tracking unit 130.

  The condition determination unit 145 determines whether or not there is a significant difference between the predicted position 631 calculated by the observation angular velocity position prediction unit 143 and the predicted position 633 calculated by the linear position prediction unit 148 by chi-square test or the like. Determine. When there is a significant difference between the two predicted positions 631 and 633, the condition determination unit 145 determines to estimate an error in the gyro angular velocity.

The angular velocity error estimating unit 146 estimates the error of the gyro angular velocity when the condition determining unit 145 determines that the error of the gyro angular velocity is estimated. For example, the angular velocity error estimation unit 146 directly uses the gyro angular velocity ω _J observed by the gyro sensor 820 as an observation value of the bias error Δω of the gyro angular velocity. That is, the bias error Δω of the gyro angular velocity is estimated on the assumption that the moving body is moving linearly.

When the moving body does not perform constant velocity linear motion, the motion model of the tracking filter 131b and the actual motion of the moving body do not match, so that the accuracy of estimation by the tracking filter 131b is reduced, and the error variance of estimation is large. Become. Since it cannot be said that there is a significant difference between the predicted position 631 and the predicted position 633, the condition determining unit 145 determines not to estimate the error of the gyro angular velocity.
In other words, when the condition determination unit 145 determines that the error of the gyro angular velocity is estimated, it means that the motion model of the tracking filter 131b matches the actual motion of the mobile body. It is thought that it is in a fast linear motion.

The angular velocity error estimation unit 146 calculates an estimated value of the bias error Δω of the gyro angular velocity using, for example, a Kalman filter. The angular velocity error estimation unit 146 uses, for example, an observation model defined by the following equation.

  In addition, as described in Embodiment 2, the condition determination unit 145 may be configured to determine whether or not there is a moving body in the straight section based on information stored in the map storage unit 170. The condition determination unit 145 determines to estimate an error in the gyro angular velocity when it is determined that there is a moving body in the straight section.

The target tracking device 100 calculates an estimated value from the observed value of the target (moving body) using the tracking filter 131b of the constant velocity linear motion model, discriminates the motion state from the angular velocity of the gyro, and determines the target based on the motion state. Predict the future position of
The motion state determination unit 140 corrects the bias of the angular velocity of the gyro using the estimated value obtained from the tracking unit 130, and determines the motion state using the angular velocity after the bias correction.
The tracking unit 130 calculates an estimated value based on the constant velocity linear motion model from the motion state.
The motion state extrapolation unit 180 extrapolates time based on the estimated value and the motion state.

  The tracking unit 130 uses an α-β filter as the tracking filter 131b.

  The motion state determination unit 140 calculates a difference between a predicted value calculated by extrapolating time from the estimated position obtained from the tracking unit 130 by using the angular velocity of the gyro and a predicted value calculated by extrapolating time by assuming constant velocity linear motion. When the chi-square determination based on the prediction error variance is not considered to be the same, the bias error included in the gyro angular velocity is estimated.

  The motion state determination unit 140 performs bias correction processing when the target exists on a straight road using map data, and does not perform bias correction processing on other routes.

Embodiment 4 FIG.
The fourth embodiment will be described with reference to FIG.
Note that portions common to Embodiments 1 to 3 are denoted by the same reference numerals, and description thereof is omitted.

FIG. 18 is a system configuration diagram showing an example of the configuration of the prediction system 801 in this embodiment.
The prediction system 801 predicts future positions of a plurality of observation objects (moving bodies) and determines whether there is a risk of collision between the observation objects.

  The prediction system 801 includes a position observation device 811. The position observation device 811 observes the position of the observation target. The position observation device 811 is, for example, a radar or a GPS. The position observed by the position observation device 811 is called an observation position.

  The prediction system 801 has an angular velocity observation device 821. The angular velocity observation device 821 observes the angular velocity to be observed. The angular velocity observation device 821 is, for example, a gyroscope. The angular velocity observed by the angular velocity observation device 821 is called an observation angular velocity.

The prediction system 801 includes a prediction device 101. The prediction device 101 estimates the current position and motion state of the observation target based on the observation position observed by the position observation device 811 and the observation angular velocity observed by the angular velocity observation device 821. The prediction device 101 predicts the future position of the observation target based on the estimated result. The prediction apparatus 101 determines whether there is a risk of collision between observation targets based on the predicted result.
The prediction apparatus 101 is a computer, for example.
The prediction device 101 has a storage device (not shown). The storage device stores computer programs and data. The storage device is, for example, a volatile memory (RAM), a nonvolatile memory (ROM), a magnetic disk device, an optical disk device, or the like.
The prediction device 101 has a processing device not shown. The processing device executes the computer program stored in the storage device, thereby processing the data stored in the storage device and controlling the prediction device 101 as a whole.
The prediction device 101 has an input device (not shown). The input device inputs information such as data, signals, and operations from the outside of the prediction device 101 and converts the information into data to be processed by the processing device. The data converted by the input device may be processed directly by the processing device, or may be temporarily stored by the storage device.
The prediction device 101 has an output device (not shown). The output device converts data processed by the processing device, data stored by the storage device, and the like into a format that can be output to the outside of the prediction device 101, such as data, signal, image, and sound, and outputs the data to the outside.
The functional blocks of the prediction device 101 described below are realized when the processing device executes a computer program stored in the storage device. Note that the prediction apparatus 101 may be a single computer, or may be a collection of a plurality of computers that implement one or more functional blocks of the prediction apparatus 101.
The functional blocks of the prediction device 101 may not be realized by a computer, but may be realized by an electronic circuit such as an analog circuit or a digital circuit, or may be realized by a configuration other than an electrical configuration such as a mechanical configuration. Good.

  The prediction device 101 includes a tracking device 231. The tracking device 231 estimates the current motion state of the observation target such as the position, velocity, and angular velocity of the observation target based on the observation position observed by the position observation device 811 by processing the data with the processing device. A numerical value representing the current motion state of the observation target estimated by the tracking device 231 is called an estimated value. The tracking device 231 estimates the estimated value using, for example, a Kalman filter. In addition, the tracking device 231 estimates the accuracy of estimation of the current motion state of the observation target. The tracking device 231 calculates, for example, the expected value variance or covariance of the estimated value error.

The prediction device 101 includes an angular velocity error estimation device 246. The angular velocity error estimation device 246 estimates the error of the observed angular velocity based on the observation angular velocity observed by the angular velocity observation device 821 and the estimated angular velocity estimated by the tracking device 231 as the processing device processes the data. . The angular velocity error estimation device 246 uses, for example, the difference between the observed angular velocity observed by the angular velocity observation device 821 and the estimated angular velocity estimated by the tracking device 231 as an observation value. Etc. The error in the observed angular velocity estimated by the angular velocity error estimating device 246 is called an error estimated value.
If the time when the angular velocity observation device 821 observes the observation angular velocity is different from the time when the tracking device 231 observes the observation position on which the estimated value of the angular velocity is estimated, the angular velocity error estimation device. 246, for example, by smoothing the observation angular velocity observed by the tracking device 231 before and after the time when the position observation device 811 observed the observation position as a reference, at the time when the position observation device 811 observed the observation position. Calculate the observation angular velocity. The observation angular velocity calculated by the smoothing process is called a smooth angular velocity. The angular velocity error estimation device 246 estimates the observation angular velocity error based on the difference between the calculated smooth angular velocity and the estimated angular velocity value estimated by the tracking device 231. Note that the angular velocity error estimation device 246 may be configured to match the observation time by the method described in the first embodiment.

The prediction device 101 includes a condition determination device 245. The condition determination device 245 determines whether or not to estimate the error estimated value based on the estimation accuracy estimated by the tracking device 231 by processing the data by the processing device. If the accuracy of the estimated value of the angular velocity estimated by the tracking device 231 is low, estimating the error estimated value based on the estimated value may reduce the accuracy of the error estimated value. It is determined not to estimate. For example, the condition determining device 245 compares the error variance value calculated by the tracking device 231 with a predetermined threshold value, and determines that the error estimated value is estimated when the error variance value is smaller than the threshold value.
The angular velocity error estimation device 246 estimates the observation angular velocity error when the condition determination device 245 determines that the error estimation value is estimated based on the determination result of the condition determination device 245.

  The prediction device 101 includes an angular velocity error storage device 250. The angular velocity error storage device 250 stores the estimated error value estimated by the angular velocity error estimation device 246 when the storage device stores data.

  The angular velocity error estimation device 246 may be configured to always estimate the error estimation value without providing the condition determination device 245. The angular velocity error estimation device 246 may be configured to change the value of the observation error variance covariance matrix used in the Kalman filter based on the estimation accuracy estimated by the tracking device 231. For example, the angular velocity error estimation device 246 uses the error variance value calculated by the tracking device 231 as the element value of the observation error variance covariance matrix.

  The prediction device 101 includes an angular velocity correction device 247. The angular velocity correction device 247 corrects the error in the observed angular velocity observed by the angular velocity observation device 821 using the estimated error value stored in the angular velocity error storage device 250 by the processing device processing the data. For example, the angular velocity correction device 247 obtains an observation angular velocity in which the error is corrected by calculating a difference obtained by subtracting the error estimated value from the observation angular velocity. The observed angular velocity corrected by the angular velocity correcting device 247 is called corrected angular velocity.

  The prediction device 101 includes a predicted position calculation device 280. The predicted position calculation device 280 processes the data, and the predicted position calculation device 280 determines the observation target based on the current motion state of the observation target estimated by the tracking device 231 and the corrected angular velocity corrected by the angular velocity correction device 247. Predict future motion status. A numerical value representing the future motion state of the observation target predicted by the predicted position calculation device 280 is called a predicted value.

  The prediction device 101 includes a collision risk determination device 290. The collision risk determination device 290 determines whether or not there is a risk that a plurality of observation targets collide based on the future motion state of the observation target predicted by the predicted position calculation device 280 when the processing device processes the data. Determine. When the collision risk determination device 290 determines that there is a risk of collision, the collision risk determination device 290 outputs a warning message to warn about this. The warning message output from the collision risk determination device 290 is displayed on the control screen or transmitted to the observation target, for example, and prompts an action to avoid the collision risk.

The prediction device 101 includes a collision risk determination device 290 and predicts the future position of the observation target for each of a plurality of observation targets.
The collision risk determination device 290 determines whether or not there is a risk that the plurality of observation targets collide based on the position predicted by the predicted position calculation device 280.
Since the future position of the observation target is predicted with high accuracy, the risk of collision can be determined with high accuracy.

When the computer program is executed by the computer, the computer functions as the prediction device 101.
Since the future position of the observation target is predicted based on the highly accurate angular velocity corrected for the error of the angular velocity observed by the angular velocity observation device, the future position of the observation target can be accurately determined even when the observation target is turning. A prediction device capable of prediction can be realized.

  DESCRIPTION OF SYMBOLS 100 target tracking apparatus, 101 prediction apparatus, 130 tracking part, 131 tracking filter, 132 model selection part, 140 motion state determination part, 141 bias correction part, 142 turning determination part, 143 observation angular velocity position prediction part, 144 estimated angular velocity position prediction Unit, 145 condition determination unit, 146 angular velocity error estimation unit, 147 angular velocity correction unit, 148 linear position prediction unit, 150 bias correction value memory, 160 motion performance storage unit, 170 map storage unit, 180 motion state extrapolation unit, 191 maximum Angular velocity acquisition unit, 192 abnormal value determination unit, 193 threshold comparison unit, 195 number determination unit, 196 determination result output unit, 211 transition probability storage unit, 212 transition probability update unit, 213 model probability storage unit, 214 model probability update unit, 215 Weighted average calculation unit, 231 tracking device, 245 Conditional format Device, 246 angular velocity error estimation device, 247 angular velocity correction device, 250 angular velocity error storage device, 280 predicted position calculation device, 290 collision risk determination device, 600 moving body, 606 true angular velocity, 611 observation position, 616 gyro angular velocity, 621, 622 estimated position, 626 tracking angular velocity, 631 to 633 predicted position, 800 tracking system, 801 prediction system, 810 observation apparatus, 811 position observation apparatus, 820 gyro sensor, 821 angular velocity observation apparatus.

Claims (15)

In the prediction device for predicting the future position of the observation target based on the position of the observation target observed by the position observation device and the angular velocity of the observation target observed by the angular velocity observation device,
The prediction device includes an angular velocity error estimation device, an angular velocity correction device, and a predicted position calculation device,
The angular velocity error estimation device estimates an angular velocity error observed by the angular velocity observation device based on the position observed by the position observation device,
The angular velocity correction device corrects the angular velocity observed by the angular velocity observation device based on the error estimated by the angular velocity error estimation device,
The predicted position calculation device predicts a future position of the observation target based on the angular velocity corrected by the angular velocity correction device. The prediction device has an angular velocity error storage device,
The angular velocity error estimation device estimates an angular velocity error observed by the angular velocity observation device when a predetermined condition is satisfied,
The angular velocity error storage device stores the error estimated by the angular velocity error estimation device,
The prediction apparatus according to claim 1, wherein the angular velocity correction device corrects the angular velocity observed by the angular velocity observation device based on an error stored in the angular velocity error storage device. The prediction device includes a tracking device and a condition determination device,
The tracking device estimates the angular velocity of the observation target based on the position observed by the position observation device,
The condition determination device determines whether or not a predetermined condition is satisfied based on the angular velocity observed by the angular velocity observation device and the angular velocity estimated by the tracking device,
3. The prediction apparatus according to claim 2, wherein the angular velocity error estimation device estimates an error of an angular velocity observed by the angular velocity observation device when the condition determination device determines that the predetermined condition is satisfied. . The prediction device includes an estimated angular velocity position prediction device and an observation angular velocity position prediction device,
The estimated angular velocity position prediction device predicts the future position of the observation target based on the angular velocity estimated by the tracking device,
The observation angular velocity position prediction device predicts the future position of the observation target based on the angular velocity observed by the angular velocity observation device,
The condition determination device is configured to determine whether or not the predetermined condition is satisfied based on a position predicted by the estimated angular velocity position prediction device and a position predicted by the observation angular velocity position prediction device. The prediction device according to claim 3. The condition determination device tests whether or not there is a significant difference between the position predicted by the estimated angular velocity position prediction device and the position predicted by the observation angular velocity position prediction device,
The angular velocity error estimation device, when the condition determination device determines that there is a significant difference between the position predicted by the estimated angular velocity position prediction device and the position predicted by the observation angular velocity position prediction device, The prediction device according to claim 4, wherein an error in angular velocity observed by the angular velocity observation device is estimated. The prediction device includes a straight line determination device,
The straight line determination device determines whether the observation target is in a linear motion based on the position observed by the position observation device,
The angular velocity error estimation device estimates an error of an angular velocity observed by the angular velocity observation device when the straight line determination device determines that the observation target is in a linear motion. The prediction device according to claim 5. The prediction device has a turning determination device,
The turning determination device determines whether the observation target is turning based on the angular velocity corrected by the angular velocity correction device,
7. The prediction apparatus according to claim 1, wherein the predicted position calculation apparatus predicts a future position of the observation target based on a result determined by the turning determination apparatus. The prediction device has a linear position estimation device,
The linear position estimation device estimates the position of the observation target based on the position observed by the position observation device, assuming that the observation target is in a linear motion,
The predicted position calculation device predicts the future position of the observation target based on the position estimated by the linear position estimation device when the turning determination device determines that the observation target is not turning. The prediction apparatus according to claim 7. The prediction device has a plurality of predetermined angular velocity position estimation devices,
The plurality of predetermined angular velocity position estimation devices estimate the position of the observation target based on the position observed by the position observation device, assuming that the observation target is swiveling at different predetermined angular velocities,
The predicted position calculation device calculates the position of the observation target based on the angular velocity corrected by the angular velocity correction device and the positions estimated by the plurality of predetermined angular velocity position estimation devices, and calculates the calculated position of the observation target. The prediction apparatus according to claim 1, wherein a future position of the observation target is predicted based on the above. The prediction device includes a weight calculation device,
The weight calculation device calculates a weight of each of the plurality of predetermined angular velocity position estimation devices based on the angular velocity corrected by the angular velocity correction device,
The predicted position calculation device calculates the position of the observation target by combining the positions estimated by the plurality of predetermined angular velocity position estimation devices according to the weight calculated by the weight calculation device. Item 10. The prediction device according to Item 9.   The predicted position calculation device selects a predetermined angular velocity position estimation device to be adopted from the plurality of predetermined angular velocity position estimation devices based on the angular velocity corrected by the angular velocity correction device, and the selected predetermined angular velocity position estimation device is The prediction device according to claim 9 or 10, wherein a future position of the observation target is calculated based on the estimated position. The prediction device includes a collision risk determination device, predicts the future position of the observation target for each of a plurality of observation targets,
12. The collision risk determination device according to claim 1, wherein the collision risk determination device determines whether or not there is a risk that the plurality of observation targets collide based on the position predicted by the predicted position calculation device. The prediction apparatus in any one.   A prediction system comprising: a position observation device for observing the position of an observation target; an angular velocity observation device for observing the angular velocity of the observation target; and the prediction device according to claim 1. .   A computer program which, when executed by a computer, functions as the prediction device according to any one of claims 1 to 12. A prediction device having an angular velocity error estimation device, an angular velocity correction device, and a predicted position calculation device, based on the position of the observation target observed by the position observation device and the angular velocity of the observation target observed by the angular velocity observation device, In the prediction method for predicting the future position of the observation target,
The angular velocity error estimation device estimates the angular velocity error observed by the angular velocity observation device based on the position observed by the position observation device,
The angular velocity correction device corrects the angular velocity observed by the angular velocity observation device based on the error estimated by the angular velocity error estimation device,
A prediction method, wherein the predicted position calculation device predicts a future position of the observation target based on the angular velocity corrected by the angular velocity correction device.

Images