Title Author Keyword ::: Volume ::: Vol. 17Vol. 16Vol. 15Vol. 14Vol. 13Vol. 12Vol. 11Vol. 10Vol. 9Vol. 8Vol. 7Vol. 6Vol. 5Vol. 4Vol. 3Vol. 2Vol. 1 ::: Issue ::: No. 4No. 3No. 2No. 1

A Position and Velocity Estimation Using Multifarious and Multiple Sensor Fusion

Youngwan Cho1, and Heejin Lee2

1Department of Computer Engineering, Seokyeong University, Seoul, Korea, 2Department of Electrical, Electronic, and Control Engineering, Hankyong National University, Ansung, Korea
Correspondence to: Heejin Lee, (lhjin@hknu.ac.kr)
Received May 18, 2017; Revised June 23, 2017; Accepted June 23, 2017.
This is an Open Access article distributed under the terms of the Creative Commons Attribution Non-Commercial License (http://creativecommons.org/licenses/by-nc/3.0) which permits unrestricted non-commercial use, distribution, and reproduction in any medium, provided the original work is properly cited.
Abstract

In this paper, we propose a fusion algorithm of multifarious and multiple sensors to enhance the accuracy and reliability of position and velocity estimation for the vehicles. We proposed an adaptive Kalman filter for multiple sensor fusion to provide a fault tolerant estimation. We verified the multiple sensor fusion estimator can provide a fault tolerant estimation through Matlab simulation and laboratory equipped experiments. We also proposed a fusion algorithm of multifarious sensors in order to enhance the velocity estimation accuracy. We proposed a Kalman filter error correction for compensate the accumulative error in the main sensor with the other type of sensor which has characteristic of biased error. We also developed a fusion algorithm for compensate the error in the position measuring with the velocity measuring. We made experiments for estimating position and velocity of vehicle simultaneously through the fusion of multifarious and multiple sensors and showed that average position error was 1.5764 m and average velocity accuracy was 99.81%.

Keywords : Position estimation, Velocity estimation, Kalman filter, Sensor fusion, Map based GPS error correction
1. Introduction

Autonomous driving has been said to be the next big disruptive innovation in the years to come. The technology for autonomous vehicle navigation gains increasing importance in various growing application areas [1]. An intelligent autonomous vehicle can be defined as a vehicle that navigates to its destination without human operator’s manipulation and determines its own decision on path generation, environment recognition, and obstacle avoidance. The autonomous vehicle generally includes a detection system for measuring its own position and recognizing an obstacle, a central control device for giving commands such as acceleration, deceleration and steering based on the information, and devices for following the control commands.

The most important information for autonomous navigation is the reliable and accurate measuring for position and velocity of the vehicle. The central control unit first determines most of the control commands based on the position and the velocity information of the vehicle. Then, the central control unit combines the other information and finally issues the modified control command. Therefore, it is basically required to detect position and velocity information with reliability and accuracy. A single sensor based position and velocity detection system is difficult to guarantee stability and reliability due to limitations such as time, space constraints, inaccuracies and uncertainties of single sensor, and danger of sensor failure.

Therefore, in this study, we propose a sensor fusion model using multiple Doppler, tachometer and GPS sensors to improve the reliability and accuracy of position and velocity measuring. There are many studies on the vehicle state estimation with various methods and approaches. Feng et al. [2] proposed an algorithm to consider the correlation between sensors in a sequential filter, and Smyth and Wu [3] designed a multi-rate Kalman filter for sensor fusion at different periods. Olfati-Saber [4] presented a modified DKF algorithm that uses two identical consensus filters for fusion of the sensor data and covariance information. Lee [5] applied reliability indexed sensor fusion to estimate the vehicle velocity.

In particular, various researches have been conducted on a technique of fusing GPS and other sensors to detect the position and velocity of an autonomous vehicle. Thrapp et al. [6] used the Extended Kalman filter(EKF) [7] to integrate DGPS and odometry information for position estimation and improved accuracy by setting the number of satellites used for position detecting as a measure of DGPS accuracy, and changing the measurement error covariance. Ohno et al. [8] also used EKF to integrate DGPS and odometry information and improved accuracy by excluding DGPS information, which is considered to be inaccurate. Laneurit et al. [9] presented an approach of multi-sensor fusion able to locate a vehicle with a decimeter precision. They got the precision by taking into account a complete model of errors on GPS data as well as the data provided by an original approach coupling a vision algorithm with a precise numerical map.

In this paper, we propose a technique to integrate information provided by multiple sensor types in multiple to obtain reliability and accuracy. The proposed fusion model consists of a multiple sensor fusion model that integrates the same type of sensors and a multifarious sensor fusion model that integrates the different type of sensors. We propose a distributed Kalman filter for the multiple sensor fusion that uses weighted averages to reflect the reliability of sensors as a way to improve fault tolerance. The multifarious sensor fusion model uses a Kalman filter error correction for compensate the accumulative error in the main sensor with the other type of sensor which has characteristic of biased error. We utilize the works in [12] to integrate digital map, GPS sensor data, and speed sensor information to acquire the accurate position information. We also propose a Kalman filter model to integrate the local multiple sensor fusion estimates.

2. Multiple Sensor Fusion Model

To have no performance degradation even in single sensor failure, and to acquire the sensor information secured with fault tolerance with high reliability, this can be solved through the homogeneous multiple sensor fusion. The multiple sensor fusion model with fault tolerance provided in this study is based on the distributed Kalman Filter [7] as the basic structure. The distributed Kalman filter is the form of performing the estimate process in the local filter on the mutually independent sensors to integrate the information provided from the local filter by the master filter. These structures can handle the faults occurred from the individual sensor in the local filter level, and can provide continuous solution even when the fault is occurring, so it has the characteristics of having tolerance to faults.

In this study, the reliability of each sensor used in the multiple sensor fusion is evaluated and weighted value is granted to select the main sensor, and the method that the error weighted average with each sensor is compensated to find the fusion sensor result value is presented.

Figure 1 shows the adaptive Kalman filter structure that is provided for the multiple sensor fusion. The master filter outputs the final sensor value minimized with the sensor effect occurred with fault based on the error estimate value on each sensor value inputted from each local Kalman filter. The local Kalman filter estimates the measurement value of each sensor and the error value with main sensor, and the main filter calibrates this to be used as the estimate value Ŝi of each sensor. The reliability on the estimate value Ŝi of each sensor is defined by the total of deviation on the estimate value for each sensor used in the fusion as shown in Eq. (1). The local Kalman filter selects the sensor with maximum reliability as the main sensor to be used as the standard signal, and the error on the standard signal of each sensor is estimated.

$SDk=∑i=1n(S^k-S^i), k=1,2,⋯N,$

where SDk is the reliability of sensor k, and Ŝi shows the estimate value of sensor i. The master filter reflects the reliability SDk defined in Eq. (1) in the estimate value Ŝk of each sensor as the weighted value to acquire the weighted average as shown in Eq. (2), and then, determines the final estimate value Sout. The proposed multiple sensor fusion model expresses the reliability of each sensor measurement value as the relative deviation, and selects the sensor with high reliability as the sensor with low possibility of fault with high measurement accuracy to have the adaptive Kalman filter structure of calibrating the error on each sensor.

$Sout=∫i=1n1∣SDi∣·S^i∫i=1n1∣SDi∣.$

The proposed multiple sensor fusion model was validated through the simulation. The velocity pattern was produced on one random section, and in the process of multiple 5 sensors measuring this, the case of having each difference pattern of error on two random sensors was performed with the simulation on the signal estimation performance of the proposed fusion model. Figure 2 shows the sensor measurement values used in the simulation. In a specific point, the sensor1 was supposed to show abnormal high value to assume the fault situation, and the sensor2 was supposed to model a big noise. Figure 3 shows the simulation result, and it was verified that the final output signal did not deviate greatly from the standard signal even though the two sensors among the five sensors had malfunction.

3. Multifarious Sensor Fusion Model

In this study, the method is proposed on having the tachometer as the main sensor and the Doppler sensor as the auxiliary sensor to compensate the position error of the main sensor through the auxiliary sensor. The tachometer has the advantage of measuring the moving distance and velocity of the vehicle continuously, but there is a problem of having difficulty in applying independently in the automatic unmanned driving system requiring precision control due to the occurrence of error on the velocity and position information when there is slip or slide in the wheels on the acceleration or braking of the vehicle. The Doppler sensor has the possibility to have error over the tolerance on measuring the sensor in the non-contact method, so it has the disadvantage of not being able to guarantee the safety of the measurement signal when used independently. In this study, the method is proposed on improving the performance by enabling the fusion of heterogeneous sensors with difference characteristics for measuring the velocity and position.

The position measurement error δr and the velocity measurement error δv of the tachometer are shown as follows.

$∂r =r-rm,∂v =v-vm,$

where v and r represent the vehicle velocity and position, respectively. In Figure 4, a, rm, and rd represent the vehicle acceleration, the position measurement value of the tachometer and Doppler sensor, respectively. By taking into account the noise wm and wd of the tachometer and Doppler sensor, we can establish the following equation:

$v˙m=a+wm=v˙+wm.$

The difference of the tachometer position measurement value and the Doppler sensor position measurement value of z is shown as follows:

$z=rm-rd=(r+δr)-(r-wd)=δr+wd.$

From this, the state equation (6) can be obtained.

$[δr˙δv˙]=[0100] [δrδv]+[01] wm,z=[10] [δrδv]+wd.$

We can derive discrete form of the sate equation (6) as follows:

$xk+1=Axk+Bwk,zk=Cxk+dk,$

where the state variable is $xk=[δrkδvk]T$, and the system matrix A, B and C are defined as follows:

$A=[1Δt01], B=[0Δt] C=[10].$

By applying the system model (7) to the Kalman filter, we can obtain the estimates δr̂ and δv̂ of the position measurement error and velocity measurement error, respectively. In this study, performance of the proposed sensor fusion filter is verified by a simulation.

The simulation environment was developed to evaluate the performance of the position estimation system. The resolution of the tachometer was assumed to be 200 pulse/revolution and vehicle movement velocity as 2 m/sec. Also, the variance of the tachometer of 0.001 m/sec was added for the noise. The error caused by the slip or slide that can occur on acceleration and deceleration was also considered by adding noise on the acceleration and deceleration profile. We modeled the characteristic of Doppler sensor that is not accumulated by adding the noise of 0 mean and 0.01 m/sec variance.

In this simulation, it is verified that the proposed estimation model can compensate for the disadvantages of the two sensors by integrating a tachometer with high uncertainty but low inaccuracy and Doppler sensor with relatively low uncertainty but inaccuracy. Figure 5 shows the result of the simulation. It can be seen that the error of the fused position estimate through the Kalman filter is less dispersed and stable than the data of the simple position information without fusion.

4. Map based GPS Error Correction

We utilize the works in [12] to integrate digital map, GPS sensor data, and speed sensor information to acquire the accurate position estimate. In the paper, latitude longitude directional moving patterns from the velocity and position data of the vehicle moving on a specific path are applied to the state transition matrix of the Kalman filter to estimate the position and velocity. The estimated position is used for the map-matching technique to estimate the final position.

The data obtained from GPS is matched to digital map to reduce an error contained in GPS. The matched position data is used for the input of the adaptive Kalman filter of which system model is represented as Eq. (12).

$xk+1=Axk,$

where $A=[1Δt0α]$ and $x=[sv]T$.

The α represents an estimated acceleration obtained by approximating the moving pattern. In Eq. (12), the longitude and latitude directional position is represented as s and the velocity is represented as v. When a vehicle is operated at a specific moving pattern on a predetermined path, the velocity at a point on the specific route can be estimated.

In Figure 6, the sA, vA and sB, vB represent the estimated position and velocity at the sampling time k−1 and k, respectively, and vC represent the velocity projected on the approximated velocity profile. Then, we can obtain the approximated acceleration α as shown in Eq. (13).

$α=vCvA.$

The α plays a role of changing the estimated velocity in state transition matrix depending on the moving pattern. The measurement equation applied to the Kalman filter is as shown in Eq. (14).

$zk=Hxk, H=[11].$

The map based GPS error correction filter was applied to an experiment of measuring the position of vehicle moving on predefined path. In the experiment, the error was defined as the distance between the measured position and the reference absolute position. Figure 7 shows the variation of measurement error with points for the cases of point-to-curve matching and GPS error filter matching.

5. Position and Velocity Estimation and Experiments

Figure 8 shows the overall structure of the multifarious and multiple sensor fusion model proposed in this study. In the hierarchical sensor signal processing structure for permitting single sensor fault, the lower part enables multiple sensor fusion to improve the fault tolerance, and each sensor data passing through the local filter in the lower structure is ultimately fused in the upper position and velocity filter to provide the position and velocity estimation improved with precision and reliability.

The sensor data provided from the lower local filter passes through the velocity estimation filter, position estimation filter and direction estimation filter to provide the final position and velocity estimation. The state variables to be assumed can be expressed as follows:

$x^k=[VnRn],$

where Vn and Rn represent the vehicle velocity and position in the navigation coordinate, respectively. The acceleration fb measured in the vehicle coordinate can be transformed into navigation coordinate, and the gravity can be removed to obtain the acceleration vn in the navigation coordinate as shown in Eq. (13).

$v˙n=CBN·fb-gn,$

where $CNB$ is the coordinate transform matrix to the vehicle coordinate from the navigation coordinate. Eq. (13) can be transformed into the satellite coordinate as shown in Eq. (14).

$Vn=[Slat000Slong000Sh] [CBN•fb-gn]=S·CBN·fb-S·gn.$

We can express Eq. (14) as the state equation (15) which can be applied as the system model of Kalman filter for estimating the velocity.

$[Vn.Rn.]=[00I0] [VnRn]+[S•CNB-S00] [fbgn]=F [VnRn]+G [fbgn].$

We can obtain the discrete form of Eq. (15) as in Eq. (16).

$X (k+1)=(I+FΔt) X (k)+GΔtu (k)=AX (k)+Bu (k).$

The velocity vector of the system model is the reference of navigation system coordinate, and $Vn=CBN·Vb$, so the measurement model for estimating the velocity can be expressed in latitude and the longitude coordinate system as in Eq. (17).

$Z=S·(CNB)-1·Vb.$

In this study, the estimate value of the system model based the adaptive Kalman filter is used as the measurement value to estimate the final position through the main filter. The measurement model of the main filter estimating the final position is shown as follows:

The latitude ϕ, longitude λ and height h can be transformed into the TM coordinate system as in Eq. (18).

$[xTMyTMzTM]=[RN+h000(RE+h)cosϕ000-1] [ϕ-ϕ0λ-λ0h-0],$

where RN and RE are the radius of curvature in the north direction of the meridian and east, respectively, and ϕ0 and λ0 are the latitude and longitude of the origin of the TM coordinate system.

Converting the TM coordinates in Eq. (18) to the navigation coordinates yields Eq. (19).

$[xyz]=[1000-1000-1] [xTMyTMzTM].$

By introducing the matrix D to denote the process of converting the position r that is estimated by the GPS and map fusion position filter into the navigation coordinate system, the position measurement model zk can be expressed as Eq. (20).

$zk=Dr.$

In order to verify the validity of the proposed multifarious and multiple sensor fusion model in this paper, three times of experimental navigating were conducted on the specific route of approximately 3.7 km length from Gireum IC to Kookmin University as shown in Figure 9. The accuracy of the position estimation was compared with the reference absolute position and the accuracy of the velocity estimation was compared with the average velocity between the specific points on the moving range to test the validity of the proposed sensor fusion model.

Figure 10 shows the test result in a graph, comparing the GPS data, the GPS information corrected through the map matching algorithm, and the information estimated through the fusion filter proposed in this paper. As shown in Figure 10, the GPS information contains errors and shows a significant deviation from the actual road, and the errors are irregular. The correction position to which the map matching algorithm is applied does not deviate from the road but still contains errors for the reference position. In this paper, we show that the proposed location estimation results show more accurate results.

6. Conclusions

In this paper, we propose a method of estimating sensor information with reliability and accuracy through fusion of multiple sensors in realizing position and velocity device of autonomous navigation vehicle. In order to obtain reliable information without performance deterioration in sensor failure or fault, we propose a method of integrating multiple sensors by applying an adaptive Kalman filter. We proposed multiple sensor fusion method for the same kind of sensors to overcome the disadvantages of different sensors with different characteristics and to utilize the advantages. We also proposed a fusion model that integrates GPS information with digital map information to improve the accuracy of position measurement.

The multiple sensor fusion and the multifarious sensor fusion model were verified to be improved in fault tolerance performance and accuracy of velocity estimate through each simulation, and the performance of the map information based GPS fusion algorithm was verified through the actual driving experiment. In this paper, we propose a multifarious and multiple sensor fusion that estimates the position and velocity using information from each local filter. As a result of experiment on the driving vehicle mounting the device implemented with the proposed estimation algorithm to perform the actual road driving test for verifying the performance, the average position error was 1.5764 m and the accuracy of the speed estimate was 99.81% in the total area.

Acknowledgements

This research was supported by Seokyeong University in 2017.

Conflict of Interest

Figures
Fig. 1.

Multiple sensor fusion model.

Fig. 2.

Simulated sensor profiles.

Fig. 3.

Simulation result of sensor fusion.

Fig. 4.

Multifarious sensor fusion model.

Fig. 5.

Simulation result of multifarious sensor model.

Fig. 6.

Approximation of moving pattern and the α.

Fig. 7.

Measurement error of the compared map matching algorithms. (a) Point-to-curve matching, (b) GPS error filter matching.

Fig. 8.

Block diagram of the position and velocity estimator.

Fig. 9.

Fig. 10.

Experimental results of position estimating.

References
1. Rosenzweig, J, and Bartl, M (2015). A review and analysis of literature on autonomous driving. The Making-of Innovation, E-Journal, 1-57.
2. Feng, XL, Ge, QB, and Wen, CL 2009. An optimal sequential filter for the linear system with correlated noises., Proceedings of 2009 Chinese Control and Decision Conference, Guilin, China, Array, pp.5073-5078.
3. Smyth, A, and Wu, M (2007). Multi-rate Kalman filtering for the data fusion of displacement and acceleration response measurements in dynamic system monitoring. Mechanical Systems and Signal Processing. 21, 706-723.
4. Olfati-Saber, R 2007. Distributed Kalman filtering for sensor networks., Proceedings of 2007 46th IEEE Conference on Decision and Control, New Orleans, LA, Array, pp.5492-5498.
5. Lee, H (2006). Reliability indexed sensor fusion and its application to vehicle velocity estimation. Journal of Dynamic Systems, Measurement, and Control. 128, 236-243.
6. Thrapp, R, Westbrook, C, and Subramanian, D 2001. Robust localization algorithms for an autonomous campus tour guide., Proceedings of 2001 IEEE International Conference on Robotics and Automation, Seoul, Korea, Array, pp.2065-2071.
7. Choset, H, Lynch, K, Hutchinson, S, Kantor, G, Burgard, W, Kavraki, L, and Thrun, S (2005). Principles of Robot Motion: Theory, Algorithms, and Implementation. Cambridge, MA: MIT Press
8. Ohno, K, Tsubouchi, T, Shigematsu, B, and Yuta, S (2004). Differential GPS and odometry-based outdoor navigation of a mobile robot. Advanced Robotics. 18, 611-635.
9. Laneurit, J, Chapuis, R, and Chausse, F (2005). Accurate vehicle positioning on a numerical map. International Journal of Control, Automation, and Systems. 3, 15-31.
10. White, CE, Bernstein, D, and Kornhauser, AL (2000). Some map matching algorithms for personal navigation assistants. Transportation Research Part C. 8, 91-108.
11. Jun, J, Guensler, R, and Ogle, J (2006). Smoothing methods to minimize impact of global positioning system random error on travel distance, speed, and acceleration profile estimates. Transportation Research Record: Journal of the Transportation Research Board. 1972, 141-150.
12. Cho, Y, and Choi, H (2014). Accuracy enhancement of position estimation using adaptive Kalman filter and map matching. International Journal of Control and Automation. 7, 167-178.
Biographies

Youngwan Cho was born in Hamyang, Korea, in 1968. He received the B.S., M.S., and Ph.D. degrees in electronic engineering from Yonsei University, Seoul, Korea, in 1991, 1993, and 1999, respectively. He worked as a Senior Research Engineer in the Control System Group at SAMSUNG Electronics, Seoul, Korea, from 1999 to 2003. He was a visiting scholar at Department of Mechanical Engineering, Michigan State University from 2016 to 2017. He is currently working as an Associate Professor in the Department of Computer Engineering, Seokyeong University, Seoul, Korea. His research interests include fuzzy control theory and applications, intelligent control systems, machine learning, and robotics and automation.

E-mail: ywcho@skuniv.ac.kr

Heejin Lee received the B.S., M.S., and Ph.D. degrees in electronic engineering from Yonsei University, Seoul, Korea, in 1987, 1989, and 1998, respectively. He worked as a Senior Research Engineer in the Daewoo Telecom Ltd., Seoul, Korea, from 1989 to 1993. He is currently working as a Professor in the Department of Electrical, Electronic and Control Engineering, Hankyong National University, Ansung, Korea. His current research interests include fuzzy control theory, fuzzy application system, adaptive and robust control, and intelligent robotics.

E-mail: lhjin@hknu.ac.kr

July 2017, 17 (2)