Improved Different Dimensional Sensors Combined Space Registration Algorithm

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.


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][3][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][6][7][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.

UKF (Unscented Kalman Filter)
Consider a discrete time nonlinear system as follows: where in  +1 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: (5)

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,  ≥ 1; if the above equation is false, then adjust  |−1 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  |−1 , accordingly increasing the use of the current observed quantity and inhibiting the filtering divergence.  therein can be determined by the following equation: in which 0 <  < 1 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.

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 (0, 0, 0), IR sensor B locating at ( 0 ,  0 ,  0 ) 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 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   (  ) = √ 2  (  ) +  2  (  ) +  2  (  ).For the same object, since the object positions in the common coordinate system are the same, thus the following equation is true:

Mathematical Problems in Engineering
Deducing from the equation above, we have Introducing the equation above into (10),

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 (  ) = [ (1) (  )  ,  (2) (  )  ], in which  (1) (  ) is the state vector of the object, and  (2) is the time and space deviation vector, then the system state equation in dimension expansion state is shown in wherein Φ = [  0 0  ], (  ) = [Γ(  ) 0], and  and Γ are determined by the specific object motion model.

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.
Therefore, giving a fixed dimension expansion state vector (  ), the object state vector  (1) (  ) can be estimated by improved UKF, and then KF can be used to estimate the space deviation vector  (2) (  ).The specific steps are given as follows.
(3) Along the time update process, according to the state equation, the updated sigma point is (4) Compute the state variance prediction and covariance (1) wherein  is the covariance of process noise.
(26)  (7) Then make use of Kalman filtering method to estimate space deviation vector.The specific steps are given as follows: (2)

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 V() 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 RMSE = [(1/) ∑  =1 (  −  ) 2 ] 1/2 .Giving the particle number  = 100, and process noise variance values  = 10,  = 20, and measurement noise variance  = 1, 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.
In order to verify the effectiveness of the algorithm, UKFbased registration algorithm mentioned in literature [19] and 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,  (1) ( +1 |   ) 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.

Figure 6 :
Figure 6: RMSE of the range bias estimation of radar.

Figure 7 :
Figure 7: RMSE of the azimuth bias estimation of radar.

Figure 8 :
Figure 8: RMSE of the elevation bias estimation of radar.

Figure 9 :
Figure 9: RMSE of the azimuth bias estimation of IR.

Table 1 :
Comparison of simulation parameters by UNGM model.