JPH0695138B2 - Tracking filter - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial field of application] The present invention inputs target position information including observation noise and a target distance change rate from a target observation arrangement to obtain various target motions such as a true value and velocity of the target position. This paper proposes a method for improving the accuracy of the tracking filter that estimates the element.

[Conventional technology]

Figure 2 shows, for example, IEEE TRANSACTIONS ON AEROSPACE AND
ELECTRONIC SYSTEMS VOL AES-13 NO.3, MAY 1977, P310〜
P317 "MANEUVERING TARGET TRACKING USING ADAPTIVE STATE
FIG. 6 is a configuration diagram of a conventional tracking filter having a plurality of motion models indicated by “ESTIMATON”. In the figure
(1) is a target observation device that measures the target position and target distance change rate including observation noise, and (2) is the position of each motion model for evaluating the target position observation error for each of multiple motion models. Observation error evaluator, (3) reliability calculator for each motion model to calculate the reliability of each motion model, (4) first delay circuit, (5) target motion parameters The gain matrix calculator for calculating the Kalman gain matrix used to calculate the smoothed value of (6), the smoother for calculating the smoothed value of the target motion specifications, and (7) for each of the multiple motion models Prediction value calculator for each motion model that calculates the predicted values of the target motion parameters after sampling, (8) is the third delay circuit, and (9) is the difference between the smooth value and the true value for each of the multiple motion models. Smoothing error evaluation device for each motion model that evaluates the smoothing error The prediction error evaluator for each motion model that evaluates the prediction error, which is the difference between the prediction value and the true value of each motion model, (11) is the second delay circuit, and (12) is the fourth delay circuit. .

A conventional tracking filter having a plurality of motion models is configured as described above, and, for example, using fixed Cartesian coordinates, a model in which a constant acceleration vector independent of the sampling time is added to a constant velocity linear motion model is used. The case of having each acceleration level is used as a plurality of motion models. In the target observation device (1), the target position information including the observation noise in polar coordinates is converted into Cartesian coordinates.
In the position observation error evaluator (2) for each motion model, 1 from the current time obtained from the predicted value calculator (7) for each motion model
The target position prediction vector calculated before sampling is used as the average vector of the target position, and the prediction error covariance matrix calculated one sampling before the current time obtained from the prediction error evaluator (10) for each motion model and preset The covariance matrix of the difference between the target position observation vector and the target position prediction vector is obtained from the covariance matrix of the observation noise obtained from the observation model, and the target position observation vector is assumed to follow a three-dimensional normal distribution. The error of the target observation position input from the observation device with respect to the target predicted position is evaluated. In the reliability calculator (3) for each motion model,
The reliability of each of a plurality of motion models calculated one sampling before the present time, the Markov property in the motion model transition is assumed, and the preset transition probability of the motion model and the position observation error evaluator for each motion model (2) The reliability of each of the plurality of motion models is calculated from the target position observation error evaluation value for each of the plurality of motion models input. In the gain matrix calculator (5), the observation noise covariance matrix obtained from the preset observation model and the motion model calculated by the prediction error calculator (10) for each motion model one sampling before the current time The gain matrix is calculated from the Kalman filter theory from the prediction error covariance matrix for each. The smoother (6) calculates the smoothed value one sampling before the present time, the target observation position input from the target observation device (1), the gain matrix input from the gain matrix calculator (5) and each Smoothness value of target position and target velocity by summation of constant acceleration vectors constituting a plurality of motion models weighted by the reliability of each motion model obtained by the motion model reliability calculator (3) To calculate. The predicted value calculator (7) for each motion model uses the target position and target speed calculated by the smoother (6) and the constant acceleration vector for each motion model
The predicted values of the target position and the target speed after sampling are calculated. In the smoothing error evaluator (9) for each motion model, the covariance matrix of the difference between the smoothed value and the true value of the target position and target velocity is calculated by the gain matrix calculator (5) and one sampling before the present time. Prediction error calculator for each motion model
It is calculated by the prediction error covariance matrix calculated in (10). The prediction error calculator (10) for each motion model uses the smoothing error covariance matrix input from the smoothing error evaluator (9) for each motion model and the driving noise covariance matrix obtained from the preset motion model. A covariance matrix of the difference between the predicted value and the true value after one sampling at the present time is calculated.

[Problems to be Solved by the Invention]

In the conventional tracking filter as described above, even when the target distance change rate is observed by the target observing device (1) like the pulse Doppler radar, the tracking filter is configured only by the target position information, so the observation is performed when the target turns. The rapid change in the target distance change rate is calculated by the reliability calculator of each motion model.
It is not reflected in the reliability of each of the multiple motion models obtained in (3), and the smoothed values of the target position and target velocity calculated by the smoother (6) are calculated by the reliability calculator (3) of each motion model. Despite the values that reflect the reliability of each of the obtained multiple motion models, multiple motions are required to evaluate the error of the smoothed value, calculate the predicted value of the target position and target speed, and evaluate the error of the predicted value. The reliability of each model was not reflected. For this reason, the tracking accuracy with respect to the turning target must be degraded.

The present invention has been made in order to solve such a problem, and an object thereof is to obtain a tracking filter capable of accurately tracking even a turning target.

[Means for Solving the Problems]

The tracking filter of the present invention uses a target distance change rate to calculate the reliability of each of a plurality of motion models, and a smooth error covariance matrix that is an evaluation of a smooth error that is a difference between a smooth value and a true value.
The reliability of each of the plurality of motion models is used for the calculation of the predicted value and the calculation of the prediction error covariance matrix that is the evaluation of the prediction error that is the difference between the predicted value and the true value.

[Action]

In the present invention, the probability density of the difference between the observed value of the target distance change rate and the predicted value of the distance change rate of each of the plurality of motion models (that is, the target distance change rate observation error evaluation value) is set to the plurality of motion models. Calculated from the prediction error covariance matrix and the target distance change rate observation noise for each motion model, which is the prediction error that is the difference between the prediction value of each motion model and the prediction value of each motion model and the true value, It is used as a weighting factor for calculating the reliability of each of a plurality of motion models and is used together with the target position observation error evaluation value for each of a plurality of motion models calculated from the target position information. The reliability for each of a plurality of motion models is calculated more appropriately than the case where only the target position information is calculated. Calculation of the parameters for increasing the smooth error covariance matrix, which is the evaluation of the smooth error, by using the reliability for each of the motion models and the constant acceleration vector that constitutes a plurality of motion models, and the motion model is not uniquely determined. Then, by adding the smooth error covariance matrix for each motion model to this specification, the smooth error covariance matrix obtained by integrating the effects of each motion model is calculated. Using the reliability for each model and the constant acceleration vector that composes multiple motion models, the influence term of acceleration in the prediction is calculated, and the influence of each motion model at the current time is integrated into this specification and obtained. Calculated by constant velocity linear motion prediction based on the smoothed value 1
The predicted values obtained by integrating the influences of multiple motion models are calculated by adding the predicted values after sampling, and the reliability for each of the motion models and the constant acceleration that constitutes the motion models are calculated. By using the vector, the parameters that increase the prediction error covariance matrix, which is the evaluation of the prediction error by not uniquely determining the motion model, are calculated. Is added to calculate the prediction error covariance matrix obtained by integrating the effects of the plurality of motion models.

〔Example〕

FIG. 1 is a block diagram showing an embodiment of the present invention. (1)
(12) is exactly the same as the above conventional device. (13) is a smoothing error evaluator that evaluates the smoothing error of the smoothed value, (14) is a predictive value calculator that calculates a predicted value one sampling after the current time, and (15) is a predicted value one sampling after the current time. The prediction error evaluator that evaluates the prediction error, and (16) is the distance change rate observation error evaluator for each motion model.

The motion model in the case of N pieces is x _k = Φ _k-1 x _k-1 + P _k-1 W _k-1 + P ′ _k-1 u _k-1 (1).

Where • x _k is the state vector that represents the true value of the target motion specifications at the sampling time t _k , and the target position vector in Cartesian coordinates is The target velocity vector in Cartesian coordinates When Is.

・ Φ _k-1 is a transition matrix of the state vector x _k from sampling time t _k-1 to t _k , and it is assumed that the target performs a uniform linear motion. Is.

· W _k is driving noise vector P _k at the sampling time t _k for example a transformation matrix driving noise vector at sampling time t _k, the truncation error term due to the assumed constant velocity linear motion target motion model P _{k- If we consider 1} W _k-1 , W _k is equivalent to the acceleration vector. Is.

-U _k is a constant acceleration vector forming N motion models at the sampling time t _k , and u _k = α ₁ or u _k = α ₂ or ... or u _k = α _N (2) and P ′ _k Is the transformation matrix of the constant acceleration vector at sampling time t _k Is.

Fig. 3 is a diagram for explaining the constant acceleration vector in the plane parallel to the horizontal plane. In the figure, 0 is the origin of coordinates 0-xy with the target observation device as the origin, and X is the east direction with the positive x-axis. 0-
x-axis of xy, Y is y of coordinate 0-xy with north direction as positive y-axis
Axis, A1 is a constant acceleration vector in the positive direction of the y-axis, A2 is a constant acceleration vector in the positive direction of the x-axis, A3 is a constant acceleration vector in the negative direction of the y-axis, A4 is a constant in the negative direction of the x-axis It is an acceleration vector. The magnitude of the constant acceleration vector in Fig. 3 is 10g (g is the gravitational acceleration), and in the case of a motion model that considers the constant acceleration vector with zero acceleration, N = 5 (3) (2) is And can be written regardless of the sampling time t _k .

Next, the hypothesis that u _k-1 = α _a (a = 1,2, ..., N) (5) at the sampling time t _k is true is Φ k, a (a = 1,2, ……, N) …… (6) is written.

Let E be the symbol for the mean, and w _k is the three-dimensional normally distributed white noise with mean 0 E [ W _k ] = 0 (7) E [ W _k ▲ W ^T _l ▼] = Q _k (k = 1 , 0I (when k ≠ l)
…… (8)

Here, 2.0 is zero vector, Q _k is the process noise covariance matrix at sampling time t _k, a ^T represents the transposed vector of the vector a.

Let the target position observation model be Z _k = H _k x _k + v _k (9).

Here, · Z k is the target position observation vector · H _k by configured orthogonal coordinate than the observed value of the target position information at sampling time t _k is H _k = (I 0I) in the observation matrix at sampling time t _k .

・V _k is the target position observation noise vector at the sampling time t _k , and is a three-dimensional normally distributed white noise with mean 0 E [ v _k ] = 0 E [ v _k ▲ v ^T _l ▼] = R _k (k = 1), 0I (when k ≠ l)
… (10).

Note that R _k is the target position observation noise covariance matrix at the sampling time t _k , and is a value that does not depend on the motion model.

Target distance change rate observation model_φ(K) = (k) + v (K) Set as (11).

here, ·_φ(K) is the sampling time t_kTarget distance change in
Observed value of conversion rate ・ (k) is sampling time t_kChange in target distance in
True value of rate v (K) is the sampling time t_kTarget distance change in
One-dimensional normally distributed white noise with zero mean
So E [v (K)] = 0 (12) E [v (K) v (L)] ▲ V^Two ▼ (k) (k = 1
, 0 (when k ≠ l) (13).

In addition, ▲ V^Two ▼ (k) is sampling time t_kIn
The variance of the target distance change rate, which is a value that does not depend on the motion model.
It

If the target distance is R (k), that is, R ² (k) = ▲ x ² _k ▼ + ▲ y ² _k ▼ + ▲ z ² _k ▼ (14), then both sides of equation (14) are differentiated. do it Is.

Let Z ^{k be} the entire target position observation vector from sampling time t ₁ to t _k, and ▲ Z be the entire observed value of the target distance change rate.
^k _D ▼, that is, Z ^k = [ Z ₁ , Z ₂ , ・ ・ ・, Z _k ] ... (16) ▲ Z ^k _D ▼ ＝ [ _φ (1), _φ (2), ……,
_φ (k)] …… (17) is written.

From the above, from the usual theory of Kalman filter ▲ ^a _k▼ (-) = Φ_{k-1 k-1}(+) + P '_k-1 α _a
…… (18) ▲P ^a _k▼ (-) = Φ_k-1P_k-1(+) ▲ Φ^T _k-1▼ + P_k-1Q
_{k-1 ▲}P^T _k-1▼ …… (19) ▲ ^a _k▼ (＋) ＝ ▲ ^a _k▼ (-) + K_k(Z _k−H_k▲
^a _k▼ (−)) …… (20) K_k= ▲ P^a _k▼ (-) ▲ H^T _k▼ (H_k▲ P^a _k▼ (-) ▲ H^T _k▼ +
R_k)^-1 …… (21) ▲ P^a _k▼ (＋) ＝ ▲ P^a _k▼ (-)-K_kH_k▲ P^a _k▼ (-) ...
(22) (a = 1,2, ..., N)

Where: ▲ ^a _k▼ (-) is hypothesis φ_{k, a}Under the sun
Time t_kAgainstx _kThe prediction vector of
If you write in Koutor, from the theory of Karman Filter ▲ ^a _k▼ (-) = E [x _k/ Φ_{k, a}， Z^k-1, ▲ Z^k-1 _D▼]
... (23).

・ _k(+) Is sampling time t_kAgainstx _kThe smoothness of
In Khutorx _k (+) = E [x _k/ Z^k, ▲ Z^k _D▼] It is (24).

・ ▲ ^a _k▼ (+) is hypothesis φ_{k, a}Under the sun
Time t_kAgainstx _kWith the smooth vector of ▲ ^a _k▼ (+) = E [x _k/ Φ_{k, a}， Z^k, ▲ Z^k _D▼] ……
(25)

・ P_k(+) Is sampling time t_kCovariance of the smooth error at
It is a dispersion matrix, and from the theory of Kalman filter P_k(+) = E [(x _k− _k(+)) (x _k− _k(+))^T
/ Z^k, ▲ Z^k _D▼] It is (26).

・ ▲ P^a _k▼ (-) is sampling time t_kHypothesis φ in
_{k, a}Prediction error covariance matrix under^a _k▼ (-) = E [(x _k-▲ ^a _k▼ (-)) (x_k−
▲ ^a _k▼ (-))^T/ Φ_{k, a}， Z^k-1, ▲ Z^k-1 _D▼] ……
(27).

・ ▲ P^a _k▼ (+) is sampling time t_kHypothesis φ in
_{k, a}Is the smooth error covariance matrix under^a _k▼ (+) = E [(x _k-▲ ^a _k▼ (+)) (x _k−
▲ ^a _k▼ (+))^T/ Φ_{k, a}， Z^k, ▲ Z^k _D▼] …… (28)

・ K _k is the gain matrix at the sampling time t _k .

Note that A _UT represents the transposed matrix of matrix A.

Also, in the same way as when the Maruman filter is normally applied.
Initial value ₀(+), P₀(+) Is determined separately
To do.

Equation (19) from ▲ P ^a _k ▼ (-) because value that does not depend on the hypothesis φ _{k, a,} formula (21) from K _k, equation (22) from ▲ P ^a _k ▼ (+) similarly hypothesis The value does not depend on φ _{k, a} .

The reliability β _{k, a} of the hypothesis φ _{k, a} based on the observation information up to the sampling time t _k is determined by the conditional probability density function β _{k, a} = P [φ _{k, a} / Z ^k , ▲ Z ^k _D ▼]… … (29) (a = 1,2, ……, N) is written.

Markov property is assumed for the transition of the motion model. That is, the motion model φ _{k, a} is determined from the motion model at the sampling time t _k-1 and does not depend on the motion model up to the sampling time t _k-2 . At this time, the transition probability P _ab of the motion model is P _ab = P [φ _{k, a} / φ _k-1 ,, b] (30) (a = 1,2, ..., N; b = 1,2 , ……, N) is written.

Hypothesis φ_{k, a}Evaluation of target position observation error under f_a(K) (a = 1,2, ..., N) ...... (31) is approximated by a three-dimensional normal distribution, the theory of Kalman filter
Than f_a(K) = g (z _k; H_k▲ ^a _k▼ (-), H_k▲ P^a _k▼
(-) ▲ H^T _k▼ + R_k) …… (32).

Where g ( z _k ; a _k , A _k ) is 3 of the mean vector a _k and the covariance matrix A _k
Probability density at Z _k of the dimensional normal distribution.

As can be seen from equation (32), the observation vector of the target positionZ _kBut
Target position prediction vector H_k▲ ^a _k▼ The closer to (-)
f_aThe value of (k) becomes large and the target is hypothesis φ_{k, a}Exercise under
It is highly evaluated for its reliability.

Figure 4 shows the evaluation value f of the target position observation error._a(k) in one dimension
In the figure, G indicates the target position.
The horizontal axis represents F, the vertical axis represents probability density, and Z is the observation vector.
z _kIs the target position, PA is hypothesis φ_{k, a}Target prediction position under
, FA is the probability density f_a(k), N is the average H_k▲ ^a _k▼ (-) dispersion
H_k▲ P^a _k▼ (-) ▲ H^T _k▼ + R_kNormal distribution song determined by
It is a line.

The predicted value of the target distance change rate for the sampling time t _k under the hypothesis φ _{k, a} is ▲ ^a _p (k) ▼, that is, ▲ ^a _p (k) ▼ = E [(k) / φ _{k, a} , Z ^k-1 , ▲ Z
^k-1 _D ▼] …… If we write (33), from equation (15) Is.

here Is the target position prediction vector Is the target velocity prediction vector.

Note that from equation (14) Is.

In addition, if the first-order approximation is performed from the property of total differentiation, (k)-▲^a _p▼ (k) ≒ h_k(▲ ^a _k▼ (-))
(x _k− ^a _k▼ (-)) ... (37).

Where h_k(▲ ^a _k▼ (-) isofX _k= ▲ ^a _k▼ (-) is the valueIs.

Formula (11) and Formula (37)_φ (K)-▲^a _p▼ (k) ≒ h_k(▲^a _k▼ (-))
(x _k-▲ ^a _k▼ (-)) + v (K) It is (39).

Formula (39), Formula (23), Formula (12) and v (K) is white noise
By definition E [_φ(K)-^a _p(K) / φ_{k, a}， Z^k-1, ▲ Z
^k-1 _D▼] = 0 (40) Therefore, from equation (33), E [_φ(K) / φ_{k, a}， Z^k-1, ▲ Z^k-1 _D▼] ＝ ▲^a _p▼
(K) It is (41).

Expression (39), Expression (41), Expression (27), Expression (13) and v (K) is white
From the assumption of noise E [(_φ(K) -E [_φ(K) / φ_{k, a}， Z^k-1, ▲ Z
^k-1 _D▼])²/ Φ_{k, a}， Z^k-1, ▲ Z^k-1 _D▼] ＝ h_k(▲ ^a
_k▼ (−)) ▲ P^a _k▼ (−) ▲ h^T _k(▲ ^a _k▼ (-))
+ ▲ V^Two ▼ (k) …… (42).

From Equation (41) and Equation (42), hypothesis φ_{k, a}Target distance change under
Evaluation of conversion rate observation error ▲ f^a ₂▼ (k) (a = 1,2, ..., N) …… (43) is approximated by a one-dimensional normal distribution f_a(K) = g₂(_φ(K); ▲^a _p▼ (k), h_k(▲
^a _k▼ (−)) ▲ P^a _k▼ (−) ▲ h^T _k▼ (▲ ^a _k▼
(-)) + ▲ V^Two ▼ (k) …… (44).

Here, g ₂ (l; m, n) is the probability density in 1 of the one-dimensional normal distribution with mean m and variance n.

The reliability P [φ _{k, a} / Z ^k-1 , ▲ Z ^k-1 _D ▼] of the hypothesis φ _{k, a} at the time when the observation information z _k and _φ (k) at the sampling time t _k cannot be obtained is , From Markov property, each hypothesis φ _{k, a} is one sampling before φ _{k-1, b} (b = 1,2,
, N), it is obtained from β _{k-1, b of} equation (29), which is the reliability of φ _{k-1, b ,} and P _{ab of} equation (30), which is the transition probability. Is obtained as.

Observation value of target position observation vector z _k and target distance change rate
Reliability β of hypothesis φ _{k, a} when _φ (k) is obtained
_{k, a} is the target position observation vector of the hypothesis φ _{k, a based on} the reliability P [φ _{k, a} / Z ^k-1 , ▲ Z ^k-1 _D ▼] of the hypothesis when the observation vector Z _k is not obtained. A value _obtained by multiplying the evaluation value f _a (k) by Z _k and the evaluation value ▲ f ^a ₂ ▼ (k) by the observed value _φ (k) of the target distance change rate, that is, formula (45), formula (31) and formula From (43) It can be considered to be proportional to.

From the nature of probability Therefore, normalize equation (46) Is.

From equation (29) and Bayes' theorem Is.

From equation (24), equation (25) and equation (49) So Is.

From equation (50) and equation (20), equation (18), and equation (47) _k (+) = Φ_{k-1 k-1}(+) + P '_k-1
k-1＋ K_k(Z _k−H_k(Φ_k-1 x _k-1(+) + P '_k-1
k-1)] ... (51).

here Is.

Constant acceleration vector that composes multiple motion models
The conditional probability density function of α _a (a = 1,2, ..., N) is _{represented by} P [ U _k-1 / φ _{k, a} , Z ^k-1 , ▲ Z ^k-1 _D ▼ according to the probability theory in the discrete system. ] = Δ ( U _k-1 −
α _a ) …… (53) And

Here, the delta function δ ( x ) has the property of ∫ ... ∫G ( x ) δ ( x ) d x = G ( 0 ) ... (55).

From equations (53) and (55), E [ U _k-1 / φ _{k, a} , Z ^k-1 , ▲ Z ^k-1 _D ▼] = ∫ ... ∫ U _k-1 P [ U _k-1 / φ _{k, a} , Z ^k-1 , ▲ Z ^k-1 _D ▼] d U _k-1 = ∫… ∫ U _k-1
δ ( U _k-1 −α _a ) d U _k-1 = α _a …… (56).

That is ▲ ^a _k-1▼ ＝ [U _k-1/ Φ_{k, a}， Z^k-1, ▲ Z^k-1 _D▼] ……
(57) When ▲ ^a _k-1▼ ＝α _a … (58).

From equation (54) and equation (55) Is.

That is, _k-1 = E [U _k-1/ Z^k-1, ▲ Z^k-1 _D▼] ………… (60)Is.

From equation (53), equation (58) and equation (55), E [(U_k-1-▲ ^a _k-1▼) (U_k-1-▲ ^a _k-1▼)^T/ Φ
_{k, a}， Z^k-1, ▲ Z^k-1 _D▼] = ∫… ∫ (U _k-1−α _a) (U _k-1−α _a)^TP [U _k-1/
φ_{k, a}， Z^k-1, ▲ Z^k-1 _D▼] dU _k-1 = ∫… ∫ (U _k-1−α _a) (U _k-1−α _a)^Tδ (U _k-1−α _a)
dU _k-1 = 0I …… (62).

From equation (54) and equation (55)Is. _k (-) Is the sampling time t_kAgainstx _kPrediction of
Tor, ie _k (-) = E [x _k/ Z^k-1, ▲ Z^k-1 _D▼] ………… （64）

Applying formula (64), formula (24), formula (7) and formula (60) to formula (1)
do it _k (-) = Φ_{k-1 k-1}(+) + P '_k-1
k-1 I get (65).

From equation (1) and equation (65)x _k − _k(-) = Φ_k-1(x _k-1− _k-1(+)) + P
_k-1 W _k-1+ P '_k-1(U _k-1− _k-1) …… (66).

P_k(-) Is the sampling time t_kPrediction error covariance in
Matrix, ie P_k(-) = E [(x _k− _k(-)) ( _k−x _k(-))
^T/ Z^k-1, ▲ Z^k-1 _D▼] …… (67)

Substituting equation (66) into equation (67) and applying equations (26), (8), and (63)), x _k-1 , W _k-1 , and U _k-1 are If it ’s a correlation To get

Therefore, from equation (45), equation (19) and equation (68) Is.

Here, from equation (61) and equation (45) Is.

From Equation (26) and Equation (24), P_k(+) = E [x_k▲ x^T _k▼ / Z^k, ▲ Z^k _D▼] − _k(+)
^T _k(+) It is (71).

From equation (49) Therefore, from equation (71) Is.

From equation (28) and equation (25) ▲ P^a _k▼ (+) = E [x _k▲x ^T _k▼ / φ_{k, a}， Z^k, ▲ Z^k _D▼]
-▲ ^a _k▼ (+) ▲ ^a _k▼^T(+) …… (73) That is ▲ P^a _k▼ (＋) ＝ ∫… ∫x _k▲x ^T _k▼ P [x _k/ Ψ_{k, d}， Z^k，
▲ Z^k _D▼] dx _k-▲ ^a _k▼ (+) ▲ ^a _k▼^T(+) ……
(74) Therefore, from equation (72)Is.

From equation (47) and equation (50) Therefore, from the fact that ▲ P ^a _k ▼ (+) is unrelated to a, from equation (47) and equation (75) Is.

From Equation (20) and Equation (18) ▲ ^a _k▼ (+) = Φ_{k-1 k-1}(+) + P '
_k-1 α _a ＋ K_k〔Z _k−H_k(Φ_{k-1 k-1}(+) + P '_k-1 α _a)]
… (78).

From equation (78) and equation (51) Therefore, from equation (77) Is.

Note that when the motion model is uniquely determined, for example, when β _{k, 1} = 1 β _{k, a} = 0 (a = 2,3, ..., N), P _k can be calculated from Eqs. (80) and (52). (+) = ▲ it is a _{^{P a k ▼ (+) ......}} (81).

That is, the second term on the right side of Expression (80) is a term that increases the smoothing error covariance matrix because the motion model is not uniquely determined.

Next, the operation of the tracking filter of the present invention will be described with reference to FIG.

It is assumed that the initial value is previously determined in the same manner as when the Kalman filter is normally applied to the tracking filter.

The target observation device (1) uses the target distance change rate and polar coordinates to target
Observe the position information, convert the target position information to Cartesian coordinates,
Observed value of target distance change rate in equation (11)_φ(K) and equation (9)
Target position observation vectorZ _kIs output. For each exercise model
The position observation error evaluator (2) of
One sampling before the current time obtained from the value calculator (7)
Prediction vector calculated for each motion model of Eq. (18) ▲
^a _k▼ (-) is input through the 4th delay circuit (12) and the target
Position prediction vector H_k▲ ^a _k▼ (-) is calculated and
From the present time obtained from the prediction error evaluator (10) for each Dell
For each motion model of equation (19) calculated before sampling
Prediction error covariance matrix ▲ P^a _k▼ (-) to the second delay circuit (1
1) through the covariance of the predicted position error for each motion model
Matrix H_k▲ P^a _k▼ (-) ▲ H^T _k▼ is calculated and this
Target position of equation (10) obtained from the set observation model
Observation noise covariance matrix R_kMore observation vectorZ _kAnd exercise model
Target position prediction vector H for each_k▲ ^a _k▼ (-) difference
Covariance matrix H_k▲ P^a _k▼ (-) ▲ H^T _k▼ + R_kAnd the target observation device
Target position observation vector input from (1)Z _kA plurality of
Target position observation, which is the degree of agreement with each motion model
F in Eq. (31), which is the evaluation value of the error_aCalculate (k) according to equation (32)
Put out. Distance change rate observation error evaluator for each motion model (1
In 6), it is obtained from the predicted value calculator (7) for each motion model.
Exercise of formula (18) calculated one sampling before the current moment
Prediction vector for each model ▲ ^a _k▼ (-) to the 4th delay
Input through the delay circuit (12) and set the target distance according to equations (34) and (35).
Predicted rate of separation change ▲^a _p▼ (k) is calculated and equation (38),
From equations (35) and (36), the transformation matrix h_k(▲ ^a _k▼ (-))
Is calculated and obtained from the prediction error evaluator (10) for each motion model.
Of the formula (19) calculated one sampling before the current
Prediction error covariance matrix for each motion model ▲ P^a _k▼ (-)
Input through the second delay circuit (11) and preset
Change in target distance in Eq. (13) obtained from a certain observation model
Rate observation noise ▲ V² Observed value of target distance change rate from (k)
_φ(K) and predicted value ▲^a _p▼ Expression that is the variance of the difference of (k)
(42) h_k(▲ ^a _k▼ (−)) ▲ P^a _k▼ (−) ▲ h^T _k▼
(▲ ^a _k▼ (-)) + ▲ V² ▼ (k) is calculated and the target
Observation value of the target distance change rate input from the observation device (1)
_φThe degree of agreement with each of the plurality of motion models in (k)
Of Eq. (43), which is the evaluation value of the target distance change rate observation error
▲ f^a ₂▼ (k) is calculated according to equation (44). Each movement model
In the reliability calculator (3) of 1
Reliability for each of several motion models calculated in
Β in equation (29)_{k-1, a}Through the first delay circuit (4)
Force and the transition probability P of the preset equation (30)_ab
More target observation informationZ _kas well as_φWhen (k) cannot be obtained
The value of Eq. (45), which is the reliability of each motion model of
Target position observation vector to the value of (45)Z _kFor each motion model of
Positioning error evaluator for each motion model
F in equation (31) input from (2)_a(K) and target distance change
rate_φMotion that is the degree of agreement with each motion model in (k)
Input from the distance change rate observation error evaluator (16) for each model.
(43) ▲ f^a ₂▼ Equation (48) weighted by (k)
According to the target position observation vectorZ _kAnd target distance change rate_φ
Each of the multiple motion models at the time when (k) was obtained
Β in Eq. (29), which is the reliability of_{k, a}To calculate. Gain matrix
In the calculator (5), from the preset observation model
The target position observation noise covariance matrix R of the obtained equation (10) is obtained._kAnd 1
Prediction for each motion model calculated before sampling
Input from the error evaluator (10) through the second delay circuit (11).
Prediction error covariance matrix for each motion model ▲ P^a _k▼
(-) And smooth vector according to Eq. (21) _k(+) Arithmetic
The output gain matrix K_kTo calculate. With smoother (6)
Is the smooth vector of Eq. (24) calculated one sampling before.
Toll _k-1Input (+) through the third delay circuit (8)
Φ by prediction of uniform linear motion_{k-1 k}(+) Is calculated and
Each of the equation (29) obtained from the motion model reliability calculator (3)
Motion model reliability β_{k, a}And the preset equation (5)
Constant vector of motion models
Leα _aThe target acceleration according to equation (52)U _k-1Recommend
FixedU _k-1Is a term P ′ that affects the prediction_{k-1 k-1}Calculate
Then, these and the eye of the equation (9) obtained from the target observation device (1)
Position observation vectorZ _kFor a smooth vector
_kObservation vector in (+) calculationZ _kDegree of contribution
Gain row obtained from the gain matrix calculator (5) that determines
Row K_kIs applied according to Eq. (51) and the smooth vector _kCalculate (+)
Put out. In the predictive value calculator (7) for each motion model,
Vector obtained from the instrument (6) _kStraight line than (+)
Φ by line motion prediction_{k k}(+) Is calculated and
A constant acceleration vector that composes multiple set motion models.
Cuttleα _aIs the term P ′ that affects the prediction_k α _aIs calculated as
Sampling time t after 1 sampling from the time point_{k + 1}Against
Prediction vector for each motion model ^a
_{k + 1}▼ (-) is calculated according to equation (18). Exercise model
The smoothing error evaluator (9) with
Prediction error covariance matrix for each motion model in Equation (27)
▲ P^a _k▼ (-) is the prediction error evaluator for each motion model (10)
Input through the second delay circuit (11), and gain matrix calculation
Gain matrix K obtained from the output device (5)_kAnd the luck of equation (25)
Smooth vector for each dynamic model ▲ ^a _k▼ (+) smoothing error
The difference of smoothing error is
Covariance matrix ▲ P^a _k▼ (+) is calculated according to equation (22).

The smoothness error evaluator (13) is a reliability calculator for each motion model.
The reliability β of each motion model obtained from (3)_{k, a}， Gain line
Gain matrix K obtained from column calculator (5)_kAnd in advance
Set constant acceleration vectorα _aMore motion vector
If the smoothing error becomes large because it is not uniquely determined
Item to be evaluatedAnd the smoothing error evaluator for each motion model (9)
Smooth error covariance matrix for each motion model obtained by^a
_k▼ (+) is added and smoothing error co-division, which is the evaluation of smoothing error
Matrix P_k(+) Is calculated according to the equation (80). Exercise model
For each prediction error evaluator (10), the smoothing error evaluator (13)
Obtained smooth error covariance matrix P_k(+) And preset
Driving noise co-component of Eq. (8) obtained from the defined motion model
Matrix Q_kAnd according to formula (19), 1 sample from the present
Sampling time t after_{k + 1}Multiple motion models for
Prediction vector for each ^a _{k + 1}▼ (-) prediction error
Prediction error covariance matrix ▲ P^a _{k + 1}▼ (-) is calculated
Put out.

The predicted value calculator (14) uses the smoother (6) to calculate the smoothness vector of Eq. (24).
Cuttle _kInput (+) and predict Φ by constant velocity linear motion prediction_k
kCalculating (+) and calculating the reliability of each motion model (3)
The reliability β of each motion model of equation (48) obtained from_{k, a}Ah
The transition probability P of formula (30) that has been set_abMore observation
TollZ _{k + 1}Of each motion model of Eq. (45) when
Degree of reliability And a plurality of preset motion models.
Constant acceleration vector of equation (5)α _aMore view
Measurement vectorZ _{k + 1}The target acceleration at the time whenAnd the term that this acceleration affects the prediction.Is calculated according to formula (65) and formula (70)
Prediction vector one sampling after the pointx _{k + 1}Calculate (-)
Put out.

The prediction error evaluator (15) is a reliability calculator for each motion model.
The reliability β of each motion model in Eq. (48) obtained from (3)_{k, a}Ah
The transition probability P of formula (30) that has been set_abAnd synopsis
Configure multiple motion models that have been set for
Equation (5) constant acceleration vectorα _aUse equation (70) from
Prediction vector after one sampling from the current point _{k + 1}
Prediction error covariance matrix P, which is the evaluation of the prediction error of (-)_{k + 1}
The term to increase (-)And the prediction error estimator for each motion model (10)
Prediction error covariance matrix for each motion model obtained by^a
_{k + 1}▼ (-) is added to calculate the prediction error
Covariance matrix P_{k + 1}Calculate (-) according to equation (69).

〔The invention's effect〕

As described above, according to the present invention, it is possible to improve the accuracy of target motion parameter calculation at low cost without attaching a special additional device to the normal target automatic tracking device.

In the above, the case of a model in which a constant acceleration vector is added to the constant velocity linear motion model has been described. However, tracking that calculates the target motion parameters from the information of the target observation device with multiple motion models other than this Applicable to filters.

[Brief description of drawings]

FIG. 1 is a diagram for explaining the configuration of an embodiment of the present invention, and FIG.
FIG. 4 is a diagram illustrating a conventional tracking filter, FIG. 3 is a diagram illustrating a constant acceleration vector, and FIG. 4 is a diagram illustrating a target position observation error evaluation value. In the figure, (1) is a target observation device, (2) is a position observation error evaluator for each motion model, (3) is a reliability calculator for each motion model, (4) is the first delay circuit, (5 ) Is a gain matrix calculator,
(6) is a smoother, (7) is a predictive value calculator for each motion model,
(8) Third delay circuit, (9) Smoothing error evaluator for each motion model, (10) Prediction error evaluator for each motion model, (11) Second delay circuit, (12) Second 4 is a delay circuit, (13) is a smoothing error evaluator, (14) is a prediction value calculator, (15) is a prediction error evaluator, and (16) is a distance change rate observation error evaluator for each motion model. In the drawings, the same or corresponding parts are designated by the same reference numerals.

Claims (1)

[Claims] 1. A target observing device for observing target position information and a target distance change rate, a plurality of different motion models each composed of a plurality of constant vectors of the same dimension, and the target observing device. Match between the position observation error evaluator for each motion model that calculates the target position observation error evaluation value that is the degree of matching with the target position information, and each of the motion models and the target distance change rate input from the target observation device. Target distance change rate, which is the degree of observation error, The target position observation error for each of the above motion models calculated by the distance change rate observation error evaluator for each motion model and the position observation error evaluator for each motion model Evaluation value and distance change rate observation error for each movement model Evaluation target distance change rate observation error for each movement model calculated by the evaluator To calculate the reliability of each of the above exercise models, the reliability calculator of each of the exercise models, the gain matrix calculator of calculating the same gain matrix in each of the exercise models, and the reliability of each of the exercise models It is obtained by inputting the reliability of each of the above-mentioned motion models calculated by the calculator, the gain matrix calculated by the gain matrix calculator, and the target position information observed by the target observation device, and integrating the influence of each of the motion models. The smoothing device for calculating the smoothed value of the target motion data such as the target position and speed, and the smoothing value of the target motion data calculated by the smoother are input to input the position observation error evaluator for each motion model and the motion. Distance change rate for each model Calculation of predicted value for each motion model to calculate predicted value after one sampling of target motion specifications for each of the above motion models used in the evaluation error evaluator And, the reliability of each of the above-mentioned motion models calculated by the reliability calculator of each of the above-mentioned motion models and the smooth value of the target motion parameters calculated by the above-mentioned smoother are input to integrate the influence of each of the above-mentioned motion models. A predicted value calculator that calculates a predicted value after one sampling of the obtained target motion specifications and the gain matrix calculated by the gain matrix calculator are input and the error of the smoothed value of the target motion specifications for each of the above motion models is input. Smoothing error evaluator for each motion model to be evaluated, reliability of each of the motion models calculated by the reliability calculator of each motion model, gain matrix calculated by the gain matrix calculator, and smoothing error for each motion model The smoothing error evaluation value for each of the above motion models calculated by the evaluator is input, and the error of the smooth value of the target motion parameters obtained by integrating the effects of each of the above motion models is input. The smoothing error evaluator that evaluates the difference, the smoothing error evaluation value of the target motion data calculated by the smoothing error evaluator, and the delay change circuit are input through the delay circuit. Prediction for each motion model that evaluates the error in the target motion parameter prediction value for each motion model used by the position observation error evaluator for each model, the gain matrix calculator, and the smoothing error evaluator for each motion model The error evaluator and the reliability of each motion model calculated by the reliability calculator of each motion model and the prediction value of the prediction error for each motion model calculated by the prediction error evaluator for each motion model A tracking error file which is provided with a prediction error evaluator that evaluates an error in a predicted value of target motion data obtained by integrating the effects of the respective motion models described above. .