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