JPH0690279B2 - Tracking filter - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial application] The present invention inputs target position information including observation noise from a target observation device, and estimates a true value of a target position and target motion parameters such as velocity and acceleration. We propose a method for improving the accuracy of the tracking filter.

[Conventional technology]

Figure 2 shows, for example, IEEE TRANSACTIONS ON AEROSPACE AND E
LECTRONIC SYSTEMS VOL.AES-13 NO.3 「MANEUVERING TAR
FIG. 1 is a block diagram of a conventional tracking filter having a plurality of motion models shown in “GET TRACKING USING ADAPTIVE STATE ESTIMATION”. In the figure, (1) is a target observation device for measuring a target position including observation noise, (2) is an observation error evaluator for each motion model for evaluating the observation error for each of the motion models, and (3) is each motion model for calculating the reliability of each of the motion models. Reliability calculator, (4) is the first delay circuit,
(5) is a gain matrix calculator for calculating the Kalman gain matrix used to calculate the smooth value of the desired motion parameter, (6) is a smoother for calculating the smooth value of the target parameter, and (7) is a plurality. Prediction value calculator for each motion model that calculates the prediction value of the target motion parameters after one sampling from the present time, (8) is the third delay circuit, and (9) is each a plurality of motion models. Smoothing error evaluator for each motion model that evaluates the smoothing error that is the difference between the smoothed value and the true value of
(10) is a prediction error evaluator for each motion model that evaluates the prediction error that is the difference between the prediction value and the true value of each of the plurality of motion models, (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. The target observing device (1) converts target position information including observation noise in polar coordinates into Cartesian coordinates. In the observation error evaluator (2) for each motion model, the target position prediction vector calculated one sampling before the current time obtained from the prediction value calculator (7) for each motion model is set as the average vector of the target positions, and each motion model is calculated. Target position observation vector and target position prediction from the prediction error covariance matrix obtained from the prediction error evaluator (10) calculated 1 time before the present time and the observation noise covariance matrix obtained from the preset observation model The covariance matrix of the vector difference is obtained, and the error of the target observation position input from the target observation device with respect to the target predicted position is evaluated for each of a plurality of motion models assuming that the target position observation vector follows a three-dimensional normal distribution. In the reliability calculator (3) of each motion model, the reliability of each of the motion models calculated one sampling before the current time, and the transition probability of the motion model preset assuming Markov property in the motion model transition And observation error evaluator for each motion model (2)
The reliability of each of the multiple motion models is calculated from the observation error evaluation value input for each of the multiple motion models. 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 evaluator (10) for each motion model one sampling before the present time The gain matrix is calculated from the Kalman filter theory from the prediction error covariance matrix for each. In the smoother (6), the smoothed value calculated one sampling before the current time, the target observation device (1)
Target observation position and gain matrix calculator input from (5)
Based on the sum of constant acceleration vectors that make up a plurality of motion models weighted by the gain matrix and the reliability of each motion model (3) The smoothed value of the target position and the target speed is calculated. 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 to calculate the target position and target speed one sampling after the current time. Calculate the predicted value. 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 the 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. In the prediction error evaluator (10) for each motion model, 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 are used. 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, the smoothed values of the target position and the target velocity calculated by the smoother (6) are the reliability for each of the plurality of motion models obtained by the reliability calculator (3) of each motion model. However, the reliability of each motion model was not reflected in the evaluation of the error of the smoothed value, the calculation of the predicted value of the target position and the target speed, and the evaluation of the error of the predicted value. For this reason, the tracking accuracy with respect to the turning target must be degraded.

[Means for solving problems]

The tracking filter of the present invention is a smoothing error covariance matrix which is a smoothing error evaluation which is a difference between a smoothed value and a true value, a prediction error which is a calculation of a predicted value and a prediction error which is a difference between a predicted value and a true value. The reliability of each of a plurality of motion models is used to calculate the covariance matrix.

[Operation] In the present invention, the reliability of each of the plurality of motion models and the constant acceleration vector that forms the plurality of motion models are used, and the motion model is not uniquely determined. The smooth error covariance matrix is calculated by calculating the parameters for increasing the covariance matrix and adding the smooth error covariance matrix for each of the multiple motion models to this parameter, and the reliability for each of the multiple motion models is calculated. The degree of acceleration and the constant acceleration vector that composes multiple motion models are used to calculate the influence term of the acceleration in the prediction, and one sampling after the constant velocity linear motion prediction is calculated based on the smoothed value at the current time in these specifications. The predicted value is calculated by adding the predicted values of the above, and the reliability for each of the multiple motion models and the constant acceleration that constitutes the multiple motion models The parameters that increase the prediction error covariance matrix, which is the estimation of the prediction error, are calculated by using the motion vector that cannot be uniquely determined, and the prediction error covariance of each motion model is calculated based on these parameters. The prediction error covariance matrix is calculated by adding the matrices.

〔Example〕

FIG. 1 is a block diagram showing an embodiment of the present invention,
(1) to (12) are 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. It is a prediction error evaluator that evaluates a prediction error.

The motion model when the number of the plurality is N is x k = Φ _k-1 x _k-1 + Γ _k-1 W _k-1 + Γ ′ _k-1 u _k-1 (1).

Where: • x k is a 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 the sampling time t _k-1 to t _k , and it is assumed that the target makes a uniform linear motion. Is.

· W k is a sampling time t drive noise vector Γk in _k is 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 gamma _k-1 If we look at 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) Γ ′ _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. Coordinate 0-
The x-axis of xy, Y is the y-axis of coordinates 0-xy, where the north direction is the positive y-axis, A
1 is the constant acceleration vector in the positive direction of the y-axis, A2 is the constant acceleration vector in the positive direction of the x-axis, A3 is the constant acceleration vector in the negative direction of the y-axis, A4 is the constant acceleration vector in the negative direction of the x-axis Is. In the case of a motion model in which the magnitude of the constant acceleration vector in FIG. 3 is 10 g (g is the gravitational acceleration) and the constant acceleration vector with zero acceleration is considered, 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 φ _k , a (a = 1,2, ... …, N)… (6) is written.

E is a symbol representing the average, and W k is a three-dimensional normally distributed white noise with an average of 0 E [ W k] = 0 (7) E [ W k ▲ W ^T _l ▼] = Qk (when k = 1 ), 0I (when k ≠ l) ... (8).

Here, 0 is a zero vector. Qk is a driving noise covariance matrix at sampling time tk, and a ^T represents a transposed vector of vector a.

The observation model is z k = Hk x k + v k (9).

Where: z k is an observation vector in Cartesian coordinates composed of the observed values of the target position information at sampling time tk. Hk is the observation matrix at sampling time tk, where Hk = (I 0I).

・V k is the observation noise vector at the sampling time tk, and is three-dimensional normally distributed white noise with an average of 0 E [ v k] = 0 E [ v k ▲ v ^T _l ▼] = Rk (when k = 1) , 0I (when k ≠ l) ... (10).

Note that Rk is the observation noise covariance matrix at the sampling time tk and is a value that does not depend on the motion model.

Also, writing a whole observation vectors from the sampling time t ₁ to tk zk, i.e. zk = [z 1, z 2, ......, z k ] ... (11).

From the above, the usual Kalman filter theory ^a _k▼ (-) = Φ_{k-1 k-1}(+) Γ ′_k-1 αa… (1
2) ▲ P^a _k▼ (-) = Φ_k-1P_k-1(+) ▲ Φ^T _k-1▼ + Γ_k-1Q
_k-1▲ Γ^T _k-1▼… (13) ▲ ^a _k▼ (＋) ＝ ▲ ^a _k▼ (-) + Kk (zk-Hk ▲ ^a _k
▼ (−))… (14) Kk ＝ ▲ P^a _k▼ (-) ▲ H^T _k▼ (Hk ▲ P^a _k▼ (-) ▲ H^T _k▼ +
Rk)^-1 … (15) ▲ P^a _k▼ (＋) ＝ ▲ P^a _k▼ (−) − KkHk ▲ P^a _k▼ (-) ...
(16) (a = 1,2, ..., N).

Where ▲ ^a _k▼ (-) is the observation vector under hypothesis φk, a
z1,z2 ……z _k-1Thanxa prediction vector of k
If you write it with the averaged vector, then from the theory of Kalman filter ▲ ^a _k▼ (-) = E [xk l φk, a, z^k-1] (17)

・ k (+) is the observation vectorz1,z2 ...,zthan k
xwith a smooth vector of k  k (+) = E [xk l z^k] (18)

・ ▲ ^a _k▼ (+) is the observation vector under hypothesis φk, a
z1,z2 ……zthan kxk smooth vector ▲ ^a _k▼ (+) = E [xk l φk, a, z^k] (19).

・ Pk (+) is the smooth error co-component at sampling time tk
It is a dispersion matrix, and from the theory of Kalman filter, Pk (+) = E [(xk- k (+)) (xk- k
(+))^T l z^k] (20).

・ ▲ P^a _k▼ (-) is hypothesis φ at sampling time tk
Prediction error covariance matrix under k, a ▲ P^a _k▼ (-) = E [(xk- ▲ ^a _k▼ (-)) (xk
-▲ ^a _k▼ (-))^T l φk, a, z^k-1] (21)

・ ▲ P^a _k▼ (+) is hypothesis φ at sampling time tk
The smooth error covariance matrix under k, a ▲ P^a _k▼ (+) = E [(xk- ▲ ^a _k▼ (+)) (xk
-▲ ^a _k▼ (+))^T l φ_{k, a}, z^k] (22)

・ Kk is the gain matrix at the sampling time tk.

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

Also, in the same way as when applying the Kalman filter normally.
Initial value 0 (+) and P0 (+) are determined separately
To do.

Equation (13) from ▲ P ^a _k ▼ (-) because value that does not depend on the hypothesis φ _{k, a,} Kk the equation (15), ▲ P ^a from the equation (16) _k ▼ (+) likewise hypothesis phi _The value does not depend on _{k or a} .

Hypothesis φ when the observation vectors z 1, z 2, ..., Z k are obtained
_{k, β k, a} = P by the conditional probability density function of the reliability β _{k, a} of _a [φ _{^k, a} lz _k] ... (23) write (a = 1,2, ......, N ) and.

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 Pab of the motion model is Pab = P [φk, al φk−1, b]… (24) (a = 1,2, ……, N; b = 1,2, ……, N) Write.

Evaluation of observation error under hypothesis φk, a Approximating fa (k) (a = 1,2, ..., N) (25) with a three-dimensional normal distribution is the theory of Kalman filter.
From fa (k) = g (zk; Hk ▲ ^a _k▼ (−), Hk ▲ P^a _k▼ (-)
▲ H^a _k▼ + Rk) It is (26).

Here, g ( z k; ak.Ak) is 3 of the mean vector a k and the covariance matrix Ak.
It is the probability density in z k of the dimensional normal distribution.

As can be seen from equation (26), the observation vector of the target positionz
k is the target position prediction vector Hk ▲ ^a _k▼ Close to (-)
However, the value of fa (k) becomes large and the target is under the hypothesis φk, a.
The reliability of having exercised is highly evaluated. Figure 4 shows incorrect observation
It is a figure explaining the evaluation value fa (k) of a difference in one dimension.
In the figure, G is the horizontal axis representing the target position, and F is the probability density.
Degree of vertical axis, Z is the observation vectorzgoal indicated by k
, PA is the target predicted position under the hypothesis φk, a, and FA is the probability density.
Degree fa (k), N is the average Hk ▲ ^a _k▼ (-) Dispersion Hk ▲ P^a _k▼
(-) ▲ H^T _k▼ + Rk is a normal distribution curve determined by
It Observation vectorzHypothesis φ when k is not obtained
k, a reliability P {φk, a l z^k-1} Is the Markov property,
Hypothesis φk, a is one hypothesis φ before one sampling_k-1, b (b = 1,
2, ..., N)_k-1trust in b
Β in equation (23), which is the degree_k-1, b and the transition probability equation (2
From 4) PabIs obtained as.

Measurement hypothesis at the time the vector z k is obtained .phi.k, a reliability .beta.k, a is the observation vector z k can not be obtained when the hypothesis .phi.k, a reliability P [φk, alz ^k-1] to A value obtained by multiplying the evaluation value fa (k) by the observation vector z k of the hypothesis φ k, a, that is, from Equation (27) and Equation (25) It can be considered to be proportional to.

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

From equation (23) and Bayes' theorem Is.

From equation (18), equation (19) and equation (31) So Is.

From equation (32) and equation (14), equation (12), and equation (29)  k (+) = Φk-1 k-1 (+) + Γ '_{k-1 k-1}＋ K_k
〔zk-H_k(Φk-1 k-1 (+) Γ ′_{k-1 k-1}) ... (3
3)

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 as follows: P [ u k-1 l φk, a, Z ^k-1 ] = δ ( u k-1 − Α a)… (3
Five) And

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

From equations (35) and (37), E [ u k-1 l φk, a, Z ^k-1 ] = ∫ ... ∫ u k-1P [ u k-1 l φ
k, a, Z ^k-1 ] d u k-1 = ∫ ... ∫ u k-1δ ( u k-1− α a) d
u k-1 = α a (38)

That is ▲ ^a _k-1▼ = E [uk-1 l φk, a, Z^k-1]… (39) ▲ ^a _k-1▼ ＝αa ... (40).

From equation (36) and equation (37) Is.

Ie  K-1 = E [uk-1 l Z^k-1 … (42)Is.

From equation (35), equation (40) and equation (37), E [(uk-1− ▲ ^a _k-1▼) (uk-1− ▲ ^a _k-1▼)^T l
 φk, a, Z^k-1] = ∫… ∫ (uk-1−αa) (uk-1−α
a)^TP [uk-1 l φk, a, Z^k-1] Duk-1 ＝ ∫… ∫ (uk
-1-αa) (uk-1−αa)^Tδ (uk-1−αa) duk-
1 = 0I (44).

From equation (36) and equation (37)Is.  k (-) is the observation vectorz1,z2 ...,z _k-1Than
xthe prediction vector of k, ie  k (−) = E [xk l Z^k-1] (46)

In equation (1), equation (46), equation (18), equation (7) and equation (42)
Apply  k (−) = Φk−1 k-1 (+) + Γ '_k-1 K-1 ...
Get (47).

From equation (1) and equation (47)x k- k (−) = Φk−1 (xk-1− K-1
(+)) + Γ_k-1 Wk-1 + Γ '_k-1(uk-1 k-1) ...
(48)

Pk (−) is the prediction error covariance at sampling time tk
Matrix, that is, Pk (−) = E [(xk- k (-)) (xk- k
(-))^T l Z^k-1]… (49)

Substituting equation (48) into equation (49) and applying equation (20), equation (8) and equation (45), x _k-1 , W _k-1 , and u _k-1 are uncorrelated with each other. if To get

Therefore, from equation (27), equation (13) and equation (50) Is.

Here, from equation (43) and equation (27) Is.

From equation (20) and equation (18), Pk (+) = E [xk ▲x ^T _k▼ l Z^k] − k (+) ^T _k
(+) ... (53).

From formula (31) Therefore, from equation (53) Is.

From equation (22) and equation (19) ▲ P^a _k▼ (+) = E [xk ▲x ^T _k▼ l φ_{k, a}, Z^k]-▲
^a _k▼ (+) ▲ ^a _k▼ (+)^T … (55) That is ▲ P^a _k▼ (＋) ＝ ∫… ∫xk ▲x ^T _k▼ P [xk l φ_{k, a}， Z
^k] Dx _k-▲ ^a _k▼ (+) ▲ ^a _k▼ (+)^T Therefore, from equation (54)Is.

From equation (29) and equation (32) Therefore, ▲ P ^a _k ▼ (+) is unrelated to a, equation (29)
And from equation (56) Is.

From formula (14) and formula (12) ▲ ^a _k▼ (+) = Φ_{k-1 k-1}(+) + Γ ′_k-1 αa + Kk
〔Z _k−H_k(Φ_{k-1 k-1}(+) + Γ ′_k-1 αa)]… (5
9)

From Equation (59) and Equation (33) ▲ ^a _k▼ (+)- _k+ = (I−KkHk) Γ ′_k-1(αa-
_k-1) (60) Therefore, from equation (58)Is.

Note that when the motion model is uniquely determined, for example, when βk, 1 = 1, βk, a = 0 (a = 2,3 ..., N) (62), Pk is calculated from equations (61) and (34). (+) = ▲ P ^a _k a ▼ (+) ... (63) , a plurality of motion models each error covariance matrix ▲ P ^a _k ▼ (+) and the error covariance matrix Pk (+) Will be equal. That is, the second term of equation (61) 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) observes the target position information in polar coordinates.
Then, the result is converted into target position information in Cartesian coordinates,
Observation vector of (9)ZOutput k. For each exercise model
In the observation error evaluator (2), the predicted value for each motion model
One sampling before the current time obtained from the calculator (7)
Prediction vector calculated for each motion model of equation (12) ▲
^a _k▼ Input (-) through the fourth delay circuit (12) and
Position prediction vector Hk ▲ ^a _k▼ (-) is calculated and
The present time obtained from the prediction error evaluator (10) for each Dell
The motion model of equation (13) calculated before sampling
Prediction error covariance matrix with ▲ P^a _k▼ (-) to the second delay time
The predicted position error of each motion model is input through the road (11).
Covariance matrix Hk ▲ P^a _k▼ (-) ▲ H^T _k▼ is calculated and this
In equation (10) obtained from the observation model that has been set
Observation vector from observation noise covariance matrix RkZk and motion model
Target position prediction vector Hk for each ^a _k▼ (-) difference
The covariance matrix is calculated and input from the target observing device (1).
Observation vectorZFor each of several motion models of k
Fa in Eq. (25) that is the evaluation value of the observation error
(K) is calculated according to the equation (26). Trust of each movement model
In the degree calculator (3), sampling time t_k-1Calculated by
An expression that is the reliability for each of a plurality of motion models
Β in (23)_{k-1, a}Input through the first delay circuit (4)
Shib and the transition probability Pab of the preset equation (24)
More observation vectorzEach motion model when k is not obtained
Value of equation (27), which is the reliability of the
To the observation vectorZis the degree of agreement of k with each motion model.
Input from the observation error evaluator (2) for each motion model
Observed according to formula (30) by weighting fa (k) of formula (25)
vectorZA plurality of motion models at the time when k is obtained
Calculate βk, a in equation (23), which is the reliability of each. Ge
The in-matrix calculator (5) uses preset observations
The observation noise covariance matrix of Equation (10) obtained from the model and
Preliminary calculation for each exercise model calculated before 1 sampling
Through the second delay circuit (11) from the measurement error evaluator (10)
Prediction error covariance matrix for each input motion model ▲ P^a _k
▼ (-) and smooth vector according to equation (15) k
(+) The gain matrix Kk used for calculation is calculated. Smoother
In (6), equation (18) calculated before 1 sampling
Smooth vector of _k-1(+) To the third delay circuit (8)
Φ through constant velocity linear motion prediction_{k-1 k-1}(+)
Calculated and obtained from the reliability calculator (3) for each motion model
Reliability of each motion model in equation (30)_{k, a}And in advance
Configures multiple motion models of the set equation (5)
Constant acceleration vectorαFrom a, add the target according to equation (34).
speed _k-1And estimate _k-1Is the term Γ ′ that affects the prediction_k-1
k-1Obtained from these and the target observing device (1)
(9) observation vectorZsmooth vector for k
_kObservation vector in (+) calculationZdegree to which k contributes
Gain obtained from the gain matrix calculator (5) that determines
Apply the matrix Kk according to equation (33) and apply the smooth vector _k(+)
To calculate. Prediction value calculator (7) for each motion model
Is a smooth vector obtained from the smoother (6) _k(+)
More uniform linear motion prediction Φ_{k k}(+) Is calculated and
Constants that make up multiple motion models that are randomly set
Acceleration vectorαa is a term that influences the prediction Γ ′_k αCalculate a
Sampling time, sampling time after 1 sampling from the present time
t_{k + 1}Prediction vector for multiple motion models for
^a _{k + 1}▼ (-) is calculated according to equation (12). Exercise model
The smoothing error evaluator (9) for each
Prediction error co-component for each motion model of equation (21)
Matrix ▲ P^a _k▼ (-) is the prediction error evaluation for each motion model
Input from the device (10) through the second delay circuit (11)
According to the gain matrix Kk obtained from the IN matrix calculator (5)
Then, the smooth vector for each motion model in Eq. (19) ▲ ^a _k▼
For multiple motion models that are evaluation values of (+) smoothing error
Smooth error covariance matrix with and ▲ P^a _k▼ (+) according to formula (16)
Calculate.

In the smoothing error evaluator (13), the reliability βk, a of each motion model obtained from the reliability calculator (3) of each motion model, the Kane matrix Kk obtained from the gain matrix calculator (5) and the preset (34) from the constant acceleration vector α a
The term that evaluates that the smoothing error increases because the motion vector is not uniquely determined by using Is calculated, and the smoothing error covariance matrix ▲ P ^a _k ▼ (+) for each motion model obtained from the smoothing error evaluator (9) for each motion model is added to this to calculate the smoothing error covariance that is the evaluation of the smoothing error. The matrix Pk (+) is calculated according to the equation (61).

In the prediction error evaluator (10) for each motion model, smoothing error
Smoothing error covariance matrix Pk (+) obtained from the evaluator (13)
And the expression obtained from the preset motion model
According to the equation (13) by the driving noise covariance matrix Qk of (8)
Sampling time t after 1 sampling from the present time_{k + 1}To
Prediction vector for each of multiple motion models ▲ ^a _{k + 1}
▼ Prediction error covariance matrix that is the evaluation of (-) prediction error ▲
P^a _{k + 1}▼ (-) is calculated. In the predicted value calculator (14),
Smoothing vector of equation (18) from smoother (6) k (+)
Φk by inputting and predicting uniform linear motion Calculate k (+)
Then, the formula obtained from the reliability calculator (3) for each motion model
(30) Reliability βk, a of each motion model and preset
From the transition probability Pab of the equation (24)z _{k + 1}But
The reliability of each motion model in equation (27) when it is not obtainedAnd a plurality of preset motion models.
Constant acceleration vector of equation (5) that composesαa
Observation vectorZ _{k + 1}The target acceleration at the time whenIt is estimated that this acceleration affects the predictionIs calculated according to formula (47) and formula (52)
Prediction vector one sampling after the point _{k + 1}(-)
calculate.

The prediction error evaluator (15) calculates the reliability of each motion model.
Βk, b obtained from the instrument (3), preset equation
(24) Transition probability Pabs and preset multiples
Constant acceleration vector of equation (5) that constitutes the motion model of
TollαFrom equation (52) from a, using one sample from the present
Prediction vector after _{k + 1}It is an evaluation of the prediction error of (-)
Prediction error covariance matrix P_{k + 1}The term to increase (-)And the prediction error evaluator (1
0) prediction error covariance matrix for each motion model
▲ P^a _{k + 1}▼ (-) is added to estimate the prediction error
Difference covariance matrix P_{k + 1}Calculate (-) according to equation (51).

〔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 an observation error evaluation value. In the figure, (1) is a target observation device, (2) is an observation error evaluator for each motion model, (3) is a reliability calculator of each motion model, (4) is a first delay circuit, (5) Is a gain matrix calculator, (6) is a smoother, (7) is a predicted value calculator for each motion model, (8) is a third delay circuit, (9) is a smoothing error evaluator for each motion model, ( 10) is a prediction error evaluator for each motion model, (11) is a second delay circuit, (12) is a fourth delay circuit, (13) is a smoothing error evaluator, (14) is a prediction value calculator, (15) is a prediction error evaluator. 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,
An observation error for each motion model that calculates an observation error evaluation value that is the degree of agreement between each of a plurality of different motion models composed of a plurality of constant vectors of the same dimension and the target position information input from the above target observation device The reliability calculator for each motion model that inputs the observation error evaluation value for each motion model from the evaluation device and the observation error evaluator for each motion model, and calculates the reliability for each motion model, and the above A gain matrix calculator that calculates the same gain matrix in the motion model, the reliability of each motion model calculated by the reliability calculator of each motion model, the gain matrix calculated by the gain matrix calculator, and the target observation Smoothed values of target motion parameters such as target position and velocity obtained by integrating the effects of each motion model using target position information input from the device. The smoothing device to be calculated and the smoothing value of the target motion data calculated by the smoothing device are input to calculate the predicted value of the target motion data for each of the above motion models after one sampling, and the observation error for each of the motion models is calculated. Prediction value calculator for each motion model that outputs the prediction value for each of the above motion models to the evaluator, and the gain matrix is input from the above gain matrix calculator, and the error of the smoothed value of the target motion parameters for each of the above motion model A smoothing error evaluator for each motion model, and evaluating the error of the predicted value of the target motion specifications for each of the above motion models, and calculating the gain matrix calculator, the smoothing error evaluator for each motion model, and the above motion model. The prediction error evaluator for each motion model that outputs the evaluation value of the prediction error for each motion model above to the observation error evaluator for each The reliability of each motion model calculated by the reliability calculator of each motion model, the evaluation value of the smoothing error for each motion model calculated by the smoothing error evaluator for each motion model, and the gain matrix calculated by the gain matrix device And a smoothing error evaluator that outputs the evaluation value of the smoothing error to a prediction error evaluator for each of the motion models and evaluates the error of the smoothing value obtained by integrating the effects of the respective motion models, and each of the motions. Using the reliability of each motion model calculated by the model reliability calculator and the smoothed value of the target motion parameters calculated by the smoother, the target motion parameters obtained by integrating the effects of each motion model are calculated. Calculated by a predictive value calculator that calculates a predicted value after one sampling, a reliability of each exercise model calculated by the reliability calculator of each exercise model, and a prediction error evaluator for each exercise model A tracking filter comprising: a prediction error evaluator that inputs a prediction error evaluation value for each of the motion models and evaluates the error of the prediction value obtained by integrating the effects of the motion models.