Roll & Bank Estimation Using GPS/INS and Suspension Deflections
by
Lowell S. Brown
A thesis submitted to the Graduate Faculty of
Auburn University
in partial fulfillment of the
requirements for the Degree of
Master of Science
Auburn, Alabama
August 4, 2012
Keywords: rollover, relative roll, state estimation, suspension deflections, vehicle dynamics,
kalman filter, INS/GNSS
Copyright 2012 by Lowell S. Brown
Approved by:
David Bevly, Chair, Professor of Mechanical Engineering
Subhash Sinha, Professor of Mechanical Engineering
David Beale, Professor of Mechanical Engineering
George Flowers, Professor of Mechanical Engineering
Abstract
Thisthesispresentsthreemethodsthatprovideandestimateofroadbankbydecoupling
the vehicle roll due to dynamics and roll due road bank. Suspension deflection measurements
were used to provide a measurement of the relative roll between the vehicle body frame and
the axle frame, or between the sprung mass and the unsprung mass respectively. A method
of scaling the suspension deflection measurements to vertical wheel motion was explored. A
deflection scaling parameter was found by both a dynamics based method and a suspension
geometry based method. The parameter was determined to effectively scale the suspension
deflection measurements with minimum error variances over varying vehicle speeds. The
relative roll measurement was then incorporated into three different estimation architectures.
A vehicle model based Kalman filter (KF) observer and two kinematic navigation model
based extended Kalman filters (EKF) were developed. The first EKF used a cascaded
approachtoincorporatetherelativerollmeasurement. TheEKFsecond, acoupledapproach,
augmented the state vector with a state for the road bank. The road bank was modeled as a
time varying disturbance and a measurement update for the relative roll measurement was
developed.
All the estimators were used to decouple the vehicle roll due to dynamics and the roll
due to bank. Each algorithm was tested in simulation with data from CarSim 6, a vehicle
dynamic modeling software package. The estimators were then tested on the Prowler ATV
experimental platform at the National Center for Asphalt Technology (NCAT). The KF
vehicle model based estimator correctly estimated the road bank under low dynamics in
simulation but was susceptible to vehicle model uncertainties and nonlinearities. Both the
cascaded and coupled approach performed well for both simulation and experimental data.
ii
The EKFs correctly estimated the road crown and banked turns of the NCAT Oval track.
The coupled EKF displayed the added benefit of filtering the noise on the bank estimate.
Between the three estimation approaches the coupled kinematic based EKF approach
was determined to be the best method. The vehicle model based approach proved to be very
sensitivetothevehiclemodel. Smalldeviationsinthemodelledtolargebankerrorsandpoor
performance under high dynamics. Both of the kinematic based approaches performed well
across all ranges of dynamics and road bank disturbances. However, the coupled approach
filtered the noise on the bank estimate which was determined to be advantageous.
iii
Acknowledgments
I dedicate this thesis to my mother, the late Elinor Brown, and must first thank and
acknowledge my father, Carry Brown, for pushing me in my academic studies early on in life
I generally looked forward getting the report card signed. A?s on the report card equaled
cash in hand! Also, I thank and love all of my family for the continuing guidance and support
throughout early adulthood.
Thank you to Dr. Bevly for exposing me to the GAVLAB as an undergraduate and
offering me the research assistant position and the opportunity to work on fascinating engi-
neering problems and platforms.
To the GAVLAB members, thank you for the technical discussions as well as the often
heated debates! To Gabe Hien, David Hodo and Will Travis for the help and assistance
in automating and controlling the Prowler ATV (The first two Revisions). Also thanks
to the Frenchies, Fahd and Anthony, for their work on the I2C architecture. To Ryan
Hill and David Broderick for their help and assistance in all Prowler revisions including the
most recent conversion to the CAN communications architecture and MOOS logging/control
architecture. Also, thanks to all the GAVLAB members who contributed to getting the
general CAN and MOOS architectures up and running. Thanks to Jonathan Ryan and
Scott Martin for discussions on the Kalman filter. Thank you to those who reviewed and
made comments on the thesis as well.
Thank you to my committee members Dr. David Beale and Dr. Subhash Sinha for
reading, reviewing and adding to the quality of my thesis.
Finally, thank you to Christ, Jesus, who while fully atoning also acts as my measure-
ment update snapping me back to truth from the error growth that too often occurs during
iv
this wonderful but brief experience. (Hopefully, the variances in my covariance matrix are
decaying with respect to time!)
v
Table of Contents
Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ii
Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iv
List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ix
List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii
1 Background & Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Rollover Algorithms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.4 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2 Roll, Relative Roll and Road Bank . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2 Methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2.1 Relative Roll . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.2.2 Scale Factor ? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3 The Eta (?) Parameter for Roll Scaling . . . . . . . . . . . . . . . . . . . . . . . 12
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3.2 The Effect of Speed on ? . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3.3 The effect of Mass and CG height on ?. . . . . . . . . . . . . . . . . . . . . . 14
3.4 ? as the Kinematic Suspension Ratio . . . . . . . . . . . . . . . . . . . . . . 18
3.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
4 Road Bank Estimation using Suspension Deflections and a Cascaded Navigation
EKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
vi
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
4.2 Methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
4.2.1 Coordinate Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4.2.2 Inertial Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
4.2.2.1 Mechanization Equations . . . . . . . . . . . . . . . . . . . 23
4.2.2.2 Update Equations . . . . . . . . . . . . . . . . . . . . . . . 26
4.2.2.3 Error Equations . . . . . . . . . . . . . . . . . . . . . . . . 29
4.2.3 Integration of Inertial Navigation and GPS . . . . . . . . . . . . . . 31
4.2.4 Kalman Filter Overview . . . . . . . . . . . . . . . . . . . . . . . . . 31
4.2.5 Extended Kalman Filter (EKF) Overview . . . . . . . . . . . . . . . 37
4.2.6 Navigation Error State EKF with Closed Loop Feedback . . . . . . . 39
4.2.7 NAV EKF Cascaded with Relative Roll . . . . . . . . . . . . . . . . . 44
4.3 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.3.1 Navigation EKF Tuning . . . . . . . . . . . . . . . . . . . . . . . . . 45
4.3.2 Cascaded EKF Road Bank Estimation . . . . . . . . . . . . . . . . . 56
4.4 Robustness . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
4.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
5 Road Bank Estimation using Suspension Deflections and a Coupled Navigation
EKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
5.2 Methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
5.3 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
5.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
6 Roll and Road Bank Estimation using Suspension Deflections, Roll model and
Yaw model Coupled with a KF disturbance observer . . . . . . . . . . . . . . . 70
6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
6.2 Methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
vii
6.2.1 Vehicle Fixed Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
6.2.2 Roll and Yaw Models . . . . . . . . . . . . . . . . . . . . . . . . . . 75
6.2.3 Kalman Filter Disturbance Observer Setup . . . . . . . . . . . . . . . 79
6.3 Simulations & Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
6.3.1 Model Validation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
6.3.2 Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
6.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
7 Experimental . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
7.2 Equipment & Test Facilities . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
7.3 Sensor Mounting and Test Procedure . . . . . . . . . . . . . . . . . . . . . . 93
7.4 Results & Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
7.4.1 Coupled Vehicle Model Kalman Filter Observer Results . . . . . . . . 100
7.4.2 Comparison of Cascaded and Suspension Coupled Navigation EKF . 105
7.4.2.1 Navigation EKF Cascaded . . . . . . . . . . . . . . . . . . . 105
7.4.2.2 Navigation EKF Coupled . . . . . . . . . . . . . . . . . . . 108
7.4.3 Additional Validation of the Coupled Navigation EKF . . . . . . . . 110
7.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
8 Conclusions & Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
8.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
8.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121
Appendices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123
A Vehicle Properties . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
A.1 CarSim?s Small SUV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
A.2 Prowler ATV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
viii
List of Figures
2.1 Roll Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.2 Suspension Deflection Diagram . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3 Simulated Vehicle Performing a Double Lane Change Maneuver on a Flat
Surface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.1 Double Lane Change ISO 3888-2 . . . . . . . . . . . . . . . . . . . . . . . . 12
3.2 ? Values for DLC on Flat Surface (10 km/hr to 250 km/hr) . . . . . . . . . . . . 13
3.3 RMS Error for DLC on Flat Surface vs. Speed . . . . . . . . . . . . . . . . . 14
3.4 ?Values for DLC on Flat Surface (Different Payload Heights) . . . . . . . . . 16
3.5 RMS Error for DLC on Flat Surface vs. Payload Height & Speed . . . . . . 16
3.6 ? Values for DLC on Flat Surface (Different Payload Longitudinal Displace-
ments) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.7 RMS Error for DLC on Flat Surface vs. Payload Longitudinal Offset & Speed 18
3.8 Suspension Diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
4.1 Positions on Road Course with IMU Bias & Walk from CarSim Simulation . 49
4.2 Velocity on Road Course with IMU Bias & Walk from CarSim Simulation . . 50
4.3 Position Estimates on Road Course with IMU Bias & Walk . . . . . . . . . . 51
4.4 Position Errors on Road Course with IMU Bias & Walk . . . . . . . . . . . . 51
4.5 Velocity Estimates on Road Course with IMU Bias & Walk . . . . . . . . . . 52
4.6 Velocity Errors on Road Course with IMU Bias & Walk . . . . . . . . . . . . 52
4.7 Attitude Estimates Road Course with IMU Bias & Walk . . . . . . . . . . . 53
4.8 Attitude Errors Road Course with IMU Bias & Walk . . . . . . . . . . . . . 54
ix
4.9 Accelerometer Bias Estimates on Road Course with IMU Bias & Walk . . . 55
4.10 Roll Gyro Bias Estimate Road Course with IMU Bias & Walk . . . . . . . . 55
4.11 Roll Estimate from DLC on 15% (8.62 deg) Bank using CarSim . . . . . . . 57
4.12 Attitude Errors from DLC on 15% (8.62 deg) Bank using CarSim . . . . . . 58
4.13 Simulation DLC on Increasing Bank Bank Estimate using CarSim . . . . . . 59
4.14 Simulation DLC on Increasing Bank Bank Errors using CarSim . . . . . . . 59
4.15 Road Course Bank Estimate using CarSim . . . . . . . . . . . . . . . . . . . 60
4.16 Road Course Bank Errors using CarSim . . . . . . . . . . . . . . . . . . . . 60
4.17 Vehicle Performing Fishhook Maneuver on Flat Surface . . . . . . . . . . . . 62
4.18 Vehicle Performing a Fishhook Maneuver on a 30% Bank . . . . . . . . . . . 62
5.1 Coupled EKF Bank Estimate DLC on 15% (8.62 deg) Bank . . . . . . . . . 66
5.2 Coupled EKF Bank Errors DLC on 5% (8.62 deg) Bank . . . . . . . . . . . 66
5.3 Coupled EKF Bank Estimate DLC on Increasing Bank . . . . . . . . . . . . 67
5.4 Coupled EKF Bank Errors DLC on Increasing Bank . . . . . . . . . . . . . . 67
5.5 Coupled EKF Bank Estimate Road Course . . . . . . . . . . . . . . . . . . . 68
5.6 Coupled EKF Bank Errors Road Course . . . . . . . . . . . . . . . . . . . . 68
6.1 Navigation Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
6.2 Schematic of Bicycle Model (with all Forces and Angles Shown in Positive
Direction) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
6.3 Roll Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
6.4 Model Comparison Double Lane Change at 20 Km/hr . . . . . . . . . . . . . 83
6.5 Model Comparison Double Lane Change at 20 Km/hr . . . . . . . . . . . . . 83
6.6 Model Comparison Double Lane Change at 70 Km/hr . . . . . . . . . . . . . 84
6.7 Model Comparison Double Lane Change at 70 Km/hr . . . . . . . . . . . . . 85
6.8 Straight Path on Increasing Bank at 20 Km/hr . . . . . . . . . . . . . . . . 86
x
6.9 Straight Path on Increasing Bank at 20 km/hr . . . . . . . . . . . . . . . . . 86
6.10 Straight Path on Increasing Bank at 20 km/hr . . . . . . . . . . . . . . . . . 87
6.11 Straight Path on Increasing Bank at 70 km/hr . . . . . . . . . . . . . . . . . 87
6.12 Straight Path on Increasing Bank at 70 Km/hr . . . . . . . . . . . . . . . . 88
6.13 Straight Path on Increasing Bank at 70 Km/hr . . . . . . . . . . . . . . . . 88
6.14 Bank Estimate Double Lane Change at 20 Km/hr . . . . . . . . . . . . . . . 89
6.15 Bank Estimate Double Lane Change at 70 Km/hr . . . . . . . . . . . . . . . 89
6.16 Axle Roll Rate Estimate Double Lane Change at 20 km/hr . . . . . . . . . . 90
6.17 Axle Roll Rate Estimate Double Lane Change at 70 km/hr . . . . . . . . . . 91
7.1 Sensor Locations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
7.2 Sensor Logging . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95
7.3 Suspension Potentiometer CAN Enclosure . . . . . . . . . . . . . . . . . . . 95
7.4 Skid Pad Constant Radius-Steady State Turns . . . . . . . . . . . . . . . . . 96
7.5 NCAT Oval 1-2 deg Banked Straights . . . . . . . . . . . . . . . . . . . . . . 97
7.6 NCAT Oval 8 deg Banked Turn . . . . . . . . . . . . . . . . . . . . . . . . . 98
7.7 Double Lane Change Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
7.8 Double Lane Change Maneuver . . . . . . . . . . . . . . . . . . . . . . . . . 99
7.9 Position of Prowler on NCAT Oval Track Kalman Filter Observer . . . . . . 101
7.10 Steer Angle of Prowler on NCAT Oval Track Kalman Filter Observer . . . . 101
7.11 Model Output of the the Prowler on NCAT Oval Track Kalman . . . . . . . 102
7.12 Model Output of the the Prowler on NCAT Oval Track Kalman . . . . . . . 102
7.13 State Estimates of Kalman Filter Observer for Prowler on NCAT Oval Track 103
7.14 State Estimates of Kalman Filter Observer for Prowler on NCAT Oval Track 104
7.15 Bank Angle Estimate of Kalman Filter Observer for Prowler on NCAT Oval
Track . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
xi
7.16 Position NCAT Oval Track 15 State EKF Cascaded . . . . . . . . . . . . . . 105
7.17 Roll and Pitch Comparison on NCAT Oval Track 15 State EKF Cascaded . 106
7.18 Yaw Comparison on NCAT Oval Track 15 State EKF Cascaded . . . . . . . 107
7.19 Bank Estimate on NCAT Oval Track 15 State EKF Cascaded . . . . . . . . 108
7.20 Roll & Pitch Comparison on NCAT Oval Track 16 State Coupled EKF . . . 109
7.21 Yaw Comparison on NCAT Oval Track 16 State Coupled EKF . . . . . . . . 109
7.22 Bank Estimate on NCAT Oval Track 16 State Coupled EKF . . . . . . . . . 110
7.23 Coupled EKF on NCAT Skid Pad 50 Ft. Circle Position and Speed . . . . . 111
7.24 Coupled EKF on NCAT Skid Pad 50 Ft. Circle Bank Estimate . . . . . . . 111
7.25 Tire Relaxation and Asymmetrical Steering . . . . . . . . . . . . . . . . . . 112
7.26 Coupled EKF on NCAT Skid Pad 10 Ft. Circle Position and Speed . . . . . 113
7.27 Coupled EKF on NCAT Skid Pad 10 Ft. Circle Bank Estimate . . . . . . . 113
7.28 Coupled EKF on NCAT Oval Track Acceleration on Banked Left Turn Posi-
tion and Speed . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114
7.29 Coupled EKF on NCAT Oval Track Acceleration on Banked Left Turn Bank
Estimate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115
7.30 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116
7.31 Coupled EKF on NCAT Oval Track Roll Excitation Bank Estimate . . . . . 116
7.32 Coupled EKF on NCAT Oval Track High Speed Position and Velocity . . . . 117
7.33 Coupled EKF on NCAT Oval Track High Speed Bank Estimate . . . . . . . 117
7.34 Wheel Lift off in Straight on NCAT Oval Track . . . . . . . . . . . . . . . . 118
xii
List of Tables
3.1 Payload Height . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.2 Payload Longitudinal Displacement . . . . . . . . . . . . . . . . . . . . . . . 17
4.1 Simulation Noise Settings . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.2 Time Constants . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.3 Tuned Measurement Covariances . . . . . . . . . . . . . . . . . . . . . . . . 46
4.4 Error Covariances for P . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
4.5 Root Mean Squared Position Error . . . . . . . . . . . . . . . . . . . . . . . 50
4.6 Root Mean Squared Velocity Error . . . . . . . . . . . . . . . . . . . . . . . 52
4.7 Root Mean Squared Attitude Error . . . . . . . . . . . . . . . . . . . . . . . 53
4.8 Root Mean Squared Position Error . . . . . . . . . . . . . . . . . . . . . . . 56
4.9 Root Mean Squared Velocity Error . . . . . . . . . . . . . . . . . . . . . . . 56
4.10 Root Mean Squared Attitude Error . . . . . . . . . . . . . . . . . . . . . . . 56
A.1 Small SUV Vehicle Parameters . . . . . . . . . . . . . . . . . . . . . . . . . 124
A.2 Small SUV Suspension Parameters . . . . . . . . . . . . . . . . . . . . . . . 124
A.3 Small SUV Roll Stiffness and Damping . . . . . . . . . . . . . . . . . . . . . 125
A.4 Small SUV Cornering Stiffness . . . . . . . . . . . . . . . . . . . . . . . . . . 125
A.5 Prowler Vehicle Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
xiii
Chapter 1
Background & Motivation
1.1 Introduction
Vehicle rollover is a topic which has been widely researched by the vehicle community
for some time. With the rise in popularity of high center of gravity vehicles such as sport
utility vehicles, rollover has become an important issue for vehicle safety.
In 2009, 2.954 trillion miles were traveled by motorists in the United States. Among
those miles, motor vehicle crashes resulted in 30,797 fatalities [1]. Rollover crashes account
for only 3% of vehicle crashes. However, they lead to approximately one third of all occupant
deaths. This amount of rollover related deaths is a disproportionately large number of
highway fatalities.
Some vehicle safety features recently introduced by manufacturers into motor vehicle
fleets may be contributing to a reduction in rollover crashes and the harm they cause. These
features include rollover sensors to trigger inflatable side curtain airbags (SCABs, known as
rollover protection) to mitigate occupant injury, electronic stability control (ESC) to reduce
loss of yaw control, and roll stability control (RSC) to minimize the number of rollover
crashes that occur. These safety features are typically installed in Light Trucks and Vans
(LTVs) [2].
A review of empirical evidence that ESC has the potential to reduce the number of
rollover accidents and several other fatal types of accidents is shown in [3]. In fact, the Na-
tionalCenterforHighwaySafety(NTSHA)hasissuedafinalruleintheFederalRegisterthat
requires manufacturers to implement ESC systems for passenger cars, multipurpose passen-
gervehicles, trucks, andbuseswithagrossvehicleweightratingof4,536Kg(10,000 pounds).
According to NHTSA research, preventing single-vehicle loss-of-control crashes is the most
1
effective way to reduce deaths resulting from rollover crashes. ?This is because most loss-of-
control crashes culminate in the vehicle leaving the roadway, which dramatically increases
the probability ofarollover. Basedonthe bestavailable datadrawnfromcrash datastudies,
NHTSA estimates that the installation of ESC will reduce single-vehicle crashes of 2 passen-
gercarsby34percentandsinglevehiclecrashesofsportutilityvehicles(SUVs)by59percent,
withamuchgreaterreductionofrollovercrashes. NHTSAestimatesthat ESChasthepoten-
tial to prevent 71 percent of the passenger car rollovers and 84 percent of the SUV rollovers
that would otherwise occur in singlevehicle crashes [4].?
1.2 Rollover Algorithms
The rollover issue has led to the development of several metrics and methodologies to
predict and prevent vehicle roll over. Although typically the motivation for these studies
has been passenger vehicle safety, the use of these metrics can also be used to aid in the
navigation and control of unmanned ground vehicles (UGV). In order for the UGV to be
effective in performing its task, it must remain in a usable and controllable state. Thus the
vehicle must avoid rolling over. Additionally, the appropriate control effort required to keep
the vehicle from rolling is dependent on the bank on which the vehicle is operating.
Work has been done to develop metrics and algorithms for rollover indication and pre-
diction. The most basic is the Static Stability Factor (SSF). The SSF is a function of the
track width and the vertical height of the center of gravity (CG) of the vehicle. Increasing
CG height or decreasing the track width will cause an increase in rollover propensity [5].
Other metrics including the Lateral Load Transfer Ratio (LLT, LTR) and the Roll Stability
Factor (RSF) use the difference in normal forces on the left and right sides of the vehicle
to indicate rollover [5, 6]. Two wheel lift off can be determined through the observation of
the RSF. A Two Wheel Lift-off Velocity (TWLV) value can then be assigned and used in
characterization of rollover propensity [5]. To predict a Time-To-Rollover (TTR), G?sp?r et
al. [7] used a simple model to calculate a TTR assuming the speed and steer angle remain
2
constant. In that work, a neural network is used to make up for the lack of complexity off
the simple vehicle model needed to run faster than real time.
Rajamani et al. [8] determined that if the roll motion of the sprung mass is caused
entirely by the lateral acceleration, ignoring road and other external inputs, the rollover
index can be approximated as a function of lateral acceleration and CG height. Thus the
roll angle estimates and CG height estimates need to be accurate. They then propose a
dynamic observer and a CG height estimator to estimate the real-time roll angle and CG
height in order to calculate an accurate rollover index value.
These rollover metrics and algorithms work well with the assumption the road is flat and
level. This assumption will hold for small bank angles. However, in the more extreme cases
where bank angles exceed threshold of the small angle approximation, rollover characteristics
change. In order to improve the accuracy of the roll indicator, it is beneficial to have an
understanding of the bank on which the vehicle is traveling. A vehicle operating on a large
bank will have an increased tendency to roll compared to a vehicle operating on a flat
surface. Peters and Iagnemma [9] developed a stability metric based on the distribution of
wheelterraincontactforces. Theyshowedthatthemetriccansuccessfullypredictrolloveron
surfaces with arbitrary geometries. Ryu and Gerdes investigated the problem of estimating
vehicle and road bank [10]. They modeled the road bank as a disturbance and used a
disturbance observer to estimate the road bank. Using a two antenna GPS system they were
able to accurately estimate roll and road bank.
1.3 Contributions
It is desirable to develop a method of estimating roll and road bank which is not re-
liant on multiple GPS antenna systems, as a multi GPS antenna system is more expensive
then a single antenna system. Reducing the number of GPS antennas needed would be
advantageous.
3
This work explores two methods of estimating the vehicle road bank. The first is a
vehicle model based observer method. The vehicle model observer method uses an inertial
measurement unit (IMU), vehicle speed sensor and potentiometers to measure suspension
deflections. The IMU measures the specific force and rotation rates of the vehicle. The
vehicle?srelativeroll, theamountofrollthatexistsbetweenthesprungandunsprungmasses,
is measured from suspension deflections and geometry. Previously, suspension deflections
have been used as an estimate of the total vehicle roll with the assumption that the road
bank was near zero and could be neglected [11]. The vehicle speed sensor could be a Hall
effect wheel speed sensor. But, for the experimental results of this work, access to the wheel
speed sensor was not available and the GPS velocity was used.
The second method for road bank estimation utilizes the kinematics of the vehicle in
lieu of a vehicle model. The method is based off the extended Kalman filter (EKF) used
for navigation which provides estimates of the attitude states of the vehicle. The total roll
angle acquired from the EKF attitude vector is then combined with a measurement of the
vehicle?s relative roll and an estimate of road bank is acquired. As in the first method, the
suspension deflections and geometry are used to provide the measurement of the vehicle?s
relative roll. This method uses a single antenna GPS and a six degree of freedom (DOF)
IMU.
This thesis makes several contributions to the research in this field:
? A vehicle model based Kalman filter observer method for road bank estimation is
modified to use the relative roll measurement with the potential to not need any GPS
measurements.
? This vehicle model based Kalman filter method was tested in simulation, but a poor
vehicle model prevented validation of experimental data.
? A kinematic model based navigation EKF cascaded with relative roll measurements is
developed to estimate road bank.
4
? A kinematic model based navigation EKF coupled with relative roll measurements is
developed to estimate road bank.
? Both the cascaded and coupled kinematic model based navigation EKF approches are
validated in simulation and with experimental data.
? The cascaded and coupled approaches are compared against each other with the ex-
perimental data.
? The coupled kinematic model based navigation approach is recommended for road
bank estimation due to the filtering of sensor noise it provides on the bank estimate.
1.4 Thesis Outline
The outline of this thesis is as follows: the relationship of roll and bank is discussed
in Chapter 2. The concept of relative roll is explained and a geometry-based method that
uses suspension deflections to acquire a measurement of relative roll is presented. A scaling
parameter, ?, is shown to be necessary for use of the relative roll measurement. In Chapter 3
the relationship of vehicle dynamics and? scaling factor is explored. A complete formulation
of the Kalman filter and the extended Kalman filter for navigation comprises the first half of
Chapter 4. The latter half demonstrates the navigation extend Kalman filter in simulation.
The roll estimate is then combined with the relative roll measurement and is shown to
correctly provide an estimate of bank in simulation.
In Chapter 4 the navigation filter is augmented with a road bank state. This coupled
EKF approach to the bank estimation is then simulated and shown to provide the advantage
of noise filtering on the bank state. The vehicle model approach is described in Chapter
6. The chapter then concludes with a simulation of the vehicle model observer approach.
Both the cascaded and coupled versions of the navigation EKF as well as the vehicle model
observer method are tested with experimental data in Chapter 7. The data comes from
tests with the Prowler ATV on on oval track at the National Center for Asphalt Technology
5
(NCAT). The vehicle model observer performs poorly due to inaccurate knowledge of the
vehicle parameters for the Prowler ATV. The navigation filter approaches are both validated
with the experimental data. The coupled EKF is recommended as the preferred approach
due to the filtering of noise on the bank state. Finally, Chapter 8 consolidates the conclusions
from preceeding chapters and highlights areas of future work.
6
Chapter 2
Roll, Relative Roll and Road Bank
2.1 Introduction
This chapter discusses the relationship between the sprung and unsprung mass of the
vehicle and the effects of road bank on each mass. A method is presented that uses the
suspension deflections in conjunction with the knowledge of the total body roll to provide an
estimate of the road bank angle. Also, it should be noted that the methodology presented
can be modified and applied to vehicle pitch and road grade estimation.
2.2 Methodology
Figure 2.1 represents the roll model of the vehicle. In this model the sprung mass rotates
about the roll center. The road bank is described by the angle ?r that exist between the
road surface and the horizontal. The angle ?s describes the suspension roll, or relative roll,
that is due to the suspension deflections caused by the dynamics of the vehicle. The sum of
the road bank and the relative roll is the total roll indicated by ?v. The center of gravity is
denoted by CG and the gravity vector points downward and is denoted by g. The distance
from the ground surface to the roll center is hrc, the distance from the roll center to the
center of gravity is hrg and the distance between the two wheels is track width, denoted as
tw.
7
Figure 2.1: Roll Model
2.2.1 Relative Roll
The vehicle roll can be estimated using suspension deflections [11]. This measurement
of roll can be determined based on the geometry of the suspension and can be estimated
using Eq. (2.1),
??s = sin?1
parenleftBigg?L
LF??LRF + ?LLR??LRR
2tw
parenrightBigg
(2.1)
where ??s is the relative roll of the suspension. It should be noted that the suspension
roll is the roll of the body relative to the surface on which it is driving. The deflections ?Lij
are the suspension deflections at respective corners of the vehicle. The suspension roll for
the front axle is the inverse sine of the left deflection minus the right deflection divided by
the vehicle track width (tw) as observed from Figure 2.2.
8
Figure 2.2: Suspension Deflection Diagram
The suspension roll for the rear axle can be determined similarly but the j subscript is
denoted by an R for rear axle. The front and rear suspension roll values can be averaged for
a suspension roll calculation of the whole vehicle. It should also be noted that this method
captures the dynamics of the suspension relative to the sample time of the deflection sensors.
2.2.2 Scale Factor ?
Based on the vehicle suspension geometry, Eq. (2.1) will underestimate the true sus-
pension roll by a factor that can be determined experimentally [11]. Thus, Eq. (2.1) must
be multiplied by the scale factor ? to represent the true suspension roll.
?s = ? ??s (2.2)
Notethat?isaparameterassociatedwiththestaticgeometryofthevehicle?ssuspension
and therefore is vehicle suspension dependent. Initial results suggest ? is a constant value
unique to the vehicle and will not vary based on bank angle or other road inputs. This scale
factor can be determined by taking the ratio of the un-scaled suspension roll to the difference
of the true body roll and the bank angle, as shown in Eq. (2.3).
? = ?v??r??
s
(2.3)
9
In practice calculating ? is most easily performed on a flat surface with no bank angle.
When the bank angle (?r) is zero, the suspension roll (?s) becomes equal to the total roll
(?v). Note the total roll (?v) should be a direct measurement or an estimate with suitable
accuracy. Figure 2.3 shows the CarSim results of the vehicle performing a double lane
change on a flat surface. The effect of the scaling factor on the suspension roll estimate can
be seen in this plot. An average of Eq. (2.3) over the length of the run in Figure 2.3 was
used to calculate the ? value used in the corrected suspension roll. Although the un-scaled
suspension roll captures the shape of the vehicle roll, it under-predicts the magnitude. After
scaling the suspension roll closely matches the total roll as expected. Further details on how
the simulation was conducted is presented in the Section 4.3.
9 10 11 12 13 14 15 16 17 18 19?2
?1.5
?1
?0.5
0
0.5
1
1.5
2
?=1.736
Speed=70.0m/s
Time(s)
Roll
Angl
e(de
g)
??sScaledwith?Equals:?s
?r:Truth?r:Calculated
?v:Truth??s:RawMeasurement
?s:Scaled
Figure 2.3: Simulated Vehicle Performing a Double Lane Change Maneuver on a Flat Surface
Using the suspension deflections as the sole estimation of roll becomes problematic when
there are additional inputs into the suspension, such as bumps or if the vehicle is traveling
on a banked road. For a banked road, a method of measuring or estimating the total roll of
the vehicle must be used to decouple the roll due to bank and the roll due to dynamics.
10
2.3 Conclusion
If the road surface is a assumed to be flat, a low fidelity roll model can be used in
conjunction with susension deflection measurements to calculate the roll of a vehicle. The
roll scaling parameter eta (?) is needed to account for the effects of the suspension geometry
on the deflection measurements. It is possible to calculate the parameter ? experimentally
if a perfectly flat surface is available. For banked road surfaces the roll calculated from
suspension deflections will be corrupted due to the bank disturbance input to the suspension.
A method for measureing or estimating the total roll is needed to decouple the roll from bank
and the roll due to dynamics of the vehicle.
11
Chapter 3
The Eta (?) Parameter for Roll Scaling
3.1 Introduction
The ? parameter is a peculiar characteristic of the relative roll equation. The dynamic
characteristics of the ? parameter were studied by altering the vehicle mass, location of the
CG and inertial properties of a simulated vehicle. It is unusual that a constant parameter
can correct for the errors from the suspension deflections. The nature and characteristics of
this scale factor, ?, is discussed in this chapter.
3.2 The Effect of Speed on ?
A manuever called the double lane change (DLC) is the industry standard for testing
dynamic roll properties for various vehicles. Figure 3.1 reprents the ISO 3888-2 standard for
a DLC [12]. Observe that the width of the starting lane and intermediate lane are function
of the vehicle width.
Figure 3.1: Double Lane Change ISO 3888-2
12
Individual double lane change runs were created in CarSim at increments of 10 km/hr
from 10 km/hr to 250 km/hr on a flat surface. Eq. (2.3) was then used to calculate the
average ? value for each run. For the ? calculation, only the peaks of the Euler roll and
suspension roll were used. The average ? value across all speeds was then calculated to be
1.736. The slower speeds seem to generate lower? values, but the values still remain grouped
close to the average.
0 50 100 150 200 2501.7
1.71
1.72
1.73
1.74
1.75
1.76
1.77
1.78
1.79
AVG=1.736
Speed(km/hr)
?,Et
a?Un
itless
?(ra
d/ra
d)
Velocity&? ?Eta
Figure 3.2: ? Values for DLC on Flat Surface (10 km/hr to 250 km/hr)
The average ? was then applied to each of the runs for 10 - 250 km/hr. The root mean
squared (RMS) error of the bank estimate was then calculated at each speed as shown in
Figure 3.3. The plot of RMS error shows a trend of increasing RMS as speed increases, but
the RMS remains small relative to the roll angles, which are near 8 and 9 deg at the higher
speeds.
13
0 50 100 150 200 2500
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
?=1.736
Speed(m/s)
?rR
MSE
rror
(deg)
Velocityvs.BankRMSError?
rRMSError
Figure 3.3: RMS Error for DLC on Flat Surface vs. Speed
3.3 The effect of Mass and CG height on ?.
InCarSim, apayloadboxwasusedtochangetheeffectiveCGheightofthesprungmass,
i.e. the vehicle body, of CarSim?s small SUV. Initially the box has dimensions of 1.2m x 0.9m
x0.3m (L x W x H) where the length is parallel to the wheel base and the width is parallel
to the track width of the vehicle. With a mass of 250 kg, the roll, pitch and yaw inertia are
18.75 kg?m2, 31.87 kg?m2,46.87 kg?m2 respectively. The location of the payload?s center of
mass was placed directly on the vehicle center of mass which lies in the center of the vehicle
from driver side to passenger side, 0.88 m back from the front axle to the rear and 0.64
meters off the ground. The height of the payload was then altered according to Table 3.1
and a double lane change was performed at 50 km/hr, 115 km/hr and 140 km/hr. Note that the
payload height effectively changes the CG height.
14
Table 3.1: Payload Height
Height Above CG Units
-150 mm
150 mm
640 mm
750 mm
1.05 m
1.35 m
1.65 m
1.95 m
2.25 m
2.55 m
2.85 m
As shown on Figure 3.4 the average ? parameter across all the CG height runs was
1.871. It seems that adding weight increases ?, especially at payload heights below and near
the CG. When the average of 1.871 is used on the same payload runs, the RMS remains
below 2 degrees except when wheel lift off occurs in the runs with a payload height greater
than 2 m for speeds of 115 km/hr and 140 km/hr as shown in Figure 3.5. Wheel lift off was
determined to be when the suspension defelction measurement reached its maximum value.
Note the data for the ? calculation in which a suspension deflection measured full extension
were not used. However deflections near full extension could explain the increase in ? after
2 m payload height for speeds of 115 km/hr and 140 km/hr.
15
?0.5 0 0.5 1 1.5 2 2.5 31.8
1.85
1.9
1.95
2
2.05
2.1
2.15
AVG=1.871
PayloadHeight(m)
?,Et
a?Un
itless
?(ra
d/ra
d)
CGHeight&?
50km/hr115km/hr
140km/hr
Figure 3.4: ?Values for DLC on Flat Surface (Different Payload Heights)
?0.5 0 0.5 1 1.5 2 2.5 30
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
0.45
0.5
?=1.871
PayloadHeight(m)
?rR
MSE
rror
(deg)
CGHeightvs.BankRMSError50km/hr
115km/hr140km/hr
WheelLiftOffOccured
Figure 3.5: RMS Error for DLC on Flat Surface vs. Payload Height & Speed
Next, the 250 kg payload was positioned at the vertical height of the CG and then
moved towards the rear in the increments found in Table 3.2. Figure 3.6 confirms that at
lower speeds a payload affects the ? more than at higher speeds. However, the longitudinal
16
offset does not have much affect on ?. Figure 3.7 shows that using the average of 1.873
produces minimal RMS errors less than 0.1 deg across the runs.
Table 3.2: Payload Longitudinal Displacement
Longitudinal Displacement From CG to Rear Units
10 mm
50 mm
150 mm
500 mm
750 mm
1 m
1.5 m
2 m
3 m
0 0.5 1 1.5 2 2.5 31.84
1.86
1.88
1.9
1.92
1.94
1.96
AVG=1.875
PayloadDisplacement(m)
?,Et
a?Un
itless
?(ra
d/ra
d)
CGLongitudinalDisplacement&?
50km/hr115km/hr
140km/hr
Figure3.6: ?ValuesforDLConFlatSurface(DifferentPayloadLongitudinalDisplacements)
17
0 0.5 1 1.5 2 2.5 30
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
0.18
?=1.875
PayloadDisplacement(m)
?rR
MSE
rror
(deg)
PayloadDisplacementvs.BankRMSError
50km/hr115km/hr
140km/hr
Figure 3.7: RMS Error for DLC on Flat Surface vs. Payload Longitudinal Offset & Speed
3.4 ? as the Kinematic Suspension Ratio
In practice, both an accurate measurement of total roll, ?v, and a flat surface may
not be available. These issues present a challenge to finding the ? factor. However an
alternate method is available which uses measurements from the suspension links. Known as
the kinematic suspension ratio, the method relates the vertical deflection of the suspension
spring and damper to the vertical deflection that is experienced at the tire through Eq.
(3.1). Figure 3.8 shows the geometry vertical deflections of the suspension component and
the vertical deflection at the tire.
cos
parenleftBigg ?Z
a+b
parenrightBigg
= cos
parenleftBigg?L
a
parenrightBigg
(3.1)
?Z
a+b ?
?L
a (3.2)
18
After assuming small angles the expression for the vertical deflection of the tire becomes:
?Z = (a+b) ?La (3.3)
?Z = ?r?L (3.4)
The ?r scaling factor is thus the ratio of the upper A-arm length, (a+b), to the length from
the upper A-arm link and vehicle frame joint to the point where the suspension component
attaches to the upper A-arm link joint, a as seen in Eq.(3.5).
?r = (a+b)a (3.5)
Figure 3.8: Suspension Diagram
The scale factor ?r should be applied directly to the suspension deflections prior to the
relative roll calculations as shown in Eq. (3.6). However, suspension roll on vehicles does
not exceed the small angle approximation. Thus the scaling method from Section 2.2.2and
19
the kinematic suspension ratio method are roughly equivalent for small angles. And, ?r is
equivalent to ? from Eq. (2.2) and Eq.(2.3).
?s = sin?1
parenleftBigg?
r (?LLF??LRF + ?LLR??LRR)
2tw
parenrightBigg
(3.6)
Therefore, the scaling method in Section 2.2.2 was used for the CarSim simulations in which
no physical measurements of the suspension links exist to be measured. The kinematic
suspension ratio method was used to find ? for the experimental data with the Prowler
ATV, because the suspension could be directly measured and a sufficiently flat surface could
not be found.
3.5 Conclusions
The effects of speed and payload loadings on the derivation of the? factor were explored.
The ? factor was found to vary but the resulting errors in the bank estimate were small.
Practical issues such as finding a completely flat surface and the sensor cost of measuring
the total roll with an expensive device like a three antenna GPS receiver were found to be
non-issues with the kinematic suspension ratio. The kinematic suspension ratio was found
to be equivalent to ? described in Section 2.2.2.
20
Chapter 4
Road Bank Estimation using Suspension Deflections and a Cascaded Navigation EKF
4.1 Introduction
This chapter begins with the thorough description of the navigation filter methodology.
The navigation filter methodology covers the topics of inertial navigation, the formulation
of the Kalman filter (KF) to blend inertial and GPS navigation, the extension to nonlinear
system equations made possible by the extended Kalman filter (EKF), and the cascaded
integration of the navigation EKF and relative roll measurement. The cascaded navigation
EKF with relative roll measurement is then simulated to validate its efficacy at estimating
road bank.
4.2 Methodology
The EKFs used in this work utilize the methods from [13] with supplemental references
to [14]. The navigation filters are error state filters, which means that the error states are
added to the actual states during each measurement update. The filters are constructed
with a six degree of freedom IMU time update and a GPS measurement update.
The filter described in detail below is used to blend global navigation satellite system
(GNSS) solutions with inertial navigation system (INS) solutions. GNSS is comprised of
the constellations of satellites designed to provide positioning and timing information for
users on Earth or in space. The global positioning system (GPS) is the mostly widely used
GNSS currently and was designed and produced by the U.S. Department of Defense. Inertial
navigation is a method that utilizes a history of acceleration and angular velocity measure-
ments to determine the current position of the user or object. Using kinematic relationships
21
the acceleration measurements, resolved in the appropriate navigation coordinate frame, are
integrated once to yield velocity. Note the navigation coordinate frame is determined from
angular velocity measurements. The velocity is then integrated again to determine position.
Acceleration is measured using an accelerometer and angular velocity is measured using
a rate gyro. When these sensors are combined they are called inertial sensors. Typically
three orthogonal axes of accelerometers and three orthogonal axes of gyros are combined to
measure the full vector of acceleration and angular velocity. This six-axis setup is called an
inertial measurement unit (IMU). The combination of an IMU, navigation algorithms and
the computer that executes the algorithms is termed the inertial navigation system (INS)
[13] .
4.2.1 Coordinate Frames
Inertial navigation algorithms use a few coordinate frames. The Earth-centered inertial,
ECI, used in navigation applications is an inertial reference frame that does not accelerate
or rotate with respect to the rest of the universe [14]. The origin of the ECI frame is at the
center of mass of the Earth with the z-axis collinear with the average spin axis of the Earth.
The ECI frame does not rotate or accelerate with the Earth. The ECI frame is denoted
by the symbol i as a superscript or a subscript. The axes of the IMU form the body fixed
frame. The assumption is made that the x, y and z axes align with the front, right and down
axes of the vehicle [13]. The body frame is denoted by the symbol b. The Earth frame or
Earth-centered Earth-fixed (ECEF) is similar to the ECI frame. However, the ECEF frame
remains fixed to the earth and thus spins as the earth spins. The x-axis points from the
center of the Earth to the intersection of the equator, 0 deg latitude, and the conventional
zero median (CZM) that defines 0 deg longitude. The z-axis points from the center towards
the true North Pole, and the y-axis points from the center to the intersection of the equator
and 90 deg longitude east completing the orthogonal triad. The ECEF frame is denoted with
the symbol e. The local navigation frame is a geodetic or geographic frame and is denoted
22
with the symbol n. The origin of the body frame is termed the ?point of interest? which is
typically the center of mass of the vehicle. The z-axis is referred to as the down axis and is
normal to the surface of the reference ellipsoid and points towards the center of the Earth.
The x-axis is the axis that points north in the plane orthogonal to the z-axis and is also
called the North axis. The y-axis or east axis always points east [14].
4.2.2 Inertial Navigation
The velocities and angular rate measurements of an IMU can be used for stand alone
navigation for short durations of time. This type of navigation solution is called dead-
reckoning. How long a dead-reckoning solution is suitable for use depends on the quality
of the accelerometers and the gyros in the IMU and the knowledge of corresponding biases
in each axis. The suitable length for dead-reckoning also depends on the application. For
example, given both applications share the same IMU the allowable duration of error growth
that occurs in an autonomous vehicle for lane following is different from an autonomous geo-
physical grid survey application where path errors less than 20 cm need to be maintained.
In brief, integrating the angular rates yields attitude updates, the first integration of the ac-
celerations produces velocities and positions are acquired from integration of these velocities
or the second integration of the accelerations. The integrated noise and bias degrades the
solution with each subsequent integration. However, an integrated navigation system can
provide corrections to the IMU outputs and the inertial navigation solutions from estimates
of IMU bias that can be determined with the GNSS/INS integration algorithm [13]. This
method is discussed in Section 4.2.5.
4.2.2.1 Mechanization Equations
The equations of motion expressed in the north-east-down (NED) local navigation frame
are:
23
?p = Tvn (4.1)
?v =Cnbfb?[(2?nie + 2?nen)?] vn + gn (4.2)
?Cnb = Cnb parenleftBigbracketleftBigparenleftBig?bibparenrightBig?bracketrightBig?bracketleftBigparenleftBig?binparenrightBig?bracketrightBigparenrightBig (4.3)
Note that ?cba denotes the rotation rate of frame b relative to frame a parameterized
in frame c, thus ?nib is the rotation rate of the inertial frame, or the ECI frame, relative to
the body frame, or the vehicle/IMU frame, parametrized in the local navigation frame. The
vector p is the position of interest, which is the user, expressed in the geodetic coordinates
of latitude (L), longitude (?) and altitude (h).
p =
?
??
??
??
?
L
?
h
?
??
??
??
?
The vector v represents the NED components of the users velocity vN, vE and vD.
v =
?
??
??
??
?
vN
vE
vD
?
??
??
??
?
Local gravity is represented by the vector g. The matrix T is the transformation matrix that
converts the linear velocities of the NED frame to angular changes in latitude and longitude
of the local navigation frame.
T =
?
??
??
??
?
1
RN+h 0 0
0 1(R
E+h)cosL
0
0 0 ?1
?
??
??
??
?
(4.4)
24
The values of RN and RE are the radii of curvature of the reference ellipsoid for the
north/south (meridian) and the east/west (prime vertical) respectively [14]. These radii
are functions of latitude and are computed using standard ellipsoid models of the Earth?s
surface. In [13] the radii of curvature are given as:
RN (L) = Ro (1?e
2)
(1?e2 sin2L)3/2 (4.5)
RE (L) = Ro?1?e2 sin2L (4.6)
where Ro, the equatorial radius, and e, the eccentricity of the ellipsoid are:
Ro = 6,378,137.0 m
e = 0.0818191908425
The matrix Cnb is the rotation matrix. It is of the form Cba such that the matrix rotates
from frame a to frame b. Thus Cnb completes the angular rotation from the body frame to
the local navigation frame. The notation [(?cba)?], used in [13], is the skew symmetric matrix
of the vector ?cba =
bracketleftbigg
??cba ??cba ??cba
bracketrightbiggT
and is defined as:
[(?cba)?] =
?
??
??
??
?
0 ??cba,3 ?cba,2
?cba,3 0 ??cba,1
??cba,2 ?cba,1 0
?
??
??
??
?
=
?
??
??
??
?
0 ???cba ??cba
??cba 0 ???cba
???cba ??cba 0
?
??
??
??
?
(4.7)
25
In this work attitude is parametrized using Euler angles. Specifically the aerospace
sequence of yaw, pitch and roll (?, ? and ?) are used. Accordingly, the attitude vector is:
?nb =
?
??
??
??
?
?nb
?nb
?nb
?
??
??
??
?
(4.8)
4.2.2.2 Update Equations
Assuming the initial position is known three steps are needed to implement and solve
the mechanization equations. The steps include an attitude update, a velocity update and
a position update. The attitude update uses the gyro measurements from the IMU. These
gyro measurements form an angular velocity vector of the body frame relative to an inertial
frame of reference, ?bib. For attitude determination, the attitude of the body frame relative
to the navigation frame and expressed in the body frame, ?bnb, is needed. To obtain ?bnb
the rotation rate of the navigation frame relative to the inertial frame expressed in the body
frame,?bin, is subtracted from?bib, the angular rate of the body frame relative to the inertial
frame.
?bnb = ?bib??bin (4.9)
Note that?bin is essentially the rotation of the Earth. When using low cost gyros the rotation
rates from Earth are often less than the noise sensitivity of the gyros such that the following
approximation can be made.
?bnb??bib (4.10)
The Euler rates are then computed from the angular velocity vector as:
??nb = F(?nb)?bnb (4.11)
26
where
F(?nb) = 1cos?
nb
?
??
??
??
?
1 sin?nb sin?nb cos?nb sin?nb
0 cos?nb cos?nb ?sin?nb cos?nb
0 sin?nb cos?nb
?
??
??
??
?
(4.12)
For consumer grade and automotive gyros Euler integration of Eq.(4.11) is sufficient to
generate an estimate of attitude.
?
??
??
??
?
?nb
?nb
?nb
?
??
??
??
?
k+1
=
?
??
??
??
?
?nb
?nb
?nb
?
??
??
??
?
k
+?F(?nb (tk))?bnb (tk) (4.13)
In Eq. (4.13) tk is an instance in time at step k and ? is the change in time such that
? = tk+1?tk. The attitude estimate ?nb (tk) =
bracketleftbigg
?nb ?nb ?nb
bracketrightbiggT
k
is used for the rotation
matrix Cnb thatrotatesvectorsfromthebodyframetothenavigationframeintheproceeding
velocity and position updates. The Cnb transformation matrix is populated as:
Cnb =
?
??
??
??
?
cos?nb sin?nb 0
?sin?nb cos?nb 0
0 0 1
?
??
??
??
?
k
?
??
??
??
?
cos?nb 0 ?sin?nb
0 1 0
sin?nb 0 cos?nb
?
??
??
??
?
k
?
??
??
??
?
1 0 0
0 cos?nb sin?nb
0 ?sin?nb cos?nb
?
??
??
??
?
k
(4.14)
The acceleration of the vehicle in the navigation frame is a function of the specific force
measurement vector from the IMU, fb =
bracketleftbigg
fx fy fz
bracketrightbiggT
, the angular rate of the earth,?nie, the
transport rate, ?nen, and the gravity vector, gn, and is given by:
?vn = Cnbfb?[(2?nie +?nen)] vn + gn (4.15)
27
At time stepk the following relationships are used to calculate the earth rate, transport rate
and the gravity vector:
?nie|k= 7.292115?10?5
?
??
??
??
?
cosL
0
sinL
?
??
??
??
?
k
(4.16)
?ken|k=
?
??
??
??
?
vE
RE+h |k
? vNR
N+h
|k
vE tanL
RE+h |k
?
??
??
??
?
(4.17)
g|k=
?
??
??
??
?
0
0
g|k
?
??
??
??
?
(4.18)
g|k?9.78032533591 + 1.931853?10
?3 sinLk
?1?e2 sinL
k
(4.19)
Again e is the eccentricity of the reference ellipsoid. For low cost IMUs the measurement
errors overpower the Coriolis effect such that the acceleration can be simplified to
?vn?Cnbfb + gn (4.20)
Applying Euler integration to Eq.(4.15) yields
?
??
??
??
?
vN
vE
vD
?
??
??
??
?
k+1
=
?
??
??
??
?
vN
vE
vD
?
??
??
??
?
k
+? ?vn (4.21)
The position update requires applying Euler integration to Eq. (4.1) which yields
Lk+1 = Lk +?
parenleftbigg v
n
RN +h
parenrightbigg
|k (4.22)
28
?k+1 = ?k +?
parenleftBigg v
E
(RE +h) cosL
parenrightBigg
|k (4.23)
hk+1 = hk +? (?vD)|k (4.24)
It is important to remember that precision of latitude and longitude at the level of
meters represented in the local navigation frame will require seven or more significant digits
(1 m?1.6e?7 rad).
4.2.2.3 Error Equations
The manner in which errors propagate through an INS is useful for navigation because
the mechanization equations, Eq. (4.1)-(4.3), represent the ideal case. Perturbation analysis
of the mechanization equations yields the first-order Taylor series expansion of the mecha-
nization equations:
??p = Tprime?pn + T?vn (4.25)
??vn =
bracketleftBigparenleftBig
Cnbfb
parenrightBig
?
bracketrightBig
??nnb + Cnb?fb?[2 (?nie +?nen)?]?vn?[(2??nie +??nen)?] vn +?gn (4.26)
? ??nnb??[(?nin)?]??nnb +??nin?Cnb??bib (4.27)
Error is represented by?, and the matrix Tprime is the time derivative that relates positional
errors into corresponding velocities. In practice the negative skew of the transport rate,
?[(?nen)?], can be used as Tprime. The error in gravity is often approximated as a function of
the magnitude of gravity at the users latitude and the error in altitude.
?gn =
?
??
??
??
?
0
0
?2gRo?h
?
??
??
??
?
(4.28)
Note that ??nnb, the attitude errors, are the errors resolved about the NED frame not errors
in the Euler angles themselves. Also, note that?nin is equal to the sum of the earth rate and
29
transport rate.
?nin = ?nie +?nen (4.29)
The variables??bib and?fb are the errors from the accelerometer and rate gyro output errors.
These errors ultimately determine the performance of inertial navigation. Simple models of
the accelerometer and gyro are:
fb = fbt + Mafbt + bba + wba (4.30)
?bib = ?bibt + Mg?bibt + bbg + wbg (4.31)
The subscripts a and g denote terms from the accelerometer and gyro respectively. The
subscripttrepresents the true value that is being measured and the superscriptbmeans that
the term is expressed in the body frame. The matrices Ma and Mg account for the effects
of scale factors and any non-orthogonality errors. The b term is the bias vector and the w
term is the uncorrelated output noise vector. The bias vector and the uncorrelated noise
form the error model for the accelerometer and gyro, ?fband ??bib used in Eq. (4.25)-4.27.
?fb = bba + wba (4.32)
??bib = bbg + wbg (4.33)
These errors limit the performance of inertial navigation. Because they are integrated
twice accelerometer errors cause position errors to grow as a function of the time squared.
Position errors from angular velocity will grow as a function of the time cubed. Including
accurate position and velocity updates will help to mitigate the error growth problem [13].
30
4.2.3 Integration of Inertial Navigation and GPS
By themselves, INS and GPS are limited in their capabilities. However, when integrated
together they provide navigation solutions that perform better than the standalone solution
from each. The INS can provide solutions at high frequencies of 50 Hz or more. The GPS
navigation solution is typically 1-20 Hz. The errors of the GPS solution are bounded while
the INS errors grow as a function of time. Also, the INS provides an attitude solution while
a GPS receiver typically does not. When INS and GPS are fused together, the position and
velocity estimates from GPS are used to estimate and reduce the effect of the INS errors.
The INS produces attitude, velocity and position estimates at a high output rate and also
provides solutions during momentary GPS signal outages.
There are three main types of integration architectures: loose, tight and deep. Some-
times deeply integrated is also called ultra tight. This work focuses only on the loose
GPS/INS integration. Loose GPS/INS integration occurs at the PVT (position, velocity
and time) level. This means that the PVT estimates from GPS are blended with the PVA
(position, velocity and attitude) estimate of the INS or IMU. The loosely coupled architec-
ture can be open or closed loop. In the closed loop configuration the inertial sensor errors
estimated by the filter are fed back to the INS equations to compensate for the accelerometer
and gyro errors. If there is no feedback than the loosely coupled filter is an open loop archi-
tecture. The open loop form can be used when a high quality INS sensor is present. In this
work, an automotive grade INS is used. Therefore the loosely coupled filter with feedback is
utilized. The most common fusing filter used for loosely coupled GPS/INS is the extended
Kalman filter (EKF) [13] and is the filter of choice for this work.
4.2.4 Kalman Filter Overview
The primary assumption of the Kalman filter is that errors of the system that are
modeled are either systemic, white noise or Markov processes or any linear combination
of each. The algorithm estimates a set of parameters known as the state vector, x. The
31
actual Kalman filter estimate of the state vector is denoted ?x. A total-state implementation
estimates the absolute properties of the system. This work is mainly concerned with the
error-state implementation which is the estimation of the errors in a measurement made by
the system. The state vector residual, ?x, is the difference between the true state vector and
the estimated states.
?x = x??x (4.34)
For the error-state implementation the state vector residual consists of the errors that remain
in the system after the Kalman filter estimates have been used to correct the system.
The error covariance matrix, P, is the covariances and variances of the square of the
state vector residual. Specifically it is the expectation of the deviation of the state vector
estimate from the true value of the state vector.
P = E
parenleftBig
(?x?x) (?x?x)T
parenrightBig
= E
parenleftBig
?x?xT
parenrightBig
(4.35)
The diagonal of P contains the variances of each state estimate. The square roots of these
variances represent the uncertainty or standard deviation for each state. The off-diagonal
terms are the covariances which give information on the correlations between errors of the
individual states of the state vector. If state estimates become overly correlated then the
system becomes unobservable meaning there is not enough information to estimate the states
independently. In other words, the states have become too dependent on one another.
The state vector, x requires a-priori information for initialization. In the error-state
implementation, the state vector is typically initialized to zeros. The state vector of total-
state implementation is most often initialized with a best guess or previous estimates from
that last time the equipment was used. The covariance matrix, P, is initialized by the user
and indicates the confidence of the initial state vector, x.
The measurement vector is denoted as the vector y and is a set of measurements of the
system described by the state vector. It is comprised of a relating function h (x) and the
32
noise vector, wm.
y = h (x) + wm (4.36)
The measurement innovation, ?y?, is the difference between the true measurement
vector and the expected measurement vector, h (?x?), which is computed from the state
vector estimate prior to the measurement update.
?y? = y?h
parenleftBig
x?
parenrightBig
(4.37)
The measurement residual, ?y+, is the difference between the true measurement vector and
updated measurement vector, h (?x+).
?y+ = y?h
parenleftBig
x+
parenrightBig
(4.38)
Both the measurement innovations and the measurement residuals contain state estimate
errors and measurement errors that are assumed to be uncorrelated with the state estimates.
These measurement errors are also assumed to be characterized by a zero-mean Gaussian
distribution and are uncorrelated in time. The Kalman filter models the standard deviation
of these measurement errors with the measurement noise covariance matrix, R, which is the
expectation of the square of the measurement noise.
R = E
parenleftBig
wmwTm
parenrightBig
(4.39)
The diagonal terms of the measurement noise covariance matrix, R, are the variances of each
measurement. The off-diagonal terms are the covariance terms that represent the correlation
between different components of the measurement noise vector. Typically, the measurement
noises are assumed to be independent of each other, and R is a diagonal matrix.
The first steps of the Kalman filter algorithm involve propagating the system forward
in time which is the system update (also called the time-update or the time-propagation
33
phase.) The transition matrix, ?k?1, is a function of the system model and defines how the
state vector changes with time. The transition matrix is a function of the time step interval,
?s. If the transition matrix is composed of parameters that are functions of time then ?k?1
will have to be calculated for each iteration.
The system model assumes that the time derivative of each state is a linear function of
the other states and of white noise sources. The true state vector, x (t), at time, t, for the
Kalman filter is described by
?x (t) = F (t) x (t) + G (t) ws (t) (4.40)
where ws (t) is the system noise vector, F (t) is the system matrix and G (t) is the system
noise distribution matrix. Each source of noise is assumed to have a zero-mean Gaussian
distribution. The system matrix and system noise distribution matrix parameters must
both be derived from the properties and characteristics of the system being modeled. The
expectation value of the true state vector, x (t), is the estimated state vector, ?x (t). Taking
the expectation of Eq. (4.40) yields
E( ?x (t)) = ??t?x (t) (4.41)
E( ?x (t)) = F (t) ?x (t) (4.42)
Solving Eq. (4.42) gives
?x (t) = exp
?
??
t
t??s
F (tprime)dtprime
?
???x (t??
s) (4.43)
For the discrete Kalman filter the state vector estimate is modeled as a linear function of
the previous state vector value coupled with the state transition matrix, ?k?1, and is known
34
as the state propagation equation as shown in Eq. (4.44).
?x?k = ?k?1?x+k?1 (4.44)
Because the discrete and continuous forms of Kalman filter are equivalent the state transition
matrix is defined as
?k?1?exp (Fk?1?s) (4.45)
The system noise covariance matrix, Qk?1, represents how the uncertainties of the state
estimates increase with time due to noise sources such as unmeasured dynamics and sensor
noise. It too, is a function of the time step interval, ?s, and is most often a diagonal matrix.
The simplest and most commonly used covariance propagation method is
P?k = ?k?1P+k?1?Tk?1 + Qk?1 (4.46)
The final steps in the Kalman filter algorithm are called the measurement update or
correction phase. The measurement matrix, Hk, defines how the measurement vector varies
with the state vector.
h
parenleftBig
x?k
parenrightBig
= Hkx?k (4.47)
The Kalman gain matrix is
Kk = PkHTk
parenleftBig
HkP?k HTk + Rk
parenrightBig?1
(4.48)
Note that the measurement matrix Hk can vary and must be calculated at each iteration.
The covariance matrix, Rk, may also vary as a function of kinematics or signal to noise
measurements in which case it too will need to be calculated at each iteration. The Kalman
35
gain matrix serves to weight the measurement information based on the uncertainties of
those measurements at the current iteration.
Next, the measurement vector, yk, must be populated with measurements. Then the
state vector, ?x?k, is updated with the measurement vector and the Kalman gain.
?x+k = ?x?k + Kk
parenleftBig
yk?Hk?x?k
parenrightBig
(4.49)
= ?x?k + Kk?y?k (4.50)
The final step of the measurement update is to update the error covariance matrix, P?k, with
Kalman gain and the measurement matrix.
P+k = (I?KkHk) P?k (4.51)
In summary the steps of the Kalman filter are:
1. Calculate the transition matrix, ?k?1
2. Calculate the system noise covariance matrix, Qk?1
3. Propagate the state vector estimate from x+k?1 to x?k
4. Propagate the error covariance matrix from P+k?1 to P?k
5. Calculate the measurement matrix, Hk?1
6. Calculate the measurement noise covariance matrix, Rk
7. Calculate the Kalman gain matrix, Kk
8. Formulate the measurement, yk?1
9. Update the state vector estimate from x?k to x+k
10. Update the error covariance matrix from P?k to P+k
36
A closed loop implementation is common for an error state Kalman filter. The reason for
feedback is to minimize the effect of neglected higher order terms from a linearized system
model. Feedback of some or all states keeps the the filter states small. It is advantageous
to feedback the states immediately following the measurement update, Eq. (4.50). Doing so
produces zero state estimates at the start of the state propagation step, Eq. (4.44), which
implies that the state propagation phase can be omitted. However, the error covariance
matrix, P, decribed by Eq. (4.46) will still need to be propagated [14].
4.2.5 Extended Kalman Filter (EKF) Overview
In general a Kalman filter consists of a system model and a measurement model. The
system model is a representation of how the system states change with respect to time. The
measurement model describes how the observation measurements relate to the estimated
states. Specifically the extend Kalman filter (EKF) is more apt to handle nonlinear system
models.
In the extended Kalman filter the system matrix, F, and the measurement matrix, H,
are replaced by nonlinear functions of the state vector f (x) and h (x). The EKF system
dynamic model is accordingly given by
?x (t) = f (x (t)) + G (t) ws (t) (4.52)
where f (x) is a nonlinear function of the state vector. The state vector propagation is then
calculated by
?x?k = ?x+k?1 +
t
t??s
f (?x,tprime)dtprime (4.53)
For the EKF, the assumption is made that the error in the state vector estimate is much
smaller than the state vector. This enables a linear system model to be applied to the state
vector residual. The error state EKF dynamic model is defined by
37
??x (t) = F (t)?x (t) + G (t) ws (t) (4.54)
The standard Kalman filter error covariance propagation equation
P?k = ?k?1P+k?1?Tk?1 + Qk?1 (4.55)
may be used with the transition matrix linearized about the state vector estimate using
?k?1 t exp (Fk?1?s) (4.56)
where Fk?1is computed by
Fk?1 = ?f (x)?(x) |x=?x+
k?1
(4.57)
The measurement model of the EKF is
y (t) = h (x (t)) + wm (t) (4.58)
where h is a nonlinear function of the state vector. The state vector is updated with the
true measurement vector using
?x+k = ?x?k + Kk
bracketleftBig
yk?h
parenleftBig
?x?k
parenrightBigbracketrightBig
= ?x?k + Kk?y?k (4.59)
where the measurement innovation is
?y?k = yk?h
parenleftBig
?x?k
parenrightBig
= h (xk)?h
parenleftBig
?x?k
parenrightBig
+ wmk (4.60)
When the state vector estimate has converged to the true state vector values the mea-
surement innovations will be small. Thus the measurement innovation can be modeled as a
38
linear function of the state vector even though the full measurements cannot. The measure-
ment innovation then becomes [14]
?y?k = Hk?x?k + wmk (4.61)
where
Hk = ?h (x)?(x) |x=?x?
k?1
= ?y (x)?(x) |x=?x?
k?1
(4.62)
As a result of the linearization of F and H, the error covariance matrix, P, and Kalman
gain, K, are functions of the state estimates. Thus, the linearization can cause stability
issues as the EKF is more sensitive to the tuning of the initial covariance matrix than a
standard Kalman Filter [15].
4.2.6 Navigation Error State EKF with Closed Loop Feedback
The system model for navigation states consist of the mechanization equations, Eq.
(4.1)-(4.3), which are nonlinear, and the models for the IMU biases. The linearized error
state formulation of the mechanization equations was discussed as an item of interest of
the stand alone inertial navigation algorithm and formed Eq. (4.25)-(4.27). While the INS
states are error states, the remaining states are the full bias states of the accelerometer and
gyro from the IMU. Accordingly for the cascaded navigation EKF the error state vector is
defined as
x =
?
??
??
??
??
??
??
??
?
?p
?vn
??nb
bba
bbg
?
??
??
??
??
??
??
??
?
(4.63)
39
Remember that the position vector p is a vector of latitude, longitude and altitude, vn
is a velocity vector of North, East and Down velocities, and ?nb is an attitude vector of
roll, pitch, and yaw. The two bias vectors are included to account for the biases present in
the IMU where bba is comprised of the three bias states corresponding to the accelerometer
biases and similarly bbg is a comprised of the gyro biases.
bba =
?
??
??
??
?
bbax
bbay
bbaz
?
??
??
??
?
bbg =
?
??
??
??
?
bbg?
bbg?
bbg?
?
??
??
??
?
The IMU model consists of random noise and a bias added to the accelerations and angular
velocities. Thebiasesaremodeledasrandomwalk. Specificallythebiasmodelsforalow-cost
inertial IMU are
bba (t) = bbas + bbad + wba (4.64)
bbg (t) = bbgs + bbgd + wbg (4.65)
Note that the biases are a function of time with some components that are modeled as
stochastic processes. Both the accelerometer biases and the gyro biases are the sum of
three random processes. The terms bbas and bbgs represent the constant null shifts of the
accelerometer and gyro. They are essentially static biases that are modeled as random
constants. The terms bbad and bbgd are the dynamic ?in-run? biases. These two time varying
components are modeled as an exponentially correlated random process, i.e. a first order
Gauss-Markov process specifically with a standard deviation ?ad for the accelerometer and
40
?gd for the gyro. The Gauss-Markov process for the accelerometer and gyro bias models are
dbbad
dt =
?bbad = ?1
?a I3?3b
b
ad + I3?3?
b
a (4.66)
dbbgd
dt =
?bbgd = ?1
?g I3?3b
b
gd + I3?3?
b
g (4.67)
As discussed in Section 4.2.5, the system matrix F is the Jacobian of the system model
equations which are Eq. (4.25)-(4.27), (4.66)-(4.67). For clarity the system model equations
are listed again
??p =?[(?nen)?]?pn + T?vn
??vn =
bracketleftBigparenleftBig
Cnbfb
parenrightBig
?
bracketrightBig
??nnb+Cnb
parenleftBig
bba + wba
parenrightBig
?[2 (?nie +?nen)?]?vn?[(2??nie +??nen)?] vn+?2gR
o
?h
? ??nnb??[(?nie +?nen)?]??nnb +??nin?Cnb
parenleftBig
bbg + wbg
parenrightBig
?bbad = 1
?aI3?3b
b
ad + I3?3?
b
a
?bbgd = 1
?gI3?3b
b
gd + I3?3?
b
g
The Jacobian of the system model equations yields the system model matrix F
F =
?
??
??
??
??
??
??
??
?
?[(?nen)?] T 03 03 03
Gg ?[2 (?nie +?nen)?]
bracketleftBigparenleftBig
Cnbfb
parenrightBig
?
bracketrightBig
Cnb 03
03 03 ?[(?nie +?nen)?] 03 ?Cnb
03 03 03 ?1?a I3 03
03 03 03 03 ?1?g I3
?
??
??
??
??
??
??
??
?
(4.68)
41
where 03 is a 3 by 3 null matrix, I3 is a 3 by 3 identity matrix and
Gg =
?
??
??
??
?
0 0 0
0 0 0
0 0 ?2gRo
?
??
??
??
?
(4.69)
For small propagation times the F matrix can be simplified to
F =
?
??
??
??
??
??
??
??
?
03 T 03 03 03
Gg 03
bracketleftBigparenleftBig
Cnbfb
parenrightBig
?
bracketrightBig
Cnb 03
03 03 03 03 ?Cnb
03 03 03 ?1?a I3 03
03 03 03 03 ?1?g I3
?
??
??
??
??
??
??
??
?
(4.70)
The corresponding process noise, ws, of the system is
ws =
?
??
??
??
??
??
??
??
?
03?1
Cnbwa
Cnbwg
Cnb?a
Cnb?g
?
??
??
??
??
??
??
??
?
(4.71)
The process noise is used to define the process covariance matrix, Q. Taking the expec-
tation of the system noise vector defines the diagonal terms of Q as power spectral densities.
The expectations, E
braceleftBig
wawTa
bracerightBig
and E
braceleftBig
wgwTg
bracerightBig
are the accelerometer and gyro noise power
spectral densities or standard deviations. According to [14], the process noise of the bias
states, ?a and ?g are 2?2ad/?a and 2?2gd/?g respectively. The terms ?a and ?g are the time con-
stants for the Markov process error model for acceleration and gyro errors respectively. The
discrete process noise matrix, Qk, is calculated from the continuous process noise matrix,
Q, using the time interval ?s = tk+1?tk.
42
Qk = ?sQ (4.72)
Neglecting any lever arm effects (i.e. the kinematic displacement between the center of
gravity and the sensor location) the measurement matrix H is
H =
?
??
?
I3 03 03 03 03
03 I3 03 03 03
?
??
?
6?15
(4.73)
For the closed loop error state navigation EKF, all the states except the attitude states
of the state vector, x, need to be feedback following the measurement update prior to the
mechanization phase.
x+k+1 = x+k+1??x+k+1 (4.74)
The attitude errors, ?nnb, are corrections to the rotation matrix, Cnb, and therefore cannot
simply be added to the previous Euler angle estimates. The attitude states are updated by
executing the following equations:
Cn+b = (I3 + [??nnb?]) Cn?b (4.75)
The Euler angles are then extracted from Cn+b by:
? = tan?1
parenleftbiggc
32
c33
parenrightbigg
(4.76)
? =?sin?1c31 (4.77)
? = tan?1
parenleftbiggc
21
c11
parenrightbigg
(4.78)
where cij represents the element in the ith row and jth column of the rotation matrix Cn+b .
Once the update is complete the Kalman filter state vector, ?x+k+1 is reset to zero and P+k+1
is used to initiate another time update cycle [13].
43
4.2.7 NAV EKF Cascaded with Relative Roll
In lieu of using a two or three antenna GPS attitude measurement the vehicle roll can
alternatively be estimated using a single antenna GPS/INS extended Kalman filter (EKF).
This method provides an estimate of the attitude of the vehicle body in an absolute sense
based on the kinematic relationships between positions and velocities measured from a single
antenna GPS and the accelerations and roll rates measured from an inertial measurement
unit (IMU). The EKF is used for sensor fusion of the GPS and IMU for the purpose of
obtaining an estimate/observation of total roll which is not measured.
The difference between the total roll (?v) and the relative roll (?s) of the suspension
deflections is the road bank.
?r = ?v??s (4.79)
Since the suspension roll can be determined from Eq. (2.2) we can subtract it from the total
body roll estimated by the EKF to yield an estimate of road bank:
?r,est = ?v,EKF??s (4.80)
The suspension deflections and the total roll capture the dynamic motions so the dy-
namics of the vehicle body will subtract out assuming the tires remain in contact with the
ground. Note that errors in the estimate of road bank will be related to the accuracy of the
total roll estimate, ?v, and how well the suspension scaling parameter,?, scales the relative
roll measurement, ?s .
4.3 Simulations
In order to validate the presented methodology, vehicle simulations were conducted us-
ing the commercial software CarSim. The software, developed by Mechanical Simulation
Corporation, provides a high fidelity simulation environment, and allows the user to view
44
a number of vehicle outputs. The software has been accepted by industry as accurately
representing vehicle dynamics. CarSim data is often used as a truth measurement in simu-
lation. The vehicle used in the simulations was the small SUV in CarSim which has vehicle
parameters typically associated with the small SUV class of vehicle. The parameters for the
small SUV are provided in Table A.1of Appendix A.
Since the data used is from CarSim, it is necessary to emulate the results which would be
expected from an actual GPS receiver and IMU. Therefore CarSim positions and velocities
are corrupted based on the typical noise variances associated with each of the measurements.
To emulate the IMU, noise variances and biases were added to the accelerations and gyro
rates from CarSim. The simulated IMU runs at 20 Hz and the GPS at 1Hz.
4.3.1 Navigation EKF Tuning
For the sensor model shown in Section 4.2.6tThe noise, bias and random walk character-
istics used in the simulation are given in Table 4.1. The time constants for the accelerometer
and gyro bias walk and the accelerometer and gyro measurements are representative of an
automotive grade IMU (specifically the CrossBow 440) and are shown in Table 4.2. The
values used for the initial process covariance matrix Q were the exact values from the sim-
ulation settings, while the values for the measurement covariance matrix R and covariance
matrix P are found in Table 4.3 and Table 4.4 respectively.
45
Table 4.1: Simulation Noise Settings
Name Value Units
?a 0.3 m/s2
?g 0.3 deg/s
?Vel 0.05 m/s
?ba 0.1 m/s2
?bg 0.05 deg/s
?L 0.000001 deg
?? 0.000001 deg
?h 1 m
?Def 0.0002 m
??s sin?1
parenleftBig4?
Def
2tw
parenrightBig
rad
Table 4.2: Time Constants
Name Value Units
?a 500 s
?g 1000 s
Table 4.3: Tuned Measurement Covariances
Name Value Units
?Pos. Horz. 0.0000052 deg
?Pos. Z 1 m
?Vel. Horz. 0.05 m/s
?Pos. Z 0.1 m/s
46
Table 4.4: Error Covariances for P
Name Value Units
?2L,?P 0.001 rad
?2hP 2.8 m
?2VelP 0.1 m/s
?2?,?P 0.01 rad
?2?P 0.05 rad
?2ba
P
0.000001 m/s2
?2bg
P
0.000001 rad/s
The process covariance matrix Q is:
Q =
?
??
??
??
??
??
?
?2accel 0 ??? 0
0 ?2gyro ... ...
... ... ?2
biasa 0
0 ??? 0 ?2biasg
?
??
??
??
??
??
?
12?12
(4.81)
where
?2accel = diag
parenleftbiggbracketleftbigg
?2a ?2a ?2a
bracketrightbiggparenrightbigg
(4.82)
?2gyro = diag
parenleftbiggbracketleftbigg
?2g ?2g ?2g
bracketrightbiggparenrightbigg
(4.83)
?2biasa = diag
parenleftbiggbracketleftbigg
2?2ba
?a
2?2ba
?a
2?2ba
?a
bracketrightbiggparenrightbigg
(4.84)
?2biasg = diag
parenleftbiggbracketleftbigg
2?2bg
?g
2?2bg
?g
2?2bg
?g
bracketrightbiggparenrightbigg
(4.85)
47
The measurement covariance matrix R is:
R =
?
??
??
??
??
??
?
?2Pos. Horz. 0 ??? 0
0 ?2Pos. Z ... ...
... ... ?2
Vel. Horz. 0
0 ??? 0 ?2Vel. Z
?
??
??
??
??
??
?
6?6
(4.86)
where
?2Pos. Horz. =
?
??
?
?2Pos. Horz. 0
0 ?2Pos. Horz.
?
??
? (4.87)
?2Vel. Horz. =
?
??
?
?2Vel. Horz. 0
0 ?2Vel. Horz.
?
??
? (4.88)
The covariance matrix P is:
P =
?
??
??
??
??
??
??
??
??
??
??
??
?
?2L,?P 0 ??? 0
0 ?2hP
?2VelP ... ...
... ?2
?,?P
... ?2
?P
?2ba
P
0
0 ??? 0 ?2bg
P
?
??
??
??
??
??
??
??
??
??
??
??
?
15?15
(4.89)
where
?2L,?P =
?
??
?
?2L,?P 0
0 ?2L,?P
?
??
? (4.90)
?2VelP =
?
??
??
??
?
?2VelP ??? 0
... ?2
VelP
...
0 ??? ?2VelP
?
??
??
??
?
(4.91)
48
?2?,?P =
?
??
?
?2?,?P 0
0 ?2?,?P
?
??
? (4.92)
?2ba
P
=
?
??
??
??
?
?2ba
P
??? 0
... ?2
baP
...
0 ??? ?2ba
P
?
??
??
??
?
(4.93)
?2bg
P
=
?
??
??
??
?
?2bg
P
??? 0
... ?2
bgP
...
0 ??? ?2bg
P
?
??
??
??
?
(4.94)
To demonstrate the performance of the navigation EKF, two cases are presented. Both
runs consist of seven laps around the the CarSim 6 road course track, as shown in Figure
4.1, with the CarSim stock small SUV. Noise is added to all signals. The first demonstrates
the estimation of accelerometer and gyro biases when Markov random walk and bias are
artificially added to each simulated measurement of the IMU. The second run is similar
except that in addition to the Markov random walk and bias on the IMU measurements
noise is also added.
?2 ?1.5 ?1 ?0.5 0 0.5 1 1.5 2 2.5 3x 10?3?1.5
?1
?0.5
0
0.5
1
1.5
2
2.5x 10?3
Latitude(deg)
Long
itude
(deg)
Position2D NoiseyCarsimReference
Estimate
Figure 4.1: Positions on Road Course with IMU Bias & Walk from CarSim Simulation
49
120 140 160 180 200 220 240
10
12
14
16
18
20
22
24
26
28 HorizontalVelocity
Time(s)
Veloc
ity(m
/s)
NoiseyCarSimReferenceEstimate
Figure 4.2: Velocity on Road Course with IMU Bias & Walk from CarSim Simulation
The position solutions and position errors are shown in Figures 4.3-4.4 for the first road
course run. Note that the navy dots are the corrupted CarSim value and are shown for
reference with the estimate in orange. The error plot shows the true uncorrupted signal
subtracted from the EKF estimate. Table 4.5 shows the root mean squared error for the
errors in latitude, longitude, and altitude. For latitude and longitude the table gives the
RMS in the meter equivalent.
Table 4.5: Root Mean Squared Position Error
RMS Units
Latitude, L 1.051e-007 rad
Longitude,? 8.304e-008 rad
Altitude,h 0.245 m
Latitude, L 0.657 m
Longitude,? 0.519 m
50
0 100 200 300 400 500 600 700 800 90032.668
32.6732.672
32.674 Position
Latit
ude(
deg) CarsimNoiseyReferenceEstimate
0 100 200 300 400 500 600 700 800 900?85.442
?85.44
?85.438
Long
itude
(deg)
0 100 200 300 400 500 600 700 800 900
230235
240245
Time(s)
Altit
ude(
m)
Figure 4.3: Position Estimates on Road Course with IMU Bias & Walk
0 100 200 300 400 500 600 700 800 900?1
0
1x 10?5 Position
Latit
ude(
deg) Error
0 100 200 300 400 500 600 700 800 900?2
0
2x 10?5
Long
itude
(deg)
0 100 200 300 400 500 600 700 800 900?1
01
23
Time(s)
Altit
ude(
m)
Figure 4.4: Position Errors on Road Course with IMU Bias & Walk
The velocity solutions for the road course in CarSim are shown in Figures 4.5 and 4.6
with the root mean sqaured shown in Table 4.6.
51
Table 4.6: Root Mean Squared Velocity Error
RMS Units
North Velocity,vN 0.002145 m/s
East Velocity, vE 0.05826 m/s
Down Velocity,vD 0.05887 m/s
0 100 200 300 400 500 600 700 800 900?50
0
50 VelocityNED
Time(s)No
rthV
elocit
y(m
/s) CarsimNoiseyReference
Estimate
0 100 200 300 400 500 600 700 800 900?50
0
50
Time(s)Ea
stVe
locity
(m/s
)
0 100 200 300 400 500 600 700 800 900?2
0
2
Time(s)Do
wnV
elocit
y(m
/s)
Figure 4.5: Velocity Estimates on Road Course with IMU Bias & Walk
0 100 200 300 400 500 600 700 800 900?0.5
0
0.5 ErrorVelocityNED
Nort
hVe
locity
(m/s
)
Error
0 100 200 300 400 500 600 700 800 900?0.2
0
0.2
East
Veloc
ity(m
/s)
0 100 200 300 400 500 600 700 800 900?0.2
0
0.2
Time(s)Do
wnV
elocit
y(m
/s)
Figure 4.6: Velocity Errors on Road Course with IMU Bias & Walk
52
Figures 4.7 and 4.8 provide the attitude angles and errors estimated by the EKF using
data from CarSim. This data run is a vehicle negotiating a various of turns and straight
sections. The truth used for the attitudes is the Euler attitudes generated by the CarSim
model. Note that the estimates have less errors once the filter settles out. For the RMS
calculation in Table 4.7 any jumps in error to 360 deg caused by the wrap have been padded
with empty values. The empty spaces do not affect the RMS estimate as the empty values
effectively reduced the total length of the vector. Also, for the yaw error plot the error
jumps from wrapping were replaced with the value NaN (?Not a Number?) which does not
get plotted and does not alter the vector length.
Table 4.7: Root Mean Squared Attitude Error
RMS Units
Roll,?v 0.07575 deg
Pitch, ? 0.08400 deg
Yaw,? 0.21770 deg
0 20 40 60 80 100 120?5
0
5 Attitude
Roll
(deg)
CarSimTruthEstimate
0 20 40 60 80 100 120?10
0
10
Pitch
(deg)
0 20 40 60 80 100 1200
200
400
Yaw
(deg)
Time(s)
Figure 4.7: Attitude Estimates Road Course with IMU Bias & Walk
53
0 100 200 300 400 500 600 700 800 900?0.4
?0.20
0.2 ErrorAttitude
Roll
(deg)
0 100 200 300 400 500 600 700 800 900?0.2
00.2
0.40.6
Pitch
(deg)
0 100 200 300 400 500 600 700 800 900?0.5
0
0.5
Time(s)
Yaw
(deg)
Error
Figure 4.8: Attitude Errors Road Course with IMU Bias & Walk
The last remaining Kalman filter states are the bias states. The first simulation on
the road course includes only bias and the Markov random walk to show that the filter
can estimate both constant bias and drift because it is hard to see the bias and walk when
noise is present. Figure 4.9 represents the bias estimation results of the artificially corrupted
accelerometer. In general the filter estimates the bias well and is able to somewhat track the
Markov walk. However, there appears to be an estimation error of roughly 0.02 m/s2 for the
X bias estimate. This discrepancy is most likely due to poor excitation in the x-axis of the
accelerometer [16].
54
0 100 200 300 400 500 600 700 800 900?0.02
0
0.02
0.04
0.06
0.08
0.1
0.12
0.14
Time(s)
Acce
lerat
ion(
m/s2
)
AccelerometerBiasEstimates
XBiasYBiasZBias
XBiasEstimateYBiasEstimateZBiasEstimate
Figure 4.9: Accelerometer Bias Estimates on Road Course with IMU Bias & Walk
Figure 4.10 shows the estimates of the angular rate bias estimates. Note that roll, pitch
and yaw gyro bias estimates converge to the bias at similar rates despite the different levels
of bias present in each axis.
0 100 200 300 400 500 600 700 800 900
?2
0
2
4
6
8
10
12
14
x 10?4
Time(s)
Angu
larR
ate(
deg/
s)
GyroscopeBiasEstimates
?vBias?Bias?Bias
?vBiasEstimate?BiasEstimate?BiasEstimate
Figure 4.10: Roll Gyro Bias Estimate Road Course with IMU Bias & Walk
55
For the second run on the road course noise was added on top of the bias and Markov
walk. The RMS values in Tables 4.8-4.10 show that the noise did not alter the errors of
estimates.
Table 4.8: Root Mean Squared Position Error
RMS Units
Latitude, L 1.0493e-007 rad
Longitude,? 8.32718e-008 rad
Altitude,h 0.2486 m
Latitude, L 0.6558 m
Longitude,? 0.5204 m
Table 4.9: Root Mean Squared Velocity Error
RMS Units
North Velocity,vN 0.002145 m/s
East Velocity, vE 0.06165 m/s
Down Velocity,vD 0.06460 m/s
Table 4.10: Root Mean Squared Attitude Error
RMS Units
Roll,?v 0.11596 deg
Pitch, ? 0.10298 deg
Yaw,? 0.24104 deg
4.3.2 Cascaded EKF Road Bank Estimation
To validate the methodology for estimating bank with the roll estimate from the navi-
gation EKF and the measurement of relative roll from the suspensions potentiometers, the
vehicle was simulated driving on 15 % bank at 25 km/hr. A double lane change maneuver
was performed near the 30 second point. Note that bank percentage is the ratio of elevation
change to the horizontal distance or the slope. A 15% bank translates into a bank of 8.46
deg, or 180/pi?sin?1 0.3 = 8.62. Figure 4.11 shows the total vehicle roll (?v), suspension roll
56
(?s), estimated road bank (?r,truth), and the estimated road bank (?r,est) for this simulation.
The road bank truth comes from an averaging of the road bank reported at each wheel from
the CarSim variables.
0 10 20 30 40 50 60 70 80 90
1
2
3
4
5
6
7
8
9
10
Time(s)
Roll
Angl
e(de
g)
RoadBankEstimate
?rTruth?
vTotalRollEstimate?
sSuspensionRollMeasurement?
rEstimate
Figure 4.11: Roll Estimate from DLC on 15% (8.62 deg) Bank using CarSim
The estimated bank angle is fairly accurate. However, one may notice the variation in
the estimate during the maneuver. This can be partially attributed to the vehicles change in
heading on the slope. As the vehicle turns, the bank angle which is calculated with respect
to the body frame of the vehicle is different from the slope on which the vehicle is driving. If
the vehicle is driving down the bank instead of across it the measured bank angle would be
zero degrees. This reinforces the concept that the bank angle estimated from this method is
always relative to the vehicle.
The errors in the vehicle total roll estimate are expected to be present in the road bank
estimate. Figure 4.12 shows the error for both the total roll and the bank estimates. Notice
that there is only a small difference between the bank error and the total roll error. This
small difference confirms that the error in the bank estimate is due largely from the errors
in the roll estimate. Thus, the road bank estimation performance is limited by the EKF
performance.
57
0 10 20 30 40 50 60 70 80 90
?0.25
?0.2
?0.15
?0.1
?0.05
0
0.05
0.1
0.15
0.2
0.25
RollError
Roll
Angl
e(de
g)
Time(s)
?vTotalRollError?rBankError
Figure 4.12: Attitude Errors from DLC on 15% (8.62 deg) Bank using CarSim
To validate the road bank estimation methodology in a more complex scenario, simula-
tions were run with the vehicle performing a double lane change on an increasing bank angle.
This was used to determine how well a dynamic bank angle can be estimated during dynamic
maneuvers. Figure 4.13 represents the roll and bank estimates and Figure 4.14 represents
the errors on the roll and bank estimates. Notice that during the lane change maneuver (14 -
23 seconds) the bank and total roll errors look to be out of ?phase?. This is a small deviation
but could be the effect of either tire bounce, the axle does not remain parallel to the road
surface, or the effects of the independent front suspension on CarSim?s small SUV model.
The independent front suspension has an effective track width but the dynamics could cause
the independent suspension to behave such that the track width changes. This would affect
the suspension roll measurement as Eq. (2.1) assumes a constant track width. Since the
difference between total roll error and bank error are so small, the assumption appears to
hold.
58
0 5 10 15 20 25 30 35 40 45?5
0
5
10
15
20
25
Time(s)
Roll
Angl
e(de
g)
RoadBankEstimate?
rTruth?vTotalRollEstimate?
sSuspensionRollMeasurement?rEstimate
Figure 4.13: Simulation DLC on Increasing Bank Bank Estimate using CarSim
0 5 10 15 20 25 30 35 40 45?0.1
?0.05
0
0.05
0.1
0.15
0.2 RollError
Roll
Angl
e(de
g)
Time(s)
?vTotalRollError?rBankError
Figure 4.14: Simulation DLC on Increasing Bank Bank Errors using CarSim
To test the performance of the bank estimation under changing velocities the CarSim
road course shown in Figure 4.1 was used. Figures 4.15-4.16 represent the estimates and
errors respectively from a single lap on the road course. Again under higher dynamics the
59
errors in total roll and bank diverge from time to time but remain in the same bounds as
observed in Figure 4.12.
120 140 160 180 200 220?6
?4
?2
0
2
4
6
Time(s)
Roll
Angl
e(de
g)
RoadBankEstimate?
rTruth?vTotalRollEstimate?
sSuspensionRollMeasurement?rEstimate
Figure 4.15: Road Course Bank Estimate using CarSim
120 140 160 180 200 220
?0.2
?0.1
0
0.1
0.2
0.3
0.4
0.5 RollError
Roll
Angl
e(de
g)
Time(s)
?vTotalRollError?rBankError
Figure 4.16: Road Course Bank Errors using CarSim
60
4.4 Robustness
To test the robustness of the method, simulations were run with the vehicle performing
a fishhook maneuver. This is a maneuver which is used to test the rollover propensity of
vehicles. In the maneuver the driver steers left and quickly back to the right causing the
vehicle to produce the potential for two wheel lift off and subsequently roll over. The top of
Figure 4.17 shows the roll and bank estimate of the vehicle during the maneuver, and the
bottom shows the vertical tire forces. Figure 4.18 shows the results for the vehicle performing
a fishhook maneuver on a 30% (16.7?) bank. The vertical line on both figures represents
the point at which two wheel lift off occurs. Two wheel lift off is identified by the vertical
tire forces on one side of the vehicle both going to zero. On the banked surface the lift
off occurs during the initial turn while on the flat surface it occurs during the second turn.
This difference in initial time that two wheel lift off occurs demonstrates that the vehicle
can have a significantly different response when operating on a banked terrain versus flat
ground. Additionally the difference suggests that the appropriate counter maneuver to keep
the vehicle from rolling may change dependening on the severity and direction of the bank.
The vehicle response and appropriate counter maneuver is further complicated when the
road grade and vehicle pitch is considered.
4.5 Conclusion
This chapter has shown that suspension deflection measurements and an estimate of the
total vehicle body roll can be compbined to obtain an accurate estimate of road bank. An
Extended Kalman Filter was developed to estimate the total roll state of the vehicle. Sus-
pension deflections were used to measure the relative roll between the sprung and unsprung
mass of a vehicle. Simulation results of various vehicle maneuvers demonstrated the effec-
tiveness of the method in estimating road bank. Additionally, the methodology presented
here can be modified and applied to vehicle pitch and road grade estimation.
61
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5?80
?60
?40
?20
0
20
Angle (deg) ?vEstimatedRoll?sSuspensionRoll
?rBankEstimate
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50
5000
10000
15000
time (sec)
Normal Force (N)
LeftFrontLeftRear
RightFrontRightRear
Figure 4.17: Vehicle Performing Fishhook Maneuver on Flat Surface
0 0.5 1 1.5 2 2.5 3?60
?40
?20
0
20
Angle (deg) ?vEstimatedRoll?sSuspensionRoll
?rEstimate
0 0.5 1 1.5 2 2.5 30
1
2
3x 104
Time (s)
Normal Force (N)
LeftFrontLeftRear
RightFrontRightRear
Figure 4.18: Vehicle Performing a Fishhook Maneuver on a 30% Bank
62
Chapter 5
Road Bank Estimation using Suspension Deflections and a Coupled Navigation EKF
5.1 Introduction
Thischapterpresentsanestimationschemefordeterminingroadbankusinganavigation
EKF architecture with an additional road bank state. The basic 15 state architecture of the
navigation EKF presented in Chapter 4 is augmented with the 16th state of road bank. The
goal of the coupled approach is to see if any useful relationship between the relative roll angle
given from the suspension deflections and the total roll state of the navigation filter could
be exploited. Since the deflections are measured from potentiometers that operate at 100Hz
or better, it seemed logical to have the suspension deflections implemented inside the INS
navigation portion of the EKF filter, the time propagation phase. However, implementing
the relative roll from the suspension deflections as a second measurement update provided
better filtering of the bank estimate state.
5.2 Methodology
The coupled or augmented approach uses the same principle equations and structure
as the cascaded method described in Section 4.2.6. However there are some details that are
important to know for the coupled approach. First, the error state vector is augmented with
road bank as the 16th state.
63
x =
?
??
??
??
??
??
??
??
??
??
?
?p
?vn
??nnb
bba
bbg
?nr
?
??
??
??
??
??
??
??
??
??
?
(5.1)
The system model, F matrix, must be adjusted as well. Ideally there would exist an equation
that propagates the road bank rate as a function of the states in the state vector. No such
function exist so the road bank rate is modeled as a disturbance with the time constant ?s.
??nr = ?1
?s ?
n
r (5.2)
The new model matrix is
F =
?
??
??
??
??
??
??
??
??
??
?
?[(?nen)?] T 03 03 03 03?1
Gg ?[2 (?nie +?nen)?]
bracketleftBigparenleftBig
Cnbfb
parenrightBig
?
bracketrightBig
Cnb 03 03?1
03 03 ?[(?nie +?nen)?] 03 ?Cnb 03?1
03 03 03 ?1?a I3 03 03?1
03 03 03 03 ?1?g I3 03?1
01?3 01?3 01?3 01?3 01?3 ?1?s
?
??
??
??
??
??
??
??
??
??
?
(5.3)
where 03 is a 3 by 3 null matrix, 03?1 is a 3 by 1 null matrix, 01?3 is a 1 by 3 null matrix,
I3 is a 3 by 3 identity matrix and
Gg =
?
??
??
??
?
0 0 0
0 0 0
0 0 ?2gRo
?
??
??
??
?
64
For small propagation times the F matrix can be simplified to
F =
?
??
??
??
??
??
??
??
??
??
?
03 T 03 03 03 03?1
Gg 03
bracketleftBigparenleftBig
Cnbfb
parenrightBig
?
bracketrightBig
Cnb 03 03?1
03 03 03 03 ?Cnb 03?1
03 03 03 ?1?a I3 03 03?1
03 03 03 03 ?1?g I3 03?1
01?3 01?3 01?3 01?3 01?3 ?1?s
?
??
??
??
??
??
??
??
??
??
?
(5.4)
The measurement matrix for the GPS measurement is
H =
?
??
?
I3 03 03 03 03 0
03 I3 03 03 03 0
?
??
?
6?16
(5.5)
Thesuspensiondeflectionmeasurementareincorporatedthroughasecondmeasurement
update. Like the GPS update this update will occur whenever a new deflection measurement
exists. The measurement matrix for the second update is
H2 =
bracketleftbigg
01?3 01?3 1 0 0 01?3 01?3 ?1
bracketrightbigg
1?16
(5.6)
The primary advantage of adding the road bank as a state is that the EKF is to provide a
cleaner estimate by filtering the noise on road bank state estimate.
5.3 Simulations
For comparison, the same CarSim runs presented in Section 4.3.2 for the cascaded
approach are used here for the coupled estimate algorithm. Note that the noises added to
the coupled simulations have the same characteristics but were generated separately from
the cascaded simulations. Figures 5.1-5.2 represent a double lane change performed on 15%
bank, 8.62 deg, at 25 km/hr. The bank estimate matches the bank truth just as well as the
65
cascaded approach in Section 4.3.2. However, Figure 5.2 shows that the advantage provided
by the coupled approach is the filtering of the bank estimate.
0 10 20 30 40 50 60 70 80 900
1
2
3
4
5
6
7
8
9
10
Time(s)
Roll
Angl
e(de
g)
RoadBankEstimate
?rTruth?
vTotalRollEstimate?
sSuspensionRollMeasurement?
rEstimate
Figure 5.1: Coupled EKF Bank Estimate DLC on 15% (8.62 deg) Bank
0 10 20 30 40 50 60 70 80 90?0.4
?0.2
0
0.2 RollError
Time(s)
Roll
Angl
e(de
g)
?vTotalRollError?rBankError
Figure 5.2: Coupled EKF Bank Errors DLC on 5% (8.62 deg) Bank
66
The filtering provided by the coupled EKF approach is consistent for both the double
lane change on increasing bank at 50 km/hr shown in Figures 5.3-5.4 and the lap around
the CarSim road course shown in Figures 5.5-5.6.
0 5 10 15 20 25 30 35 40 45
0
5
10
15
20
25
Time(s)
Roll
Angl
e(de
g)
RoadBankEstimate?
rTruth?vTotalRollEstimate?
sSuspensionRollMeasurement?rEstimate
Figure 5.3: Coupled EKF Bank Estimate DLC on Increasing Bank
0 5 10 15 20 25 30 35 40 45
?0.25
?0.2
?0.15
?0.1
?0.05
0
0.05
0.1
0.15
0.2
0.25
RollError
Time(s)
Roll
Angl
e(de
g)
?vTotalRollError?rBankError
Figure 5.4: Coupled EKF Bank Errors DLC on Increasing Bank
67
120 140 160 180 200 220?6
?4
?2
0
2
4
6
Time(s)
Roll
Angl
e(de
g)
RoadBankEstimate?
rTruth?vTotalRollEstimate?
sSuspensionRollMeasurement?rEstimate
Figure 5.5: Coupled EKF Bank Estimate Road Course
120 140 160 180 200 220?0.4
?0.3
?0.2
?0.1
0
0.1
0.2
0.3
0.4
0.5 RollError
Time(s)
Roll
Angl
e(de
g)
?vTotalRollError?rBankError
Figure 5.6: Coupled EKF Bank Errors Road Course
5.4 Conclusion
The navigation extended Kalman filter was augmented with a 16th state, the road bank.
This coupled EKF architecture required a second measurement update from the suspension
roll measurements. The approach was simulated with the types of runs used for the cascaded
68
EKF simulation. The coupled EKF simulations displayed smoother bank estimates than the
cascaded approach. Thus, the primary advantage of adding the road bank as a state is that
the coupled EKF will provide a cleaner estimate by filtering the noise on the road bank state
estimate.
69
Chapter 6
Roll and Road Bank Estimation using Suspension Deflections, Roll model and Yaw model
Coupled with a KF disturbance observer
6.1 Introduction
This chapter presents a novel estimation scheme for road bank. In [10] the author uses
a disturbance observer to estimate unmeasured vehicles states such as the road bank. The
author uses a two antenna GPS receiver integrated with an IMU to output vehicle side slip
and the total roll of the vehicle. Additionally the IMU provides yaw rate and roll rate of
the vehicle. In this chapter, a method is presented that builds off the disturbance observer
concept. The difference is that the measurements used are the yaw rate and roll rate from
an IMU and the relative roll angle from suspension deflections (as opposed to measurements
from a dual antenna GPS).
6.2 Methodology
Coordinate frames and the transformations between various frames and intermediate
frames are used to describes the rotations and rotation rates for the vehicle axle and vehicle
body (i.e. the unsprung and sprung masses of the vehicle respectively). The angles and
angular rates will then be incorporated in to a coupled roll and yaw rate vehicle model. The
vehicle model uses steer angle as an input and equations of motion that are functions of
velocity. This model is then incorporated into an observer architecture to estimate states
not directly measured or generated from the model.
70
6.2.1 Vehicle Fixed Frames
The method in this chapter builds off the roll model in Figure 2.1 and the inverse of the
rotation matrix from Eq. (4.14). The notationarepresents the axle frame of the vehicle that
originates at the unsprung mass. The notation b still represents the body frame however the
body frame is now specified as originating on the sprung mass of the vehicle. Remember
that ?nb is equivalent to ?v. The Euler angle rotation is given by
Can =
?
??
??
??
?
1 0 0
0 cos?v sin?v
0 ?sin?v cos?v
?
??
??
??
?
?
??
??
??
?
cos?v 0 ?sin?v
0 1 0
sin?v 0 cos?v
?
??
??
??
?
?
??
??
??
?
cos?v sin?v 0
?sin?v cos?v 0
0 0 1
?
??
??
??
?
(6.1)
= Ca2C21C1n
The sub and superscript 1 denote the intermediate coordinates given by the rotation about
the z axis from the navigation coordinates n. The sub and superscript 2 represent the inter-
mediate coordinates given the rotation about the y axis from the intermediate coordinates
1.
The angular velocity vector of the fixed vehicle axle frame from the navigation frame to
the body frame and expressed in the body frame is defined as
?aan =
?
??
??
??
?
?a
?a
?a
?
??
??
??
?
(6.2)
71
By using the angular rate of the Euler angles, the axle frame fixed velocity vector can be
written as
?aan =
?
??
??
??
?
??a
??a
??a
?
??
??
??
?
= Ca2
?
??
??
??
?
??v
0
0
?
??
??
??
?
+ Ca2C21
?
??
??
??
?
0
??v
0
?
??
??
??
?
+ Ca2C21C1n
?
??
??
??
?
0
0
??v
?
??
??
??
?
(6.3)
?
??
??
??
?
??a
??a
??a
?
??
??
??
?
= J
?
??
??
??
?
??v
??v
??v
?
??
??
??
?
The Euler rates can then be determined from the axle fixed angular velocity vector by
inverting J, the sum of successive rotations in Eq. (6.4).
?
??
??
??
?
??v
??v
??v
?
??
??
??
?
=
?
??
??
??
?
1 sin?v tan?v cos?v tan?v
0 cos?v ?sin?v
0 sin?v/sin?v cos?v/cos?v
?
??
??
??
?
?
??
??
??
?
??a
??a
??a
?
??
??
??
?
(6.4)
?
??
??
??
?
??v
??v
??v
?
??
??
??
?
= J?1
?
??
??
??
?
??a
??a
??a
?
??
??
??
?
Note that because the road bank angle is defined between the vehicle frame and the inter-
mediate coordinates 1, ??r is not the same as ??v unless ?v the Euler pitch is zero. The road
bank rate ??r is the x component of the angular velocity, ?1a1 which represents the angular
velocity of the axle frame with respect to the intermediate coordinates 1. The angular frame
rate of the vehicle axle fixed frame is
72
?1a1 =
?
??
??
??
?
??r
??r
??r
?
??
??
??
?
= C12
?
??
??
??
?
??v
0
0
?
??
??
??
?
+
?
??
??
??
?
0
??v
0
?
??
??
??
?
(6.5)
=
parenleftBig
C21
parenrightBig?1
?
??
??
??
?
??v
0
0
?
??
??
??
?
+
?
??
??
??
?
0
??v
0
?
??
??
??
?
(6.6)
?
??
??
??
?
??r
??r
??r
?
??
??
??
?
=
?
??
??
??
?
cos?v ??v
??v
?sin?v ??v
?
??
??
??
?
(6.7)
Thus the road bank rate is defined as
??r = cos?v ??v (6.8)
and from Eq.(6.4) the road bank equation can also be written as
??r = cos?v ??a + sin?v sin?v ??a + cos?v sin?v ??a (6.9)
Typically the mounting location for an IMU in a vehicle is somewhere on the vehicle
body not the vehicle axle. The relationship between the vehicle body fixed frame and the
vehicle axles fixed frame is the transformation
Cba =
?
??
??
??
?
1 0 0
0 cos?s sin?s
0 ?sin?s cos?s
?
??
??
??
?
(6.10)
73
where Cba is the transformation from the vehicle axle fixed frame to the vehicle body fixed
frame. Using Eq. (6.10) the IMU measurements in the body fixed frame can be expressed
as
?bab =
?
??
??
??
?
??b
??b
??b
?
??
??
??
?
=
?
??
??
??
?
??s
0
0
?
??
??
??
?
+ Cba
?
??
??
??
?
??a
??a
??a
?
??
??
??
?
(6.11)
This reveals that the roll rate and yaw rate equations for the body fixed frame are
??b = ??s + ??a (6.12)
??b = cos?v ??a?sin?s ??a (6.13)
Note that Eq. (6.12) shows that the roll rate of the body is the sum of the roll rate from the
sprung mass (vehicle body) and the roll rate of the axle. Even though the bank of a road is
built with ?x? amount of bank, the amount ?x? is measured perpendicular to the direction
of travel on the road. The bank experienced by a vehicle is only equal to the amount ?x?
when the orientation of the vehicle is such that the x-axis of the vehicle is parallel to the
direction of travel of the road.
To implement the equations into vehicle models, some simplifications need to be made.
Eq. (6.9) can be rewritten assuming that the total pitch angle, ?v, of the vehicle vehicle is
small such that cos?v ? 1 and the variable epsilon1r is created to lump the effects of axle pitch
rate, axle yaw rate, total roll and total pitch ,??a, ??a, ?v and ?v, together . This yields
??r? ??a +epsilon1r (6.14)
epsilon1r = sin?v sin?v ??a + cos?v sin?v ??a (6.15)
Finally, assuming that the vehicle roll angle ?s and the vehicle axle pitch rate ??a are small,
Eq. (6.13) can be simplified to
74
??b? ??a (6.16)
6.2.2 Roll and Yaw Models
The vehicle dynamics are modeled with a bicycle model coupled with a roll model as in
[17]. In the bicycle model the inner and outer slip angles at the wheels are assumed to be
equal. Figures 6.2 and 6.3 represent the lateral and roll vehicle model. Figure 6.2 displays
the two dimensional relationship between the vehicle frame and the navigation frame for
reference. The navigation frame, shown in Figure 6.1, is the north-east-down frame thus the
lateral velocity for the velocity vector, V, is negative in the vehicle z-up frame. Observe that
the vehicles course, the direction of travel and denoted by ?, is not equal to the heading
of the vehicle, ?b. This results in vehicle side-slip, ?, and means that a velocity exists not
only in the longitudinal, x direction, of the vehicle but also in the lateral, y direction, of the
vehicle. The change in this lateral velocity, ?Vy, will be used to formulate the equations of
motion.
Figure 6.1: Navigation Frame
75
In Figure 6.2 the steer angle is represented by ?, ??b is the yaw rate of the vehicle body
frame, ? is the side slip angle at the center of the mass, Vx and Vy are the longitudinal and
lateral velocity components of the velocity vector V. The lateral tire forces are denoted as
FyF for the front and FyR for the rear. Similarly ?f and ?r are the front and rear axle slip
angles.
Figure 6.2: Schematic of Bicycle Model (with all Forces and Angles Shown in Positive
Direction)
The tire force modeled using the cornering stiffness C? as a linear relationship between
the slip angle and tire force such that
FyF =?C?f?f (6.17)
FyR =?C?r?r (6.18)
where C?f and C?r are the cornering stiffness parameters for the front and rear axle respec-
tively. Eq. (6.17) and (6.18) assume small slip angles ?f and ?r such that vehicle operates
in the linear region of the tire curve moleded by the cornering stiffness.
76
Figure 6.3: Roll Model
Referring back to the roll model in Figure 2.1 and the bicycle model from Figure 6.2,
the coupled equations of motion for the bicycle and roll model are
?Vy = IeqC0
IxxmVxVy?
parenleftbigg
Vx + IeqC1I
xxmVx
parenrightbigg
??a+?hrgB??
Ixx
??sh(mghrg?K?)
Ixx ?s+
IeqC?f
Ixxm ??g?r (6.19)
??a =? C1
IzzVxVy?
C2
IzzVx
??a + aC?f
Izz ? (6.20)
??v =?C0hrg
IxxVxVy?
C1hrg
IxxVx
??a?B??
Ixx
??s + mghrg?K?
Ixx ?s +
C?fhrg
Ixx ??
??a (6.21)
where
C0 = C?f +C?r (6.22)
C1 = aC?f?bC?r (6.23)
77
C2 = a2C?f +b2C?r (6.24)
Ieq = Ixx +mh2rg (6.25)
In these equations, Izz is the moment of inertial about the z axis, the yaw axis. The total
mass of the vehicle is represented by m. The terms a and b denote the front and rear weight
splits relative to the center of gravity. The effect of the unsprung mass is assumed to be
minimal. Therefore, the model assumes a mass-less axle. The moment of inertia about the
x axis, the roll axis, is represented by Ixx. K? is the roll stiffness and B?? is the roll damping.
The term hrg is the distance from the roll axis to the center of gravity on the sprung mass.
The angular acceleration of the axle, ??a, is related through Eq. (6.14) because the axle frame
is assumed to remain in contact with the ground surface by means of the tires.
The coupled roll and bicycle model is represented in state space with the state transition
matrix A, input vector B, and two disturbance vectors Bw1 and Bw2 such that
?x = Ax + B? + Bw1?r + Bw2 ??a (6.26)
where
A =
?
??
??
??
??
??
?
IeqC0
IxxmVx ?
parenleftBig
Vx + IeqC1IxxmVx
parenrightBig
?hrgB??Ixx h(mghrg?K?)Ixx
? C1IzzVx ? C2IzzVx 0 0
?C0hrgIxxVx ?C1hrgIxxVx ?B??Ixx mghrg?K?Ixx
0 0 1 0
?
??
??
??
??
??
?
(6.27)
B =
?
??
??
??
??
??
?
IeqC?f
Ixxm
aC?f
Izz
C?fhrg
Ixx
0
?
??
??
??
??
??
?
(6.28)
78
Bw1 =
?
??
??
??
??
??
?
?g
0
0
0
?
??
??
??
??
??
?
(6.29)
Bw2 =
?
??
??
??
??
??
?
0
0
?1
0
?
??
??
??
??
??
?
(6.30)
The state vector x is
x =
?
??
??
??
??
??
?
Vy
??a
??s
?s
?
??
??
??
??
??
?
(6.31)
6.2.3 Kalman Filter Disturbance Observer Setup
Using the system modeled from the coupled roll and bicycle model described in Section
6.2.2, adisturbanceobservercanbeimplementedifthefollowingmeasurementsareavailable.
y =
?
??
??
??
?
?s
??b
??b
?
??
??
??
?
=
?
??
??
??
?
?s
??f
??s + ??a
?
??
??
??
?
(6.32)
The yaw rate, ??f, and the sum of the roll rate of the sprung mass and the angular rate of
the axle frame, ??s + ??a, are measurements from the gyro according to Eq. (6.12) and Eq.
(6.16). The sprung mass roll angle is a measurement from the suspension deflection sensors
attached to the spring and damper at each corner of the vehicle. Section 7.3 gives more
detail on mounting of the suspension deflection sensors.
79
The disturbances of the road bank angle, ?r, and the angular acceleration of the axle,
??a, found in the state space model of the coupled roll and bicycle models, Eq. (6.26), vary
independently of the system model matrix A. However ?r and ??a are not independent of
each other as seen in Eq. (6.14). Thus, as long as the disturbances due to the road bank
variations consist of white noise forcing the angular jerk of the axle, ...?a, and the road bank
error rate, ?epsilon1r, the dynamics of the disturbance developed by[18] are modeled as
?w = Aww (6.33)
where
w =
?
??
??
??
??
??
?
?r
??a
??a
epsilon1r
?
??
??
??
??
??
?
(6.34)
Aw =
?
??
??
??
??
??
?
0 1 0 1
0 0 1 0
0 0 0 0
0 0 0 0
?
??
??
??
??
??
?
(6.35)
By augmenting the statespace model Eq. (6.26)with the disturbancesmodeled in Eq. (6.33)
the observer is modeled as
?z =
?
??
?
A Bw
04 Aw
?
??
?z +
?
??
?
B
04?1
?
??
?? (6.36)
?z = Fz + G?
80
such that
z =
?
??
?
x
w
?
??
? =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
?
Vy
??a
??s
?s
?r
??a
??a
epsilon1r
?
??
??
??
??
??
??
??
??
??
??
??
??
??
?
(6.37)
Bw =
bracketleftbigg
Bw1 04?1 Bw2 04?1
bracketrightbigg
(6.38)
Bw =
?
??
??
??
??
??
?
?g 0 0 0
0 0 0 0
0 0 ?1 0
0 0 0 0
?
??
??
??
??
??
?
The measurement equation is
y = Hz (6.39)
?
??
??
??
?
?s
??b
??b
?
??
??
??
?
=
?
??
??
??
??
??
?
0 0 0 1 0 0 0 0
0 1 0 0 0 0 0 0
0 0 1 0 0 1 0 0
0 0 0 1 1 0 0 0
?
??
??
??
??
??
?
z (6.40)
The full observer can now be implemented with the standard Kalman filter structure de-
scribed in Section 4.2.4.
81
6.3 Simulations & Results
Simulations were again performed using CarSim 6. First the coupled roll and bicycle
model was validated against the CarSim high fidelity model for the small SUV. Steer angle
and velocity output from CarSim simulations were used as inputs to the coupled vehicle
model simulations performed in Matlab. The output states from the coupled vehicle model
were then compared to the corresponding vehicle states from the same CarSim simulations.
For testing of the Kalman filter observer, CarSim steer angle and vehicle speed were
used as inputs to the Kalman filter observer. Some simulations assume a perfect vehicle
model represented by the coupled bicycle and roll vehicle model. When the perfect vehicle
model assumption was made the measurements of roll rate, yaw rate and the roll due to
suspension deflections were created by corrupting the outputs of the coupled vehicle model
with noise. Other simulations assume that the vehicle model is imperfect. For the case that
the vehicle model is imperfect, noise was added to the output data from CarSim vehicle
states of roll rate, yaw rate and suspension deflections for the small SUV.
6.3.1 Model Validation
Figures 6.4 and 6.5 represent a double lane change (DLC) maneuver at 20 km/s. At this
speed the coupled vehicle model and the CarSim model compare quite well. Note though
that the CarSim roll does not return to zero after the DLC maneuver is completed while the
coupled vehicle model does. This is most likely a numerical issue within the CarSim software.
Also note that the roll calculated from the CarSim suspension deflections using the method
discussed in Section 2.2.1 closely matches the CarSim roll because the road surface is flat.
82
0 10 20 30 40 50 60 70 80 90?0.2
?0.1
0
0.1
0.2
(m/s)
Vehicle Model Comparison
Vy
VyCarSim
0 10 20 30 40 50 60 70 80 90-10
-5
0
5
10
(deg/s)
??YawRate
??YawRateCarSim
Figure 6.4: Model Comparison Double Lane Change at 20 Km/hr
0 10 20 30 40 50 60 70 80 90?0.5
0
0.5
(deg/s)
Vehicle Model Comparison
??RollRate
??RollRateCarSim
0 10 20 30 40 50 60 70 80 90-0.5
0
0.5
Timesuppress(s)
(deg)
?Roll?RollCarSim
?RollDeflections
Figure 6.5: Model Comparison Double Lane Change at 20 Km/hr
At higher speeds the coupled vehicle model and the CarSim models deviate. Specifically
the lateral velocity from the coupled vehicle model does not match the lateral velocity for
CarSim. The lateral velocity for CarSim model is greater in magnitude during the dynamic
event indicating that the vehicle is slipping laterally. This separation is due to the non-linear
83
saturationfromthetireforcesthatareunmodeledinthelineartiremodelusedinthecoupled
vehicle model as well as simplifications such as combining the inner and outer wheels into
front and rear axles. Figure 6.6 represents the same double lane change maneuver performed
at 70 km/hr. The amount of deviation seen in the vehicle states will cause significant errors
in the estimation of the unmodeled states. From this, it can be concluded that at low speeds
and low dynamics the coupled vehicle model is a valid model, but at high speeds and high
dynamics the coupled vehicle model is unreliable.
0 5 10 15 20 25 30?0.4
?0.2
0
0.2
0.4
(m/s
)
VehicleModelComparison V
yVyCarSim
0 5 10 15 20 25 30-20
-10
0
10
20
(deg/
s)
??YawRate??YawRateCarSim
Figure 6.6: Model Comparison Double Lane Change at 70 Km/hr
84
0 5 10 15 20 25 30?10
?5
0
5
10
(deg/
s)
VehicleModelComparison ?
?RollRate??RollRateCarSim
0 5 10 15 20 25 30-2
-1
0
1
2
Time(s)
(deg)
?Roll?RollCarSim
?RollDeflections
Figure 6.7: Model Comparison Double Lane Change at 70 Km/hr
6.3.2 Estimation
Corrupted outputs from CarSim were used as the measurements for the coupled roll and
bicycle model. The results of this simulation were less favorable than simulations when the
model was identical to the measurements. For each run, parameters such as the axle height
and cornering stiffness needed to be adjusted due to the nonlinearities of the CarSim model.
These adjustments made estimation difficult and very impractical as an overall solution
method for estimating the bank angle.
The Kalman filter model based observer estimates the road bank well at low dynamics
and when the vehicle model output closely matches the measured states. Figures 6.8-6.11
represent the CarSim small SUV traversing a bank the increases from 0% to 30% bank while
maintaining a straight path. In this run the CarSim vehicle states of roll rate, yaw rate and
the roll due to suspension deflections were corrupted with noise and used as measurements
for the Kalman filter observer. Notice in Figure 6.10 that the axle roll rate state accounts for
the discrepancy between roll rate predicted by the roll model of Kalman filter observer and
the ?gyro measurement? from CarSim shown in Figure 6.9. In Figure 6.11 the error in bank
85
is misrepresented due to the fact that the CarSim?s roll and suspension deflection variables
do not settle out correctly as discussed in Section 6.3.1.
0 10 20 30 40 50 60 70 80 90?0.2
?0.15
?0.1
?0.05
0
0.05
(m/s
)
KalmanFilterEstimatesofModeledStates V
yVyCarSim
0 10 20 30 40 50 60 70 80 90-0.3
-0.2
-0.1
0
0.1
(deg/
s)
??YawRate??YawRateCarSim
Figure 6.8: Straight Path on Increasing Bank at 20 Km/hr
0 10 20 30 40 50 60 70 80 90?0.5
0
0.5
1
(deg/
s)
KalmanFilterEstimatesofModeledStates ??
sRollRate??vRollRateCarSim
0 10 20 30 40 50 60 70 80 90-10
0
10
20
30
(deg)
Time(s)
?sRoll?vRollCarSim
Figure 6.9: Straight Path on Increasing Bank at 20 km/hr
86
0 10 20 30 40 50 60 70 80 90?0.5
0
0.5
1
(deg/
s)
KalmanFilterEstimatesofUn-ModeledStates ??
aAxleRollRate??aAxleRollRateCarSim
0 10 20 30 40 50 60 70 80 90-20
0
20 ??aAxleRollAccel
??aAxleRollAccelCarSim
0 10 20 30 40 50 60 70 80 90-0.5
0
0.5
(deg/
s)
Time(s)
epsilon1ErrorinRollRateepsilon1CalcCalculatedCarSimEpisilon
Figure 6.10: Straight Path on Increasing Bank at 20 km/hr
0 10 20 30 40 50 60 70 80 90?10
0
10
20
30
Bank
(deg)
KalmanFilterEstimateofBank ?
rBank?rBankCarSim
0 10 20 30 40 50 60 70 80 90-1.5
-1
-0.5
0
0.5
1
Time(s)
Erro
r(de
g)
Error ?
r,ErrorBankError
Figure 6.11: Straight Path on Increasing Bank at 70 km/hr
When the same path on the increasing bank is taken at a higher velocity, the error in
the suspension deflection measurement from CarSim is collected in the axle roll rate state
which is represented in Figure 6.12. The collection of this error into the axle roll rate states
reduces the error in the bank estimate as represented by Figure 6.13.
87
0 5 10 15 20 25 30?2
0
2
4
(deg/
s)
KalmanFilterEstimatesofUn-ModeledStates ??
aAxleRollRate??aAxleRollRateCarSim
0 5 10 15 20 25 30-100
-50
0
50 ??aAxleRollAccel
??aAxleRollAccelCarSim
0 5 10 15 20 25 30-1
0
1
(deg/
s)
Time(s)
epsilon1ErrorinRollRateepsilon1CalcCalculatedCarSimEpisilon
Figure 6.12: Straight Path on Increasing Bank at 70 Km/hr
0 5 10 15 20 25 30?10
0
10
20
30
Bank
(deg)
KalmanFilterEstimateofBank ?
rBank?rBankCarSim
0 5 10 15 20 25 30-1
-0.5
0
0.5
1
Time(s)
Erro
r(de
g)
Error ?
r,ErrorBankError
Figure 6.13: Straight Path on Increasing Bank at 70 Km/hr
Analyzing Kalman filter observer results of the 20 km/hr and 70 km/hr runs from the
double lane change maneuver described Section 6.3.1 confirms that the presence of high
dynamics reduces the accuracy of the the bank estimate. This could be due to the large
88
difference in the lateral velocity predicted by the coupled vehicle model and the actual
lateral velocity of the CarSim small SUV.
0 10 20 30 40 50 60 70 80 90?2
?1
0
1
2
Bank (deg)
Kalman Filter Estimate of Bank
?rBank?
rBankCarSim
0 10 20 30 40 50 60 70 80 90-2
-1
0
1
2
Timesuppress(s)
Errorsuppress(deg)
Error
?r,ErrorBankError
Figure 6.14: Bank Estimate Double Lane Change at 20 Km/hr
0 5 10 15 20 25 30?20
?10
0
10
20
Bank
(deg)
KalmanFilterEstimateofBank ?
rBank?rBankCarSim
0 5 10 15 20 25 30-20
-10
0
10
20
Time(s)
Erro
r(de
g)
Error ?
r,ErrorBankError
Figure 6.15: Bank Estimate Double Lane Change at 70 Km/hr
Another source for the error in bank during dynamics could be that the axle roll rate is
not being correctly captured by the observer. The assumption of the model is that the axle
89
moves with the surface of the road and that the stiffness of the tires is negligible. However,
in Figure 6.17, the axle roll rate from CarSim indicates that there is significant compliance
in the small SUV tires used in the model. This compliance is mostly likely from the effects
of the roll stiffness and roll dampening due to the tires. The coupled vehicle model does not
account for this effect and thus does not correctly estimate the axle roll rate. Including a
measurement of lateral velocity (through the relationship of measurements of side-slip and
longitudinal speed) would enhance the observers estimate of lateral velocity and may also
enhance the estimates of the axle roll acceleration and velocity. However, in practice this
enhancement will require a sensor capable of measuring side-slip (or lateral velocity) such
as a multi-antenna GPS which will add significantly to the sensor costs. The results show
how critical the importance of an accurate vehicle model is to producing accurate estimates.
The results also confirm that the kinematic navigation EKF based methods presented in
Chapters 4 and 5 provide much better estimates of road bank.
0 10 20 30 40 50 60 70 80 90?0.5
0
0.5
(deg/s)
Kalman Filter Estimates of Un?Modeled States
??AxleRollRate
??AxleRollRateCarSim
0 10 20 30 40 50 60 70 80 90-2
0
2
(deg/s
2 )
??AxleRollAccel
??AxleRollAccelCarSim
0 10 20 30 40 50 60 70 80 90-1
0
1
(deg/s)
Timesuppress(s)
epsilon1ErrorinRollRateepsilon1
CalcCalculatedCarSimEpisilon
Figure 6.16: Axle Roll Rate Estimate Double Lane Change at 20 km/hr
90
0 5 10 15 20 25 30?10
0
10
(deg/
s)
KalmanFilterEstimatesofUn-ModeledStates ??AxleRollRate
??AxleRollRateCarSim
0 5 10 15 20 25 30-100
-50
0
50 ??AxleRollAccel
??AxleRollAccelCarSim
0 5 10 15 20 25 30-10
0
10
(deg/
s)
Time(s)
epsilon1ErrorinRollRateepsilon1CalcCalculatedCarSimEpisilon
Figure 6.17: Axle Roll Rate Estimate Double Lane Change at 70 km/hr
6.4 Conclusion
A vehicle model based observer method for estimating bank was presented and sim-
ulated. Simulation revealed that for low dynamic maneuvers, the coupled vehicle model
Kalman filter observer can sufficiently estimate the bank. However, the observer does not
perform well under high dynamic maneuvers. The coupled vehicle model Kalman filter
method presented is not recommend for road bank estimation. Adding a measurement of
the side-slip or lateral velocity could improve the axle roll acceleration and velocity estimates
such that the error in the bank estimation is reduced during the higher dynamics.
91
Chapter 7
Experimental
7.1 Introduction
The Prowler ATV, a light tactical all terrain vehicle for the military, was used to test
the road bank algorithms. Maneuvers similar to those seen in simulation were performed
to validate the algorithms with real data. The Prowler has been automated and is outfit-
ted with various sensors. To test the road bank algorithm the following sensors were used:
single antenna GPS receiver, six degree of freedom IMU, and four linear potentiometers to
measure suspension deflections. In addition a three antenna GPS attitude system was uti-
lized to obtain accurate attitude truth measurements for comparison with the EKF attitude
estimates.
Since the ATV typically operates primarily off-road, there are a few issues that arise
when going to an experimental study. First, the roughness of the surface adds a significant
amount of excitation to the data of both the IMU and the suspension deflection measure-
ments. Additionally, in an off-road scenario there are likely to be localized events such as
rocks or logs which induce suspension deflections. Most importantly, the angle of bank while
off road is unknown. For these reasons the tests were performed on a road where the road
bank and road crown have been surveyed.
7.2 Equipment & Test Facilities
The 2003 ATV Prowler is a rugged all-terrain vehicle designed by ATV Corporation
for superior off-road maneuverability and durability. The power source for this ATV is a
660cc 4-stroke Yamaha engine with approximately 35 horsepower. The Prowler has a fully
92
automatic transmission with Hi/Low range, reverse, and park with push button four wheel
drive(4WD).Thefrontandrearaxlesbothhaveanindependentdoublewishbonesuspension
with off road tires. The vehicle is designed with a low roll center to minmize rollover events
and 12 inches of ground clearance.
The area selected for testing of the vehicle occured at the National Center for Asphalt
Technology track (NCAT). The facility has a closed 1.7 mile oval track and a skid pad area
whichallowsforsafetestingofautonomousvehicleoperationwithsufficientGPSaccessibility
for various testing scenarios.
7.3 Sensor Mounting and Test Procedure
The Prowler vehicle has a range of sensors to provide information about the vehicle
states. The 6 DOF Crossbow 440 IMU provides accelerations and gyroscopic rates about
all 3 vehicle axes. As seen in Figure 7.1 the Crossbow is mounted directly behind the
driver seat and is centered with respect to the lateral axis. Various GPS sensors have
been implemented on the vehicle, including a Novatel system with integrated real-time-
kinematic (RTK) corrections and a Septentrio system which provides GPS based roll, pitch,
and yaw measurements. The Novatel antenna is located on top of the Prowler roll cage and is
laterally and longitudinally aligned with the Crowssbow IMU. Hall effect sensors mounted on
behind the wheel hubs yield wheel speeds at each wheel. Celesco potentiometers mounted on
the suspension components produce measurements of suspension deflections. Brackets were
created that position the potentiometer such that it is collinear with the shock and spring
assembly as seen in Figure 7.1. A Celesco string potentiometer is mounted on the steering
rack and measures the turns of the steering column. The string potentiometer provides a
measurement of the steering angle at the wheels with the assumption that steer angles at
the left and right wheels are equal.
93
Figure 7.1: Sensor Locations
The black case mounted on the front of the Prowler holds almost all of the sensor
electronics as shown in Figure 7.2. The Septentrio and Novatel receivers are stacked on top
of one another and connect via serial port to the Advantech fan-less computer, the silver
brick furthest from the viewer in Figure 7.2. The Crossbow IMU communicates via a serial
connection also. The steering potentiometer and suspension potentiometers are both read
by an A/D converter on a Microchip PIC and custom board. Each board is housed in an
aluminum enclosure shown in Figure 7.3. The PIC sends the converted reading to the CAN
Bus which is read via a PCAN USB dongle by the Advantech computer.
The Advantech computer runs Ubuntu which is a Linux operating system. The MOOS
architecture is the C++ platform used to log the data. Sensor interfaces that inherit from a
base sensor class were written to read and log each sensor.
94
Figure 7.2: Sensor Logging
Figure 7.3: Suspension Potentiometer CAN Enclosure
95
It was desirable to perform tests similar to those that were performed in simulation.
However, this was not always possible due to available terrain and topography characteristics
of the NCAT track. In spite of this limitation several test sites were found to be adequate.
For maneuvers on flat terrain, the NCAT skid pad (See Figure 7.4) and the straights on the
the NCAT oval track were used (See Figure 7.5) . Note that the flat surface of the skidpad
appears to have roughly a 2 deg bank with the high portion in the north west corner and the
low portion in the south east corner. The straights of the NCAT oval track have a 1-2 deg
crown with the peak running between the two lanes. This crown is somewhat distinguishable
in Figure (7.5).
Figure 7.4: Skid Pad Constant Radius-Steady State Turns
96
Figure 7.5: NCAT Oval 1-2 deg Banked Straights
Maneuvers involving bank were performed on turns of the NCAT oval track. Both
turning sections transition from the 1-2 deg crown of the straights to roughly 8 deg of bank
and then back to 1-2 deg of crown. Figure 7.6 via a chase vehicle shows the Prowler driving
through the turn on the west end of the track.
97
Figure 7.6: NCAT Oval 8 deg Banked Turn
Specific maneuvers of inteterest were the double lane change (DLC), sinusoidal steer
inputs, constant radius steady state turns and straight driving at various speeds. The double
lane change maneuver was scaled down to the dimensions of the Prowlers base length and
track width. The prowler parmaters are shown in Appendix A and the DLC dimensions and
scaling are shown in Figure 3.1. The DLC is suitable for generating large amounts of roll.
Figure 7.7 shows the DLC implemented on the skidpad and Figure 7.8 shows the Prowler
during the lane change event. The forward direction of the DLC pointed roughly in the
?uphill? direction on the skidpad to avoid bias in the roll estimation. Double lane changes
were also performed on the straights and banked turns of the NCAT oval track but they did
not use any measured spacings or lengths.
The runs involving sinusoidal steer inputs were intended to provide insight into the
effects on bank estimation with respect to the high and low frequency steering inputs. The
sinusoids were created by a human driver and took place in the ?uphill? and ?downhill?
directions of the skidpad.
98
Figure 7.7: Double Lane Change Setup
Figure 7.8: Double Lane Change Maneuver
99
7.4 Results & Discussion
The results of the vehicle based Kalman filter observer with the experimental data were
very poor. As shown previously in Section 6.3.2 a near perfect vehicle model is needed to
yield useful estimates. The bank estimates from the cascaded navigation EKF and the 16
state coupled navigation EKF are comparable. As shown in Section 5.3, the coupled EKF
filters out some of the noise present in the suspension deflection measurement. For this
reason the two EKF results are initially compared and then only the coupled EKF is used
for analysis of experimental data.
From Eq. (3.5) and suspension component measurements, ? was found to be 2.3. The
8 deg banked turns on the NCAT oval track were used to verify the ? scale factor for the
suspension deflection measurements.
7.4.1 Coupled Vehicle Model Kalman Filter Observer Results
A portion of data from the end of a four lap run on the NCAT oval is shown in Figure 7.9
and the steer angle measurement that is the input to the coupled vehicle model is represented
by Figure 7.10. The prowler traveled in the counter clockwise direction of the the track
such that the the vehicle turned to the left through each turn. Prior to the banked turn
a sinusoid steer angle of diminishing amplitude was applied. During the banked turn a
relatively constant steer angle was maintained and after the banked turn another sinusoidal
steer input was applied.
100
?10 ?8 ?6 ?4 ?2 0 2 x 10?3
?3
?2.5
?2
?1.5
?1
?0.5
0x 10?3
Longitude(deg)
Latit
ude(
deg)
Position
720 740 760 780 800 82013
14
15
16
17
18
Time(s)
(m/s
)
HorizontalVelocity
Figure 7.9: Position of Prowler on NCAT Oval Track Kalman Filter Observer
720 730 740 750 760 770 780 790 800 810 820?5
?4
?3
?2
?1
0
1
2
3
4
5
Time(s)
Steer
Angl
e(de
g)
SteerAngleattheTire ?SteerAngleMeasurement
Figure 7.10: Steer Angle of Prowler on NCAT Oval Track Kalman Filter Observer
Figures 7.11 and 7.12 represent some of the outputs from the coupled vehicle model
that are produced with the measured steer angle as the input. Notice that the yaw rate
output does not match the measured yaw rate during the banked turn and during the high
amplitude sinusoids after the banked turn. Some of this error is from the assumption of flat
101
ground made by the model and some of the error could be from unmodeled higher dynamics.
In Figure 7.12 the Septentrio roll measurements are plotted for a reference to where the
banked turn begins and ends. Neglecting the error in the peak values, the model outputs
seem to trend well with the measurements.
720 730 740 750 760 770 780 790 800 810 820
?10
0
10
(deg/
s)
ModelOutput ?
?MeasYawRateIMU??YawRateModel
720 730 740 750 760 770 780 790 800 810 820-10
-5
0
5
10
(deg/
s)
Time(s)
??v,MeasRollRateIMU??
sRollRateModel
Figure 7.11: Model Output of the the Prowler on NCAT Oval Track Kalman
720 730 740 750 760 770 780 790 800 810 820?10
?8
?6
?4
?2
0
2
4
6
8
Time(s)
(deg)
ModelOutput ?
v,MeasRollSeptentrio?s,MeasRollDeflections
?sModel
Figure 7.12: Model Output of the the Prowler on NCAT Oval Track Kalman
102
Figures 7.13-7.15 highlight several of the estimated states from the Kalman filter ob-
server. Note that during low dynamics through the banked turn the observer estimates and
the measurements trend nicely in Figures 7.14-7.14. The process variances were chosen such
that the measurements were trusted more than the model. However even with this tuning
the dynamics of the sinusoids are more than the observer can handle as evidence by the poor
estimates of yaw rate and roll rate during the sinusoids. Again the Septentrio measurement
is given as a reference in Figure 7.14 where the estimate of the relative roll matches a little
better than the yaw rate and roll rate estimates. Finally the estimate of bank is represented
by Figure 7.15. Here it is clear that the observer performs poorly for purpose of estimating
the bank angle. As discussed in the simulation results from Section 6.3.2 the estimates vary
greatly, but for the experimental data even during the low dynamic case the estimate of
bank still varies significantly.
720 730 740 750 760 770 780 790 800 810 820?20
?10
0
10
20
(deg/
s)
Estimates ??
MeasYawRateIMU??YawRateEstimate
720 730 740 750 760 770 780 790 800 810 820-40
-20
0
20
40
(deg/
s)
Time(s)
??v,MeasRollRateIMU??
sRollRateEstimate
Figure 7.13: State Estimates of Kalman Filter Observer for Prowler on NCAT Oval Track
103
720 730 740 750 760 770 780 790 800 810 820
?10
?5
0
5
10
Time(s)
(deg)
Estimates ?
v,MeasRollSeptentrio?s,MeasRollDeflections
?sEstimate
Figure 7.14: State Estimates of Kalman Filter Observer for Prowler on NCAT Oval Track
720 730 740 750 760 770 780 790 800 810 820?14
?12
?10
?8
?6
?4
?2
0
2
4
6
8
Time(s)
(deg)
RoadBankEstimate ?
v,MeasRollSeptentrio?rBankEstimate
Figure 7.15: Bank Angle Estimate of Kalman Filter Observer for Prowler on NCAT Oval
Track
The errors in the estimates are most likely due to the inability of the model to match
precisely with the dynamics of the real vehicle. For this reason the vehicle model based
method with the coupled bicycle model is not recommend for use on vehicle as nonlinear as
the Prowler ATV.
104
7.4.2 Comparison of Cascaded and Suspension Coupled Navigation EKF
Both architectures presented and Chapters 4 and 5 were adjusted to handle the timing
and structure of real data. The filter states are very similar in comparison. The bank
estimates of each architecture match the expected measurements of bank on the NCAT oval
track. For reference, Figure 7.16 represents a plot of the measured and estimated positions
of the Prowler on the NCAT oval track. The estimates are from the 15-state Navigation
EKF.
?10 ?8 ?6 ?4 ?2 0 2 x 10?3
?3
?2.5
?2
?1.5
?1
?0.5
0x 10?3
Latit
ude(
deg)
Longitude(deg)
HorizontalPosition&Altitudevs.Time
200 300 400 500 600 700 800190
200
210
Time(s)Al
titud
e(m) NovatelGPSEstimate
Figure 7.16: Position NCAT Oval Track 15 State EKF Cascaded
7.4.2.1 Navigation EKF Cascaded
Recall that a three antenna Septentrio unit was mounted to the prowler to provide truth
measurements of roll, pitch and yaw of the vehicle. To demonstrate that the Navigation
EKF filter is correctly estimating the states correctly, the attitude measurements from the
Septentrio are compared with the attitude estimates of the 15 state navigation EKF. Figure
7.17 represents the roll and pitch comparison while Figure 7.18 represents comparison of
yaw. The data in both figures are from single lap around the NCAT oval track. The banked
turns are marked by the sections where the roll angle is roughly eight degrees. The sinusoids
105
are purposefully induced to excite the vehicle states for the estimation of biases. Note for
the data shown in Figures 7.17-7.18 the bias states have been previously estimated and were
used as the initial biases for this data set. Accordingly the covariance matrix was set to
small initial values indicating the biases were known.
Observe in Figure 7.17 that the pitch estimate is roughly one degree different from the
Septentrio measurement. Note that the difference in the pitch data from the Septentrio
is due to slight misalignment of the Septentrio antenna frame and the IMU frame. The
mounting location behind the seat of the Prowler is not completely coplanar in the direction
of pitch with the vehicle body. The mounting surface pitches downward roughly one to two
degrees.
460 480 500 520 540 560 580 600 620
?10
?5
0
5 Roll&Pitch
?v(d
eg)
460 480 500 520 540 560 580 600 620?6
?4?2
02
4
?(de
g)
Time(s)
SeptentrioNoFixSeptentrioFixedSolution
Estimate
Figure 7.17: Roll and Pitch Comparison on NCAT Oval Track 15 State EKF Cascaded
Figure 7.18 represents the yaw estimate and gives further confidence that the EKF filter
is estimating properly. The Estimates of yaw match quite well with the measurements from
the Septentrio.
106
460 480 500 520 540 560 580 600 6200
50
100
150
200
250
300
350
?(de
g)
Time(s)
Yaw
SeptentrioNoFixSeptentrioFixedSolution
Estimate
Figure 7.18: Yaw Comparison on NCAT Oval Track 15 State EKF Cascaded
The estimate of bank is shown in Figure 7.19. For reference the estimate of total roll, the
Euler roll, and the measured roll from the suspension deflections are displayed. The figure
shows a section of straight driving followed by sinusoidal motion. Then straight driving is
resumed during the banked turn after which sinusoidal motion is resumed. While in the
straight section, the one to degrees of road crown is observed in the estimate of bank from
time 460 seconds to 465 seconds. During the sinusoids the bank estimate remains within
reasonable bounds. Note that due to the orientation of the vehicle as it oscillates from left to
right the bank estimate is a combination of the crown and vehicle orientation. The vehicle
appears the oscillate in the right side lane from 460 - 475 seconds. This is evident from the
average bank estimate of two degrees. Evident from the average bank estimate of 0 degrees,
the vehicle oscillates about the center of the crown from 475 seconds till the bank begins
at 485 seconds. The NCAT oval track has banks of eight degrees through the turns. The
estimate of bank during this steady state turn is eight degrees. Note that roll measured by
suspension deflections is not zero.
107
460 470 480 490 500 510 520?12
?10
?8
?6
?4
?2
0
2
4
6
8
Time(s)
Roll
Angl
es(d
eg)
EstimatedRoll&Bank ?
vEulerRoll?sRoll(Deflections)
?rBank
Figure 7.19: Bank Estimate on NCAT Oval Track 15 State EKF Cascaded
7.4.2.2 Navigation EKF Coupled
The measurements and estimates of roll, pitch and yaw are again presented to validate
that the 16 state Coupled Navigation EKF is functioning as expected. The estimates of
roll pitch and yaw in Figures 7.20 - 7.21 match very closely the estimates of the 15 state
navigation filter estimates shown in Figures 7.17 - 7.18.
108
460 480 500 520 540 560 580 600 620
?10
?5
0
5 Roll&Pitch
?v(d
eg)
460 480 500 520 540 560 580 600 620?6
?4?2
02
4
?(de
g)
Time(s)
SeptentrioNoFixSeptentrioFixedSolution
Estimate
Figure 7.20: Roll & Pitch Comparison on NCAT Oval Track 16 State Coupled EKF
460 480 500 520 540 560 580 600 6200
50
100
150
200
250
300
350
?(de
g)
Time(s)
Yaw
SeptentrioNoFixSeptentrioFixedSolution
Estimate
Figure 7.21: Yaw Comparison on NCAT Oval Track 16 State Coupled EKF
Figure 7.22 represents a section from the NCAT oval track. The same time interval of
460 seconds to 520 seconds is used to compare the result with Figure 7.19. The estimate of
bank for the navigation EKF coupled with the bank state looks very similar to the cascaded
bankestimateapproachwithstandardnavigationEKF.However, asseenpreviouslywiththe
109
simulation data the 16 state coupled EKF filters the noise off the bank estimate. The noise
filtering is the primary advantage to the coupled approach. For this reason the remaining
experimental result analysis is presented with the coupled 16 state navigation filter.
460 470 480 490 500 510 520?12
?10
?8
?6
?4
?2
0
2
4
6
8
Time(s)
Roll
Angl
es(d
eg)
EstimatedRoll&Bank ?
vEulerRoll?sRoll(Deflections)
?rBank
Figure 7.22: Bank Estimate on NCAT Oval Track 16 State Coupled EKF
7.4.3 Additional Validation of the Coupled Navigation EKF
A circle with a radius of 50 ft was marked on the skid pad area at NCAT. Laps were
driven around the circle. The velocity was increased about every two laps until the tires
break loose and are unable to provide enough lateral force to hold the turn for the ATV.
The idea is to demonstrate the steady state cornering capabilities of a vehicle in both the
linear section of the tire curve and the nonlinear. As the speed increases more steer angle
is required to hold the turn. In the nonlinear portion of the tire curve the required steer
angle is no longer linearly proportional to the step increase in velocity. Figure 7.23 shows
Novatel measurements of position as well as horizontal velocity profile for the duration of
the 50 ft circle run. Figure 7.24 shows the bank and total roll estimates and the relative roll
measurement from the suspension potentiometers. The peaks of the bank are represented by
triangles. InthisruntheProwleristurningleftwhichmeanspositiverollisrolltotheoutside
110
of the circle. Thus at 205.8 s the vehicle was pointed roughly east and experienced positive
1.5 deg of bank. At 215.0 s the vehicle was pointed roughly west the vehicle experienced
-1.25 deg of bank. Thus this area of the skidpad appears to have roughly a bank of 1 to 2
deg. Static measurements observed while testing confirm that the skidpad area does tend to
slope about 1-2 deg in direction running from the south east to the north west.
0 1 2 3 4x 10?4
?1
0
1
2
3x 10
?4
201.0s
210.8s
205.8s
215.0s
Longitude(deg)
Latit
ude(
deg)
Position Novatel
EstimateStartEnd
BankZeroBankZeroBankPeak
BankMin
50 100 150 200 250 3000
2
4
6
8
Time(s)
(m/s
)
HorizontalVelocity
Figure 7.23: Coupled EKF on NCAT Skid Pad 50 Ft. Circle Position and Speed
200 220 240 260 280 300?3
?2
?1
0
1
2
3
4
5
201.0s
210.8s
205.8s
215.0s
Time(s)
Roll
Angl
es(d
eg)
EstimatedRoll&Bank
?vEulerRoll?sRoll(Deflections)
?rBank
Figure 7.24: Coupled EKF on NCAT Skid Pad 50 Ft. Circle Bank Estimate
111
A second circle with radius of 10 ft was marked off. Again the speed was increased
about every two laps in the same manner as the 50 ft circle. The vehicle performed laps
around the 10 ft. circle turning to the right. The smaller radius required more steer angle
at lower speeds. The prowler suspension is nonlinear in that at extreme turn angles the
outer wheel turns in more than the inner wheel as seen in Figure 7.25. Also present in
Figure 7.25 is tire deformation in the outer front tire. These properties could effect the bank
angle estimate. However, Figure 7.27 shows no noticeable growth in peak bank estimates
as the vehicle increases speed towards the conditions where asymmetrical steering and tire
relaxation are prevalent.
Figure 7.25: Tire Relaxation and Asymmetrical Steering
112
?15 ?10 ?5 0x 10?5
0
2
4
6
8
10
x 10?5
120.3s
123.2s
121.4s
124.6s
Longitude(deg)
Latit
ude(
deg)
Position Novatel
EstimateStartEnd
BankZeroBankZeroBankPeak
BankMin
60 80 1001201401601800
2
4
6
Time(s)
(m/s
)
HorizontalVelocity
Figure 7.26: Coupled EKF on NCAT Skid Pad 10 Ft. Circle Position and Speed
110 120 130 140 150 160 170 180?7
?6
?5
?4
?3
?2
?1
0
1
2
120.3s 123.2s
121.4s
124.6s
Time(s)
Roll
Angl
es(d
eg)
EstimatedRoll&Bank
?vEulerRoll?sRoll(Deflections)
?rBank
Figure 7.27: Coupled EKF on NCAT Skid Pad 10 Ft. Circle Bank Estimate
Revisiting the NCAT oval track run, the coupled EKF bank estimate was tested against
a change in velocity. Figure 7.28 shows the position and velocity of the Prowler as it travels
through one of the banked turns. At 585.0 s the vehicle begins to travel at a constant
velocity of 2 m/s. Time 594.0s marks the acceleration start, and 605.0 s is the end of the
113
acceleration. In Figure 7.29 it is observed that the bank estimate does not change during
accelerations. More importantly notice that there is evidence in the estimate of total roll,
?v, that the increased speed causes the sprung mass of the vehicle to shift to the outside
of the turn which is positive roll for a left turn such as this. Note that there is road crown
present through the banked turns. The bank should be either roughly -7 deg, the outside
lane, or -8 deg, the steeper inside lane.
?1.5 ?1 ?0.5 0 0.5 1 1.5 2x 10?3
?2.5
?2
?1.5
?1
?0.5
0x 10?3
585.0s594.0s
605.0s
Longitude(deg)
Latit
ude(
deg)
Position Novatel
EstimateStartEnd
SlowAccelerateFast
5705805906006106206300
5
10
15
20
585.0s 594.0s
605.0s
Time(s)
(m/s
)
HorizontalVelocity
Figure 7.28: Coupled EKF on NCAT Oval Track Acceleration on Banked Left Turn Position
and Speed
114
565 570 575 580 585 590 595 600 605?12
?10
?8
?6
?4
?2
0
2
4
585.0s 594.0s
605.0s
Time(s)
Roll
Angl
es(d
eg)
EstimatedRoll&Bank?
vEulerRoll?sRoll(Deflections)
?rBank
Figure 7.29: Coupled EKF on NCAT Oval Track Acceleration on Banked Left Turn Bank
Estimate
Figure 7.30 shows the position and velocity for a banked turn on the NCAT oval track.
On this particular turn the roll axis is excited through sinusoidal steering inputs. The
sinusoids start during the transition from the crowned straight to the banked left turn and
end just before the transition back to the crowned straight. The bank estimate through the
turn shown in Figure 7.31 closely resembles the bank estimate in Figure 7.29. Notice how
even the bank estimate for the transition is unaffected by the roll excitation. Also observe
the lane changes performed in the straight after the banked turn. There appears to be a
large change in bank from the center of the road to the outer lane since the bank oscillates
from 0 to 2 deg.
115
?1.5?1 ?0.5 0 0.5 1 1.5 2 2.5 3x 10?30
0.5
1
1.5
2
2.5
x 10?3
672.6s
707.8s713.5s716.9s
Longitude(deg)
Latit
ude(
deg)
Position Novatel
EstimateStartEnd
BankBeginBankEndLeftLane
RightLane
67068069070071072014
16
18
20
22
672.6s
707.8s 713.5s716.9s
Time(s)
(m/s
)
HorizontalVelocity
Figure 7.30:
670 680 690 700 710 720?12
?10
?8
?6
?4
?2
0
2
4
672.6s 707.8s
713.5s
716.9s
Time(s)
Roll
Angl
es(d
eg)
EstimatedRoll&Bank
?vEulerRoll?sRoll(Deflections)
?rBank
Figure 7.31: Coupled EKF on NCAT Oval Track Roll Excitation Bank Estimate
The last run shown in Figures 7.32-7.33 demonstrates double lane changes made from
the inner to outer lanes. In Figure 7.33 the estimate of crown for this section of straight is
observed to be about 1.5 deg.
116
?4 ?3 ?2 ?1 0 1 x 10?3
?2.5
?2
?1.5
?1
?0.5
0x 10?3 756.0s
798.0s 809.9s812.7s
Longitude(deg)
Latit
ude(
deg)
Position Novatel
EstimateStartEnd
BankBeginBankEndLeftLane
RightLane
75076077078079080081012
14
16
18
756.0s 798.0s
809.9s
812.7s
Time(s)
(m/s
)
HorizontalVelocity
Figure 7.32: Coupled EKF on NCAT Oval Track High Speed Position and Velocity
750 760 770 780 790 800 810
?8
?6
?4
?2
0
2
4
756.0s 798.0s
809.9s
812.7s
Time(s)
Roll
Angl
es(d
eg)
EstimatedRoll&Bank
?vEulerRoll?sRoll(Deflections)
?rBank
Figure 7.33: Coupled EKF on NCAT Oval Track High Speed Bank Estimate
The last thing to make note of is that one of the assumptions inherent in the use
of suspension potentiometer measurements is that the wheels remain in contact with the
ground. As shown in Figure 7.34 it is possible to violate this condition without rolling over.
When this occurs the estimate of bank will not be accurate. But, the value for full suspension
117
extension is a known constant and the measurement can be flagged when the measurement is
greater than full extension which would indicate that the tire and surface contact condition
is violated.
Figure 7.34: Wheel Lift off in Straight on NCAT Oval Track
7.5 Conclusions
The test equipment, sensors and experiments were discussed in this chapter. The vehicle
model based observer was shown to be inadequate due to both the need for a better vehicle
model and the nature of the dynamics of the maneuvers performed during testing. The
cascaded and coupled EKF bank estimators were compared and again the filtering of the
bank estimate proved to be an advantage. Thus the coupled EKF is the preferred method.
118
Chapter 8
Conclusions & Future work
8.1 Conclusions
In conclusion, several methods that utilize relative roll information given by suspension
potentiometer measurements were explored, simulated and experimentally tested. First the
? parameter that describes the reletionship between the suspension deflections and relative
roll was explored. Two methods for finding ? were presented: an experimental method that
requires truth measurements of Euler roll and road bank and an analytical method based
on the suspension geometry of the vehicle. The architecture of the extended Kalman filter
was explained and then implemented alongside the relative roll measurement in a cascaded
architecture and a coupled architecture. Simulation revealed that the filtering provided by
the coupled EKF provided a preferred solution. Thus, the coupled EKF was chosen as the
recommended method between the two.
Additionally a vehicle model based-observer was constructed and tested in simulation.
Simulations showed that the vehicle model based observer could estimate the bank angle
for very low vehicle dynamics. However, as the model and real vehicle were not close the
performace of the algorithm was poor. This was evident under high dynamic maneuvers
where the assumptions of the model were violated.
All three methods were tested with real data off the Prowler ATV from runs made at
the National Center of Asphalt Technology. The experimental data confirmed what was
observed in simulation. The vehicle model based observer performed much worse than the
kinematic based-EKF approaches. The coupled EKF was determined to be the best method
for estimating road bank due to its accuracy and filtering of the bank estimate. Several
experimental runs at the NCAT facility demonstrate this efficacy.
119
8.2 Future Work
For future work, the model from the vehicle model based observer could be improved.
Specific improvements to the vehicle model may include more accurate model parameter
values, a more complex model than the coupled roll and bicycle model presented in Chapter
6 and a nonlinear tire model. Also, adding a measurement of lateral acceleration via a
measurement of side-slip could aid the current model such that the observer is useful under
high dynamic maneuvers.
The cascaded and coupled EKF bank estimation methods presented in Chapter 4 and
5 could be altered to estimate road grade. This would be accomplished by subtracting the
deflections from front and rear ends of the vehicle and dividing by the wheel base, wb, for
both the left and right sides. Doing so would generate a relative pitch , ?s, represnented by
Eq. (8.1).
??s = sin?1
parenleftBigg?L
LF??LLR + ?LRF??LRR
2wb
parenrightBigg
(8.1)
A new scaling value and the relationship between the suspension geometry for the rela-
tive pitch measurement would also need to examined. Finally, the architectures could be
implemented for online use to provide estimates in real-time.
120
Bibliography
[1] NHTSA. Fatality analysis reporting system (fars) encyclopedia. Technical report,
http://www-fars.nhtsa.dot.gov/Main/index.aspx, April 2012.
[2] NHTSA. Rollover data special study final report. Technical Report DOT HS 811 435,
National Highway Traffic Safety Administration, January 2011.
[3] A. Erke. Effects of electronic stability control (esc) on accidents: A review of empirical
evidence. Accident Analysis & Prevention, 40(1):167?173, 2008.
[4] Docket no. nhtsa 2007 27662, 2007.
[5] R.P. Marimuthu, B. Jang, and S.J. Hong. A Study on SUV Parameters Sensitivity on
Rollover Propensity. 2006.
[6] N. Bouton, R. Lenain, B. Thuilot, and M Berducat. A rollover indicator dedicated to
all-terrain vehicles including sliding effects and pilot behavior. 2008.
[7] P. G?sp?r, Z. Szab?, J. Bokor, et al. Brake control combined with prediction to prevent
the rollover of heavy vehicles. In Proc. of the IFAC World Congress, Praha, 2005.
[8] R. Rajamani, D. Piyabongkarn, V. Tsourapas, and JY Lew. Real-Time Estimation of
Roll Angle and CG Height for Active Rollover Prevention Applications. 2009.
[9] S.C. Peters and K. Iagnemma. An analysis of rollover stability measurement for high-
speed mobile robots. In Proceedings of the IEEE International Conference on Robotics
& Automation, 2006.
[10] J. Ryu and J.C. Gerdes. Estimation of vehicle roll and road bank angle. In Proc. Amer.
Control Conf, pages 2110?2115, 2004.
[11] A. Hac, T.D. Brown, and J. Martens. Detection of vehicle rollover. 2004.
[12] ISO 3888-2:2011. Passenger cars ? test track for a severe lane-change manoeuvre ? part
2: Obstacle avoidance.
[13] S. Gleason and D. Gebre-Egziabher, editors. GNSS Applications and Methods. Artech
House Publishers, 2009.
[14] P. D. Groves, editor. Principles of GNSS, Inertial, and Multisensor Integrated Naviga-
tion Systems. Artech House Publishers, 2008.
121
[15] P. S. Maybeck. Stochastic Models, Estimation and Control, volume 1-3. New York:
Academic Press, 1979.
[16] I. Rhee, M.F. Abdel-Hafez, and J.L. Speyer. Observability of an integrated gps/ins
duringmaneuvers. Aerospace and Electronic Systems, IEEE Transactions on,40(2):526?
535, 2004.
[17] C.R. Carlson and J.C. Gerdes. Optimal rollover prevention with steer by wire and
differential braking. In Proceedings of IMECE, pages 16?21, 2003.
[18] J. Hahn, R. Rajamani, S. You, and K. Lee. Road bank angle estimation using distur-
bance observer. In Proceedings of AVEC 6th International Symposium, 2002.
122
Appendices
123
Appendix A
Vehicle Properties
A.1 CarSim?s Small SUV
Table A.1: Small SUV Vehicle Parameters
Parameter Symbol Value Units
mass m 1142 Kg
Inertia x axis Ixx 439.9 Kg?m2
Inertia y axis Iyy 1296 Kg?m2
Inertia z axis Izz 1296 Kg?m2
Track Width tw 1.79 m
Wheel Base wbor L 2.20 m
Front Split a 0.88 m
Rear Split b 1.32 m
CG Height hrg 0.64 m
Axle Height haxle .34 m
Roll Center Height hrc .3 m
Table A.2: Small SUV Suspension Parameters
Parameter Symbol Value Units
Spring Mount Separation Front s1,s 1.79 m
Damper Mount Separation Front s1,d 1.79 m
Spring Stiffness Front k1 10,000 N/m
Damper Coefficient Front b1 6000 N?s/m
Auxiliary Roll Stiffness Front (Anti-Sway Bar) k?,1,aux 251 N?m/deg
Spring Mount Separation Rear s2,s 1.00 m
Damper Mount Separation Rear s2,d 1.79 m
Spring Stiffness Rear k1 10,000 N/m
Damper Coefficient Rear b1 6000 N?s/m
Auxiliary Roll Stiffness Rear (Anti-Sway Bar) k?,1,aux 225 N?m/deg
124
Table A.3: Small SUV Roll Stiffness and Damping
Parameter Symbol Value Units
Auxiliary Roll Stiffness Front (Anti-Sway Bar) k?,aux N?m/deg
Roll Stiffness k? 1.00 N?m/deg
Roll Damping b?? 1.79 N?m/deg
k?,aux = (k?,1,aux +k?,2,aux)2 (A.1)
k? =
parenleftBig
k1s21,s +k2s22,s
parenrightBig
2 +k?,aux (A.2)
b?? =
parenleftBig
b1s21,d +b2s22,d
parenrightBig
2 +k?,aux (A.3)
Table A.4: Small SUV Cornering Stiffness
Parameter Symbol Value Units
Cornering Stiffness Front Axle C?f 1280.3 N?m/deg
Cornering Stiffness Rear Axle C?r 1280.3 N?m/deg
125
A.2 Prowler ATV
Table A.5: Prowler Vehicle Parameters
Parameter Symbol Value Units
mass m 1142 Kg
Inertia x axis Ixx 439.9 Kg?m2
Inertia y axis Iyy 1296 Kg?m2
Inertia z axis Izz 1296 Kg?m2
Track Width tw 1.118 m
Wheel Base wb 1.448 m
Front Split a 0.8397 m
Rear Split b .6949 m
CG Height hcg 0.64 m
Axle Height haxle .34 m
Roll Center Height hrc .3 m
126