Abstract
To address the problem of deviation and registration of 3D radar and infrared sensor, this paper presents and improves a method based on the state value and space deviation of federated filtering of unscented Kalman filter and standard Kalman filter, which conduces to real time registering of system deviation of radar and IF sensors. In the method presented here, a covariance matching criteria-based approach was employed for judgment of filtering divergent trend, while self-adaptive attenuation factor was introduced for correction of the predicted error covariance so as to inhibit the divergent phenomenon. The experiment results indicated that the method presented here conduces to improvement of the precision and speed of space registration, showing practical value in deviation registration of 3D radars and infrared sensors.
1. Introduction
Heterogeneous multisensor system overcomes the defect of single sensor that can provide unilateral information of the tracking object image [1, 2]. However, employment of heterogeneous multisensor system in measurement requires calibration of the data information of such sensors. Due to the fact that the transferred data form, narration, and description of the environment by every sensor vary [2–4], if the space deviation undergoes information fusion directly without registration, then it may result in obvious tracking error or even the presence of multiple false points [5–8].
3D radar is able to provide the complete position information of the tracking objects. Nevertheless, since radar always radiates high-power magnetic wave into the air during working, it is apt to magnetic interference and attack by antiradiation missiles. Infrared sensors can be easily concealed and effective in resisting interference, yet unable to offer the distance data of the tracking object to the system [8]. Moreover, it can be easily influenced by climate due to over close distance. Deviation fusion mainly relies on two methods: least square method and exact maximum likelihood method [9]. Unfortunately, least square method overlooks the measurement noise of sensors and the impact of deviation relative to the common coordinate system of each sensor on information fusion. Exact maximum likelihood method though takes the measurement noise of sensors into consideration [10], yet fails to resolve the problem of error in coordinate conversion, which inevitably results in wrong data and inaccurate models [11]. Moreover, least square method and exact maximum likelihood method all require the sensors to be homogeneous sensors which are less capable in measurement of deviation registration among sensors at different dimensions.
To address the issues stated above, this paper employed an improved federated filter based on unscented and standard Kalman filters, the former wherein was used for accuracy estimation relying on its self-adaptive divergence-inhibition ability, the later wherein was used for deviation vector estimation depending on its high speed, thereby actualizing accurate registration of space deviation.
2. UKF (Unscented Kalman Filter)
Consider a discrete time nonlinear system as follows:where in is the state vector of the system, is the state transition matrix, is system process noise with as variance, and is measurement noise with as variance.
After state estimation of the above nonlinear system, the linear approximation in EKF (Extended Kalman Filter, EKF) algorithm was replaced by UT [12], and then the UKF [13] algorithm for nonlinear system state estimation was acquired [14]. Now simply after sigma point sampling of the system state vector, the process noise and measurement noise can be separated.
The detailed operation steps of UKF algorithm are given as follows.
Initializing:
Computing sigma point set:
Prediction updating:
Measurement updating:
3. Improved Unscented Kalman Filter
When UKF algorithm is applied to target tracking, the mathematical model of the studied object and the a priori knowledge of noise statistics should be known at first. If the filter was designed based on inaccurate mathematical model or noise statistics, then a larger state estimation error or even filter divergence can occur [15].
Divergence of UKF occurs frequently. A covariance registration criterion-based method was presented here for judgment of filter divergence trend:wherein is the preset adjustability coefficients, ; if the above equation is false, then adjust to be
When the filtering diverges, attenuation factor calculation formula can be used to acquire self-adaptive weighting coefficient that can then be used for correction of , accordingly increasing the use of the current observed quantity and inhibiting the filtering divergence. therein can be determined by the following equation:in which is the attenuation coefficient which can further enhance the rapid tracking ability of filter, normally the more it close to 1, the greater proportion of the information before moment , and the influence of the current residual error vector can be highlighted better. This method shows good tracking ability of mutation state and keeps effective in tracking even though the filtering reaches a steady state.
4. Establishment of Improved Heterogeneous Sensor Deviation Registration Model
3D radar and IR sensor B are tracking and detecting the same object , the set relation of deviation registration is shown in Figure 1 It is assumed that radar A is located at the origin of the reference Cartesian coordinate system , IR sensor B locating at in the reference Cartesian coordinate system. Let be the measurement time deviation, the system deviation of radar A (distance, azimuth angle, and pitch angle), and the system deviation (azimuth angle and pitch angle) of infrared . Assuming the system deviation measurement random noises of 3D radar A and IR sensor B were and , respectively, wherein the subject means of and were 0, covariance was the Gaussian distribution of and , in which is the standard deviation of distance, is the standard deviation of radar azimuth angle, is the standard deviation of radar pitch angle, is the standard deviation of IR sensor azimuth angle, is the standard deviation of IR sensor pitch angle.
Supposing the true position coordinate of object at is , true polar coordinates of object relative to IR sensor B are and measurement values of radar A and TR sensor B were and , respectively. The true polar coordinate value of object relative to radar A is , and then (9) can be deduced:
Convert (9) into the common reference coordinate system; then
Developing (11) using the transfer of axes polar coordinate-Cartesian coordinate and Taylor first approximation formula, (12) can be acquired as follows: wherein .
For the same object, since the object positions in the common coordinate system are the same, thus the following equation is true:
Deducing from the equation above, we have
Introducing the equation above into (10),
5. Heterogeneous Sensor Deviation Registration Steps
5.1. Vector Dimension Expansion
Different from the standard state filtering, since the algorithm given in this paper is the federated computation estimation of the object state and deviation registration, dimension expansion is performed here.
Firstly establishing dimension expansion vector , in which is the state vector of the object, and is the time and space deviation vector, then the system state equation in dimension expansion state is shown in wherein , , and and are determined by the specific object motion model.
After dimension expansion, the system measurement vector is , and the system measurement equation after dimension expansion is shown in in which . The specific forms can be referred to as (12) and (15). is the measurement noise vector.
5.2. Computing Steps
The following equation can be deduced from (16):
From (18), registering deviation shows the features of fixed error and remains the same within a certain time.
Besides, from the measurement equations (17), (12), and (15), we can see that, relative to the object state vector , the measurement equation is nonlinear. But it is linear relative to space deviation vector . That is, measurement equation (17) can be developed into
is the nonlinear function matrix of the state vector of the object.
Therefore, giving a fixed dimension expansion state vector , the object state vector can be estimated by improved UKF, and then KF can be used to estimate the space deviation vector . The specific steps are given as follows.
(1) Let the state vector mean be , and its corresponding covariance :
(2) Sampling symmetric strategy can be used for computation of Sigma point and the corresponding weight:wherein is the dimension of the object state vector , is the scale parameter, and is the th column of the matrix square root.
(3) Along the time update process, according to the state equation, the updated sigma point is
(4) Compute the state variance prediction and covariance wherein is the covariance of process noise.
(5) If covariance is emanative, modify according to (7), or otherwise take the next step.
The measurement prediction and covariance corresponding to sigma point are given as follows, respectively:
(6) Measurement updating is as follows:
(7) Then make use of Kalman filtering method to estimate space deviation vector. The specific steps are given as follows:
6. Simulation Experiment
6.1. Univariate Nonstationary Growth Model
Choosing univariate nonstationary growth model [16], the process model and measurement model are given as follows:
and are zero-mean Gaussian noise. This system is highly nonlinear and the likelihood function presents bimodal [17, 18]. Using UKF and improved UKF to estimate the state, the formula of root-mean-square error is . Giving the particle number , and process noise variance values , , and measurement noise variance , simulation is conducted. The results of a single simulation are shown in Figures 2–5. After 100 times of Monte Carlo simulation, the result is given in Table 1.
From the experimental result, it is apparent that giving different noise intensities, the errors of improved UKF algorithm are lower. In the above calculation, with respect to both precision and time, the improved UKF algorithm has the most desirable performance.
6.2. Deviation Registration Model
Assuming the tracked object is in uniform linear motion within 3D space, the initial position of the object was , speed , and process noise variance km. Also assuming radar A locating at the origin, and IF sensor B at position . Sampling cycle is s and deviations of sensors are m, rad, rad, rad, and rad. Random noises of radar A and IF sensor B are and , respectively.
In order to verify the effectiveness of the algorithm, UKF-based registration algorithm mentioned in literature [19] and the improved UKF and KF federated filtering-based registration algorithm presented here were employed, respectively, for carrying out 100 simulation experiments Monte Carlo, acquiring the following simulation results (Figures 6, 7, 8, 9, and 10).
The simulation results have indicated that the improved KF and KF federated filtering deviation registration method features better registering precision in distance deviation of radar, azimuth angle, and pitch angle deviations of both radar and IF than UKF-based deviation registering method. This is because the improved UKF presented here is making judgments of the filtering discrete trend based on measurement information and attenuation factor has been introduced to inhibit the discretion, which can not only guarantee the positive semidefiniteness and positive definiteness of the noise variance matrix, but also effectively monitor the variance matrix. As a result the problem of instable filtering arising from inaccurate or unknown statistics of system noise can be effectively resolved, while the divergent speed and estimation precision can also be significantly enhanced. In addition, when the deviation vector is updated, KF can enable accurate computation of that since the deviation vector is linear. Therefore, the algorithm in this paper displays great advantages in registering precision.
In terms of real time, after repeated simulation experiment Monte Carlo and acquisition of the average value, average time consumption of UKF-based registering algorithm is 0.2836, whereas that of the method based on improved UKF and KF federated filtering is 0.2971. Apparently, compared with UKF-based deviation registering method, the computation complexity of federated filtering deviation registering method remains the same as UKF-based deviation registering method. This is because, in the improved UKF, in addition to real-time judgment of the filtering discrete trend, also needs to be modified in a discrete situation. To analyze from a comprehensive perspective, the algorithm in this paper can well meet the requirement of high precise deviation registering. At the same time the real time can also be guaranteed.
7. Conclusions
This paper presents an improved deviation registration method based on federated filtering of unscented Kalman filter and standard Kalman filter. It takes the system deviation of sensors, position, and azimuth deviations relative to the common reference coordinate into consideration, modifies the covariance self-adaptively based on the real-time discrete situation, and performs precise deviation registering as per the data at different dimensions of 3D radar and IF sensors. At the same time it features a desirable computation speed. Therefore it has a promising future in heterogeneous sensor space registering field.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This work is partially supported by the Scientific Research Common Program of Beijing Municipal Commission of Education #KM200811417011, Funding Project for Academic Human Resources Development in Institutions of Higher Learning under the Jurisdiction of Beijing Municipality, PHR (IHLB) 200906126, PHR (IHLB) 200907120, and the Young Key Teacher Program of Beijing Municipal Commission of Education. Thanks are for the help.