The Joint Adaptive Kalman Filter (JAKF) for Vehicle Motion State Estimation

This paper proposes a multi-sensory Joint Adaptive Kalman Filter (JAKF) through extending innovation-based adaptive estimation (IAE) to estimate the motion state of the moving vehicles ahead. JAKF views Lidar and Radar data as the source of the local filters, which aims to adaptively adjust the measurement noise variance-covariance (V-C) matrix ‘R’ and the system noise V-C matrix ‘Q’. Then, the global filter uses R to calculate the information allocation factor ‘β’ for data fusion. Finally, the global filter completes optimal data fusion and feeds back to the local filters to improve the measurement accuracy of the local filters. Extensive simulation and experimental results show that the JAKF has better adaptive ability and fault tolerance. JAKF enables one to bridge the gap of the accuracy difference of various sensors to improve the integral filtering effectivity. If any sensor breaks down, the filtered results of JAKF still can maintain a stable convergence rate. Moreover, the JAKF outperforms the conventional Kalman filter (CKF) and the innovation-based adaptive Kalman filter (IAKF) with respect to the accuracy of displacement, velocity, and acceleration, respectively.


Introduction
The motion state estimation of the forward-moving vehicle is a prerequisite for automatic driving vehicles, and its main outputs are the relative transverse longitudinal velocity and acceleration, and relative location. Currently, Lidar and Radar are commonly used as distance measurement sensors in automatic driving systems, which were already fully demonstrated by many teams at the DARPA Urban Challenge [1][2][3]. Lidar has high accuracy and wide measuring range, and can immediately obtain targets' displacement, and thus can simply calculate velocity, acceleration, and other states. Laser measurements however easily suffer from attenuation over the air, and the perception accuracy happens to sharply decline due to the serious noise, and thus Lidar fails to work normally in bad weather [4][5][6]. Radar is another common distance measurement sensor that can easily obtain targets' displacement, velocity, and acceleration. Despite a lower accuracy than Lidar, Radar is better at penetrating through smoke and dusts, and thus is robust against bad weather conditions. Hence, an obvious step is to fuse Lidar and Radar sensors in order to highlight their respective advantages and to compensate their mutual shortcomings. By exploiting the associated properties with the different In a word, the current main efforts are made to improving the accuracy of the filtered results. However, when suffering from continuous high noise, none of the related sensors would keep working as expected. Any sensor losing effectivity undoubtedly decreases the overall performance, so an adaptive control decision should be made by a tradeoff between accuracy and stability. Considering the two factors, the proposed JAKF employs the high-accuracy Lidar and Radar as the source of input data, and synthesizes the respective advantages of IAE and FR to realize the multi-sensor adaptation. In addition, Chi-square hypothesis test and correction decreases measurement error, and coordinate transformation and time synchronization decreases fusion error. Therefore, JAKF is more accurate and stable than the conventional federated Kalman filter and single-sensor adaptive Kalman filter using Lidar or Radar.

Method
This section first briefly introduces the structure of JAKF, and then explains the local and global filter in details, and at last provides two post-processing operations: coordinate transformation and time synchronization.

The Structure of JAKF
JAKF is a two-step processing method for partition estimation. Figure 1 shows the structure and working process of JAKF. (1) Sensors send the current collected data to the LF; (2) The LF fuses the measured data with the feedback data from the GF, and then updates the time and filtered information into the latest value; (3) The GF fuses all the corrected data into a new global state estimation, and outputs the global state estimation and meanwhile feeds them back to the LF. so an adaptive control decision should be made by a tradeoff between accuracy and stability. Considering the two factors, the proposed JAKF employs the high-accuracy Lidar and Radar as the source of input data, and synthesizes the respective advantages of IAE and FR to realize the multisensor adaptation. In addition, Chi-square hypothesis test and correction decreases measurement error, and coordinate transformation and time synchronization decreases fusion error. Therefore, JAKF is more accurate and stable than the conventional federated Kalman filter and single-sensor adaptive Kalman filter using Lidar or Radar.

Method
This section first briefly introduces the structure of JAKF, and then explains the local and global filter in details, and at last provides two post-processing operations: coordinate transformation and time synchronization.

The Structure of JAKF
JAKF is a two-step processing method for partition estimation. Figure 1 shows the structure and working process of JAKF. (1) Sensors send the current collected data to the LF; (2) The LF fuses the measured data with the feedback data from the GF, and then updates the time and filtered information into the latest value; (3) The GF fuses all the corrected data into a new global state estimation, and outputs the global state estimation and meanwhile feeds them back to the LF. In Figure 1, LFi takes the latest Zi, Qi, Pi, and Xi as input and then independently performs the IAE based on such information. Zi is the corrected measured data from Lidar or Radar by coordinate transformation and time synchronization. β is the information allocation factor of LFi. Xg is the global optimal estimation. Pg is the V-C matrix of Xg. Then, LFi sends Qi, Ri, Xi, and Pi to the GF. Qi is the system noise V-C matrix of LFi. Ri is the measurement noise V-C matrix of LFi. Xi is the state estimated value of LFi. Pi is the V-C matrix of Xi. At last, GF calculates Qi, Xi, and Pi, and feeds them back to the LFi, and finally outputs the optimal result Xg and Pg. When i = 1, the data source of LFi is Lidar, otherwise is Radar for i = 2.

Z1
Coordinate system transformation and Time unification (2) (3) Figure 1. The structure and working process of JAKF.
In Figure 1, LF i takes the latest Z i , Q i , P i , and X i as input and then independently performs the IAE based on such information. Z i is the corrected measured data from Lidar or Radar by coordinate transformation and time synchronization. β is the information allocation factor of LF i . X g is the global optimal estimation. P g is the V-C matrix of X g . Then, LF i sends Q i , R i , X i , and P i to the GF. Q i is the system noise V-C matrix of LF i . R i is the measurement noise V-C matrix of LF i . X i is the state estimated value of LF i . P i is the V-C matrix of X i . At last, GF calculates Q i , X i , and P i , and feeds them back to the LF i , and finally outputs the optimal result X g and P g . When i = 1, the data source of LF i is Lidar, otherwise is Radar for i = 2.

Local Kalman Filter
Since vehicle's transverse and longitudinal velocity changes slowly during driving, so LF can use the linear discrete Kalman filter model, as expressed by [23]: where X t+1 and X t are the state vector X = [x s x v x a y s y v y a ] T at time t + 1 and t, respectively. x s is the relative transverse displacement, x v is the relative transverse velocity, x a is the relative transverse acceleration, y s is the relative longitudinal displacement, y v is the relative longitudinal velocity, and y a is the relative longitudinal acceleration. Φ is the state transition matrix, which can be expressed by: where ∆t is the time interval of the filtered data. G is the coefficient of the system noise matrix and defined as G = [∆t 3 /6 ∆t 2 /2 ∆t ∆t 3 /6 ∆t 2 /2 ∆t] T . W t is the system noise vector at time t, Z t+1 is the measurement vector at time t + 1, H is the coefficient of the measurement matrix, which is a six-order unit matrix, and V t+1 is the measurement noise vector at time t + 1. W t and V t+1 are uncorrelated zero-mean white Gaussian noise sequence with covariance, i.e.: where the term cov is the function of calculating the covariance matrix, and δ kj is the dirichlet function. From the above linear discrete system, the CKF working behaviors can be characterized by [23]: K t`1 " P t`1{t HrHP t`1{t H`R t`1 s´1 (10) P t`1{t`1 " rI´K t`1 HsP t`1{t (11) where v t is the innovation vector, andĈ vt is the V-C matrix of v t . To improve the real-time performance and ergodicity, we set a sliding estimation window with size N for averagingĈ vt . Finally,R t`1 and Q t`1 can be calculated according toĈ vt . So v t ,Ĉ vt ,R t`1 , andQ t`1 are defined by [10]: If the innovation vector v t of Lidar includes seriously divergent noise or errors,Ĉ vt is divergent as well, so we use the Chi-square hypothesis test to mitigate this negative influence. The Chi-square Sensors 2016, 16,1103 6 of 29 hypothesis test can estimate the deviation between the observed and theoretical values of samples. The deviation decides the Chi-square values, i.e., the higher the Chi-square values, the more abnormal the samples, so a certain Chi-square distribution threshold enables us to identify abnormal samples.
Considering v t is the zero-mean white noise with Gaussian distribution, we obtain: where v t (m) is the mth element of vector v t , C vt´1 (i, i) is the ith diagonal element of matrixĈ vt , and χ 2 α p1q represents a Chi-square distribution with m freedom degree and confidential level α. If U t (m) < χ 2 α p1q, the new innovation vector is normal, otherwise it is abnormal. To mitigate the influence of the abnormal innovation vector, the abnormal element is corrected by:

Global Kalman Filter
To get the optimal estimation, the GF analyzes and integrates the estimated information of the LFs. The computational process of GF is as follows: Step 1. Optimal information fusion. GF fuses all the state estimations of the local filters into the optimal fusion state vectorX g , and meanwhile it fuses the covariance matrix into P g , as expressed by [9]:X g " P g Step 2. Calculating the information allocation factor. The information allocation factors β 1 and β 2 are used to adjust Q i , P i , and the weight ofX 1 andX 2 in the optimal fusion stateX g . β 1 is calculated according to tr(R 1 ) and tr(R 2 ). tr(R 1 ) and tr(R 2 ) indicate the trace of R 1 and R 2 , respectively. R can truly reflect the performance of the current filter. Table 1 lists the relation between R and the accuracy of Lidar. When the Signal-Noise Ratio (SNR) of Lidar spans from 80 dB to 87 dB, the value of R 1 decreases from 0.0021 to 0.0004, and accordingly the accuracy declines from 0.0362 to 0.0164. This implies that there exists a directly proportional relationship between these two quantities. If the deteriorating environment makes the accuracy of Lidar drop down to a certain threshold such as tr(R 1 ) ě 20 tr(R 2 ), we shall choose β 1 << 1, so as that the output of GF can approximate to the output of only Radar available. Generally speaking, Lidar is almost ten times as accurate as the Radar, so when tr(R 1 ) ě 20 tr(R 2 ), the accuracy of Radar becomes far better than that of Lidar, i.e., the Lidar already loses effectivity and produces divergent results. Therefore, β 1 is defined by: According to the principle of information distribution conservation β 1 + β 2 = 1 [9], so β 2 is expressed by: Step 3. Filtering state feedback. GF uses β 1 and β 2 to calculate the latest system information that will be feed back to the LFs, as expressed by:

Coordinate Transformation
The collected motion state of the forward vehicle should be placed into the current car's coordinate system, but in fact, the data collected by Lidar and Radar are based on their own coordinate systems, so in order to realize spatial synchronization of the measured data, we propose a method to transform the Lidar coordinate system C lidar (x l , y l , z l ) and the Radar coordinate system C radar (x r , y r , z r ) into the current car coordinate system C car (x c , y c , z c ). Figure 2 illustrates the relations between these coordinate systems.
According to the principle of information distribution conservation β1 + β2 = 1 [9], so β2 is expressed by: Step 3. Filtering state feedback. GF uses β1 and β2 to calculate the latest system information that will be feed back to the LFs, as expressed by:

Coordinate Transformation
The collected motion state of the forward vehicle should be placed into the current car's coordinate system, but in fact, the data collected by Lidar and Radar are based on their own coordinate systems, so in order to realize spatial synchronization of the measured data, we propose a method to transform the Lidar coordinate system Clidar(xl, yl, zl) and the Radar coordinate system Cradar(xr, yr, zr) into the current car coordinate system Ccar(xc, yc, zc). Figure 2 illustrates the relations between these coordinate systems.  First, we implement the coordinate transformation between C lidar and C car . Suppose P lidar = (x lidar , y lidar , z lidar ) T is a point in C lidar , and P car = (x car , y car , z car ) T is the corresponding point in C car , so their coordinate transformation can be achieved by: where T lidarÑcar is the mounted coordinate of Lidar onto the coordinate system C car , which can be simply measured. R lidarÑcar is the rotation matrix in C lidar relative to C car . In R lidarÑcar , the pitch angle α can be obtained by rotating around the x-axis, and similarly, the deflection angle ϕ can be obtained by rotating around the z-axis, so R lidarÑcar is defined by: More details about the calculation of α and ϕ can be found in [24]. Similarly, we can obtain T radarÑcar and R radarÑcar to execute the coordinate transformation between C radar and C car , and thus achieving the spatial synchronization of Lidar and Radar.

Time Synchronization
Since various sensors have different data sampling frequenci, the time error calibration has to be considered. To improve the accuracy of data fusion, we choose an appropriate time slice as the time interval of data fusion. Since the sampling frequency of Lidar is higher than that of Radar, the collected data from Lidar need to be calculated in accordance with the Radar by interpolation and extrapolation. Suppose the data sampling interval of Radar as the time point of data fusion t i , so we need to find a corresponding time point in data sampling interval of Lidar as interpolate point, and then calculating the measured value of Lidar at time t i through Lagrange three-point interpolation.
Assume the measured data sequence from Lidar is X i´1 , X i , and X i+1 at time t mi´1 , t mi , and t mi+1 , and t mi´tmi´1 = t mi´tmi+1 = h. t mi ď t i ď t mi+1 , i.e., t i = t mi + τh, 0 ď τ ď 1, so the estimated value X i at time t i is defined by:

Simulation
We conducted the simulation using MATLAB R2014a on a computer equipped with an Intel Core i7-4790 CPU (3.60 GHz) and Windows 64. There are two important factors that can affect the accuracy of the filtered results. At the one hand, the noise intensity is treated as a prevailing concern and can be quantified by the variance R. In the simulation, the Lidar noise variance is increased by 0.1 per time span from 0.03 to 5.93, while the Radar noise variance maintains its initial value of 0.1. The initial velocity, acceleration, and displacement of the forward vehicle are 2 m/s, 0.18 m/s 2 , and 0 m, respectively. Figure 3 shows the root mean squared error (RMSE), where JAKF produces the maximum RMSE of the displacement 0.2323 m and the average 0.0818 m, the maximum RMSE of the velocity 0.0837 m/s and the average 0.0689 m/s, and the maximum RMSE of the acceleration 0.0237 m/s 2 and the average 0.0171 m/s 2 . When the Lidar noise variance is increased to 0.93, the RMSEs of the displacement, velocity, and acceleration produced by CKF (Lidar) increase to 10-fold, so the results of CKF (Lidar) have diverged. Although IAKF (Lidar) can adapt to the increased noise intensity, the RMSE of JAKF is significantly smaller than that of IAKF (Lidar). At the other hand, the acceleration a F of the forward vehicle in the CA model is another matter. In the simulation, a F is increased by 0.1 m/s 2 per time from 0.18 m/s 2 to 6.08 m/s 2 and the noise variances of Lidar and Radar are fixed at 0.03 and 0.1, respectively. Figure 4 compares the motion state estimation against different acceleration values a F . JAKF contributes the maximum RMSE of the displacement 0.0403 m and the average 0.0261 m, the maximum RMSE of the velocity 0.0664 m/s and the average 0.0643 m/s, and the maximum RMSE of the acceleration 0.0737 m/s 2 and the average 0.0346 m/s 2 . In the increased acceleration situation, the RMSEs of CKF (Radar) and IAKF (Radar) are larger than the RMSEs of CKF (Lidar), IAKF (Lidar), and JAKF. and 0.1, respectively. Figure 4 compares the motion state estimation against different acceleration values aF. JAKF contributes the maximum RMSE of the displacement 0.0403 m and the average 0.0261 m, the maximum RMSE of the velocity 0.0664 m/s and the average 0.0643 m/s, and the maximum RMSE of the acceleration 0.0737 m/s 2 and the average 0.0346 m/s 2 . In the increased acceleration situation, the RMSEs of CKF (Radar) and IAKF (Radar) are larger than the RMSEs of CKF (Lidar), IAKF (Lidar), and JAKF.   and 0.1, respectively. Figure 4 compares the motion state estimation against different acceleration values aF. JAKF contributes the maximum RMSE of the displacement 0.0403 m and the average 0.0261 m, the maximum RMSE of the velocity 0.0664 m/s and the average 0.0643 m/s, and the maximum RMSE of the acceleration 0.0737 m/s 2 and the average 0.0346 m/s 2 . In the increased acceleration situation, the RMSEs of CKF (Radar) and IAKF (Radar) are larger than the RMSEs of CKF (Lidar), IAKF (Lidar), and JAKF.   Regarding the comparative results in Figures 3 and 4, we can conclude: (i) JAKF enables bounding the RMSE of the motion state estimation to an acceptable level, so the JAKF is robust; (ii) JAKF outputs the smallest RMSE of the motion state estimation, so the JAKF is more accurate than CKF and IAKF; (iii) JAKF uses multi-sensors data fusion to realize the global adaptation, so JAKF can be more adaptive than the single-sensor adaptive filter through performance compensation between different sensors.

Experiment
To obtain accurate data, the test car is equipped with a high precision URG-04LX Lidar developed by Hokuyo (Osaka, Japan), and a millimeter-wave Radar ESR, developed by Delphi (Warren, OH, USA). The forward car collects the displacement, velocity, and acceleration from a Controller Area Network (CAN) bus as a benchmark that is used to compare with the filtered results to evaluate JAKF. CAN bus is a vehicle bus standard designed to allow microcontrollers and devices to communicate with each other in applications. The motion states of vehicles can be easily obtained from some vehicle-sensors by CAN bus. CAN bus even can correct the current velocity and direction. Table 2 lists the parameters of URG-04LX and ESR.  Figure 5 shows the employed URG-04LX and ESR in the experiments, respectively. Figure 6 shows the test car and the forward car, respectively. Figure 7 shows the experimental scenario and route. Regarding the comparative results in Figure 3 and 4, we can conclude: (i) JAKF enables bounding the RMSE of the motion state estimation to an acceptable level, so the JAKF is robust; (ii) JAKF outputs the smallest RMSE of the motion state estimation, so the JAKF is more accurate than CKF and IAKF; (iii) JAKF uses multi-sensors data fusion to realize the global adaptation, so JAKF can be more adaptive than the single-sensor adaptive filter through performance compensation between different sensors.

Experiment
To obtain accurate data, the test car is equipped with a high precision URG-04LX Lidar developed by Hokuyo (Osaka, Japan), and a millimeter-wave Radar ESR, developed by Delphi (Ohio, USA). The forward car collects the displacement, velocity, and acceleration from a Controller Area Network (CAN) bus as a benchmark that is used to compare with the filtered results to evaluate JAKF. CAN bus is a vehicle bus standard designed to allow microcontrollers and devices to communicate with each other in applications. The motion states of vehicles can be easily obtained from some vehicle-sensors by CAN bus. CAN bus even can correct the current velocity and direction. Table 2 lists the parameters of URG-04LX and ESR.    Regarding the comparative results in Figure 3 and 4, we can conclude: (i) JAKF enables bounding the RMSE of the motion state estimation to an acceptable level, so the JAKF is robust; (ii) JAKF outputs the smallest RMSE of the motion state estimation, so the JAKF is more accurate than CKF and IAKF; (iii) JAKF uses multi-sensors data fusion to realize the global adaptation, so JAKF can be more adaptive than the single-sensor adaptive filter through performance compensation between different sensors.

Experiment
To obtain accurate data, the test car is equipped with a high precision URG-04LX Lidar developed by Hokuyo (Osaka, Japan), and a millimeter-wave Radar ESR, developed by Delphi (Ohio, USA). The forward car collects the displacement, velocity, and acceleration from a Controller Area Network (CAN) bus as a benchmark that is used to compare with the filtered results to evaluate JAKF. CAN bus is a vehicle bus standard designed to allow microcontrollers and devices to communicate with each other in applications. The motion states of vehicles can be easily obtained from some vehicle-sensors by CAN bus. CAN bus even can correct the current velocity and direction. Table 2 lists the parameters of URG-04LX and ESR.      Figure 5 shows the employed URG-04LX and ESR in the experiments, respectively. Figure 6 shows the test car and the forward car, respectively. Figure 7 shows the experimental scenario and route. In the experiments, R1, R2, Q1 and Q2 represent the noises of local filters LF1 and LF2, and their initial values are 0.03, 0.1, 0.000001 and 0.000001, respectively. R1 and R2 are measurement noise covariance in Kalman filter model and are calculated by IAE at each moment. Q1 and Q2 are constant and indicate the systemic noise covariance in Kalman filter model. The sliding window size of the innovation sequence is 30, and the given confidential level α is 0.005 in Chi-square distribution, so X 2 0.005 (1) = 7.879. We conducted three experiments to evaluate the JAKF results. In the first two experiments, the forward car moves along a straight line at a constant and varying acceleration, respectively. In the third, the forward car is permitted to change lanes and thus produces obvious displacement in the lateral.
In the first experiment, the forward car just shifts visibly in the vertical, so we only focus on the longitudinal motion information. The initial longitudinal velocity, acceleration, and displacement of the forward vehicle are 5.43 m/s, 0.18 m/s 2 , and 0 m, respectively. The test car follows the forward car so as to ensure sensors can detect the forward car. We collected 5000 samples with frequency 100 Hz. Figure 8 shows the noises of URG and ESR, respectively. URG is suffered from the continuous high noise within the 1000th~1500th time slots, the 2000th~3000th time slots, and the 3500th~4000th time slots, while the noise of ESR always holds steady, but URG has less error than ESR when working well during the 1500th~2000th, 3000th~3500th, and the 4000th~5000th time slots. Figure 9 provides the measurement noise V-C matrix R of the local-filter output of URG and ESR, which indicates the R of JAKF can adapt to the variation of the noise.  In the experiments, R 1 , R 2 , Q 1 and Q 2 represent the noises of local filters LF1 and LF2, and their initial values are 0.03, 0.1, 0.000001 and 0.000001, respectively. R 1 and R 2 are measurement noise covariance in Kalman filter model and are calculated by IAE at each moment. Q 1 and Q 2 are constant and indicate the systemic noise covariance in Kalman filter model. The sliding window size of the innovation sequence is 30, and the given confidential level α is 0.005 in Chi-square distribution, so X 2 0.005 (1) = 7.879. We conducted three experiments to evaluate the JAKF results. In the first two experiments, the forward car moves along a straight line at a constant and varying acceleration, respectively. In the third, the forward car is permitted to change lanes and thus produces obvious displacement in the lateral.
In the first experiment, the forward car just shifts visibly in the vertical, so we only focus on the longitudinal motion information. The initial longitudinal velocity, acceleration, and displacement of the forward vehicle are 5.43 m/s, 0.18 m/s 2 , and 0 m, respectively. The test car follows the forward car so as to ensure sensors can detect the forward car. We collected 5000 samples with frequency 100 Hz. Figure 8 shows the noises of URG and ESR, respectively. URG is suffered from the continuous high noise within the 1000th~1500th time slots, the 2000th~3000th time slots, and the 3500th~4000th time slots, while the noise of ESR always holds steady, but URG has less error than ESR when working well during the 1500th~2000th, 3000th~3500th, and the 4000th~5000th time slots. Figure 9 provides the measurement noise V-C matrix R of the local-filter output of URG and ESR, which indicates the R of JAKF can adapt to the variation of the noise.
Sensors 2016, 16,1103 11 of 29 Figure 5 shows the employed URG-04LX and ESR in the experiments, respectively. Figure 6 shows the test car and the forward car, respectively. Figure 7 shows the experimental scenario and route. In the first experiment, the forward car just shifts visibly in the vertical, so we only focus on the longitudinal motion information. The initial longitudinal velocity, acceleration, and displacement of the forward vehicle are 5.43 m/s, 0.18 m/s 2 , and 0 m, respectively. The test car follows the forward car so as to ensure sensors can detect the forward car. We collected 5000 samples with frequency 100 Hz. Figure 8 shows the noises of URG and ESR, respectively. URG is suffered from the continuous high noise within the 1000th~1500th time slots, the 2000th~3000th time slots, and the 3500th~4000th time slots, while the noise of ESR always holds steady, but URG has less error than ESR when working well during the 1500th~2000th, 3000th~3500th, and the 4000th~5000th time slots. Figure 9 provides the measurement noise V-C matrix R of the local-filter output of URG and ESR, which indicates the R of JAKF can adapt to the variation of the noise.    Figure 10 provides the estimation and error comparison of URG. The estimation of error is the abs value of the difference between the actual and estimation values in the same time slot. After the 500th time slot, all the focused three filters converge to a stable state. When the suffered measure noise by URG is normal, these three filtering algorithms can output the normal results. But when the continuous high noise appears during the 1000th~1500th, 2500th~3000th, and 3500th~4000th time slots, the noise variance R of URG is increased. In this case, the CKF is incapable of dealing with the continuous serious disturbance, while the JAKF still can produce higher accuracy than CKF and IAKF. At   Figure 10 provides the estimation and error comparison of URG. The estimation of error is the abs value of the difference between the actual and estimation values in the same time slot. After the 500th time slot, all the focused three filters converge to a stable state. When the suffered measure noise by URG is normal, these three filtering algorithms can output the normal results. But when the continuous high noise appears during the 1000th~1500th, 2500th~3000th, and 3500th~4000th time slots, the noise variance R of URG is increased. In this case, the CKF is incapable of dealing with the continuous serious disturbance, while the JAKF still can produce higher accuracy than CKF and IAKF.  Figure 10 provides the estimation and error comparison of URG. The estimation of error is the abs value of the difference between the actual and estimation values in the same time slot. After the 500th time slot, all the focused three filters converge to a stable state. When the suffered measure noise by URG is normal, these three filtering algorithms can output the normal results. But when the continuous high noise appears during the 1000th~1500th, 2500th~3000th, and 3500th~4000th time slots, the noise variance R of URG is increased. In this case, the CKF is incapable of dealing with the continuous serious disturbance, while the JAKF still can produce higher accuracy than CKF and IAKF. At      Table 3 lists the comparisons of CKF, IAKF and JAKF about root-mean-square error, maximum error, and variance of filtered results. In Table 3, the RMS error and the variance of JAKF is smaller than the ones of CKF and IAKF, so JAKF has better stability and fault-tolerance against the continuous varying noise, and the accuracy of JAKF is higher than that of the single-sensor filter in the situation where the acceleration of the forward car is constant.    Table 3 lists the comparisons of CKF, IAKF and JAKF about root-mean-square error, maximum error, and variance of filtered results. In Table 3, the RMS error and the variance of JAKF is smaller than the ones of CKF and IAKF, so JAKF has better stability and fault-tolerance against the continuous varying noise, and the accuracy of JAKF is higher than that of the single-sensor filter in the situation where the acceleration of the forward car is constant.   In the second experiment, the forward car moved along a straight line at a varying acceleration. As the first experiment, we only focus on the longitudinal motion in this situation. The initial longitudinal velocity, acceleration, and displacement of the forward vehicle are 0.15 m/s, 0 m/s 2 , and 0 m, respectively. The test car follows the forward car so as to ensure sensors can detect the forward car. We collected 5000 samples with frequency 100 Hz. Figure 12 shows the noises of URG and ESR. URG suffered from the continuous high noise within the 2000th~3000th time slots, and the 3500th~4400th time slots, while the noise of ESR still holds steady all the time. Figure 13 provides the measurement noise V-C matrix R of the local-filter output of URG and ESR.  In the second experiment, the forward car moved along a straight line at a varying acceleration. As the first experiment, we only focus on the longitudinal motion in this situation. The initial longitudinal velocity, acceleration, and displacement of the forward vehicle are 0.15 m/s, 0 m/s 2 , and 0 m, respectively. The test car follows the forward car so as to ensure sensors can detect the forward car. We collected 5000 samples with frequency 100 Hz. Figure 12 shows the noises of URG and ESR. URG suffered from the continuous high noise within the 2000th~3000th time slots, and the 3500th~4400th time slots, while the noise of ESR still holds steady all the time. Figure 13 provides the measurement noise V-C matrix R of the local-filter output of URG and ESR.    In the second experiment, the forward car moved along a straight line at a varying acceleration. As the first experiment, we only focus on the longitudinal motion in this situation. The initial longitudinal velocity, acceleration, and displacement of the forward vehicle are 0.15 m/s, 0 m/s 2 , and 0 m, respectively. The test car follows the forward car so as to ensure sensors can detect the forward car. We collected 5000 samples with frequency 100 Hz. Figure 12 shows the noises of URG and ESR. URG suffered from the continuous high noise within the 2000th~3000th time slots, and the 3500th~4400th time slots, while the noise of ESR still holds steady all the time. Figure 13 provides the measurement noise V-C matrix R of the local-filter output of URG and ESR.               Table 4 lists the comparisons of CKF, IAKF and JAKF about root-mean-square error, maximum error, and variance of filtered results. In Table 4, JAKF has high accuracy and stability even though the motion model of the forward car is dynamic. In a nutshell, a varying acceleration imposes little influence on JAKF.
In the third experiment, the forward car is permitted to change lanes and thus produces obvious displacement in the lateral, so we mainly focus on the transverse motion of the forward car in this situation. The initial transverse velocity, acceleration, and displacement of the forward vehicle are 0 m/s, 0 m/s 2 , and 0 m, respectively. During the whole experiment, the test car moves along a straight line and keeps the inter-distance about 3 m~8 m apart from the forward car so as to ensure sensors can detect the forward car. We collected 1000 samples with frequency 100 Hz. We collected the transverse vehicle motion information to CKF, IAKF and JAKF, and correspondingly compared the transverse results with the former two longitudinal ones. Figure 16 shows the noises of URG and ESR. URG is suffered from the continuous high noise within the 200th~400th time slots, and the 600th~800th time slots, while the noise of ESR still holds steady. Figure 17 provides the measurement noise V-C matrix R of the local-filter output of URG and ESR.  In Table 4, JAKF has high accuracy and stability even though the motion model of the forward car is dynamic. In a nutshell, a varying acceleration imposes little influence on JAKF.
In the third experiment, the forward car is permitted to change lanes and thus produces obvious displacement in the lateral, so we mainly focus on the transverse motion of the forward car in this situation. The initial transverse velocity, acceleration, and displacement of the forward vehicle are 0 m/s, 0 m/s 2 , and 0 m, respectively. During the whole experiment, the test car moves along a straight line and keeps the inter-distance about 3 m~8 m apart from the forward car so as to ensure sensors can detect the forward car. We collected 1000 samples with frequency 100 Hz. We collected the transverse vehicle motion information to CKF, IAKF and JAKF, and correspondingly compared the transverse results with the former two longitudinal ones. Figure 16 shows the noises of URG and ESR. URG is suffered from the continuous high noise within the 200th~400th time slots, and the 600th~800th time slots, while the noise of ESR still holds steady. Figure 17 provides the measurement noise V-C matrix R of the local-filter output of URG and ESR.                 Table 5 lists the comparisons of CKF, IAKF and JAKF about root-mean-square error, maximum error, and variance of filtered results. In Table 5, compared with the results of the first two experiments, the advantages of JAKF in accuracy and stability decline slightly, but the JAKF results are still better than the ones of CKF and IAKF. Table 6 summarizes the ratio of accuracy improvement of JAKF in the three experiments.  Table 5 lists the comparisons of CKF, IAKF and JAKF about root-mean-square error, maximum error, and variance of filtered results. In Table 5, compared with the results of the first two experiments, the advantages of JAKF in accuracy and stability decline slightly, but the JAKF results are still better than the ones of CKF and IAKF. Table 6 summarizes the ratio of accuracy improvement of JAKF in the three experiments. In a nutshell, the JAKF extends the single adaptive filter through integrating the respective advantages of Lidar and Radar, and uses the noise variance R and the information allocation factor β to adjust the filtered results of the local and global filters, respectively, and thus can improve the fault tolerance and accuracy.

Conclusions
The paper proposed a joint adaptive Kalman filter algorithm called JAKF that combines one Lidar and one Radar to accurately estimate the motion state of the moving car ahead, e.g., the relative position, the relative velocity, and the relative acceleration. For adaptation, JAKF adopts the modified innovation sequence to calculate the measurement noise V-C matrix of each local filter. Since the measurement noise V-C matrix can reflect the performance of the current filter, the information allocation factor can be specified to optimize the multi-sensorial fusion. The simulation and experimental results show that JAKF can always maintain the convergence even when suffering from abnormal noise. JAKF significantly improves the fault tolerance and stability of the estimation system, and meanwhile, it enhances the accuracy of the filtered results.