JPH0695137B2 - 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 observing device to obtain various target motions such as a true value and velocity of a target position. This paper proposes a method for improving the accuracy of the tracking filter that estimates the element.

[Conventional technology]

Figure 4 shows, for example, IEEE TRANSACTIONS ON AEROSPACE AND E
LECTRONIC SYSTEMS VOL AES−13 No.3, MAY 1977, P310〜
P317 「MANEUVERING TARGET TRACKING USING ADAPTIVE E
It is a block diagram of a conventional tracking filter having a plurality of motion models indicated by "STIMAION".
Is a target observation device that measures the target position and target distance change rate including observation noise, and (2) is the position observation error evaluation for each motion model to evaluate the target position observation error for each of multiple motion models. Calculator, (3) reliability calculator of each motion model to calculate the reliability of each motion model, (4) first delay circuit, (5) smoothed value of target motion parameters The gain matrix calculator for calculating the Kalman gain matrix used for the calculation, (6) is a smoother for calculating the smoothed value of the target motion specifications, and (7) is the one after sampling for each of a plurality of motion models from the present time. Prediction value calculator for each motion model that calculates the prediction values of target motion specifications, (8) is the third
Delay circuit, (9) is a smooth error evaluator for each motion model that evaluates the smooth error which is the difference between the smooth value and the true value of each motion model, and (10) is the prediction of each motion model. Prediction error evaluator for each motion model that evaluates the prediction error, which is the difference between the true value and the true value, (11) is the second delay circuit, (12)
Is a 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 is assumed for the motion model transition, 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 evaluation value of the target position observation error for each of the plurality of motion models. In the gain matrix calculator (5), the observation noise covariance matrix obtained from the preset observation model and the motion calculated by the prediction error evaluator (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 of each model. In the smoother (6), the smoothed value calculated 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 Calculator model reliability calculator (3)
The smoothed values of the target position and the target velocity are calculated from the sum of constant acceleration vectors constituting the plurality of motion models weighted by the reliability of each of the plurality of motion models obtained in step 1. In the predictive value calculator (7) for each motion model,
Predicted values of the target position and the target speed after one sampling from the present time are calculated from the target position and the target speed calculated by the smoother (6) and the constant acceleration vector for each of a plurality of motion models. 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. It is calculated by the prediction error covariance matrix calculated by the prediction error evaluator (10) for each motion model. The prediction error estimator (10) for each motion model uses the smoothing error covariance matrix input from the smoothing error estimator (9) for each motion model and the driving noise covariance matrix obtained from the preset motion model. The covariance matrix of the difference between the predicted value and the true value after one sampling from 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 value that reflects the reliability for each of the obtained multiple motion models, the mechanism for evaluating the error in the predicted values of the target position and target speed does not exist only for each motion model, so the prediction error cannot be evaluated. This is based on the conventional Kalman filter theory, and was calculated assuming that the motion model is uniquely determined. Therefore, an increase in the prediction error caused by turning the target is not reflected and a much better evaluation result than in reality is calculated. There was a drawback.

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 according to the present invention uses the target distance change rate to calculate the reliability of each of a plurality of motion models, and uses the prediction error covariance that is the evaluation of the prediction error that is the difference between the predicted value of the target motion parameters and the true value. The reliability of each of the plurality of motion models is used to calculate the matrix.

[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 together with the target position observation error evaluation value for each of the multiple motion models calculated from the target position information as a weighting factor for calculating the reliability of each of the multiple motion models. The reliability for each of a plurality of motion models is calculated more appropriately than in the case where only the target position information is calculated. Increase the prediction error covariance matrix, which is the estimation of the prediction error, by using the reliability for each motion model and the constant acceleration vector that composes multiple motion models, and the motion model is not uniquely determined. Calculate the parameters and add these parameters to the prediction error covariance matrix for each motion model to calculate the prediction error covariance matrix obtained by integrating the effects of each motion model. I have to.

〔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 the prediction error evaluator that evaluates the prediction error of the predicted value, and (14) 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} + Γ _K-1 w _K-1 + Γ ′ _{K-1 u K-1} (1).

Where • x _K is the state vector that represents the true value of the target motion parameters at 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 the transition matrix of the state vector x _K from sampling time t _K-1 to t _K , assuming that the target performs a uniform linear motion. Is.

· W _K is driven noise vector gamma _K at the sampling time t _K example a replacement matrix of drive noise vector at the sampling time t _K, the truncation error term due to the assumed constant velocity linear motion target motion model gamma _K- If we think of ₁ w _K-1 , w _K is in charge of 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 Γ ′ _K is the transformation matrix of 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. X-axis of coordinates 0-xy, Y is the y-axis of coordinates 0-xy with the north direction as the positive y-axis, A1 is a constant acceleration vector in the positive direction of the y-axis, and A2 is a constant in the positive direction of the x-axis. An acceleration vector, A3 is a constant acceleration vector in the negative direction of the y-axis, and A4 is a constant acceleration vector in the negative direction of the x-axis. 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) is true at the sampling time t _K is given by Ψ _{K, a} (a = 1,2). , ……, N) …… (6) is written.

Let E be a symbol representing the mean, and w _K is a 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 are driven noise covariance matrix at sampling time t _K, a ^T represents the transposed vector of the vector a.

The target position observation model is z _K = H _K x _K + v _K (9).

Here, · z _K is the target position observation vector · H _K by the orthogonal coordinates composed of observed values of the target position information at sampling time t _K is = H _K in observation matrix at sampling time t _K (Ι 0I) .

・V _K is the target position observation noise vector at the sampling time t _K, which is a three-dimensional normally distributed white noise with an average of 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) …… (11)

here, ·_φ(K) is the sampling time t_KChange in target distance in
Observation of rate ・ (K) is sampling time t_KChange rate of target distance in
True value of v (K) is the sampling time t_KChange in target distance in
Is one-dimensional normally distributed white noise with an average of 0 and E [v (K)] = 0 ... (12) E [v (K) v (L)] = ▲ σ^Two ▼ (K) (K = 1
0), 0 (when K ≠ l) (13) Note that ▲ σ^Two ▼ (K) is sampling time t_KEyes in
It is the variance of the gage change rate, and 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.

The whole target position observation vector from sampling time t ₁ to t _K is Z ^K , and the whole target distance change rate observation value is Z ^K _D , that is, Z _K = [ Z ₁ , Z ₂ , ……, Z _K ]. …… (16) ▲ Z ^K _D ▼ ＝ [ _φ (1), _φ (2),… _φ (K) …… (1
Write 7).

From the above, from the usual theory of Kalman filter ▲ ^a _K▼ (-) = Φ_{K-1 K-1}(+) + Γ ′_K-1 α _a
…… (18) ▲ P^a _K▼ (-) = Φ_K-1P_K-1(+) ▲ Φ^T _K-1▼ + Γ_K-1Q
_K-1▲ Γ^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}Sampling under
Time t_KAgainstx _KThe prediction vector of
If you write in Tor, from the theory of Kalman 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 Khutor _K (+) = E [x _K｜ Z^K, ▲ Z^K _D▼] It is (24).

・ ▲ ^a _K▼ (+) is hypothesis Ψ_{K, a}Sampling under
Time t_KAgainstx _KWith the smooth vector of ▲ ^a _K▼ (+) = E [x _K│Ψ_{K, a}, Z^K, ▲ Z^K _D▼] …… (2
5)

・ 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 Ψ at
_{, 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▼] It is (27).

・ ▲ P^a _K▼ (+) is sampling time t_KHypothesis Ψ at
_{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 sampling time t _K.

Note that ^AT represents the transposed matrix of matrix A.

Also, in the same way as when applying the Kalman filter normally.
Initial value _O(+), P_O(+) Is determined separately
To do.

Since Eq. (19) indicates that ▲ P ^a _K ▼ (−) is _a value that does not depend on the hypothesis Ψ _{K, a} , Eq. (21) indicates K _K , and Eq. (22) also indicates ▲ P ^a _K ▼ (+). 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} calculated 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).

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).

Here, 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 dimension 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 (-), the more f_a
The value of (K) becomes large and the goal is hypothesis Ψ_{K, a}Exercised under
Is highly evaluated. Figure 4 shows incorrect target position observation.
Difference evaluation value f_aIt is a figure explaining (K) in one dimension,
In the figure, G is the horizontal axis representing the target position, and F is the probability density.
Vertical axis, Z is the observation vectorz _KThe target position, PA is
Hypothesis Ψ_{K, a}Target predicted position under_a(K),
N is the average H_K▲ ^a _K▼ (-) dispersion H_K▲ P^a _K▼ (-) ▲ H^T _K
▼ + R_KIt is a normal distribution curve determined by.

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 ▼ = E [(K) | Ψ _{K, a} , Z ^{K -1} , ▲ Z ^K-1 _D ▼] …… (3
If you write 3), from equation (15) Is.

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

Note that from equation (14) Is.

Moreover, 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.

From equation (11) and equation (37)_φ (K)-▲^a _p▼ (K) ≒ h_K(▲^a _K▼ (-)) (x _K-▲
^a _K▼ (-)) (x _K-▲ ^a _K▼ (-)) + v (K) ......
(39).

Formula (39), Formula (23), Formula (12) and v Assumption that (K) is white noise
From 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)
… (41).

Expression (39), Expression (41), Expression (27), Expression (13) and v (K) is white
From the assumption of sound 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▼ (-)) + ▲ σ^Two ▼
(K) …… (42).

From equations (41) and (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▼ (-)) + ▲ σ^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 , 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.
Z _D ^K-1 ] is obtained by the Markov property because the hypothesis Ψ _{K, a} changes from each hypothesis Ψ _{K-1, b} (b = 1,2, ..., N) one sampling before. _{K-1, b}
Β _{K-1, b in} Eq. (29), which is the reliability of
From P _{ab in} (30) Is obtained as.

Observation value of target position observation vector z _K and target distance change rate
The reliability β _{K, a} of the hypothesis Ψ _{K, a} when _φ (K) is obtained is
The hypothesis reliability P [Ψ when the observation vector z _K cannot be obtained
_{K, a} | Z ^K-1 , ▲ Z ^K-1 _D ▼], the evaluation value ▲ f _a ▼ (K) of the hypothesis Ψ _{K, a by} the target position observation vector z _K and the observation value _φ (of the target distance change rate) The value obtained by multiplying the evaluation value ▲ f ^a ₂ ▼ (K) by K), that is, from equation (45), equation (31) and equation (43) It can be considered to be proportional to.

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

From equation (29) and Pais's 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}(+) + Γ ′_{K-1 K-1}＋ K_K〔z _K−H
_K(Φ_{K-1 K-1}(+) + Γ ′_{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 calculated according to the probability theory in the discrete system by P [ u _K-1 | Ψ _{K, a} , Z ^K-1 , ▲ Z ^K-1 _D ▼ ] = Δ ( u _K-1 −
α _-a ) …… (53) And

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

From Eqs. (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).

Ie ^a _K-1 = E [u _K-1│Ψ_{K, a}, Z^K-1, ▲ Z^K-1 _D▼] …… (57) ^a _K-1 =α _a … (58).

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

Ie _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)^T P [u _K-1│Ψ_{K, a}, Z^K-1, ▲ Z^K-1 _D▼] du _K-1 = ∫… ∫ (u _K-1−α _-a) (u _K-1-Α_-aK)^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 equation (64), equation (24), equation (7) and equation (60) to equation (1) _K (-) = Φ_{K-1 K-1}(+) + Γ ′_{K-1 K-1} I get (65).

From equation (1) and equation (65)x _K − _K(-) ＝ φ_K-1(x _K-1− _K-1(+)) + Γ_K-1 w _K-1
+ Γ ′_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(-)) (x _K- _K(-))^T| Z^K-1, ▲ Z
^K-1 _D▼] …… (67)

Substituting Eq. (66) into Eq. (67), Eq. (26), Eq. (8), and Eq.
If 3) is applied and x _K-1 , w _K-1 , and u _K-1 are uncorrelated with each other, To get

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

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

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 observation, convert the target position information to Cartesian coordinates,
Observed value of target distance change rate in equation (11)_φEye of (K) and formula (9)
Position observation vectorz _KIs output. Position per exercise model
In the position observation error evaluator (2), prediction value calculation for each motion model is performed.
1 sampling time from the present time obtained from the generator (7)
Predicted vector for each calculated motion model of equation (18) ▲ ^a _K
▼ (-) is input through the fourth delay circuit (12) and the target position
Prediction vector H_K▲ ^a _K▼ (-) is calculated and
1 sun from the present time obtained from the prediction error evaluator (10)
The prediction error for each motion model in Eq. (19) calculated before pulling
Difference covariance matrix ▲ P^a _K▼ (-) is passed through the second delay circuit (11).
Input and then the covariance matrix H of the predicted position error for each motion model_K
▲ P^a _K▼ (-) ▲ H^T _K▼ is calculated and this and preset
Of the target position in Eq. (10) obtained from an observation model
Sound covariance matrix R_KMore observation vectorz _KAnd for each exercise model
Target position prediction vector H_K▲ ^a _K▼ (-) difference covariance row
Row 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
Calculate fa (K) in equation (31), which is the evaluation value of the error, according to equation (32)
To do. Distance change rate observation error evaluator for each motion model (14)
Is obtained from the predicted value calculator (7) for each motion model
The motion model of Equation (18) calculated one sampling before the present time.
Prediction vector for each Dell ▲ ^a _K▼ (-) to the fourth delay time
Input through the road (12) and change the target distance according to Eqs. (34) and (35).
Forecasted conversion rate ▲^a _p▼ (K) is calculated, and equation (38) and equation (3
From 5) and equation (36), the transformation matrix h_K(▲ ^a _K▼ (-)) is calculated
Obtained from the prediction error evaluator (10) for each motion model
The motion model of Equation (19) calculated one sampling before the present time.
Prediction error covariance matrix for each Dell ▲ P^a _K▼ (-) to the second
Input through the delay circuit (11) and set this in advance.
(13) target distance change rate observation obtained from the observation model
Noise ▲ σ^Two Observed value of target distance change rate from (K)_φ(K)
And the predicted value_pH in equation (42), which is the variance of (K)_K(▲ ^a _K▼
(-)) ▲ P^a _K▼ (−) ▲ h^T _K▼ (▲ ^a _K▼ (-)) + ▲
σ^Two ▼ Calculate (K) and input from the target observing device (1)
Observed value of target distance change rate_φ(K) multiple motion models
Target distance change rate observation error, which is the degree of agreement with each
▲ f in equation (43), which is the evaluation value of^a ₂▼ Calculate (K) according to equation (44)
To do. The reliability calculator (3) for each motion model
Multiple motion models calculated one sampling before
Β in equation (29), which is the reliability for each_{K-1, a}The first late
Input through the delay circuit (4) and preset with this
Transition probability P in equation (30)_abMore target observation informationz _Kas well as_φ(K) is
Equation (45), which is the reliability of each motion model when it is not obtained
Of the target position observation vectorz _Kof
The position of each motion model, which is the degree of agreement with each motion model
F of the equation (31) input from the position observation error evaluator (2)_a(K) and
Target distance change rate_φDegree of agreement of (K) with each motion model
Distance change rate observation error evaluator for each motion model (14)
▲ f of the equation (43) input from^a ₂▼ (K) weighted formula
Target position observation vector according to (48)z _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 line
In the column calculator (5), the
Target position observation noise covariance matrix R in Eq. (10)_Kas well as
Preliminary calculation for each exercise model calculated before 1 sampling
Input from the measurement error evaluator (10) through the second delay circuit (11)
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. In the smoother (6),
Smooth vector of equation (24) calculated before sampling
Le _K-1Input (+) through the third delay circuit (8)
Φ by linear motion prediction_{K-1 K-1}(+) Is calculated and each exercise
Each motion in equation (29) obtained from the model reliability calculator (3)
Model confidence β_{K, a}And the preset formula (5)
Constant acceleration vectors that make up several motion modelsα
_aThe target acceleration according to equation (52) _K-1And estimate _K-1
Is the term Γ ′ that affects the prediction_{K-1 K-1}And calculate these and the eye
Target position observation vector of equation (9) obtained from the target observation device (1)
Lez _KFor a smooth vector _K(+) Observation in calculation
vectorz _KGain matrix calculator that determines the degree of contribution
Gain matrix K obtained from (5)_KIs applied according to equation (51) and
Smooth vector _KCalculate (+). Prediction for each exercise model
In the measured value calculator (7), the smooth vector obtained from the smoother (6)
Le _KΦ from (+) based on constant velocity linear motion prediction_{K K}(+)
Is calculated and a plurality of preset motion models are calculated.
Constant acceleration vectorα _aTerm affects prediction
Γ ′_K α _aIs calculated, and the
Sampling time t_{K + 1}For multiple motion models for
Prediction vector ^a _{K + 1}(-) Is calculated according to the equation (18). luck
In the smoothing error evaluator for each dynamic model (9), 1 sampling
Prediction error for each motion model of equation (27) calculated previously
Covariance matrix P^a _K(-) Is the prediction error evaluation for each motion model
Input from the device (10) through the second delay circuit (11) and gain
Gain matrix K obtained from matrix calculator (5)_KAnd the expression (2
Smooth vector for each motion model in 5) ^a _K(+) Smooth error
The difference of smoothing error is
Covariance matrix ▲ P^a _K▼ (+) is calculated according to equation (22). motion
In the prediction error evaluator (10) for each model,
Eq. (28) motion model obtained from the smooth error estimator (9)
Error covariance matrix for each^a _K▼ (+) is the one mentioned above
Hypothesis Ψ_{K, a}P is determined regardless of_K ^a(+) Is changed to the expression (2
6) Smooth error covariance matrix P_KPresumed to be (+),
The driving noise coefficient of Eq. (8) obtained from the set motion model
Covariance matrix Q_KAnd according to formula (19), 1 sample from the present
Sampling time t after_{K + 1}Multiple motion modes for
Prediction vector for each Dell ▲ ^a _{K + 1}▼ (-) of the prediction error
Prediction error covariance matrix that is the evaluationx ^a _{K + 1}Calculate (-)
It

The prediction error evaluator (13) is a reliability calculator for each motion model.
The reliability β of each motion model in Eq. (48) obtained from (3)_{K, a},
The transition probability P of the preset equation (30)_abAnd Araka
The formulas that make up multiple kinetic models that are pre-set
Constant acceleration vector of (5)α _aUsing equation (70)
Predicted vector after one sampling _{K + 1}(-)
Prediction error covariance matrix P, which is the evaluation of the measurement error_{K + 1}Large (-)
TermAnd 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}(-) Is calculated according to the 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) is the third delay circuit, (9) is the smoothing error evaluator for each motion model, (10) is the prediction error evaluator for each motion model, (11)
Is a second delay circuit, (12) is a fourth delay circuit, (13) is a prediction error evaluator, and (14) 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 a smoothing error evaluator for each motion model that inputs the gain matrix calculated by the gain matrix calculator and evaluates the error of the target motion specification smooth value for each of the above motion models, and a smoothing error evaluation for each motion model The evaluation value of the smoothing error for each of the above motion models calculated by the input device is input and through the delay circuit the distance change rate observation error evaluator for each motion model, the position observation error evaluator for each motion model, and the gain matrix calculation Error evaluator for each motion model that evaluates the error of the target motion parameter prediction value for each of the motion models used by the motion calculator and the smoothing error evaluator for each motion model, and the reliability of each motion model The reliability of each motion model calculated by the calculator and the prediction error of each motion model A tracking filter, comprising: a prediction error evaluator that inputs a prediction error evaluation value and evaluates an error of a prediction value of target motion data obtained by integrating influences of the respective motion models.