An Approach for Vehicle State Estimation Using Extended Kalman Filter Liang Tong Mechanical and Electrical Engineering School, Beijing Information Science and Technology University, Beijing, China
[email protected] Abstract. In order to meet the high cost requirement of some vehicle states measured directly in vehicle active safety control system, an approach using the Extended Kalman Filter to estimate lateral and longitudinal velocity is proposed. Firstly, a vehicle dynamic model with 3 DOF, including longitudinal, lateral and yaw motions is built with MATLAB/SIMULINK. Secondly, the vehicle state estimation algorithm by the extended Kalman state observer based on the nonlinear vehicle model is achieved and the states of longitudinal, lateral acceleration and yaw rate for the vehicle are estimated online. Finally, the estimated results are compared with the results obtained from CarSim using the same parameter to verify the practicality of the proposed method. Keywords: vehicle, extended kalman filter, state estimation.
1
Introduction
In recent years, people are paying more and more attention to the safety performance gradually with the increasing vehicle quantity and the increasing traffic accident number when they plan to buy a car. Based on the Anti-lock Braking System (ABS) and Acceleration Slip Regulation, the development of a new type of vehicle active safety control system called the Vehicle Stability Control system (VSC) that is the field of automotive active safety research focus has been applied on many foreign sedan [1-2].It not only integrates all the features of ABS and ASR, but also maintains vehicle stability in the extreme conditions. According to the characteristics and control system, the world's manufacturers put forward different names in the research and development of the vehicle stability control, such as Electronic Stability Program (ESP), Dynamic Stability Control (DSC),vehicle stability control (VSC),etc., but its composition and functions is about the same. Vehicle stability control is mainly based on the various parameters (e.g. lateral acceleration, yaw rate) of the motion to determine the appropriate control strategy and achieve the safety of vehicles traveling on the active control. Usually, the car state of motion can be measured through a variety of vehicle sensors. Constrained by the current technical level, some important variables need to use much more expensive equipment to measure (such as speed, yaw rate),or could not take direct measurements (such as the side slip angle), parameter estimation problem is the best solution to meeting a vehicle stability control system requirement. T. Xiao, L. Zhang, and S. Ma (Eds.): ICSC 2012, Part I, CCIS 326, pp. 56–63, 2012. © Springer-Verlag Berlin Heidelberg 2012
An Approach for Vehicle State Estimation Using Extended Kalman Filter
57
The classical Kalman filtering technique (KF) which is an online vehicle state estimation can solve this problem and has been widespread used in many fields. The Kalman filter dynamics results from the consecutive cycles of prediction and filtering. The dynamics of these cycles is derived and interpreted in the framework of Gaussian probability density functions. However, the Kalman Filter is only used to the estimate the signal of the linear systems because of the basis on a linear stochastic differential equation. In fact, the speed of the vehicle has significantly impact on the movement characteristics, so often needs the nonlinear models in order to more accurately describe the movement of vehicles. A non optimal approach to solve the problem, in the frame of linear filters, is the Extended Kalman filter (EKF) [3].The EKF implements a Kalman filter for a system dynamics that results from the linearization of the original non-linear filter dynamics around the previous state estimates. In this research field,Wenzel et al. investigated the application of the DEKF (Dual Extended Kalman Filter) for estimating state and parameters of the vehicle, while Best proposed a method for joint state-and-parameter estimation in parameter estimation [4]. Since the tire model of dynamic is complicated, making the EKF algorithm for solving process becomes very complicated, and thus has an impact on the real time state estimation. Therefore, based on the vehicle dynamic model with 3 DOF, the extended Kalman state observer has been designed through estimation algorithm of vehicles yaw rate, vertical speed and sideslip angle. The results show that the algorithm is better real-time computing, and the estimated effect is better.
2
Non-linear Three-Freedom Vehicle Model
The purpose of this paper is to develop an estimation method for the vehicle driving state based on a three degree-of-freedom vehicle. However, before trying to do the implementation of the estimation by using the extended Kalman filter to estimate lateral and longitudinal velocities, pitch, it is critical to specify the model that can be used for the simulation of the vehicle. Such model will be derived in this paper. 2.1
Three Degrees of Freedom Vehicle Model
This paper proposed a 3 DOF vehicle model based on the vehicle 2 DOF model by increasing a longitudinal motion which contains not only the longitudinal speed, but also the lateral speed and yaw rate. The structure of the vehicle model is shown in Fig.1[5-8]. L Fx12 δ α12
α22
Fx 22
b
a
Fy12 X
Fy 22
β
Y
Fx 21
α21
Fx11 δ
α11 Fy11
Fy21
Fig. 1. The structure of the vehicle model
58
L. Tong
The state equations of the vehicle are as follows:
a 2 k1 + b 2 k2 ak − bk2 ak β − 1δ Yaw Yaw + 1 = I xu Iz Iz ak1 − bk2 k +k k − 1)Yaw + 1 2 β − 1 δ β = ( 2 mu mu mu u = Yaw ∗ β ∗ u + ax
(1)
Where m represents the total mass of the vehicle(kg),Yaw is the yaw rate (rad/s),u is the longitudinal velocity respectively (m/s),Iz is the moment of inertia of the vehicle about its yaw axis kg*m2,a is the distance from the center of gravity to the front axle(m),b is the distance from the center of gravity to the rear axle(m), δ is the front steer angle(rad),k1 and k2 are lateral stiffness of the front and rear wheel(N/rad), β is the side slip angle, ax is the lateral acceleration (m/s2). 2.2
Tire Model
Tire model is the important sub-model of the whole vehicle because the major force and torque to the vehicle is come from the tire, such as longitudinal braking force and driving force, side force and cornering force, aligning torque and invert torque, etc.Tire model is to describe a function between these forces, torques, slip rate, slip angle, roll angle, vertical load, road friction coefficient and vehicle speed. Pacejka Magic Formula model and Dugoff tire model are good models to use [9-10].For Dugoff tire model of a single wheel, it is essential to know the friction coefficient to have a tire model. The side leaning angles of the every wheel are as follows: , (i = left , right ) Vx 0.5* B1 * Yaw
(2)
, (i = left , right ) Vx 0.5* B1 * Yaw
(3)
α fi = δ − tan −1
α fi = δ − tan −1
•
Vy + a * Yaw
•
•
Vy + a * Yaw
•
The magnitudes of the front and rear axle velocities are as follows: •
•
V fi = (Vy + 0.5* a * Yaw) 2 + (Vx 0.5* B1 * Yaw)2 , (i = left , right )
V ri =
•
•
(V y − 0.5 * b * Yaw ) 2 + (V x 0.5 * B 2 * Yaw ) 2 , ( i = left , right )
(4)
(5)
An Approach for Vehicle State Estimation Using Extended Kalman Filter
59
Therefore, the resultant front and rear friction coefficients are as follows: μresi = [C1 (1 − e−C S ) − C3 Sresi )λ 2 resi
(6)
Where C1, C2 and C3 are the Burckhardt coefficients. In the case of the known characteristics of the road, road friction coefficient is only dependent on the lateral and vertical slip rate. For the system studied in this paper, tire longitudinal force and lateral force are decided by the following equations: (1)The front and rear longitudinal and lateral wheel slips are as follows: S xij 1 = S yij V fl
ωij * Rw * cos α ij − Vij ωij * Rw * cos α ij
, (i = left , j = right )
(7)
(2) The resultant front and rear wheel slips are as follows: S resi = S xi 2 + S yi 2 , (i = 1, 2,3, 4)
(8)
(3) Now, the front and rear longitudinal and lateral tire forces can be known accurately. The tire-force equations are as follows:
Fxi μ resi S xi Fzi , (i = 1, 2, 3, 4) F = yi S resi S yi
3
(9)
Vehicle States Estimation
Usually the car state of motion can be measured through a variety of vehicle sensors to take. Due to constrained by the current technical level, some important variables need to use more expensive equipment to measure (such as speed, yaw rate), or could not take direct measurements (such as the side slip angle), parameter estimation problem is to be solved on a vehicle stability control system design. An extended Kalman filter (EKF) [11] is used to estimate the vehicle state. The EKF process is represented by a nonlinear state space description incorporating state and measurement difference equations as follows: In (10) and (11) the nonlinear function f relates the state vector x(t) and the input vector u(t). The measurement vector h relates the state to the measurements. Vectors w(t) and v(t) denote the superimposed process and measurement noise, respectively. The variance of w(t) and v(t) are Q and R, respectively. For the system model, EKF process as [12]:
x (t ) = f ( x(t ), u (t ), w(t ))
(10)
y (t ) = h( x(t ), v(t ))
(11)
Step 1: The state equation and measurement equation of the vehicle The state equation of the vehicle is (1), and the measurement equation is as follows:
60
L. Tong
y (t ) = h( x(t ), v(t )) = a y =
ak1 − bk2 k +k k Yaw + 1 2 β − 1 δ mu m m
(12)
Step 2: The linearization of the model F(t) and H(t) are the Jacobian matrix which are derivative of nonlinear function f(x(t),u(t),w(t)) and h(x(t),v(t)), t is the sampling time.
△
∂f1 ∂f1 ∂h1 ∂h1 ∂x …… ∂x ∂x …… ∂x m 1 m 1 F (t ) = …………… H (t ) = …………… ∂f m …… ∂f m ∂hm …… ∂hm ∂xm ∂xm ∂x1 ∂x1
(13)
Φ(t ) = e F (t )∗t ≈ I + F (t ) ∗ Δt
(14)
The state equation of the vehicle substitutes into the (13),and you can get the state matrix and the measurement matrix as follows: F (t ) =
a 2Yaw + b 2 β I su
ak1 − bk2 Is
ak1 − bk2 − mu 2 mu 2 βu
k1 + k2 mu x Yaw ∗ u
2(ak1 − bk2 )Yaw β (k1 + k2 ) k1δ − − + mu 3 mu 2 mu 2 Yaw ∗ β -
Yaw(a 2 k1 + b 2 k2 ) Isu2
(ak1 − bk2 ) / (mu ) H (t ) = (k1 + k2 ) / m −(ak − bk ) / ( mu 2 ) 1 2
(15)
(16)
Step 3:Choosing an initial value for x-(t0) and p-(t0) and take the process shown as Fig.2 to achieve EKF filter recursive algorithm. Gain Equation
Update
Initial input
K (t ) = P − (t ) H (t )T [ H (t )T P − (t ) H (t ) + R ]−1
xˆ − (t0 ) and P − (t0 )
Update estimate with measurement
xˆ (t ) = xˆ − (t ) + K (t )[ y (t ) − h( xˆ − (t ), 0)]
State estimate
xˆ − (t + 1) = f ( xˆ (t ), u (t ), 0)
Update the error convariance
State error convariance estiamte
P − (t + 1) = φ (t ) P (t )φ (t )T + Q
Estimate
P (t ) = [ I − K (t ) H (t )]P − (t )
Fig. 2. Working process of EKF
Based on the above equation of state vehicle model and measurement equation,we can design the extended Kalman filtering.We defined the state vector as x=[Yaw, β ,u],the measurement vector as y=[ay] that incorporates all the measurement values,the input vector as u=[ δ ,ax]T,the process noise covariance Q=I3,the measurement noise covariance as R=[10000],the initial value for error covariance as
An Approach for Vehicle State Estimation Using Extended Kalman Filter
61
p-(t0)=I3x3 and the initial value for x-(t0)=[0,0,26.4] through the two-lane condition [13-14]. On the Matlab/Simulink platform,the model is achieved and the estimation algorithm is shown in Fig.3.The estimated results are compared with the actual vehicle parameters obtained by MSCCarsim software. The special vehicle dynamics simulation software named MSCCarsim,which is developed through numerous research experiments with higher accuracy,is used to overcome the difficulty and costly expense for achievement of the vehicle experiment data.The software combines traditional and modern multi-body vehicle dynamics modeling methods towards characteristic of parametric modeling for vehicle dynamics simulation software,including three-part graphic database of full- vehicle model, the direction and speed control and the external conditions (including the road information,drag,etc.)[15].With the simple and easy to understand user interface in Carsim,users can view full-vehicle model,simulation and results,so can use the simulation software quickly and accurately to provide a reference for engineering design.
Fig. 3. Vehicle state estimation on the simulink platflorm
4
Simulink Results
The tire forces, the vehicle state histories, and the friction coefficient of the road govern all aspects of the vehicle motions and are important for vehicle simulation, handling evaluation, control system design, and safety measures. In this paper, the simulation model is a three degree-of-freedom of a full-vehicle model [16]. The estimated results are compared with the actual vehicle parameters obtained by MSCCarsim software. In this paper, the initial speed is 25Km/s, road friction coefficient is 0.3, the sampling time is 0.001s. The steering angle and longitudinal acceleration as input variables, and lateral acceleration as a measurement output are fed into EKF observer in Fig. 4.
62
L. Tong
Actual vehicle parameters are obtained by MSCCarsim software and the estimated results of the longitudinal and lateral velocities compared with the actual values are shown in Fig. 5 and Fig. 6. It can be seen from the results that the estimated value has a good match with the actual yaw rate, sideslip angle and the speed as the steering angle and longitudinal acceleration of the changes. Therefore, the estimates states of EKF follow the actual states properly, even in the face of cornering conditions. The estimated results of the yaw speed and the side slip angle compared with the actual values are shown in Fig. 7 and Fig. 8, respectively. Through those responses, it can be seen that the derived model could represent the vehicle dynamics well. The differences of these values can be accepted based on some assumption when the dynamic vehicle models were derived and the estimation processed were involved by some noises. EKF Input Variable 0.1 ax Delta
0
-0.1
-0.2
0
1
2
3
4
5 Time(s)
6
7
8
9
10
Fig. 4. The input variables into EKF The Longitudinal Velocisy
The Yaw Speed
26.5
0.35
Estimated CarSim
Estimated CarSim
0.3
26
25.5
0.2
Vx(m/s)
Yaw(rad/s)
0.25
0.15
25
0.1
24.5 0.05
0
0
1
2
3
4
5 Time(s)
6
7
8
9
24
10
0
1
Fig. 5. The yaw rate
2
3
4
5 Time(s)
6
7
8
9
10
Fig. 6. The longitudinal velocity The Side Slip Angle 0.05
The lateral Acceleration 6
Estimated CarSim
0.04
Estimated CarSim
0.03
4
0.02
Alhpa(rad)
ay(m/s2)
2
0
-2
0.01 0 -0.01 -0.02 -0.03
-4
-0.04 -6
-0.05 0
1
2
3
4
5 Time(s)
6
7
8
Fig. 7. The lateral acceleration
9
10
0
1
2
3
4
5 Time(s)
6
7
8
Fig. 8. The side slip angle
9
10
An Approach for Vehicle State Estimation Using Extended Kalman Filter
5
63
Conclusion
In this paper, a three degrees-of-freedom nonlinear vehicle model was developed. In the absence of commercially available transducers to measure some state variables, this paper proposes the estimation method for vehicle state histories and yaw rate using EKF. The obtained state estimate values can be used for advanced feedback control to make intelligent driving decisions. The comparison between the estimated results and the simulated Carsim model confirms the validity of the model, in which all the state variables follow the Carsim response well. Therefore, we can estimate more state variables to provide more comprehensive, accurate state parameters for the research and development of automotive control systems.
References 1. Onoa, E., Asanoa, K., Sugaia, M., Itob, S., Yamamotob, M., Sawadac, M., Yasui, Y.: Estimation of Automotive Tire Force Characteristics Using Wheel Velocity. Control Engineering Practice 11, 1361–1370 (2003) 2. Rajamani, R.: Vehicle Dynamics and Control. Springer, New York (2006) 3. Liu, C.-S., Peng, H.: Road Friction Coefficient Estimation for Vehicle Path Prediction. Vehicle System Dynamics 25, 413–425 (1996) 4. Pohl, R.S., Reindl, L.: The Intelligent Tire Utilizing Passive SAW Sensors Measurement of Tire Friction. IEEE Transactions on Instrumentation and Measurement 48(6), 1041– 1046 (1999) 5. Wilkin, M.A., Manning, W.J., Crolla, D.A.: Estimation of non-linear friction force between tyre and roadapplied to a performance vehicle. In: AVEC 2004, pp. 387–392 (2004) 6. Christopher, R., Carlson, J., Gerdes, C.: Nonlinear Estimation of Longitudinal Tire Slip Under Several Driving Conditions. In: Proceedings of AVEC 2002, Hiroshima, JP (September 2002) 7. Abe, M., Kano, Y.: Side-slip control to stabilize vehicle lateral motion by direct yaw 8. Ray, L.R.: Nonlinear State and Tire Force Estimation fro Advanced Vehicle Control. IEEE Transaction on Control System Technology 3(1), 117–124 (1995) 9. Hiraoka, T., Kumamoto, H., Nishihara, O.: Sideslio Angle Estimation and Active Front Steering System Based on Lateral Acceleration Data at Centers of Percussion with Respect to Front/Rear Wheels. JSAE Review 25, 37–42 (2004) 10. Dugoff, H., Fancher, P.S., Segal, L.: Tyre performance characteristics affecting vehicle response to steering and braking control inputs. Final Report, contract CST-460, Office of Vehicle System Research, US National Bureau of Standards (1969) 11. Shi, K.L.: Speed estimation of an induction motor drive using an optimized extended Kalman filter. IEEE (2000) 12. Kalman, R.E.: A new approach to linear filtering and prediction problems. Transaction of the ASME-Journal of Basic Engineering, 35–45 (1960) 13. Cherouat, H., Diop, S.: Vehicle velocity estimation and vehicle body side slip angle and yaw rate observer. In: Proc. IEEE Internat. Symposium Industrial Electronics, ISIE 2005, Mini Track on Automotive (2005) 14. Simon, D.: Kalman filter with state equality constraints. IEEE (2002) 15. Jazar, R.N.: Vehicle Dynamis:Theory and Application. Springer, New York (2008) 16. Grewal, M.S., Andrews, A.P.: Kalman Filtering: Theory and Practice Using Matlab, 2nd edn. John Wiley and Sons, Inc. (2001)
