JPH0769418B2 - 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. It relates to the tracking filter.

[Conventional technology]

Figure 4 shows, for example, IEEE TRANSACTIONS ON AE-ROSPACE AND
ELECTRONIC SYSTEMS VOL AES-13 No.3, MAY 1977, P310
~ P317 `` MANEUVERING TRRGET TRACKING USING AD-APTI
FIG. 1 is a block diagram of a conventional tracking filter having a plurality of motion models indicated by “VE STATE ESIIMION”, where (1) is a target observation device that measures 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 the reliability of each motion model for calculating the reliability of each of the motion models. Calculator,
(4) is the first delay circuit, (5) is a gain matrix calculator for calculating the Kalman gain matrix used for calculating the smoothed value of the target motion data, and (6) is the smoothed value of the target motion data. Smoothing device, (7) is a predictive value calculator for each motion model that calculates the predicted value of the target motion data after one sampling from each of the plurality of motion models, and (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 of the motion models, and (10) is the predicted value and the true value of each of the motion models. A prediction error evaluator for each motion model that evaluates a prediction error that is a difference between values, (11) is a second delay circuit, and (12) is a fourth delay circuit.

A conventional tracking filter having a plurality of motion models is configured as described above, and, for example, a model in which a constant velocity vector is added to a constant velocity linear motion model and a constant acceleration vector is added to the constant velocity linear motion model is used. The case of having different acceleration levels has been 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 present time, Markov property in the transition of the motion model is assumed, and the motion model set in advance is calculated. The reliability of each of the plurality of motion models is calculated from the transition probabilities and the observation error evaluation values of the plurality of motion models input from the observation error evaluator (2) for each motion model. 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. With the smoother (6), 1 from the present
Smoothed value calculated before sampling, target observation position input from target observation device (1), gain matrix input from gain matrix calculator (5), and reliability calculator (3) of each motion model The smoothed values of the target position and the target velocity are calculated by summing the constant acceleration vectors constituting the plurality of motion models weighted by the respective reliability of each of the plurality of motion models obtained in step 1. 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 target velocity is calculated by the gain matrix calculator (5) and the motion is performed one sampling before the current time. It is calculated by the prediction error covariance matrix calculated by the prediction error evaluator (10) for each 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 reliable for each of the plurality of motion models obtained by the reliability calculator (3) of each motion model. Despite the value reflecting the degree, there was no device to evaluate the error of this smoothed value, so
The smooth error covariance matrix P _K (+) of equation (20) is not calculated, and the smooth error covariance matrix ▲ P ^a _K ▼ for each motion model of equation (16)
(+) Was used instead of the smooth error covariance matrix P _K (+) in equation (20). Note that if the initial value P _O (+) is determined independently of the motion model as in the case of a normal Kalman filter, the smooth error covariance matrix ▲ P for each motion model can be calculated from Eqs. (13), (15), and (16). ^a _K ▼ (+) is a value that does not depend on the motion model. Therefore, the gain matrix used for the smooth value calculation in the gain matrix calculator (5) does not reflect the reliability of each of the plurality of motion models at once, and operates as if the motion model is uniquely obtained. Therefore, an error occurs in the smoothed value at every sampling, and the error is accumulated, so that the smoothed value is a value that is separated from the true value.

The present invention has been made 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 problems]

The tracking filter according to the present invention uses the reliability of each of a plurality of motion models to calculate a smooth error covariance matrix which is an evaluation of a smooth error which is a difference between a smooth value and a true value.

[Action]

In the present invention, the reliability of each of the plurality of motion models and the constant acceleration vector forming 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 matrix and adding these parameters to the smooth error covariance matrix for each of the multiple motion models.

Thus, the smooth error covariance matrix P according to the present invention is
_K(+) Reflects the reliability of each motion model
The motion mode, which is the value obtained by the conventional tracking filter.
Covariance matrix for each Dell ▲ P^a _K▼ (+) than expression (43)
And as shown in equation (45). Therefore, this invention
Where, the smooth error covariance matrix as shown in equation (13)
P_KPrediction error variance for each motion model calculated from (+)
Matrix ▲ P^a _K▼ (-) and equation (15)
Prediction error variance matrix for each rule ▲ P^a _KCalculated from ▼ (-)
Gain matrix K_KIs the value obtained by the conventional tracking filter
Is larger than the smoothed value as shown in equation (33).
_K(+) Observation vector in calculationZ _KIs heavily weighted
The followability to the turning target is improved.

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

A motion model in the case where the number 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 And 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 gamma _k in _k is for example a transformation matrix driving noise vector at sampling time t _k, 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) and Γ _k ′ is the transformation matrix of the constant acceleration vector at the sampling time t _k Is.

Fig. 2 is a diagram for explaining the constant acceleration vector in the plane parallel to the horizontal plane. In the figure, O is the origin of the coordinates O-xy with the target observation device as the origin, and X is the positive x-axis in the east direction. X-axis of coordinate O-xy, Y is the y-axis of coordinate O-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. If the magnitude of the constant acceleration vector in Fig. 2 is 10g (g is the gravitational acceleration), and in addition to this, in the case of a motion model considering a constant acceleration vector, N = 5 (3), Equation (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 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 normal distribution white miscellaneous phrase with mean 0 E [ w _k ] = 0 (7) E [ w _k ▲ w ^T _l ▼] = Q _k (k = 1 ,), And OI (when k ≠ l) ... (8).

Here, 0 is a zero vector, Q _k is a driving noise covariance matrix at sampling time t _k , and a ^T represents a transposed vector of vector a. The observation model is z _k = H _k x _k + v _k (9).

Here, · z _k 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 = the observation matrix at sampling time t _k (I OI).

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

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

Further, the entire observation vector from sampling time t ₁ to t _k is written as z _k , that is, z _k = [ z ₁ , z ₂ , ..., 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▼ (-) + k_k(z _k−H_k▲^a _k
▼ (-)) (14) K_k= ▲ P^a _k▼ (-) ▲ H^T _k▼ (H_k▲ P^a _k▼ (-) ▲ H^T _k
▼ + R_k)^-1 (15) ▲ P^a _k▼ (+) = ▲ P^a _k▼ (-)-K_kH_k▲ P^a _k▼ (-)
(16) (a = 1,2, ..., N).

Where: ▲ ^a _k▼ (-) is temporary Ψ_{k, a}Observation vector under
z ₁,z _Two…z _k-1Likex _kThe prediction vector
If it is written as a mean vector, then from the theory of Kalman filter ▲ ^a _k▼ (-) = (E) [x _k| Ψ_{k, a}, z^k-1] (17)

・ _k(+) Is the observation vectorz ₁,z _Two… 、z _kThanx
_kWith the smooth vector of _k (+) = E [x _k| z^k] (18)

・ ▲ ^a _k▼ (+) is temporary Ψ_{k, a}Observation vector z under
₁, z₂... z_kThanx _kWith the smooth vector of ▲ ^a _k▼ (+) = E [x _k| Ψ_{k, a}, Z^k] (19)

・ 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] (20).

・ ▲ P^a _k▼ (-) is sampling time t_kTemporary Ψ at
_{k, a}Prediction error covariance matrix under ▲ P^a _k▼ (-) = E [(x _k-▲ ^a _k▼ (-)) (x _k
-▲ ^a _k(-))^T │Ψ_{k, a}, Z^k-1} (21)

・ ▲ P^a _k▼ (+) is sampling time t_kTemporary Ψ 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} (22).

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

AT represents the transposed matrix of matrix A.

In addition, it is assumed that the initial values _O (+) and P _O (+) are separately determined as in the case where the Kalman filter is normally applied.

Equation (13) from ▲ P ^a _k ▼ (-) is temporary [psi _k, since the value that does not depend on _a k _k from equation ^{(15), ▼ ▲ P a} k from equation (16) (+) likewise temporary [psi _The value does not depend on _{k or a} .

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

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 _a b of the motion model is P _a b = p [Ψ _{k, a} │Ψ _k-1 ,, b] (24) (a = 1,2, ..., N; b = 1,2, …, N).

Temporary Ψ_{k, a}Evaluation of observation error under_a(K) (a = 1,2, ..., N) (25) can be 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) (26).

Here, g ( z _k ; a _k , A _k ) is 3 of the mean vector a _k and the covariance matrix A _k .
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
_kIs the target position prediction vector H_k▲x ^a _k▼ Close to (-)
Th f_aThe value of (k) becomes large, and the target is Ψ_{k, a}Under
The degree of confidence that he exercised in is highly evaluated. Fig. 3 is observation
Error 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.
Degree of vertical axis, Z is the observation vectorz _kThe goal shown by
, PA is temporary Ψ_{k, a}Target predicted position under
Degree f_a(K), N is the average H_k▲ ^a _k▼ (-) dispersion H_k▲ P^a _k▼
(−) H_k+ R_kIt is a normal distribution curve determined by. Observation
vectorz _kTemporary Ψ at the time when is not obtained_{k, a}Belief
Frequency P {Ψ_{k, a}| Z^k-1} Is a temporary Ψ due to the Markov property._{k, a}
Is each temporary Ψ before 1 sampling_{k-1, b}(B = 1,2, ...,
N), we can obtain Ψ_{k-1, b}Is the reliability of
Β in Equation (23)_{k-1, b}And the transition probability of equation (24)
Pafrom bIs obtained as.

Reliability beta _{k, a} temporary [psi _{k, a} at the time of the observation vector z _k is obtained, the reliability P temporary [psi _{k, a} point not simple vector z _k is obtained [[psi _{k, a} | Z ^k-1 ] is multiplied by the evaluation value f a (k) of the observation vector z _k of the temporary Ψ _{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〔Z
_k −H_k(Φ_{k-1 k-1}(+) + Γ ′_{k-1 k-1})] (33).

here, Is.

From equation (20) and equation (18), P_k(+) = E [x _k x _k ^T| Z_k] − _k(+) ▲ ^T _k▼
It is (+) (35).

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

From equation (22) and equation (19) ▲ P^a _k▼ (+) = E [x _k x _k ^T| Ψ_{k, a}, Z^k] − ▲ ^a _k▼ (+) ▲ ^a _k▼ (+)^T … (37) That is ▲ P^a _k▼ (＋) ＝ ∫… ∫x _k x _k ^TP 〔x _k| Ψ_{k, a}, Z^k] D
x _k -▲ ^a _k▼ (+) ▲ ^a _k▼ (+)^T Therefore, from equation (36)Is.

From equation (29) and equation (32) So, ▲ P ^a _k ▼ be irrelevant to a, from equation (29) and (38) Is.

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

From equation (41) and equation (33) ▲ ^a _k▼ (+)- _k(+) = (I-K_kH_k) Γ ′
_k-1(α a− _k-1) (42) Therefore, from equation (40)Is.

When the motion model is uniquely determined, for example, when β _{k, l} = 1 and β _{k, a} = 0 (a = 2,3, ..., N), from equation (43) and equation (34) P _k (+) = P _k ^a (+) (45), and the smooth error covariance matrix P _k ^a (+) for each of the multiple motion models becomes equal to the smooth error covariance matrix P _k (+) The tracking filter of the present invention is the same as the conventional tracking filter.

That is, the second term on the right side of the equation (43) 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)z _kIs output. 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 for each motion model of equation (12) calculated in
_k ^aInput (-) through the fourth delay circuit (12)
Position prediction vector H_{k k} ^a(-) Is calculated, and for each motion model
1 sun from the present time obtained from the prediction error evaluator (10)
Prediction for each motion model of equation (13) calculated before pulling
Error covariance matrix P_k ^a(-) Through the second delay circuit (11)
Input and then the covariance matrix H of the predicted position error for each motion model_k
P_k ^a(−) H_k ^TCalculated and this and the preset
(10) Observation noise covariance matrix R obtained from the measurement model_kYo
Observation vectorz _kAnd target position prediction for each motion model
Cuttle H_{k k} ^aCalculate the covariance matrix of the difference of (-)
Observation vector input from the surveying instrument (1)z _kA plurality of
Of the observer difference, which is the degree of conformity to each of the motion models of
F in Expression (25), which is the evaluation value_aCalculate (k) according to equation (26)
To do. The reliability calculator (3) for each motion model
Ring time t_k-1Each of the multiple motion models calculated in
Β in equation (23), which is the reliability for_{k-1, a}The first late
Input through the delay circuit (4) and preset with this
The transition probability P of equation (24)_aobservation vector from bz _kGot
Equation (27), which is the degree of signal of each motion model
Of the observation vectorz _kEach luck
The observation error for each motion model, which is the degree of agreement with the motion model
F of the equation (25) input from the difference evaluator (2)_aHeavy (k)
Observed vector according to Mitsuke formula (30)z _kWhen was obtained
An equation that is the reliability of each of several motion models at a point
Β in (23)_{k, a}To calculate. In the gain matrix calculator (5)
Is the formula obtained from the preset observation model
Observed noise covariance matrix of (10) and calculated one sampling before
Prediction error evaluator (10) for each linked model
Motion model input through the second delay circuit (11)
Prediction error covariance matrix P for each_k ^a(-) And formula (15)
According to the smooth vector _k(+) Gain row used for calculation
Row K_kTo calculate. From equation (13),
Prediction error covariance matrix ▲ P^a _k▼ (-) is based on the motion model
Since there is no value, the gain matrix K is calculated from equation (15)._KAlso exercise
The value does not depend on Dell. In smoother (6), 1 sample
Smoothing vector of equation (18) calculated before _k-1
Input (+) through the third delay circuit (8)
By motion predictionΦ _{k-1 k-1}(+) Is calculated and each motion model
Each motion model of equation (30) obtained from the reliability calculator (3) of
Dell's confidence β_{k, a}Of the preset equation (5)
Constant acceleration vector that composes multiple motion models
α _aAccording to equation (34), the target acceleration _k-1Estimate
_k-1Is the term Γ ′ that affects the prediction_{k-1 k-1}And calculate this
The observation vector of formula (9) obtained from the target observation device (1)
Cuttlez _kFor a smooth vector _k(+) Calculation smell
Observation vectorz _kGain row that determines the contribution of
Gain matrix K obtained from the column calculator (5)_kIn equation (33)
So apply and smooth vector _kCalculate (+). Exercise mode
In the predictive value calculator (7) for each Dell, from the smoother (6)
The resulting smooth vector _kConstant velocity linear motion prediction from (+)
By Φ_{k k}(+) Is calculated and preset
Constant acceleration vector that composes multiple motion modelsα _a
Is the term Γ ′ that affects the prediction_k α _aIs calculated and 1 from the present
Sampling time t after sampling_{k + 1}Multiple against
Prediction vector for each motion model ▲ ^a _{k + 1}▼ (-)
Calculate according to (12). Smoothing error evaluation for each motion model
In the instrument (9), the formula (2
Prediction error covariance matrix for each motion model in 1) ▲ P^a _k▼
(-) From the prediction error evaluator (10) for each motion model
Input through 2 delay circuit (11), gain matrix calculator
Gain matrix K obtained from (5)_kBy and,
Smooth vector for each motion model _k ^aOf the (+) smoothing error
Smooth error covariance for multiple motion models that are evaluation values
Matrix ▲ P^a _k▼ (+) is calculated according to equation (16). In addition,
From equation (13), equation (15), and equation (16)
Sliding error covariance sequence ▲ P^a _k▼ (+) does not depend on the motion model
It becomes a value.

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 gain matrix K _k obtained from the gain matrix calculator (5) and From the set constant acceleration vector α _a , formula (3
4) is used to evaluate that the smoothing error becomes large because the motion vector is not uniquely determined. Is calculated, and the smoothing error covariance matrix P _k ^a (+) 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 matrix P _k (+) is calculated according to the equation (43). When the motion model is uniquely determined, the smoothing error covariance matrix P _K (+) is calculated by the conventional tracking filter as shown in Expression (45), and the smoothing error covariance matrix ▲ P ^a for each motion model is calculated. _k ▼
It becomes the same as the value of (+).

In the prediction error evaluator (10) for each motion model, smoothing error
Smoothing error covariance matrix P obtained from the evaluator (13)_k(+)
And the expression obtained from the preset motion model
Driving noise covariance matrix Q of (8)_kAnd according to equation (13)
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.

〔The invention's effect〕

As described above, according to the present invention, the target motion source calculation accuracy can be improved without attaching a special additional device to the normal target automatic tracking device.

In the above, the case of the model in which the constant acceleration vector is added to the constant velocity linear motion model has been described, but the target motion parameters are calculated from the information of the target observation device by having multiple motion models other than this. Applicable to tracking 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 for explaining a constant acceleration vector, FIG. 3 is a diagram for explaining an observation error evaluation value, and FIG. 4 is a diagram for showing a conventional tracking filter. 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, and (13) is a smoothing 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 Input the observation error evaluation value for each of the above motion models from the evaluator and the observation error evaluator for each of the motion models, and calculate the reliability of each of the motion models, and the reliability of each motion model, and the above. A gain matrix calculator that calculates a common gain matrix for each motion model, the latest target position information input from the target observation device, and the reliability of each motion model calculated by the reliability calculator of each motion model And a target position, velocity, acceleration, etc. obtained by integrating the influence of each motion model using the gain matrix calculated by the gain matrix calculator. Input the smoother for calculating the smoothed value of the motion parameters and the smoothed value of the target motion parameter calculated by the smoother, and calculate the predicted value of the target motion parameter for each of the above motion models after one sampling. , The predicted value calculator for each motion model that outputs the predicted value to the observation error evaluator for each motion model, and the gain matrix is input from the gain matrix calculator, and the target motion parameters for each of the motion models are input. The smoothing error evaluator for each motion model that evaluates the error of the smoothed value, and the error of the predicted value of the target motion specification for each of the above motion models is evaluated, and the evaluation value of the prediction error is the gain matrix calculator, the above Calculated by the smoothing error evaluator for each motion model, the prediction error evaluator for each motion model that outputs to the observation error evaluator for each motion model, and the reliability calculator for each motion model Reliability of each motion model, inputs the gain matrix calculated in the evaluation value of the smoothing error and the gain matrix unit for each motion model each calculated in smooth error evaluator for each said motion model,
A smoothing error evaluator that evaluates an error of a smoothing value obtained by integrating the influences of the respective motion models and outputs the evaluation value of the smoothing error to a prediction error evaluator for each of the motion models is provided. A characteristic tracking filter.