Sideslip angle estimation using extended Kalman filter

Sideslip angle estimation using extended Kalman filter B.-C. Chen* and F.-C. Hsieh Department of Vehicle Engineering, National Taipei University of Technology, Taipei, Taiwan Vehicle sideslip angle can be estimated using either dynamic or kinematic models. The dynamic model requires vehicle parameters which might have uncertainties due to different load conditions, vehicle motions, and road frictions. Parameter uncertainties might result in estimation errors. Thus system identifications are required to estimate those parameters on-line. On the other hand, the kinematic model does not require these parameters. A closed-loop estimator can be formulated to estimate the sideslip angle using the kinematic model. Since the system matrix which consists of the yaw rate is time varying, the required input vector and output contain process and measurement noises, respectively, and the disturbance input matrix contains estimated states, extended Kalman filter is used to obtain the estimation gain in this paper. CarSim is used to evaluate the proposed approach under different driving scenarios and road frictions in Matlab/Simulink. The preliminary results show promising improvement of the sideslip angle estimation. Keywords: sideslip angle; estimation; extended Kalman filter

1 Introduction Since 1990s, the electronic stability control (ESC) system has been established in automotive industries to provide vehicle stability and handling predictability for drivers. ESC can utilize differential braking based on information of steering angle, vehicle sideslip angle, wheel speed, yaw rate, and lateral acceleration to generate the stabilizing yaw moment, such that the yaw rate following can be achieved while maintaining the reasonable limits of the sideslip angle [1]. Unlike yaw rate which can be measured directly with low-cost sensors, the sideslip angle needs to be measured using either optical or GPS sensors which still have practical issues of cost, accuracy, and reliability to be used in production vehicles [2]. Many approaches have been proposed to estimate the sideslip angle, or equivalently the lateral velocity, in the literature. They can be classified into three types if dynamic models are used to design the estimators. Type I uses the bicycle model with appropriately assigned parameters without using system identification. Based on the known dynamic model, various techniques can be applied to design the estimator [3-8]. However, the required vehicle parameters, which includes mass, yaw-plane rotational inertia, location of the center of gravity, and cornering stiffness at the front and rear tires, might have uncertainties due to different load conditions, vehicle motions, and road frictions. Among these parameters, cornering stiffness is influenced significantly by the road friction. Type II uses linear system identification theorem [9-12] or adaptive observer based on Lyapunov stability theory [13-15] to identify the cornering stiffness at the front and rear tires by assuming that the rest of vehicle parameters are known. Various estimators can then be designed to estimate the sideslip angle or lateral velocity. Type III is able to estimate the lateral velocity while dealing with the parameter uncertainties at the same time. Liu and Peng [16] proposed an identification scheme to estimate states and parameters simultaneously. However, their approach shows slow convergence for nonnominal conditions. Most of above approaches are based on the bicycle dynamic model or its variations. Since dynamics is concerned with the effects of forces on the motion of a body with mass and rotational inertia, parameter uncertainties might cause estimation errors. On the other hand, kinematics only considers the motion of a body and is not affected by these uncertainties. Van Zaton [17] mentioned a kinematic model which can be used to estimate the sideslip angle on a horizontal plane with zero roll and pitch angles. Because it is an open-loop estimator, large estimation errors might be resulted from not only signal offsets and noises but also large roll and pitch motions. Farrelly and Wellstead [18] proposed a kinematic model based estimator which uses the measurements of longitudinal and lateral accelerations, yaw rate, and vehicle speed to estimate the lateral velocity. The estimation gain is designed using pole placement to obtain critical damping properties. The kinematic model is shown to be observable when the yaw rate is non-zero. Their approach produces noisy estimation, but is unaffected by parameter uncertainties. When the yaw rate is * Corresponding author. Email: [email protected].

zero, the model becomes unobservable and the estimated states need to be reset to zero to prevent estimation drift. Ungoren et al. [2, 19] integrated the methodology in [18] with a physical model based estimator to avoid un-observability and the drifting issues during near-zero yaw rate conditions. A yaw rate criterion is used to switch between these two estimators. Since the system matrix of the kinematic model consists of the yaw rate which is time varying, the required input vector and output contain process and measurement noises, respectively, and the disturbance input matrix contains estimated states, extended Kalman filter is used to obtain the estimation gain in this paper. CarSim is used to evaluate the proposed approach under different driving scenarios and road frictions in Matlab/Simulink. The remainder of this paper is organized as follows: two previous approaches which include one Type I estimator for baseline comparison and the estimator based on kinematic model [18] are presented in Section 2. Followed by the description of the proposed estimator in Section 3. The simulation results using CarSim are presented in Section 4. Finally, conclusions are made in Section 5. 2 Previous Approaches

2.1 Type I estimator One Type I estimator based on the bicycle model (see Figure 1) is used for baseline comparison in this paper. Aoki et al. [5] employed lateral acceleration and yaw rate sensors to design the estimator based on a linear bicycle model as follows. (1) xɺˆ b = A b xˆ b + B b ub + Lb ( yb − yˆ b ) T where xˆ = [βˆ rˆ] is the estimated state vector; u = δ is the input; β = v / v is the sideslip angle, r is b

b

y

x

the yaw rate, and δ is the steering angle; vx and v y are the longitudinal and lateral velocities, respectively; the subscript b denotes for the bicycle model; the hat denotes for estimated values; Lb is the estimation gain matrix; A b and Bb are the system and input matrices, respectively, which can be expressed as

 −(Cα f + Cα r ) bCα r − aCα f   Cα f  −1     2 mv mv x x  , B =  mvx  (2) Ab =  b  bC − aC  aCα f  −( a 2Cα f + b 2Cα r )  αr αf     Iz I z vx    I z  where m is the vehicle mass; Iz is the yaw-plane rotational inertia; a and b are the distances from the C.G. to the front and rear axles, respectively; Cαf and Cαr are the cornering stiffness of the front and rear axles, respectively. All these parameters are assumed to be the nominal values at normal load on high road friction surface. The output equation can be expressed as follows: (3) yb = Cb xb + Db ub where yb = [a y r ]T is the output vector; Cb and Db are the output and direct feed-through matrices, respectively, and can be expressed as  −(Cα f + Cα r ) bCα r − aCα f   Cα f      Cb =  , (4) D = m mvx b   m    0 1  0   An error dynamics of the estimator was derived to study the effect of parameter uncertainties for Cα f and Cα r . One element of L matrix is selected to obtain the best condition for robustness and the other elements are selected using the pole placement, as shown in Equation (5). Their experimental results showed that the estimated results are very close to the experimental data. ( a − b) I z 1  λ1λ2   C 2(a 2 + b 2 ) + 4ab − 1  v αf x   (5) Lb =  m( a 2 + b 2 )  −λ1 − λ2   (a − b) I z  

where λ1 and λ2 are the designed poles of the closed-loop estimator. 2.2 Kinematic Model Based Estimator Farrelly and Wellstead [18] integrated lateral and longitudinal kinematics with the pole placement approach to design the estimator. The kinematic model can be expressed as follows. xɺˆ = Axˆ + Bu + L( y − yˆ ) (6) where xˆ = [vˆx vˆ y ]T and u = [ax a y ]T are the state vector and input vector, respectively; ax and a y are longitudinal and lateral accelerations, respectively; A and B are the system and input matrices, respectively, which can be expressed as follows.  0 r 1 0  , B= (7) A=    − r 0 0 1  Since the vehicle speed is used as the measurement vx , m , the output equation can be expressed as follows. (8) y = Cx where y is the output and C = [1 0] is the output matrix. The estimate of β can then be obtained as βˆ = vˆ / vˆ . The estimator gain L is selected such that both eigen values are located at λ = −α r , where y

x

α is a positive constant. Thus it can be expressed as follows.

 2α r  (9) L= 2  (α − 1)r  After comparing with the estimator based on the bicycle, the simulation results showed that their proposed algorithm produces noisier estimation, but is unaffected by parameter uncertainties. When the yaw rate is zero, the model in Equation (6) becomes unobservable and the estimated states need to be reset to zero to prevent estimation drift. Ungoren et al. [2, 19] integrated the above estimator with a physical model based estimator to these issues. If the yaw rate is less than a threshold value rth, i.e. near-zero yaw rate conditions, vˆ y can be obtained using the following equation. vˆɺ = − v r + aˆ (10) y

x ,m

y

where

aˆ y =

1  Cα f m 

   vˆ y + ar   vˆ y − br    tan −1  − δ   + Cα r tan −1        v    vx , m   x , m   

(11)

3 Estimator Design Since the measurements of r, ax, and ay are contaminated with sensor noises, they can be modeled as (12) rm = r + rn , ax , m = ax + ax , n , a y , m = a y + a y , n where the subscript m and n denote for the measurement and noise, respectively. We can then rewrite kinematic model as follows. (13) xɺ = Ax + Bu + Gw where G and w are the disturbance input matrix and the process noise vector, respectively, which can be expressed as  −v x −1 0  T (14) G=  , w =  rn ax , n a y , n  v 0 − 1  y  Since the real vehicle speed is not easy to measure, it is often approximated by the rotational speed ωt obtained from the transmission as follows. (15) vx ,t = ωt n f rw where vx ,t is the approximated vehicle speed; n f is the gear ratio of the final drive; and rw is the wheel radius. Thus we can obtain the following expression.

(16)

vx = vx ,t − ∆vx

where ∆vx is the speed error. We can further model the speed error as the measurement noise v = ∆vx in the output equation as follows. (17) y = Cx + v In order to implement the proposed approach using digital controller, we use the forward rectangular rule [20] to discretize the kinematic model as follows. ( z − 1) x = Ax + Bu + Gw (18) T where z is the advance operator and T is the sample period, which is set to be 0.04 sec in this paper. We can then obtain the discrete state-space representation as (19) xk +1 = Φ k xk + Γuk + Γ1wk where the subscript k denotes for the kth sampled variables; Φ k is the discretized time-varying system matrix, which changes with the yaw rate; Γ and Γ1 are the discretized input and disturbance input matrices, respectively. They can expressed as rk T   −vx , k T −T 0   1 T 0  , Γ= , Γ1 =  (20) Φk =    0 −T  0 T   − rk T 1   v y ,k T Since Γ1 contains states vx , k and v y , k , we can then utilize the extended Kalman filter to estimate xk as follows. (21) xˆ k +1 = Φ k xˆ k + Γuk + L k ( yk − Cxˆ k ) where L k is the estimation gain matrix, which can be designed as

 P HT Rv −1 for rk ≥ rth Lk =  k 0 for rk < rth  where Pk is the estimate error covariance matrix as

(22)

Pk = M k − M k HT (HM k HT + R ) −1 HM k

(23)

T

and R = E[vk vk ] is the variance of the measurement noise. M k is the update law for Pk as

M k +1 = Φ k Pk Φ k T + Γ1,k QΓ1, k T

(24)

where Q = E[wk wk T ] is the process noise covariance matrix and Γ1, k = Γ1 x

k

= xˆk

is updated at the kth sample

period using xˆ k . The estimate of β at the kth sample period can then be obtained as βˆk = vˆ y , k / vˆx , k . When the yaw rate is than the threshold value rth , the kinematic model becomes nearly unobservable. Thus the estimation gain is set to be zero. The discrete version of Equations (10) and (11) [2, 19] is then used to continue the estimation as follows. vɺˆ y ,k = − v x ,t ,k rm ,k + aˆ y ,k (25) where    vˆ y , k + arm , k   vˆ y − brm, k   1  (26) − δ   + Cα r tan −1  aˆ y , k = Cα f  tan −1      v   v  m  x ,t , k x ,t , k        4 Simulation Results The proposed approach is evaluated under different driving scenarios and road frictions using CarSim [CarSim user manual reference] in Matlab/Simulink. CarSim (see Figure 2) is a software program developed by the Mechanical Simulation Corporation to simulate and analyze vehicle dynamic responses. The Big SUV model is selected to verify the proposed control in this paper. This model has front independent suspensions, rear solid axles, 14 multibody degrees of freedom, and 54 state variables. Double lane change (DLC) and slalom (see Figure 3) are selected to be the test maneuvers in this paper. The built-in driver model in CarSim is used to execute the maneuver on high µ (road friction coefficient), low µ, and split µ surfaces with different vehicle entrance speeds.

4.1 High µ Surface The friction coefficient of the high µ surface is set to be 0.85 in this paper. The entrance speeds of DLC and slalom maneuvers are 80 km/hr and 90km/hr, respectively. If we assume that all the measurements are not contaminated by signal noises, the sideslip angle estimation results for both maneuvers are shown in Figures 4 and 5. Aoki's approach shows the largest estimation error. Although Cα f and Cα r are obtained on the high µ surface, they are still affected by the load transfer resulted from the vehicle motions. On the other hand, the proposed approach shows results similar to that of modified Farrelly and Wellstead's (F&W) approach which is augmented with Equations (10) and (11) and both are very closed to the response of CarSim. In order to consider the effect of sensor noises, required vehicle speed in these approaches uses the approximated speed in Equation (16) instead of the true vehicle speed. Sensor noise levels for the accelerometers, yaw rate gyro, and steering angle are set to be 0.06 m/s2, 0.2 deg/s, and 0.1 deg, respectively. The estimation results with sensor noises for both maneuvers are shown in Figures 6 and 7. Aoki's approach shows the largest estimation error. Because the proposed approach considers the process and measurement noises while designing the estimator, the estimation gain is more optimal than that of the modified F&W's approach which only considers critical damping. Thus its estimation error is smaller than that of the modified F&W's approach. Estimation errors for both maneuvers are shown Table 1. The proposed approach has the smallest maximum and mean values. The average improvements of the maximum values are 65.8 % and 14.6 % for Aoki's and modified F&W's approaches, respectively. The average improvements of the mean values are 66.7 % and 35.5 % for Aoki's and modified F&W's approaches, respectively. 4.2 Low µ Surface The friction coefficient of the low µ surface is set to be 0.35 in this paper. The entrance speeds of DLC and slalom maneuvers are 60 km/hr and 50km/hr, respectively. The lower entrance speeds are necessary to prevent vehicle to loose directional stability on low µ surface. Estimation results for both maneuvers are shown in Figures 8 and 9. Aoki's approach shows the largest estimation error which is much worse than that on the high µ surface. This is mainly because of the incorrect values of cornering stiffness which are obtained on high µ surface and significantly changed on low µ surface. Although Aoki et al. have designed the estimation gain matrix to obtain the best condition for robustness, they still cannot overcome the effect of large deviation of the cornering stiffness. On the other hand, kinematic model based estimators are not affected by these parameter uncertainties and still produce estimations closed to the response of CarSim. The estimation error of the proposed approach is smaller than that of the modified F&W's approach. Estimation errors for both maneuvers are shown Table 2. The proposed approach has the smallest maximum and mean values. The average improvements of the maximum values are 92.9 % and 35.3 % for Aoki's and modified F&W's approaches, respectively. The average improvements of the mean values are 95 % and 45.1 % for Aoki's and modified F&W's approaches, respectively. 4.3 Split µ Surface Split µ surface is a road with low µ surface on one side of the vehicle and high µ surface on the other side of the vehicle. The entrance speeds of DLC and slalom maneuvers are 65 km/hr and 55km/hr, respectively. The lower entrance speeds are necessary to prevent vehicle to loose directional stability on split µ surface. Estimation results for both maneuvers are shown in Figures 10 and 11. Aoki's approach shows the largest estimation error which is much worse than that on the high µ surface. This is mainly because of the incorrect values of cornering stiffness which are obtained on high µ surface and significantly changed on split µ surface. Besides, the bicycle model lumps the front left and right wheels into one front wheel and lumps the rear left and right wheels into one rear wheel. Thus it is not easy to for the Aoki's approach to overcome the effect of large deviation of the cornering stiffness on one side of the vehicle. On the other hand, kinematic model based estimators are not affected by these parameter uncertainties and still produce estimations closed to the response of CarSim. The estimation error of the proposed approach is smaller than that of the modified F&W's approach.

Estimation errors for both maneuvers are shown Table 3. The proposed approach has the smallest maximum and mean values. The average improvements of the maximum values are 93.9 % and 39.9 % for Aoki's and modified F&W's approaches, respectively. The average improvements of the mean values are 95.3 % and 46.5 % for Aoki's and modified F&W's approaches, respectively. 5 Conclusion A sideslip angle estimator based on the kinematic model is proposed in this paper. This estimator is similar to F&W's approach with different estimation gain design. The required vehicle speed is approximated by the rotational speed obtained from the transmission and the difference between these two speeds is modeled as measurement noise. Since the measurements of yaw rate, longitudinal acceleration and lateral acceleration are contaminated with sensor noises, they are modeled as process noises with a nonlinear disturbance matrix. Since the matrix contains states, the extended Kalman filter is used to design the estimation gain. When the yaw rate is than the threshold value, the kinematic model becomes nearly unobservable. Similar to the modified F&W approach, a discrete version of the physical model based estimator is also used to solve this problem. CarSim is used to evaluate the proposed approach under DLC and slalom maneuvers on high µ, low µ, and split µ surfaces in Matlab/Simulink. The proposed approach shows promising improvements over the baseline and the modified F&W approaches, especially on low µ, and split µ surfaces. Since the cornering stiffness is significantly affected by the road friction, it's not easy to overcome the parameter uncertainties without using system identification. On the other hand, kinematic model based estimators are not affected by these parameter uncertainties. Because the proposed approach considers the process and measurement noises while designing the estimator, the estimation gain is more optimal than that of the modified F&W's approach which only considers critical damping. Thus its estimation error is smaller than that of the modified F&W's approach. Acknowledgement This project was supported partly by the Chung-shan Institute of Science and Technology and partly by the National Science Council in Taiwan under the contract No. of NSC 95-2752-E-027-002-PAE. References [1] H. E. Tseng, The Development of Vehicle Stability Control at Ford, IEEE/ASME Transactions on Mechatronics, Vol. 4, No. 3 (1999), pp. 223-234. [2] A. Y. Ungoren, H. Peng, and H.E. Tseng, Experimental Verification of Lateral Speed Estimation, 6th International Symposium on Advanced Vehicle Control, Hiroshima, Japan, 2002. [3] L. Li and F.-Y. Wang, A Robust Observer Designed for Vehicle Lateral Motion Estimation, IEEE Proceedings of Intelligent Vehicles Symposium, 2005, pp.417-422. [4] G. Baffet, J. Stephant, and A. Charara, Vehicle Sideslip Angle and Lateral Tire-Force Estimations in Standard and Critical Driving Situations: Simulation and Experiments, 8th International Symposium on Advanced Vehicle Control, The Grand Hotel, Taipei, Taiwan, 2006. [5] Y. Aoki, T. Inoue, and Y. Hori, Robust Design of Gain Matrix of Body Slip Angle Observer for Electric Vehicles and its Experimental Demonstration, The 8th IEEE International Workshop on Advanced Motion Control, 2004. [6] U. Kiencke and A. Dai, Observation of Lateral Vehicle Dynamics, Control Engineering Practice, Vol.5, No. 8 (1997), pp. 1145-1150. [7] P. J. T. Venhovens and K. Naab, Vehicle Dynamics Estimation Using Kalman Filter, Vehicle System Dynamics, Vol.32 (1999), pp.171-184. [8] J. R. Zhang, S. J. Xu, and A. Rachid, Robust Sliding Mode Observer for Automatic Steering of Vehicles, Proceedings of 2000 IEEE Intelligent Transportation Systems Conference, Dearborn (MI), USA, 2000. [9] M. C. Best and T. J. Gordon, Real-Time State Estimation of Vehicle Handling Dynamics Using an Adaptive Kalman Filter, 4th International Symposium on Advanced Vehicle Control, Nagoya, Japan, 1998. [10] M. C. Best, T.J. Gordon, and P.J. Dixon, An Extended Adaptive Kalman Filter for Real-time State Estimation of Vehicle Handling Dynamics, Vehicle System Dynamics, Vol.34 (2000), pp. 57-75. [11] J. St´ephant and A. Charara, Observability matrix and parameter identification: application to vehicle

tire cornering stiffness, 44th IEEE Conference on Decision and Control, and the European Control Conference 2005. [12] Y. Aoki, Z. Li, and Y. Hori, Robust Design of Body Slip Angle Observer with Cornering Power Identification at Each Tire for Vehicle Motion Stabilization, 9th IEEE International Workshop on Advanced Motion Control, 2006. [13] S.-H. You, S. Yoo, J.-O. Hahn, H. Lee, and K. I. Lee, A New Adaptive Approach to Real-Time Estimation of Vehicle Sideslip and Road Bank Angle, 8th International Symposium on Advanced Vehicle Control, The Grand Hotel, Taipei , Taiwan , 2006. [14] H. E. Tseng, A Sliding Mode Lateral Velocity Observer, 6th International Symposium on Advanced Vehicle Control, Hiroshima, Japan, 2002. [15] M. Kaminaga and G. Naito, Vehicle Body Slip Angle Estimation Using an Adaptive Observer, 4th International Symposium on Advanced Vehicle Control, Nagoya, Japan, 1998. [16] C.-S. Liu and H. Peng, A State and Parameter Identification Scheme for Linearly Parameterized Systems, ASME, Journal of Dynamic System, Measurement and control Vol.120, No.4 (1998), pp524528. [17] A. T. van Zanten, Bosch ESP Systems: 5 Years of Experience, SAE Paper No. 2000-01-1633. [18] J. Farrelly, and P. Wellstead, Estimation of Vehicle Lateral Velocity, Proceedings of the 1996 IEEE International Conference on Control Applications, Dearborn, MI, 1996, pp. 552-557. [19] A. Y. Ungoren and H. Peng, A Study on Lateral Speed Estimation Methods, Internal Journal of Vehicle Autonomous Systems, Vol. 2, No.1/2 (2004), pp.126-144. [20] G. F. Franklin, J. D. Powell, and M. L. Workman, Digital Control of Dynamic System, Addison Wesley, New York, 1990.

Table 1. Estimation errors for DLC and slalom maneuvers on high µ surface (units: deg) Aoki

F&W

Proposed

max mean max mean max mean DLC 1.895 0.315 0.716 0.105 0.534 0.079 Slalom 1.421 0.220 0.611 0.171 0.599 0.099 average 1.658 0.268 0.664 0.138 0.567 0.089

Table 2. Estimation errors for DLC and slalom maneuvers on low µ surface (units: deg) Aoki

F&W

Proposed


Table 3. Estimation errors for DLC and slalom maneuvers on split µ surface (units: deg) Aoki

F&W

Proposed


r vx b

δ

a vy Figure 1. Bicycle model

Figure 2. Graphical user interface of CarSim 4 DLC

3

Lateral Offset (m)

2 1 0 0

50

100

150

200

4 3 2 1

Slalom

0 0

50

100 Station (m)

150

200

Figure 3. Test maneuvers

CarSim Aoki Modified F&W Proposed

Sideslip Angle (deg)

6 4 2 0 -2 -4 -6 0

2

4

6 time (sec)

8

10

Figure 4. DLC maneuver on high µ surface

12



6 4 2 0 -2 -4 0

2

4

6 time (sec)

8

10

12

Figure 5. Slalom maneuver on high µ surface



10

5

0 -5

-10 2

4

6 8 time (sec)

10

12

Figure 6. DLC maneuver with sensor noises



6 4 2 0 -2 -4 0

2

4

6 time (sec)

8

10

12

Figure 7. Slalom maneuver with sensor noises



6 4 2 0 -2 -4 2

4

6

8 10 time (sec)

12

14

Figure 8. DLC maneuver on low µ surface CarSim Aoki Modified F&W Proposed


10

5

0

-5

0

5

time (sec)

10

15

Figure 9. Slalom maneuver on low µ surface


8


6 4 2 0 -2 -4 -6 2

4

6

8 10 time (sec)

12

14

Figure 10. DLC maneuver on split µ surface


10


5

0

-5

0

5

time (sec)

10

15

Figure 11. Slalom maneuver on split µ surface