JP2009300380A - Target tracking device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a target tracking device which can stabilize an estimated accuracy of an unscented Kalman filter without being based on observation condition. <P>SOLUTION: When motion model and observation model in a tracking filter are expressed with a nonlinear function, the tracking filter 2 which applies the unscented Kalman filter which approximates the nonlinear function by the unscented transformation is prepared. The tracking filter 2 reads serially a positioning result of the target computed based on received signal. The tracking filter includes a predicted value adaptation degree evaluation part 22 which computes serially the adaptation degree of the predicted value to the positioning result, based on the predicted value and the positioning result which are calculated with the application of the unscented Kalman filter from the positioning result, and a parameter set part 24 which updates serially scaling parameter of the unscented transformation in the unscented Kalman filter, based on the adaptation degree which is calculated serially at the predicted value adaptation degree evaluation part 22. <P>COPYRIGHT: (C)2010,JPO&INPIT

Description

  The present invention relates to a target tracking device that tracks a target observed by an observation device.

  In the conventional target tracking device, the position and speed of the target are estimated by the following method. First, a network is formed by a plurality of receiving stations arranged at different positions, and the distance between each receiving station and the target converted from the time difference of arrival of radio waves (time difference of arrival: hereinafter referred to as TDOA). Difference (hereinafter referred to as distance difference).

  Further, a difference in radial speed between each receiving station and the target (hereinafter referred to as a radial speed difference) converted from a Doppler frequency difference (Frequency Difference Of Arrival: hereinafter referred to as FDOA) is obtained. Then, the target tracking device asynchronously inputs the distance difference and the radial speed difference obtained as described above, and estimates the target position and speed by performing tracking processing.

  When the target state variable vector is defined by the position and velocity in the north reference orthogonal coordinate system, TDOA and FDOA are nonlinear functions. The tracking target motion model is represented by a nonlinear function equation (1), and the observation model is represented by a nonlinear function equation (2).

In the above equations (1) and (2), x _k is the target state variable vector at the sampling time t _k , w _k is the driving noise vector (mean 0, covariance matrix Q _k ), and Γ ₁ is the transformation to the driving noise vector Represents a matrix. In Equation (2), z _k represents a target observed value vector at sampling time t _k , and v _k represents an observed noise vector (average 0, covariance matrix R _k ).

  As described above, when the motion model and the observation model are nonlinear functions, in the related art, an extended Kalman filter (hereinafter referred to as EKF) is generally applied as a tracking filter in consideration of the nonlinearity of the model. (For example, see Patent Document 1).

JP 2006-201084 A

However, the prior art has the following problems.
In EKF, Kalman filter theory is applied by a partial linear first-order approximation to a nonlinear function. For this reason, when the nonlinearity of the problem is strong, the error from the true value by the linear first-order approximation becomes large, and the estimated value by the tracking filter may greatly deviate from the true expected value and the error covariance.

  In order to reduce such a linear first order approximation problem of the nonlinear function in EKF, an unscented Kalman filter (hereinafter referred to as UKF) is applied as a tracking filter.

  In UKF, a nonlinear function can be approximated to a third-order term of Taylor expansion by unscented transformation (hereinafter referred to as UT). For this reason, compared with the case where EKF is applied, it can be expected that the estimation accuracy in the model that is a nonlinear function is higher.

  In the UT, 2n + 1 sigma points are first calculated according to the following equations (3) to (5).

In the above equations (3) to (5), x (^) _{k | k} represents a smoothed value vector obtained by the tracking filter at time t _k , and P _{k | k} represents a smoothing error covariance matrix (where (^ ) Means that a ^ is attached to the top of the character before the parenthesis). In particular, the smooth error covariance matrix P _{k | k} is decomposed into a lower triangular matrix √ (P _{k | k} ) and an upper triangular matrix (√ (P _{k | k} )) ^T , as shown in the following equation (6): To express.

In the above equations (4) and (5), λ is a scaling parameter, and [√ {(n + λ) P _{k | k} }] _i is the value of the lower triangular matrix √ {(n + λ) P _{k | k} }. Represents i columns.

  In UKF, 2n + 1 sigma points calculated by the above equations (3) to (5) by the UT are terms that reduce the nonlinearity of the nonlinear function of the model.

  However, as shown in the above equations (4) and (5), the sigma point includes a scaling parameter λ. And the setting of the value of an appropriate scaling parameter changes with observation conditions, such as observation accuracy and a sampling rate. Therefore, when the setting is inappropriate, there is a problem that the estimated value diverges or the accuracy of the estimated value is deteriorated.

  The present invention has been made in order to solve the above-described problems, and provides a target tracking device capable of stabilizing the estimation accuracy of an unscented Kalman filter (UKF) regardless of observation conditions. Objective.

  The target tracking device according to the present invention estimates the motion parameters of the target position and speed based on the received signals received by a plurality of receiving stations whose positions are known by radio waves transmitted from the target whose position is unknown. A tracking device that applies an unscented Kalman filter that approximates the nonlinear function by unscented transformation as the tracking filter when the motion model and the observation model in the tracking filter are represented by a nonlinear function, The tracking filter sequentially reads the target positioning result calculated based on the received signal, and based on the predicted value calculated by applying the unscented Kalman filter from the positioning result and the positioning result, the fitness of the predicted value to the positioning result Based on the predicted value suitability evaluation unit that sequentially calculates and the suitability calculated sequentially by the predicted value suitability evaluation unit. In which and a parameter setting unit for sequentially updating a scaling parameter of unscented transformation in Sen Ted Kalman filter.

  According to the target tracking device of the present invention, in the case where the motion model and the observation model are nonlinear functions, the scaling parameter of the unscented Kalman filter (UKF) is updated for each observation based on the positioning result and the estimated value. With this, it is possible to obtain a target tracking device that can stabilize the estimation accuracy of UKF regardless of the observation conditions.

  Hereinafter, a preferred embodiment of a target tracking device of the present invention will be described with reference to the drawings.

Embodiment 1 FIG.
FIG. 1 is a block diagram showing an example of the configuration of the target tracking device according to Embodiment 1 of the present invention. The target tracking device of FIG. 1 includes a target positioning device 1, N tracking filters 2 (1) to 2 (N), an integrated smoothing processing unit 3, a state quantity update unit 4, and a display unit 5.

  Here, if m = 1 to N, each tracking filter 2 (m) includes a predicted value calculation unit 21 (m), a predicted value suitability evaluation unit 22 (m), a smooth value calculation unit 23 (m), and A parameter setting unit 24 (m) is provided.

Next, the function of each component will be described.
The target positioning device 1 calculates information such as the target position and speed as a positioning result based on the signal received by the receiving station. Each tracking filter 2 (m) receives the calculated positioning result and the position information of the receiving station from the target positioning device 1, and estimates the position and speed of the target.

  In the tracking filter 2 (m), the predicted value calculation unit 21 (m) inputs the target positioning result and the position information of the receiving station from the target positioning device 1, and sets the scaling parameter of the UKF from the parameter setting unit 24 (m). Enter the settings. Then, the predicted value calculation unit 21 (m) calculates a predicted value of the target position and speed by UKF having the scaling parameter set by the parameter setting unit 24 (m).

  The predicted value suitability evaluation unit 22 (m) inputs the target positioning result from the target positioning device 1, and also inputs the target position and speed predicted values from the predicted value calculation unit 21 (m). Then, the predicted value suitability evaluation unit 22 (m) calculates the suitability of the predicted value with respect to the positioning result based on the difference or distance between the predicted value and the observed value, and the suitability is calculated with respect to the integrated smoothing processing unit 3. Output.

  The predicted value suitability evaluation unit 22 (m) modifies the setting of the scaling parameter and updates the setting of the scaling parameter in the parameter setting unit 24 (m) when the suitability is lower than the predetermined value.

  The smooth value calculation unit 23 (m) receives the predicted value of the target position and speed from the predicted value calculation unit 21 (m), and calculates the smooth value of the target position according to the procedure of UKF.

  The integrated smoothing processing unit 3 inputs smooth values of the target position and speed from the N smoothing value calculating units 23 (1) to 23 (N), and the N predicted value matching degree evaluating units 22 (1). The degree of fit of the predicted value is input from ~ 22 (N). Then, the integrated smoothing processing unit 3 weights the N smooth values according to the matching degrees corresponding thereto, calculates a weighted average value of the smooth values to obtain an integrated smooth value, and updates the state quantity updating unit 4 and the display unit 5. Output for.

  The state quantity updating unit 4 inputs the integrated smooth values of the N tracking filters 2 (1) to 2 (N) calculated by the integrated smoothing processing unit 3, and integrates the distance difference and the radial speed difference from the integrated smooth value. Calculate the predicted value. Then, the state quantity update unit 4 feeds back the calculated integrated prediction value to the target positioning device 1.

  The display unit 5 inputs the integrated smooth values of the N tracking filters 2 (1) to 2 (N) calculated by the integrated smoothing processing unit 3, and estimates the target position and speed at the current time. Display as a value.

  Next, various definition items for tracking a target by parallel UKF will be described by taking, as an example, the case where TDOA and FDOA are obtained and the distance difference between the target and the receiving station and the radial velocity difference are observed values.

  FIG. 2 is a diagram showing the positional relationship between the receiving station and the signal source target in the first embodiment of the present invention. Here, it is assumed that the target three-dimensional motion is estimated.

In FIG. 2, s _a and s _b represent position vectors of the receiving station a and the receiving station b, respectively. Further, r _a is the distance from the receiving station a to the signal source, r _b respectively represent the distance to the signal source from the reception station b. u represents a position vector of the target (signal source).

  Here, a constant velocity linear motion represented by the following expression (7) is assumed as a target motion model.

In the above equation (7), x _k represents a target state variable vector composed of position and velocity vectors. Each element of the above formula (7) is represented by the following formulas (8) to (13).

Here, u _k represents the position vector of the signal source, and u _k (•) represents the velocity vector of the signal source (where the notation (•) is a symbol at the top of the character preceding the parenthesis. Meaning it is attached). Φ _k represents a transition matrix. w _k is a driving noise vector, and Γ _{1 and k} are transformation matrices of the driving noise vector.

Next, an observation model is defined.
First, an observation model of a tracking method (hereinafter referred to as a TDOA tracking method) when TDOA is obtained is shown.

Observations vectors in TDOA scheme is the distance _r a between the receiving station and the signal source are converted from the TDOA between the receiving stations, the difference between the _{r b} (distance difference). An observation model when this distance difference is used as an observation value is defined by the following equations (14) and (15).

In the above equation (15), r _ab represents a distance difference. Further, h (x _k ) in the above equation (14) is defined by the following equation (16).

The observation noise vector v _k in the above equation (14) is given as a distance difference component as in the following equation (17).

  Next, an observation model of a tracking method (hereinafter referred to as a TDOA / FDOA tracking method) when TDOA and FDOA are obtained will be described.

TDOA / FDOA observations vector in Track method is a distance difference and the distance r _a between the receiving station and the signal source are converted from _FDOA, the difference in the rate of change of r _b (radial velocity difference), the distance difference and the radial velocity An observation model when the difference is used as an observation value is defined by the following equations (18) to (20).

The observation noise vector v _k in the above equation (18) is given as a vector having components of distance difference and radial speed difference as in the following equation (21).

Next, the tracking process in each tracking filter 2 (m) of parallel UKF will be described.
In each tracking filter 2 (m), 2n + 1 sigma points are first calculated according to the following equations (22) to (24).

In the above equations (22) to (24), x (^) _{m, k | k} is a smoothed vector by the tracking filter 2 (m) at time t _k , and P _{m, k | k} is the tracking filter 2 (m ) Represents a smooth error covariance matrix. In particular, the smoothing error covariance matrix P _{m, k | k} is decomposed into a lower triangular matrix √ (P _{m, k | k} ) and an upper triangular matrix (√ (P _{m, k | k} )) ^T , This is expressed as (25).

In the above equations (23) and (24), λ _m is a scaling parameter set by the parameter setting unit 24 (m) of the tracking filter 2 (m), and [√ {(n + λ _m ) P _{m, k | K} }] _i represents the i-th column of the lower triangular matrix √ {(n + λ _m ) P _{m, k | k} }.

  Next, for each sigma point obtained by the above equations (22) to (24), prediction processing is performed according to the following equation (26), and prediction value integration processing is performed according to the following equations (27) and (28).

Here, ω _{m, i} in the above equations (27), (28) is calculated according to the following equations (29), (30).

Then, after performing prediction processing and integration processing of the predicted value of the equation (27) ~ (30), r a and _{r b} of the distance difference of the predicted value r _{(~) m, ab,} and the radial velocity difference Predicted values r (·) (˜) _{m and ab} are calculated according to the following equations (31) and (32) based on the predictive value vector obtained by the above equation (27). It means that ~ is added to the upper part of the character before the parenthesis, and the notation (·) (~) means that ~ is added to the character that is preceded by the character before the parenthesis. Meaning).

Next, data updating for obtaining a smooth value of position and velocity when a new observation value vector is obtained will be described. First, from the predicted value vectors x (˜) _{m, k + 1 | k, i} obtained for each sigma point and the combined predicted value vectors x (˜) _{m, k + 1 | k} , the following equations (33), (34) Calculate

Next, the residual covariance matrix S _{m, k | k−1} and the gain matrix K _{m, k} are respectively expressed by the following equations (35) by P _{m, XZ} , P _{m, ZZ} and the observation error covariance matrix R _{m, k} . , (36).

  Next, a smooth value vector and a smooth error covariance matrix are calculated according to the following equations (37) and (38).

  It is assumed that the processing shown in the equations (22) to (38) described above is performed in parallel in each tracking filter 2 (m).

Next, a series of operations of the target tracking device in the first embodiment will be described.
3, the target tracking device of the first embodiment of the present invention, is a flowchart showing an exemplary flow of estimating the position and velocity of the target at time t _k. In FIG. 3, steps ST2a (m) to ST2e (m) (m = 1 to N) indicate the operations of the tracking filter 2 (m) (m = 1 to N) in FIG. Hereinafter, the operation of each step will be described.

Operation target positioning device 1 in step ST1, the result of observation by a plurality of receiving stations, if the TDOA is obtained, calculates the distance difference _{r ab} from TDOA. The target positioning device 1, when the TDOA and FDOA is obtained, calculates the distance difference _{r ab} and the radial speed difference r _{(·) a,} and _b from TDOA / FDOA.

Then, the target positioning device 1 outputs the result of multipath exclusion for the distance difference and the radial speed difference by the correlation gate to the tracking filter 2 (m) as the target positioning result. Further, the position information s _a and s _b of the receiving station is output to the tracking filter 2 (m).

Operation of Step ST2a (m) (m = 1 to N) In the tracking filter 2 (m), the predicted value calculation unit 21 (m) calculates the distance difference _rab calculated by the target positioning device 1 and the radial speed difference r ( -) Enter _{a, b} and the location information of the receiving station.

Operation of Step ST2b (m) (m = 1 to N) In the tracking filter 2 (m), the predicted value calculation unit 21 (m) receives a target positioning result (distance difference _rab and radial speed difference) from the target positioning device 1. r (·) _{a, b} ) and position information (s _a , s _b ) of the receiving station are input. Further, the predicted value calculation unit 21 (m) receives the setting λ _m of the UKF scaling parameter from the parameter setting unit 24 (m).

Then, the predicted value calculation unit 21 (m) uses the UKF having the scaling parameters set by the parameter setting unit 24 (m) to predict the target position and speed predicted values x (˜) _{m, k + 1 | k} and the prediction error. A covariance matrix P _{m, k + 1 | k} is calculated. Further, the predicted value calculation unit 21 (m) outputs the calculation result to the smooth value calculation unit 23 (m).

  As the motion model of the tracking filter, the motion model defined by the above equations (7) to (13) is used. As the observation model, when TDOA is obtained, the above equations (14) to (17) are used, and when TDOA / FDOA is obtained, the above equations (18) to (21) are used.

The procedure for calculating the predicted value of the target position and speed is based on the above equations (22) to (30).
From the target position and the predicted value of the speed, the predicted value r (˜) _{m, ab of} the distance difference and the predicted value r (·) (˜) _{m, ab} of the radial speed difference are calculated. And it outputs with respect to the predicted value adaptation evaluation part 22 (m).

  The procedure for calculating the predicted value of the distance difference and the predicted value of the radial speed difference from the predicted values of the target position and speed is based on the above equations (31) and (32).

Operation in Step ST2c (m) (m = 1 to N) The predicted value suitability evaluation unit 22 (m) calculates the predicted value r (˜) _{m, ab of} the distance difference calculated by the predicted value calculation unit 21 (m). And the predicted value r (·) (˜) of _{m, ab} is evaluated for the radial speed difference. Further, the predicted value suitability evaluation unit 22 (m) changes the setting of the scaling parameter λ _m in the parameter setting unit 24 (m) according to the evaluated suitability.

FIG. 4 is an explanatory diagram of the process of evaluating the fitness and the process of changing the setting of the scaling parameter λ _m in the predicted value fitness evaluation unit 22 (m) according to the first embodiment of the present invention. The predicted value suitability evaluation unit 22 (m) inputs the distance difference r _ab and the radial speed difference r (·) _{a, b} from the target positioning device 1. Further, the predicted value suitability evaluation unit 22 (m) receives the predicted values r (˜) _{m and ab of the} distance difference and the predicted value r (·) (˜) _{m of the} radial speed difference from the predicted value calculator 21 (m). _{, Ab} .

  Then, the predicted value suitability evaluation unit 22 (m) calculates the suitability of the predicted value with respect to the positioning result based on the difference or distance between the predicted value and the observed value. Then, the predicted value suitability evaluation unit 22 (m) outputs the calculated suitability to the integrated smoothing processing unit 3.

For example, the following may be considered as a method for calculating the fitness of the predicted value. The predicted value suitability evaluation unit 22 (m) calculates the predicted value of the tracking filter 2 (m) based on the scaling parameter λ _m set in the parameter setting unit 24 (m) of each tracking filter 2 (m). It is possible to calculate the likelihood by substituting the positioning result by the target positioning device into the following equation (39) which is N probability density function equations as the center.

Operation in Step ST2d (m) (m = 1 to N) The predicted value suitability evaluation unit 22 (m) is a threshold for evaluating the predicted value suitability calculated by the above formula (39) or other formulas. Has Th _pl . Then, the predicted value suitability evaluation unit 22 (m) continues the setting of the scaling parameter when the calculation result by the above equation (39) is equal to or greater than Th _pl .

On the other hand, when the calculation result by the above equation (39) is less than Th _pl , the predicted value suitability evaluation unit 22 (m) corrects the setting of the scaling parameter, and the correction result is displayed in the parameter setting unit 24 (m). Output for. As a result, the setting of the scaling parameter in the parameter setting unit 24 (m) is updated. Here, the threshold value Th _pl is a parameter set in advance.

A method for correcting the scaling parameter λ _m is, for example, according to the following equation (40). Δλ of the second term on the right side of the following equation (40) is a correction amount of the scaling parameter, and for example, the following equation (41) is used. Ε on the right side of the following equation (41) is a parameter set in advance.

Operation in Step ST2e (m) (m = 1 to N) The smoothing value calculation unit 23 (m) uses the predicted value calculation unit 21 (m) to predict the target position and speed predicted values x (˜) _{m, k + 1 | k} Then, the prediction error covariance matrix P _{m, k + 1 | k} is input, and smooth values x (ｘ) _{m, k | k} and smooth error covariance matrices P _{m, k | k} of the target position are calculated according to the procedure of UKF. The calculation procedure of the smooth value and the smooth error covariance matrix is based on the above equations (33) to (38).

  In the target tracking device according to the first embodiment, in each tracking filter 2 (m), the above-described steps ST2a (m) to ST2e (m) (m = 1 to N) operate in parallel. To do.

Operation in Step ST3 The integrated smoothing processing unit 3 inputs the smoothed values x (^) _{m, k | k} of the target position and speed from the smoothing value calculating unit 23 (m) (m = 1 to N), and the predicted value fit _{P m} of the predicted values from the fitness evaluation section 22 (m) _| inputting the (λm r ab, r (· ) ab). Then, the integrated smoothing processing unit 3 weights the N smooth values according to the matching degrees corresponding thereto, and calculates a weighted average value x (^) _{k | k} of the smooth values. Further, the integrated smoothing processing unit 3 outputs the weighted average value as an integrated smoothing value to the state quantity updating unit 4 and the display unit 5.

  As a calculation method of the weighted average value, when the calculation method of the fitness is the above equation (39), for example, the following equation (42) is used.

Operation of Step ST4 The state quantity update unit 4 inputs the integrated smooth value x (^) _{k | k} of the tracking filter 2 (m) (m = 1 to N) from the integrated smoothing processing unit 3. Then, the state quantity update unit 4 calculates the integrated prediction value r (˜) _{ab for the} distance difference and the integrated prediction value r (·) (˜) _ab for the radial speed difference from the integrated smooth value, and these integrated prediction values. Is fed back to the target positioning device 1. The calculation method of the integrated prediction value is based on the following equations (43) and (44).

Operation of Step ST5 The display unit 5 inputs the integrated smoothing value x (^) _{k | k} of the tracking filter 2 (m) (m = 1 to N) from the integrated smoothing processing unit 3, and outputs the integrated smoothing value at the current time. Display as an estimate of target position and speed.

  As described above, according to the first embodiment, when the motion model and the observation model are nonlinear functions, a plurality of UKFs having different scaling parameter settings are used, and the target position and velocity by each UKF are used. Each UKF scaling parameter is updated for each observation based on the degree of fit of the predicted values. With such a configuration, it is possible to stabilize the estimation accuracy of the target position and velocity by the tracking filter using the UKF in which the setting of the appropriate scaling parameter differs depending on the observation condition regardless of the observation condition.

Embodiment 2. FIG.
FIG. 5 is a block diagram showing an example of the configuration of the target tracking device according to Embodiment 2 of the present invention. The target tracking device according to the second embodiment corresponds to a case where the N tracking filters 2 (m) (m = 1 to N) in the first embodiment are one and the integrated smoothing processing unit 3 is omitted. To do.

  The functions of the prediction value calculation unit 21 and the parameter setting unit 24 in the tracking filter 2 are the same as those of the prediction value calculation unit 21 (m) and the parameter setting unit 24 (m) in the first embodiment, respectively.

  The predicted value suitability evaluation unit 22 inputs a target positioning result from the target positioning device 1 and inputs a predicted value of the target position and speed from the predicted value calculation unit 21. Then, the predicted value suitability evaluation unit 22 calculates a residual for the positioning result of the predicted value, and updates the setting of the scaling parameter in the parameter setting unit 24 (m) based on the residual.

  The smooth value calculation unit 23 receives the predicted value of the target position and speed from the predicted value calculation unit 21, and calculates the smooth value of the target position according to the procedure of UKF. Then, the smooth value calculation unit 23 outputs the smooth value to the state quantity update unit 4 and the display unit 5.

  Further, the state quantity update unit 4 feeds back the smooth value to the target positioning device 1. The display unit 5 receives the smoothing value of the tracking filter from the smoothing value calculation unit 23 of the tracking filter 2 and displays the smoothed value as an estimated value of the target position and speed at the current time.

Next, the operation of the target tracking device in the second embodiment will be described. 6, the target tracking device according to the second embodiment of the present invention, is a flowchart showing an exemplary flow of estimating the position and velocity of the target at time t _k. Hereinafter, the operation of each step will be described.

Operation of Step ST1 to Step ST2b The operation of Step ST1 to Step ST2b in FIG. 6 is the same as Step ST1 to Step ST2b (m) in the first embodiment, respectively. The mathematical formula used for the prediction process corresponds to the case where only m = 1 in the above formulas (22) to (32).

Operation of Step ST2c The predicted value suitability evaluation unit 22 inputs the distance difference _rab and the radial speed difference r (•) _{a, b} from the target positioning device 1, and the predicted value r () of the distance difference from the predicted value calculation unit 21. ~) _{M, ab} and the predicted value of radial velocity difference r (·) (~) Input _{m, ab} . Then, the predicted value suitability evaluation unit 22 calculates a residual between the predicted value and the positioning result.

Operation in Step ST2d The predicted value suitability evaluation unit 22 updates the setting of the scaling parameter in the parameter setting unit 24 based on the residual calculated in step ST2c. As a method for correcting the scaling parameter, for example, the following equation (45) is used. Δλ _{2 in} the second term on the right side of the following equation (45) is a correction amount of the scaling parameter, and for example, the following equation (46) is used. Ε _{2 on} the right side of the following equation (46) is a parameter set in advance.

Operation in Step ST2e The smooth value calculation unit 23 receives the target position and speed prediction values x (˜) _{k + 1 | k} and the prediction error covariance matrix P _{k + 1 | k} from the prediction value calculation unit 21. Then, the smooth value calculation unit 23 calculates the smooth value x (^) _{k | k} and the smooth error covariance matrix P _{k | k} of the target position according to the procedure of UKF, and the state quantity update unit 4 and the display unit 5 Output. For the calculation of the smoothing value and the smoothing error covariance matrix, those in the above equations (33) to (38) where only m = 1 is used.

Operation of Step ST4 The state quantity updating unit 4 inputs the smoothed value x (^) _{k | k} of the tracking filter 2 from the smoothed value calculating unit 23, and predicts the distance difference predicted value r (˜) _ab from the smoothed value, and the radial The predicted value r (•) (˜) _ab of the speed difference is calculated. Then, the state quantity update unit 4 feeds back these predicted values to the target positioning device 1. The calculation method of the predicted value of the distance difference and the radial speed difference is based on the above equations (43) and (44).

  As described above, according to the second embodiment, a single UKF is used for the tracking filter, and the UKF scaling parameter is updated for each observation, thereby obtaining a stable estimation accuracy regardless of the observation conditions. be able to. Furthermore, compared with the first embodiment, the configuration of the target tracking device can be simplified and the load of tracking processing can be reduced.

  As an example of the scaling parameter correction method in the second embodiment, a method based on the residual of the predicted value is shown. However, other scaling parameter correction methods may be used.

Embodiment 3 FIG.
FIG. 7 is a block diagram showing an example of the configuration of the target tracking device according to Embodiment 3 of the present invention. The target tracking device according to the third embodiment further includes a tracking filter control unit 6 as compared with the target tracking device according to the first embodiment. The target positioning device 1 is connected to the predicted value calculation unit 21 (m) of each tracking filter 2 (m) (m = 1 to N) via the tracking filter control unit 6.

  The functions of the target positioning device 1, the predicted value calculation unit 21 (m), the smooth value calculation unit 23 (m), the parameter setting unit 24 (m), and the state quantity update unit 4 in the third embodiment are This is the same as that of the target tracking device in the first embodiment.

  The function of the integrated smoothing processing unit 3 is the same as that of the target tracking device in the first embodiment until the tracking filter operated by the tracking filter control unit 6 is limited to one. The function of the integrated smoothing processing unit 3 after the number of tracking filters operated by the tracking filter control unit 6 is limited to one will be described later.

  In addition to having the function of the predicted value suitability evaluation unit 22 (m) in the first embodiment, the predicted value suitability evaluation unit 22 (m) in the third embodiment has the function of the calculated predicted value. Is input to the tracking filter control unit 6.

  The tracking filter control unit 6 inputs the target positioning result and the position information of the receiver from the target positioning device 1 and starts tracking processing until a preset time elapses until all tracking filters #m ( In m = 1 to N), the positioning result and the position information of the receiver are input.

  Further, the tracking filter control unit 6 inputs the fitness for the positioning result of the predicted value by the tracking filter 2 (m) from the predicted value fitness evaluation unit 22 (m) of each tracking filter 2 (m). Then, the tracking filter control unit 6 selects only one tracking filter with the highest predicted value fitness at the time when a preset time has elapsed since the tracking process was started, and the remaining (N−1) filters. The tracking filter operation is stopped.

  After the number of operating tracking filters is limited to one, the integrated smoothing processing unit 3 uses the smoothed value of the operating tracking filter as an integrated smoothing value as it is and outputs it to the state quantity updating unit 4 and the display unit 5. To do.

Next, the operation of the target tracking device in the third embodiment will be described. 8, in the target tracking device of the third embodiment of the present invention, is a flowchart showing an exemplary flow of estimating the position and velocity of the target at time t _k. Hereinafter, the operation of each step will be described.

Operations in Step ST1, Step ST2a (m) to Step ST2e (m), Step ST4, and Step ST5 These operations are the same as those in FIG. 3 in the first embodiment.

Operation of Step ST6 The tracking filter control unit 6 measures the time from the start of the tracking process, and after the predetermined time t _change has elapsed, as the output destination, the tracking with the highest degree of fitness of the predicted value at that time Only one filter is selected and the remaining N-1 tracking filters are stopped. The time t _change is a parameter set in advance in the tracking filter control unit 6.

Operation of Step ST3 The operation of the integrated smoothing processing unit 3 is the same as that of the target tracking device in the first embodiment until the number of tracking filters operated by the tracking filter control unit 6 is limited to one. On the other hand, after the number of tracking filters operated by the tracking filter control unit 6 is limited to one, the smooth value by the operating tracking filter is directly used as the integrated smooth value.

  As described above, according to the third embodiment, an optimal value is calculated as a UKF scaling parameter in the process of performing tracking by parallel UKF in the time zone immediately after the start of tracking where the tracking accuracy is not stable. Then, after a predetermined time has elapsed, only the tracking filter based on UKF having the optimum scaling parameter is operated. As a result, stable estimation accuracy can be obtained regardless of the observation conditions, and at the same time, the load related to the tracking process after the time zone immediately after the start of tracking can be reduced.

Embodiment 4 FIG.
FIG. 9 is a block diagram showing an example of the configuration of the target tracking device according to Embodiment 4 of the present invention. The target tracking device according to the fourth embodiment further includes a delay observation value processing unit 7 as compared with the target positioning device according to the first embodiment. Then, the target positioning device 1 is connected to the predicted value calculation unit 21 (m) of each tracking filter 2 (m) (m = 1 to N) via the delay observation value processing unit 7.

  The target positioning device 1 according to the fourth embodiment measures the target in the same manner as the target positioning device according to the first embodiment, and outputs the positioning result to the delayed observation value processing unit 7. The delayed observation value processing unit 7 inputs a positioning result for the target from the target positioning device 1, inputs an integrated prediction value after integrated smoothing processing from the state quantity update unit 4, and calculates the time of the positioning result and the latest integrated prediction value. Compare the time.

  When the time given to the positioning result is newer than the time given to the latest integrated prediction value (that is, when the positioning result is input to the tracking filter without delay), the delayed observation value processing unit 7 outputs the positioning result as it is to the predicted value calculation unit 21 (m) and the predicted value suitability evaluation unit 22 (m) of each tracking filter 2 (m).

  On the other hand, when the time given to the positioning result is older than the time given to the latest integrated prediction value, the delay observation value processing unit 7 performs the process for each receiver based on the latest integrated prediction value. Estimate radial velocity and acceleration relative to the target.

  The delayed observation value processing unit 7 extrapolates the observed values TDOA and FDOA according to the radial velocity, the radial acceleration, and the time difference between the time given to the latest integrated prediction value and the time given to the positioning result. To do.

  The delay observation value processing unit 7 calculates a distance difference from TDOA obtained by extrapolation, and calculates a radial speed difference from TDOA / FDOA. Then, the delayed observation value processing unit 7 outputs the calculation result to the predicted value calculation unit 21 (m) and the predicted value suitability evaluation unit 22 (m).

  The functions of the tracking filter 2 (m), each part of the tracking filter 2 (m), and the display unit 5 in the fourth embodiment are the same as those in the first embodiment.

  The integrated smoothing processing unit 3 has the same functions as those of the first embodiment, in addition, the matching value of the predicted value of each tracking filter 2 (m), and the parameter setting unit 24 (for each tracking filter 2 (m) The value of the scaling parameter in m) is output to the state quantity update unit 4.

  The state quantity update unit 4 inputs the degree of matching between the integrated smoothing value and the predicted value of the tracking filter 2 (m) (m = 1 to N) from the integrated smoothing processing unit 3. Then, the state quantity update unit 4 calculates an integrated prediction value of the distance difference and the radial speed difference from the integrated smooth value, and feeds back the integrated prediction value to the target positioning device 1 and the delayed observation value processing unit 7.

Next, the operation of the target tracking device in the fourth embodiment will be described. 10, in the target tracking device of the fourth embodiment of the present invention, is a flowchart showing an exemplary flow of estimating the position and velocity of the target at time t _k. Hereinafter, the operation of each step will be described.

Operations in Step ST2a (m) to Step ST2e (m), Step ST3, Step ST4, and Step ST5 These operations are the same as those in FIG. 3 in the first embodiment.

Operation of Step ST7a The delay observation value processing unit 7 inputs a positioning result for the target from the target positioning device 1, inputs a smooth value from the state quantity update unit 4, and obtains the time of the positioning result and the time of the latest smooth value. Compare.

  When the time given to the positioning result is newer than the time given to the latest smoothed value (Yes-side flow), the delayed observation value processing unit 7 uses the positioning result as it is for each tracking filter 2 (m). To the predicted value calculation unit 21 (m) and the predicted value suitability evaluation unit 22 (m).

Operation of Step ST7b When the time given to the positioning result is older than the time given to the latest smoothed value (No-side flow in (Step ST7a)), the delay observation value processing unit 7 Based on the above, the radial velocity and the radial acceleration with respect to the target for each receiver are estimated.

  The delayed observation value processing unit 7 performs positioning that is delayed from the time of the smooth value due to the estimated radial velocity, radial acceleration, and the time difference between the time given to the latest smooth value and the time given to the positioning result. Perform time correction on the result.

  The delay observation value processing unit 7 calculates a distance difference from TDOA obtained by extrapolation, and calculates a radial speed difference from TDOA / FDOA. Then, the delayed observation value processing unit 7 outputs to the predicted value calculation unit 21 (m) and the predicted value suitability evaluation unit 22 (m).

The estimation of the radial velocity and the radial acceleration in the delay observation value processing unit 7 and the time correction for the delayed positioning result are performed by the following procedure, for example. First, the time difference Δt is obtained from the following equation (47) from the time t _k given to the smooth value by the tracking filter 2 (m) and the time t _k 'given to the positioning result.

  When a delay occurs in the transmission of the positioning result, Δt <0. In this case, the following observation value correction process is performed as step ST7b. Here, the procedure of time correction for the delayed positioning result when TDOA is obtained will be described using mathematical expressions.

Assuming that the observation accuracy regarding the distance difference is constant regardless of time, first, the radial velocity difference at time t _k ′ is estimated by calculating the following equation (48).

In the above equation (48), s _{a, k ′} , s _{b, k ′} are the position vectors of the sensor at time t _k ′, and s (•) _{a, k ′} , s (•) _{b, k ′} are This is the velocity vector of the sensor at time t _k ′, both of which are known quantities.

In the above equation (48), u (˜) _{k ′ | k} and u (•) (˜) _{k ′ | k} are estimated values of the target position vector and velocity vector at time t _k ′, respectively, and time t _k Is calculated from the smooth value of the target position and the smooth value of the velocity vector. The calculation method of u (˜) _{k ′ | k} , u (•) (˜) _{k ′ | k} is based on, for example, the following equations (49) and (50).

Next, define the observation model at time _{t k} by the following equation (51).

Z _k ′ of the first term on the right side of the above equation (51) is an observation value vector at time t _k ′. In addition, v _k ′ of the second term on the right side of the above equation (51) is an observation noise vector. Further, the estimated value of the radial speed difference at time t _k ′ calculated by the above equation (48) is substituted in the parenthesis of the third term on the right side of the above equation (51), and the observed value vector obtained by delaying the time of the z k _'get the result of the correction to the current time t _k.

Next, the procedure of time correction for the delayed positioning result when TDOA / FDOA is obtained will be described using mathematical expressions. Assuming that the observation accuracy regarding the speed difference is constant regardless of time, first, the difference in radial acceleration at time t _k ′ is estimated by calculating the following equation (52).

In the equation (52), s _{a, k ′} , s _{b, k ′} are sensor position vectors at time t _k ′, and s (•) _{a, k ′} , s (•) _{b, k ′} are time The velocity vector of the sensor at t _k ′, both of which are known quantities.

U in the above equation _{(52) (~) k '} | k, u (·) (~) k' | k is the position vector and the velocity vector of the target at each time _{t k} ', of the target at time _{t k} It is calculated from the smooth value of the position and the smooth value of the velocity vector. The calculation method of u (˜) _{k ′ | k} , u (•) (˜) _{k ′ | k} is based on, for example, the above formulas (49) and (50).

Next, the observation model at time _{t k} defined by the equation (53).

Z _k ′ of the first term on the right side of the above equation (53) is an observation value vector at time t _k ′. Further, v _k ′ of the second term on the right side of the above equation (53) is an observation noise vector. Furthermore, the estimated value of the difference in radial acceleration at time t _k ′ calculated by the equation (52) is substituted in the parenthesis of the third term on the right side of the above equation (53), and the observed value vector obtained by delaying the time of the z k _'get the result of the correction to the current time t _k.

  As described above, according to Embodiment 4, TDOA or TDOA / FDOA between a plurality of receiving stations can be obtained. As a result, stable estimation accuracy can be obtained regardless of the observation conditions when the target position and velocity are estimated by UKF using the distance difference and the radial velocity difference as observation value vectors. Furthermore, tracking can be maintained even when there is a delay in the transfer of positioning results depending on network conditions, and a portion of the positioning result data obtained at the same time is missing.

  In the first to fourth embodiments described above, an example in which TDOA or TDOA / FDOA between a plurality of receiving stations is obtained, and the target position and speed are estimated using the distance difference and the radial speed difference as an observation value vector. It was. However, the type of the observation device may be other, and the elements of the observation value vector may be configured by, for example, a position or a position and a velocity, not a distance difference and a radial velocity difference.

  Further, the nonlinear term in the observation model is not limited to the form of the above formulas (16) and (20), but may be of another form.

In addition, as an example of a method for calculating the degree of fitness of the predicted value with respect to the observed value in the first to fourth embodiments, the tracking filter 2 (based on the scaling parameter λ _m set in the parameter setting unit 24 (m) is used. The method of calculating the likelihood by N probability density functions centered on the predicted value of m) was shown. However, the calculation method of the fitness is not limited to this, and other methods may be used.

  Further, in the above-described first to fourth embodiments, as an example of a method for changing the scaling parameter of the tracking filter 2 (m) having a low fitness with respect to the observation value of the prediction value, a mathematical formula based on the quadratic form of the prediction value residual Mentioned. However, other modification methods may be used.

It is a block diagram which shows an example of a structure of the target tracking apparatus in Embodiment 1 of this invention. It is the figure which showed the positional relationship of the receiving station and the target which is a signal source in Embodiment 1 of this invention. In target tracking apparatus according to the first embodiment of the present invention, it is a flowchart showing an exemplary flow of estimating the position and velocity of the target at time t _k. It is explanatory drawing of the process of changing the setting of the scaling parameter (lambda) _m , and the process of evaluating the fitness in the predicted value adaptation evaluation part 22 (m) of Embodiment 1 of this invention. It is a block diagram which shows an example of a structure of the target tracking apparatus in Embodiment 2 of this invention. In target tracking apparatus according to the second embodiment of the present invention, it is a flowchart showing an exemplary flow of estimating the position and velocity of the target at time t _k. It is a block diagram which shows an example of a structure of the target tracking apparatus in Embodiment 3 of this invention. In target tracking apparatus according to the third embodiment of the present invention, it is a flowchart showing an exemplary flow of estimating the position and velocity of the target at time t _k. It is a block diagram which shows an example of a structure of the target tracking apparatus in Embodiment 4 of this invention. In target tracking apparatus according to a fourth embodiment of the present invention, it is a flowchart showing an exemplary flow of estimating the position and velocity of the target at time t _k.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 Target positioning device, 2, 2 (1) -2 (N) Tracking filter, 21, 21 (1) -21 (N) Predicted value calculation part, 22, 22 (1) -22 (N) Predicted value adaptability Evaluation unit, 23, 23 (1) to 23 (N) Smooth value calculation unit, 24, 24 (1) to 24 (N) Parameter setting unit, 3 Integrated smoothing processing unit, 4 State quantity update unit, 5 Display unit, 6 Tracking filter control unit, 7 Delay observation value processing unit.

Claims (5)

A tracking filter for estimating movement parameters of the position and speed of the target based on reception signals received by a plurality of receiving stations with known positions of radio waves transmitted from a target whose position is unknown; and the tracking When a motion model and an observation model in a filter are represented by a nonlinear function, a target tracking device applying an unscented Kalman filter that approximates the nonlinear function by unscented transformation as the tracking filter,
The tracking filter is
The target positioning result calculated based on the received signal is sequentially read, and the predicted value for the positioning result based on the predicted value calculated by applying the unscented Kalman filter from the positioning result and the positioning result. A predicted value fitness evaluation unit that sequentially calculates the fitness of
A target tracking device comprising: a parameter setting unit that sequentially updates a scaling parameter of the unscented transform in the unscented Kalman filter based on the fitness values sequentially calculated by the predicted value fitness evaluation unit. The target tracking device according to claim 1,
The target tracking device, wherein the predicted value suitability evaluation unit sequentially calculates the suitability of the predicted value with respect to the positioning result based on a residual between the predicted value and the positioning result. The target tracking device according to claim 1,
The tracking filter is composed of a plurality of tracking filters having different scaling parameters,
Each of the plurality of tracking filters further includes a smooth value calculation unit that calculates a smooth value of the calculated predicted value,
Reading the plurality of smooth values calculated by the respective smoothing value calculation units of the plurality of tracking filters and the plurality of matching degrees calculated by the respective predicted value matching degree evaluation units of the plurality of tracking filters; A target tracking device, further comprising: an integrated smoothing unit that calculates a final predicted value of the target by calculating an integrated smoothed value obtained by performing weighted averaging of the smoothed value of the target with the plurality of matching degrees. In the target tracking device according to claim 3,
Provided in a preceding stage of the plurality of tracking filters, read the target positioning result calculated based on the received signal, and a plurality of adaptations calculated by each predicted value adaptation evaluation unit of the plurality of tracking filters Until the predetermined time has elapsed from the start of the tracking process, the positioning results are output to all of the plurality of tracking filters, and all of the plurality of tracking filters are operated. One tracking filter with the highest fitness is identified based on the read fitness, and the positioning result is output to the identified tracking filter to operate only the one tracking filter. A tracking filter control unit that stops the operation of the remaining tracking filters;
The integrated smoothing processing unit calculates the smoothed value calculated by the smoothing value calculating unit corresponding to the specified one tracking filter as the final predicted value of the target after the predetermined time has elapsed. Feature target tracking device. In the target tracking device according to claim 3,
Provided in a preceding stage of the plurality of tracking filters, and reading the positioning result of the target calculated based on the received signal, and reading the final predicted value of the target calculated by the integrated smoothing processing unit, The first time given to the positioning result is compared with the second time given to the final predicted value, and when the second time is a time earlier than the first time, the plurality of times The positioning result is output to all of the tracking filters, and when the first time is earlier than the second time, the second time is determined based on the positioning result at the first time. A target tracking device, further comprising a delayed observation value processing unit that obtains an estimated positioning result and outputs the estimated positioning result as a positioning result at the second time for all of the plurality of tracking filters.

Images