A Fully Integrated Sensor Fusion Method Combining a Single Antenna GPS Unit with Electronic Stability Control Sensors by Jonathan Ryan A thesis submitted to the Graduate Faculty of Auburn University in partial ful llment of the requirements for the Degree of Master of Science Auburn, Alabama August 6, 2011 Keywords: sensor fusion, vehicle dynamics, sideslip estimation, roll estimation, indirect tire pressure monitoring, steering misalignment detection Copyright 2011 by Jonathan Ryan Approved by David Bevly, Chair, Associate Professor of Mechanical Engineering John Y. Hung, Professor of Electrical Engineering Subhash Sinha, Professor of Mechanical Engineering Abstract This work presents a method for incorporating GPS (Global Positioning System) and standard roll stability control (RSC) sensors into the electronic stability control (ESC) and RSC systems. It is an adaptation of the very well known loosely-coupled GPS/INS (Inertial Navigation System) integration strategy which has been modi ed for the purposes of ESC systems. The rst modi cation is the removal of the pitch rate gyroscope, a sensor which is unavailable on commercial vehicles. The second modi cation deals with the observability problems of the standard loosely coupled lter by adding heading constraints when the ve- hicle is not turning. The structure and algorithm of this method is presented. Observability conditions are evaluated, and the convergence of the estimates are analyzed via simulations. The conclusions from these simulations are compared with the expectations from the lit- erature and observability condition checks. An experiment which illustrates the long term performance of the bias estimation was performed, followed by an experiment showing the roll and sideslip estimation performance during dynamic events. It is shown that over the long term the inertial bias estimates will converge if the vehicle experiences adequate dy- namics, and that the system is able to accurately estimate sideslip and roll during dynamic maneuvers. The system is also able to estimate slow sideslip buildup, an important capabil- ity for ESC systems. The uni ed system is compared with a less integrated or \modular" approach for both experiments. Furthermore, a method for using GPS to detect tire pressure changes is presented based on the hypothesis that the tire e ective radius varies according to tire pressure. A technique using GPS and wheel speed signals to estimate the e ective radius of the tires is discussed ii and validated in simulation and experiment. Experiments are given to show how the ra- dius estimate varies according to tire pressure, and a simple pressure loss detection law is discussed. A method to detect steering misalignment is also presented. iii Acknowledgments This material is based upon work supported by the National Science Foundation Grad- uate Research Fellowship under Grant No. DGE-0809382. I would rst like to acknowledge my advisor, Dr. Bevly, for his obvious role in my graduate education. In many ways he is the reason I started down this path of grad school, urging me to consider graduate studies in the rst place and also challenging me to go after fellowships I never would have otherwise dreamed of. In many ways this exempli es his main contribution to my education. That is, he believes in his students and expects them to achieve excellence. For this, and for his investment in me as a student, I am grateful. I also acknowledge his contribution to the GAVLAB intramural basketball team as one of our primary o ensive producers. I also wish to acknowledge my committee, Dr. John Hung and Dr. Subhash Sinha, for their oversight on this thesis. I would now like to acknowledge my parents, who have been encouraging and supportive throughout my entire life, and no less in graduate school. There is no doubt that I would not be where I am today without them. Likewise I want to thank my grandparents for their constant love and support, for always being there, and for always encouraging me to do my best, something which they have each modeled with great integrity. I also owe much to my fellow students in the GAVLAB. Rob, Broderick, and Will were especially helpful when I rst began and each frequently o ered their own time to help me with this or that. Likewise Hodo, Ben, Lashley, and Wei were always there when I had questions. All of these guys set an example both in excellence in research and also in the importance of working with and investing in coworkers around you. I owe a great deal also to the men of L2: Lowell, Ryan, Scott and Wei. Those were good times, and I dare say we took L2 to new heights and ushered in its golden era (mostly because Ryan and Lowell took iv the e ort to deep clean the place). I also want to thank Jeremy, Jordan, Chris, and John for their input and help over the last months. Jordan especially deserves special gratitude for taking charge of the new G35 and making it the sleek, 007 -esque data collection machine that it is. I wish to thank Ford Motor Company for sponsoring this work and giving me the opportunity to come to Dearborn for the summer. It was an invaluable experience, and I enjoyed meeting all of the good people up there. Special thanks goes to Jianbo Lu for his involvement in this work and all of the assistance he provided over the years. Finally, I wish to acknowledge my wife Ginger Ryan with deepest gratitude for her unwavering love and support during this time. She has always completely supported me and believed in me throughout my studies, even when it meant delaying some of her own dreams. She has truly exempli ed to me the love of Christ in the way that she has sacri ced for our family and cared for my interests above her own. Her companionship is sweet, her wisdom is deep, and her baking is delicious. This work would not be what it is without her, for I would not be who I am without her. For all of these people and this opportunity I give thanks to the Lord Jesus Christ. v Table of Contents Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ii Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iv List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . viii 1 Introduction and Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.1 Literature Review . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 1.2 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 2 GPS/INS Integration Algorithms . . . . . . . . . . . . . . . . . . . . . . . . . . 13 2.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 2.1.1 Kalman Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 2.1.2 Sensor Rotations and Coordinate Frames . . . . . . . . . . . . . . . . 15 2.1.3 Inertial Sensor Models . . . . . . . . . . . . . . . . . . . . . . . . . . 18 2.1.4 GPS Sensor Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18 2.2 GPS/INS: The Modi ed Modular Estimator . . . . . . . . . . . . . . . . . . 20 2.2.1 Estimation Strategy Overview . . . . . . . . . . . . . . . . . . . . . . 20 2.2.2 Road Grade Estimation . . . . . . . . . . . . . . . . . . . . . . . . . 21 2.2.3 Heading Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 2.2.4 Lateral Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 2.3 GPS/INS: The Automotive Navigation (AUNAV) Estimator . . . . . . . . . 29 2.3.1 The Loosely Coupled Algorithm . . . . . . . . . . . . . . . . . . . . . 29 2.3.2 Modi cations to the Loosely Coupled Algorithm . . . . . . . . . . . . 34 2.3.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 3 Observability of the AUNAV Estimator . . . . . . . . . . . . . . . . . . . . . . . 40 3.1 De nitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40 vi 3.2 Applications in Literature . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 3.3 Observability Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45 3.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57 4 Experimental Validation of the AUNAV Estimator . . . . . . . . . . . . . . . . 59 4.1 Initialization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60 4.2 Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 4.3 Slowly Growing Sideslip . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74 4.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78 5 Experimental Comparison with the MME Estimator . . . . . . . . . . . . . . . 79 5.1 Initialization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80 5.2 Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90 5.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95 6 Other GPS Applications: Tire Radius Estimation, Tire Pressure Monitoring, and Steering Misalignment Detection . . . . . . . . . . . . . . . . . . . . . . . . . . 98 6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98 6.2 Tire Rolling Radius . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98 6.3 Tire Pressure Monitoring . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 6.4 Steering Misalignment Detection . . . . . . . . . . . . . . . . . . . . . . . . . 106 6.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109 7 Conclusions and Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112 Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118 vii List of Figures 1.1 Sideslip De nitions in the Navigation and Body Coordinate Frames. . . . . . . . 2 1.2 SAE Coordinate System, Body Frame [32]. . . . . . . . . . . . . . . . . . . . . . 4 2.1 Diagram of the Modi ed Modular Estimator. . . . . . . . . . . . . . . . . . . . 20 2.2 Diagram of the Standard Loosely Coupled Integration Strategy. . . . . . . . . . 30 3.1 Observability of the Loosely Coupled and AUNAV Filters During Longitudinal Dynamics. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47 3.2 Observability of the Loosely Coupled and AUNAV Filters During Lateral Dynamics. 48 3.3 Observability of the LC Filter Compared with the AUNAV Filter During Longi- tudinal Dynamics when the Course Measurement is Conditionally Added to the AUNAV Filter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 3.4 Observability of the LC Filter Compared with the AUNAV Filter During Lateral Dynamics when the Course Measurement is Conditionally Added to the AUNAV Filter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 3.5 Speed and Yaw Rate Pro le of Convergence Test Simulation. . . . . . . . . . . . 51 3.6 Convergence of the AUNAV Accelerometer Bias Estimates During Simulation. . 51 3.7 Convergence of the AUNAV Gyroscope Bias Estimates During Simulation. . . . 52 viii 3.8 Convergence of the AUNAV Gyroscope Bias Estimates During Experimental Testing with Limited Dynamics. . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 3.9 Velocity and Yaw Rate from Experimental Testing. . . . . . . . . . . . . . . . . 54 3.10 AUNAV Roll Angle Estimate Convergence During Simulated Test. . . . . . . . 55 3.11 AUNAV Pitch Angle Estimate Convergence During Simulated Test. . . . . . . . 55 3.12 AUNAV Yaw Angle Estimate Convergence During Simulated Test. . . . . . . . 56 4.1 Vehicle Trajectory During Initialization Experiment. . . . . . . . . . . . . . . . 61 4.2 Pro le of Dynamic Conditions During Initialization Experiment. . . . . . . . . . 61 4.3 AUNAV Accelerometer Bias Estimation and Convergence During Initialization Experiment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 4.4 AUNAV Roll Angle Estimation and Convergence During Initialization Experiment. 64 4.5 AUNAV Gyroscope Bias Estimation and Convergence During Initialization Ex- periment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65 4.6 Vehicle Trajectory for Dynamic Test on the NCAT Skid Pad. . . . . . . . . . . 66 4.7 Pro le of Dynamic Conditions During Dynamic Test on the NCAT Skid Pad. . 67 4.8 AUNAV Velocity Estimates for Dynamic Test. . . . . . . . . . . . . . . . . . . . 68 4.9 Velocity Estimation Residuals for Dynamic Test. . . . . . . . . . . . . . . . . . 68 4.10 AUNAV Sideslip Estimation During Dynamic Test. . . . . . . . . . . . . . . . . 69 4.11 Velocity Innovations During Dynamic Test. . . . . . . . . . . . . . . . . . . . . 69 ix 4.12 AUNAV Roll Estimate for Dynamic Test. . . . . . . . . . . . . . . . . . . . . . 71 4.13 AUNAV Pitch Estimate for Dynamic Test. . . . . . . . . . . . . . . . . . . . . . 71 4.14 Pitch Estimate when Using Septentrio Pitch Information as an Extra Measurement. 73 4.15 Comparison of the AUNAV Pitch Estimate with the Septentrio Measurement and the Road Grade Estimate. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74 4.16 Rate of Sideslip Growth During Simulated Test. . . . . . . . . . . . . . . . . . . 75 4.17 AUNAV Sideslip Estimate During Simulated Test. . . . . . . . . . . . . . . . . 77 4.18 AUNAV Sideslip Estimate During Experimental Test with Slowly Growing Sideslip. 77 5.1 Yaw Rate and Yaw Constraint Signals of Initialization Simulated Test. . . . . . 82 5.2 MME Estimate of the Lumped State Compared with True Simulation Values. . 82 5.3 Convergence of the MME Roll Angle Estimate During Simulated Initialization Test. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83 5.4 Convergence of the MME Accelerometer Bias Estimates During Simulated Ini- tialization Test. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83 5.5 Convergence of the MME Gyroscope Bias Estimates During Simulated Initial- ization Test. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84 5.6 Convergence of the Pitch Angle Estimate During Simulated Initialization Test. . 84 5.7 Yaw Rate and Yaw Constraint Signals During Initialization Experiment. . . . . 86 5.8 Comparison of Velocity Estimates During Initialization Experiment. . . . . . . . 86 x 5.9 Comparison of Roll Angle Estimation Convergence During Initialization Experi- ment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87 5.10 Comparison of Accelerometer Bias Estimation Convergence During Initialization Experiment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87 5.11 Comparison of Pitch Angle Estimation Convergence During Initialization Exper- iment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89 5.12 Comparison of Gyroscope Bias Estimation Convergence During Initialization Ex- periment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89 5.13 Vehicle Trajectory During Dynamic Test on NCAT Skid Pad. . . . . . . . . . . 91 5.14 Pro le of Dynamic Conditions During Dynamic Test on NCAT Skid Pad. . . . . 91 5.15 Comparison of Forward and Vertical Velocity Estimates During Dynamic Test. . 92 5.16 Comparison of Roll Angle Estimates During Dynamic Test. . . . . . . . . . . . 93 5.17 Comparison of Pitch Angle Estimates During Dynamic Test. . . . . . . . . . . . 94 5.18 Comparison of Sideslip Angle Estimates During Dynamic Test. . . . . . . . . . 94 6.1 Undriven Wheel E ective Radius Estimate Convergence in Simulation. . . . . . 101 6.2 Undriven Wheel E ective Radius Estimate Convergence with Faster Tuning in Simulation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101 6.3 Driven Wheel E ective Radius Estimate Convergence in Simulation. . . . . . . 102 6.4 Undriven Wheel E ective Radius Estimate Convergence During Figure-8 Turning Maneuvers in Simulation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 xi 6.5 Front Left Tire Radius Estimates for Di erent Pressures. . . . . . . . . . . . . . 104 6.6 Rear Right Tire Radius Estimates for Di erent Pressures. . . . . . . . . . . . . 105 6.7 Estimates for All Four Tires for All Four Experiments. . . . . . . . . . . . . . . 105 6.8 Steer Angle Signals for All Four Misalignment Experiments. . . . . . . . . . . . 109 6.9 Averaged Steer Angle Signals and Yaw Rate Signals For All Four Misalignment Experiments. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110 xii Chapter 1 Introduction and Background Intelligent safety systems are an increasing focal point in today?s automotive industry. The motivation for these systems stems from the tragic reality that tens of thousands of people are killed in motor vehicle accidents every year. For example, over 37,000 people were killed in vehicle accidents in 2008. Rollover accounted for 33% of these deaths. In fact, motor vehicle crashes are the leading cause of death among Americans between the ages of 1 and 34 [17]. The problem has received so much attention that Congress has enacted legislation requiring all new vehicles after 2012 to include certain intelligent safety systems as standard features such as electronic stability control (ESC). Electronic stability control systems and roll stability control (RSC) systems are key elements of the modern e ort to improve and increase the safety capabilities of passenger vehicles. ESC systems seek to control unsafe yaw and lateral motions of the vehicle. These motions can occur when the vehicle begins to lose traction and a dangerous over steer situation arises. One extreme example would be the back end of the vehicle \sliding out" or \ sh-tailing" during a turn. ESC systems apply control in these situations via di erential braking of the individual tires to create the desired control moment. This is also true of RSC systems, which as their name suggests seek to minimize unsafe levels of vehicle roll. Both of these systems utilize feedback control systems, requiring information about particular states of the vehicle. Two of the most critical states for these systems are the sideslip and roll angles, which are also two of the most expensive to directly measure. In order to obtain accurate information about these states without greatly increasing the production cost of the vehicle, state estimation theory must be applied with the sensors which are already on board. This is the goal of this thesis. 1 Figure 1.1: Sideslip De nitions in the Navigation and Body Coordinate Frames. Now consider the two de nitions of sideslip followed by a discussion of the vehicle coordinate frame. Figure 1.1 shows a simple diagram of a single track vehicle situated in the North-East plane of the North-East-Down (NED) coordinate frame. Not shown is the z axis of the NED frame, which goes into the page to complete a right handed system. The blue dashed lines show the body coordinate frame, with the x axis aligned with the forward direction of the vehicle, the y axis perpendicular to right side, and the z axis pointing into the page to complete the right handed system. For the purposes of this diagram, the two z axes are collinear, but this is not true generally. In Figure 1.1, the bold vector V represents the velocity vector of the vehicle?s center of gravity, while the blue vectors Vx;Vy represent the components resolved into the body frame. The angle is de ned as the \course" angle, and represents the angle of V from North. The angle is de ned as the \heading" angle 2 or yaw angle and represents the angular direction that the vehicle is facing from North. Both angles are positive in the clockwise direction. The angle is the sideslip angle, which can now be de ned in two ways. First, it can be thought of as the di erence between the direction that the vehicle is moving and the direction that it is facing as described in (1.1). = (1.1) Sideslip can also be thought of as the ratio of the lateral and forward velocities, as in (1.2). = atan(VyV x ) (1.2) In both de nitions, the sideslip angle is positive in the clockwise direction. Equation (1.2) is more suitable to intuition, because it basically shows that sideslip is simply an angular representation of the vehicle?s lateral velocity, scaled according to forward speed. If the sideslip angle is large, it means that the vehicle is sliding no matter what the forward speed is. While there is always a small amount of sideslip during any turn, it holds true that cars are not intended to go sideways. Therefore the goal for safety systems is to keep sideslip angles low. Now consider the de nitions of the attitude angles and the body xed coordinate axis. Figure 1.2 shows the SAE coordinate axis for the body frame (original image courtesy of [32]). It is a right handed system, with the X axis pointing in the forward direction, the Y axis to the right of the vehicle, and the Z axis pointing down. The angle marks the positive roll angle direction, the positive pitch angle direction, and the positive yaw angle direction. An interesting side note involving the pitch angle is that if the vehicle is driving on a road with a positive grade (pitch) angle, the Z velocity will actually be negative and vice versa. This is a point of potential confusion which should be remembered when considering road grades. 3 Figure 1.2: SAE Coordinate System, Body Frame [32]. 1.1 Literature Review State estimation approaches for vehicle dynamics can be broadly categorized into two groups: model-based approaches and kinematic approaches. Both approaches use some sort of \model", but the di erence lies in what parameterizes the equations which make up the model. In the model-based case, the model parameters correspond to vehicle parameters such as mass, inertia, wheel base, tire sti ness, etc. Strictly speaking, model based estimators might even employ some kinematic equations in the model. What matters, however, is that these equations are inevitably parameterized by vehicle parameters. By contrast \kinematic estimators" do not use any vehicle parameters. Instead all terms in their \model" equations are constituted by sensor signals such as acceleration, rotation rate, velocity, or position. Many of these accelerations and rotation rates are directly measurable with common ESC sensors, while GPS provides information about the velocities and positions. Kinematic estimators do rely on knowledge of sensor location, although this has been shown possible to estimate accurately when it is di cult to measure. This distance known as the \lever arm" does not change, making it a relatively easy problem to overcome [27]. Kinematic estimators also rely on parameters of the sensors, such as bias time constants and noise variances, yet these are possible to identify o line. Furthermore, these parameters do not change. While it 4 is true that some of these sensor parameters are a function of temperature, this relationship can be accounted for as well. The primary advantage, then, of \kinematic estimators" is that they do not rely on changing vehicle parameters. The system is robust to di erent loadings, after market suspension modi cations, tire wear, or even a completely di erent set of tires. The signi cance of the roll and sideslip angles has led to many publications documenting various methods of their estimation. This includes a large body of research involving the use of GPS to estimate sideslip kinematicly, as opposed to using vehicle models. An important distinction among these is the number of GPS antennas used, as many current vehicles are already instrumented with a single antenna system as opposed to double or even triple antenna systems. Several researchers have proposed using single antenna GPS systems to improve sideslip estimation methods. A simple approach is presented in [2] and contrasted with results from a model based estimator. A more cascaded approach is presented in [8], where a sideslip estimate is rst obtained with a yaw rate gyroscope. This estimate is used to aid sideslip estimation with a lateral accelerometer. This approach is expanded even further in [6]. Even more authors approach kinematic sideslip estimation using dual antenna GPS systems. Ryu uses a dual antenna GPS receiver with INS to estimate vehicle velocities, sideslip, roll, and road grade [40]. In [9], Bevly and Ryu present Kalman ltering methods for vehicle state estimation using both single and dual antenna GPS/INS systems. A planar model is no longer assumed, as the inertial measurements are compensated for the roll e ects [9]. In [7] and [15], the authors use a dual antenna system to obtain estimates of the lateral states, including sideslip. These estimates are then used to determine tire parameters. The kinematic approach to state estimation has also been a popular way to approach roll angle estimation. Tseng [44] presents a novel method of estimating both roll and pitch based on the inertial mechanization equation without using GPS at all. Yet far more authors elect to take advantage of GPS information. The previously referenced works [9], [6], and [40] focus on roll estimation as well sideslip. In [6] the lumped e ects of roll and the accelerometer 5 bias are estimated using a single antenna GPS and then separated using a low pass lter. The approach in [9] rst uses a single antenna system. The estimate of the roll is then extrapolated from the estimate of the lateral accelerometer bias. Since the accelerometer is not compensated for roll, the roll e ects dominate the bias estimate and therefore this provides a good method for approximating roll. The authors then compare this method with results from integration with a dual antenna GPS system. Dual antenna systems can measure roll directly, and as a result GPS/INS integrations which use such systems can produce more accurate roll estimates. The dual antenna approach is also adopted in [40], which was one of the rst papers to demonstrate this potential. The roll information can also be used with a vehicle model to separate suspension roll from the road bank, as done in [38]. Those authors go on to show in [39] that roll and pitch have signi cant e ects on sideslip estimation. It follows then that it is important to include pitch information into the estimation algorithm. In urban areas the design limit for road grade can be up to 9% at speeds of 60 mph, and up to 12% for lower speeds around 30 mph. Rural roads can go up to 10% at design speeds of 40 mph and 8% for 60 mph design speeds [33]. Therefore knowledge of the pitch will be necessary to fully exploit these GPS/INS estimation schemes on steeper roadways. Jansson estimates the road grade by combining GPS information with barometer and torque measurements into a Kalman lter [29]. Sahlholm and Johansson take a similar model based approach using drive line sensors and GPS; however they additionally present a method for recursively improving the grade estimate with new passes over the same road [41]. Lingman and Schmidtbauer also use a longitudinal vehicle model and Kalman ltering techniques to estimate both vehicle mass and road grade. This is done without any GPS information [30]. All of these are done under the context of longitudinal vehicle control, as opposed to lateral dynamic control applications. Bae and Ryu describe two methods for road grade estimation using GPS that are much more suitable for lateral estimation and control purposes [3]. These methods involve measuring total pitch directly with a dual antenna 6 GPS receiver or taking the arctangent of the ratio of the vehicle up and forward velocities obtained from a single antenna [3]. This method is expanded in [4], where the up and forward velocities are estimated using a simple Kalman lter. Since pitch rate sensors are uncommon on most commercial vehicles, the road grade estimate a ords the opportunity of replacing the pitch rate gyro with the increasingly common single antenna GPS unit. All of the above authors use \modular" sensor fusion approaches as opposed to those that are \uni ed" to estimate the roll, pitch, and sideslip angles. Speaking very speci cally, the term \uni ed" is meant to signify a lter which incorporates all position, velocity, and attitude information of the three dimensional, six degree-of-freedom (DOF) model into one single lter. The term \modular" is meant to signify two things. First, it means that all of the states are not coupled together into one single lter, rather there are separate or cascaded lters for various sets of states. These separate lters may or may not be coupled together indirectly, sharing information about certain variables, but the distinction here is that this sharing is done outside of the lter. An example of a modular approach is [39], where the authors use one lter for the heading state and gyro bias and another for the forward and lateral velocities and accelerometer biases. Second, the term modular conveys that although there might be one single lter in the overall estimator, this lter does not include states for all six degrees of freedom. An example of this would be [15], where the lter is based on a planar model (the authors compensate for roll e ects outside of the lter). The following works consider approaches which are based on a uni ed integration scheme. The authors of [10] present a navigator based on a single antenna GPS integrated with a low cost INS in a loosely coupled integration. They evaluate its performance for positioning and present some experimental results. The authors of [16] use vehicle con- straints, such as assuming no lateral or vertical velocity, to improve a standard loosely cou- pled GPS/INS implementation. These assumptions are used for performance improvement by many authors. In [18] the authors apply the constraints to a tightly coupled GPS/INS architecture. The authors of [20] also apply to velocity constraints to the loosely coupled 7 lter and evaluate its performance. The di erence between these works and this thesis is three-fold. First, all of these works use a 6 DOF IMU/INS, whereas this work only uses a 5 DOF INS. Second, all of these works assume that the lateral velocity is zero at all times, whereas that assumption is made in this thesis only on the basis of certain conditions. Fi- nally, all of these works evaluate the performance primarily with regard to position, velocity, or attitude as opposed to sideslip. Indeed they make the assumption of zero sideslip at all times, whereas in this thesis the sideslip estimation performance is the chief objective. In [26] the authors perform an observability analysis on the loosely coupled GPS/INS lter and nd that under certain maneuvers, all states can be observed. It is also possible to use magnetometers and magnetic sensors embedded into the roadway to aid the lter. Yang and Farrell demonstrate this in [45] by creating a vehicle state estimation system having three layers of redundancy which uses magnetometers, GPS, and INS to determine the vehicle states. The accuracies and observabilities of the di erent estimates are discussed regarding the availability of each of the sensors. They show that adding the magnetometers increases the observability and eliminates the acceleration requirements. The performance of the loosely coupled lter for position and velocity determination is well studied. However, the loosely coupled lter uses a pitch rate gyroscope which is not available on commercial vehicles. Therefore the lter?s performance must be evaluated in light of this sensor reduction. Furthermore, the sideslip estimation performance of the loosely coupled lter is not well documented, because the loosely coupled lter is usually employed for general navigation purposes. The potential for sideslip estimation using this lter is brie y discussed in [19], but the performance is not analyzed. Some example plots of sideslip estimates are indeed shown, although these were not generated using a loosely coupled lter, rather they are estimates from the method presented in [15]. There is even less, if any, documented studies on the sideslip estimation performance of the loosely coupled lter when the pitch rate gyroscope is removed. Evaluating this performance is a primary goal of this thesis. The yaw information which is added to the lter based on certain conditions 8 (as described later) is also a new element. This constraint is practically the same as the one added to the estimators in [8], [2], but there is no documentation regarding how this additional yaw information a ects the performance of the loosely coupled lter. Such analysis is an important part of understanding the sideslip estimation performance, and it is included in this thesis. Finally, there is a fair amount of literature documenting the observability characteristics of the system described by the loosely coupled lter, speci cally noting that the observability of the system depends on the dynamics. Yet these results cannot be taken for granted regarding the lter used in this thesis, because of the aforementioned changes to the system. This thesis presents a summary of the observability results of the loosely coupled system, followed by an observability analysis of the presented system. Therefore this thesis is distinguished from the previous works by the lack of the pitch rate gyroscope, the sideslip estimation performance analysis, the conditionally included yaw constraint, and the observability analysis of this new system. 1.2 Contributions The goal of this work is to achieve good sideslip, attitude, and velocity estimation in addition to inertial sensor bias identi cation by combining GPS with sensors which are already present in current RSC system sensor clusters. Furthermore, the operating window of the system needs to be expanded by making the system robust to all road geometries. The sensors used in this work are the GPS (single-antenna, 1Hz), accelerometers in x;y;z, a roll rate gyroscope, and a yaw rate gyroscope. There is no pitch rate gyroscope, because these are not present on commercial vehicles. Additionally, this work shows the capability of using GPS to identify changes in tire pressure without using pressure sensors. A method of using the yaw rate signal, improved with the bias estimates from the state estimator, and the steer angle sensor to estimate steering misalignment is also presented. All of these goals seek to use GPS to increase the intelligent safety capabilities of commercial vehicles without any additional sensor costs. Following are the contributions contained in this thesis: 9 Development of an algorithm (initial results presented by the author in [37]) to provide sideslip and attitude estimates using ESC sensors and single antenna GPS. Analysis of sideslip and roll estimation performance of the algorithm. Analysis of performance improvements provided by conditionally added course mea- surements. Observability analysis of the algorithm. Performance analysis of modi ed \modular" method (initial results presented by the author in [36]). Development of a method to detect tire pressure changes using GPS and wheel speed sensors. Development of a method to detect steering misalignment using bias estimates from the state estimator, a yaw rate sensor, and the steer angle sensor. The rst contribution of this work is the development of a fully integrated state esti- mation algorithm, for the speci c purpose of estimating sideslip, roll, and inertial biases, which uses only GPS and sensors present in RSC systems. The algorithm will be referred to throughout the thesis as the (au)tomotive (nav)igation (AUNAV) estimator. The AU- NAV estimator is originally based on the well known \loosely coupled" GPS/INS integration scheme [19], [23]. The di erences between the AUNAV estimator and the basic loosely cou- pled lter are that the AUNAV estimator does not use pitch rate gyroscope information and that it incorporates course information from the GPS, when it detects that the vehicle is not turning, in order to improve the sideslip estimation performance. The \navigation" term in the acronym points back to the original loosely coupled lter?s purpose, and to the fact that the AUNAV estimator still possesses the same navigation functionality even though navigation is not its primary purpose. The AUNAV algorithm was rst presented in [37], 10 showing initial sideslip and roll estimation results. Experimental data from two tests on low friction surfaces was used to compare the sideslip and roll estimates from the AUNAV lter with those produced by the commercially available Oxford RT3000 GPS/INS unit. It was shown that for both maneuvers the AUNAV lter produced roll angle estimates within one degree of the RT estimate. The sideslip estimate was within one degree of the RT estimate for the rst run and within 2 degrees on the second run, which was much more challenging from an observability standpoint. In this thesis the estimation performance of the AUNAV lter for sideslip, roll, and inertial bias estimation is analyzed with experimental data. The observability of the AU- NAV lter is also analyzed, and the convergence expected from this analysis is studied by simulation and experiment. A comparison of the estimation performance of the modi ed \modular" estimator (MME) with the AUNAV lter is also given. The modular lter was developed by Bevly in [6] and represents a di erent approach to GPS/INS sensor fusion. In [36], the author of this thesis modi ed the lter by removing the pitch gyroscope and substituting the road grade estimate for the Euler pitch angle. The sideslip estimation per- formance of the MME when in the presence of larger road grades was analyzed. In this thesis, the MME estimation performance is analyzed for the same conditions as the AUNAV system for comparison. Another contribution is a method to indirectly detect changes in tire pressure using only the sensors stated above in addition to wheel speed measurements, which are ubiquitous on commercial vehicles today. The approach is based on the idea that the \e ective" or \rolling" radius of the tire will vary as a function of tire pressure. First, a simple method of estimating the rolling radius using GPS and wheel speed sensors is discussed. This is a recursive version of the batch least squares used in [13]. The e ects of tuning and slip on the estimate are investigated, and simulations are presented to validate the estimate. Further validation is shown with experimental data. Next, experimental data is shown which illustrates how the radius estimate varies according to tire pressure. The repeatability and potential problems 11 of this method for inferring tire pressure are discussed. A simple method of detecting tire pressure loss well within the TREAD Act requirements using only the estimate of the rolling radius is put forward. The TREAD Act is a law requiring new vehicles to have Tire Pressure Monitoring Systems (TPMS) as standard features, and it contains some required performance speci cations [1]. Future work to statistically improve the pressure loss detection algorithm is also discussed. The nal contribution is a method of detecting steering misalignments using the yaw rate sensor and the steering wheel angle sensor. The yaw rate signal is improved using the yaw rate bias estimate produced by the AUNAV estimator. The premise of the detection logic is that if the vehicle is driving straight, then the steer angle ought to be very close to zero. Steering e ects from road crown can cause an o set, but these are ignored in this work as these e ects remain the same throughout all experiments. It is shown that adjustments to the front right tire toe angle, which cause a misalignment, are detectable using the corrected yaw rate signal and the steering wheel angle signal. 12 Chapter 2 GPS/INS Integration Algorithms 2.1 Background 2.1.1 Kalman Filtering A linear Kalman lter is simply a classical estimator in state variable format that incorporates the statistical knowledge of the system and sensors into the calculation of an \optimal" estimator gain L. In doing so the Kalman lter also calculates an estimate of the variances of the state estimation errors at each time step. This covariance matrix P can provide reliable con dence bounds on the estimates under certain conditions. Kalman ltering consists of two steps, referred to here as the measurement update and the time update. For GPS/INS integration applications the measurement update usually runs at a lower frequency than the time update. A new estimator gain L is calculated every time a new measurement arrives. This is contrary to basic pole-placement estimation, where the gain is constant. The innovations (the di erences between the measurements and the state estimates) are taken and multiplied by the estimator gain, and this new quantity is added to the state estimate, just like a basic estimator. The covariance is updated at this time interval as well. The following equations describe the measurement update step [14]: L = Pk CT C Pk CT +R 1 (2.1) Pk = (I LC)Pk (2.2) 13 ^Xk = ^Xk +L Y C ^Xk (2.3) In Equations (2.1-2.3), C represents the measurement matrix, ^Xk represents the current state estimate, and Y represents the measurement vector. It should be noted that neither the gain nor the covariance matrix depends on the measurement innovations or any inputs to the system. This follows from the strict list of assumptions that must be true to satisfy both the optimality of the Kalman lter and the accuracy of the covariance estimates. The time update consists of taking a model of the system, in either continuous or discrete repre- sentation, and propagating it forward in time just like a traditional estimator. Continuous equations are used during the time update in this thesis, therefore the overall lter is a continuous-discrete Kalman lter. At each time step the system input is measured, the rate of change of the state estimate is calculated, and the result is integrated. The rate of change of the variances must also be calculated and integrated. Trapezoidal integration is found to be su cient for this work. The following are the equations for the time update [14]: _^X k = A ^Xk +Buuk (2.4) ^Xk+1 = ^Xk + 1 2 t _^ Xk + _^Xk 1 (2.5) _Pk = APk +PkAT +BwQBTw (2.6) Pk+1 = Pk + 12 t _ Pk + _Pk 1 (2.7) The matrices Bu and Bw are the system input and noise input matrices, respectively. It can be seen here that other factors a ect the quality of the estimates, the optimality of the Kalman lter, and the accuracy of the error variance estimate P. First, the lter 14 assumes a perfect model in A;Bu; and Bw. Absolute perfection in this regard is highly unlikely. However, given that the models used for this work are kinematic sensor models, modeling uncertainty is not a major problem. Second, it is assumed that the statistics of the noise are perfectly known as well. This is not that bad of an assumption either, since the process noise is actually sensor noise which can be approximated from sampled data. Although such an approximation is not perfect, it is possible to achieve su cient accuracy by analyzing sampled data. Third, it is assumed that the measurement errors are uncorrelated. The measurements in this work are the position and velocity solutions from a stand alone GPS receiver. These are likely the outputs of Kalman lters themselves, and would therefore have time correlated error, violating this assumption. Yet it has been shown that the impact from this is minimal, and may be overcome by simply increasing the tuning values for the measurements [23]. Therefore despite the stringent assumptions required to satisfy the lter?s optimality and error variance estimation, the Kalman lter does perform very well for this application. Furthermore the structure of the Kalman lter o ers many advantages for GPS/INS integration. GPS receivers o er highly accurate, unbiased estimates of a vehicle?s velocity, but these are output at slower update rates on most receivers. They can also su er from loss of signal in certain environments such as \urban canyons" or heavily wooded regions. INS systems boast much higher update rates but su er from biases and errors that grow over time. Combining the two truly o ers the best of both: an accurate, unbiased estimate of the vehicle states at a high update rate which can handle a loss of GPS satellite coverage for short periods of time. 2.1.2 Sensor Rotations and Coordinate Frames The accelerometers and the gyroscopes of the IMU provide measurements which are resolved in the coordinate frame of the IMU (the sensor frame), and not the reference coor- dinate frame (the navigation frame). The two methods of GPS/INS integration presented in this work each use a di erent navigation frame. The MME lter uses the \local tangent" 15 frame. This X axis of this system always points in the forward direction of the vehicle (see Figure 1.1), but it is not aligned in the pitch direction with the X axis of the body frame. The Z axis of the vehicle points to the center of the reference ellipsoid. The origin of this frame is at projection of the center of gravity onto the local tangent plane. This frame can be thought of as traveling with the vehicle, but it does not pitch or roll. It?s XY plane is always aligned with the local tangent plane, and it yaws with the vehicle. The AUNAV lter uses the North-East-Down frame. The X axis of this frame points due North, and the Z axis points to the center of the reference ellipsoid. The only di erence between the two coordinate frames is a yaw angle rotation, speci cally the yaw angle of the vehicle. In both navigation frames the Y axis is chosen to complete the right-handed coordinate systems. The body frame is in general not aligned with the navigation frame, potentially due to hills, banked roads, or suspension de ections caused by dynamic maneuvers or vehicle loading. It is important, then, to resolve both the IMU accelerometer and gyroscope signals into the common navigation frame through a series of steps known as IMU mechanization. The following equations show how to resolve the IMU into the navigation frame. The accelerometers are rotated using the standard series of three body xed Euler rotations. This not only brings them into the common navigation frame but also removes the gravitational e ects present in the X and Y accelerometers. For rotations into the local tangent plane, the yaw angle in (2.10) is 0. For rotations into the NED frame, represents the vehicle heading. Equation (2.8) shows how the rotation matrix Rnb is used to rotate the measured acceleration signals ab from the body frame b into the navigation frame n. an = Rnbab (2.8) Technically, the inertial sensors only provide measurements in the body frame if the IMU is perfectly aligned with the vehicle body. This is rarely the case, however misalignments are usually very small and are not a focus of this thesis. The rotation matrix Rnb is formed by a series of three body- xed rotations which are yaw ( ), pitch ( ), and roll ( ). Equation 16 (2.9) de nes the rotation matrix in terms of the three individual rotations, while equations (2.10 - 2.12) de ne the matrices for each of the three rotations. Rnb = Rbn 1 = (R R R ) 1 (2.9) R = 2 66 66 4 cos ( ) sin ( ) 0 sin ( ) cos ( ) 0 0 0 1 3 77 77 5 (2.10) R = 2 66 66 4 cos ( ) 0 sin ( ) 0 1 0 sin ( ) 0 cos ( ) 3 77 77 5 (2.11) R = 2 66 66 4 1 0 0 0 cos ( ) sin ( ) 0 sin ( ) cos ( ) 3 77 77 5 (2.12) The angular signals are resolved into navigation frame using the mechanization equa- tions, expressed in the mechanization matrix F . Equation (2.13) shows how the measured gyroscope signals !b are resolved from the body frame b into the navigation frame n. !n = F !b (2.13) The mechanization matrix is a function of the level angles (roll and pitch). It is de ned in equation (2.14). F = 1cos ( ) 2 66 66 4 1 sin ( ) sin ( ) cos ( ) sin ( ) 0 cos ( ) cos ( ) sin ( ) sin ( ) 0 sin ( ) cos ( ) 3 77 77 5 (2.14) 17 2.1.3 Inertial Sensor Models Inertial sensors are advantageous in that they do not su er from any sort of loss of signal, as long as they don?t fail, and that they output signals at a very high update rate, which is necessary for control. However they do have several disadvantages. Both accelerometers and rate gyroscopes have static or \turn on" biases in addition to a moving bias which is known as \drift". The accelerometer (and gyroscope) drifts are modeled as rst order Markov processes [6], being driven by white noise and having a time constant . They additionally su er from noise which can be modeled as additive Gaussian white noise. The sensors also potentially have a scale factor which scales the true value being measured [6]. It has been shown that this scale factor can be estimated, and the model assumed in this work assumes that no scale factor error is present or that it has already been estimated. Equations (2.15-2.18) show the inertial sensor models: abmeas = abtrue +Rbng + f + accel (2.15) !bmeas = !btrue + ! + gyro (2.16) fi; !i = b\turn on00 +bwalk (2.17) _bwalk t 1 ( bwalk) (2.18) Note that Equations (2.17-2.18) apply for the biases of both the accelerometers and gy- roscopes. The vectors a and ! represent the acceleration and rotation rate vectors. The vectors f and ! are the accelerometer and gyroscope biases. The \b" terms represent speci c components of the biases, and the vectors represent the noise. 2.1.4 GPS Sensor Models When only inertial sensors are available, their biases are usually compensated through certain assumptions such as a at road. If these do not hold, for example driving on a hill, 18 other information such as GPS is needed to compensate for the biases. Even after all of the biases are accounted for, the white noise remains and will still corrupt the solutions. Integrating this noise will potentially result in unbounded error growth. An additional sensor such as GPS is therefore necessary to bound the error growth. The GPS sensor model highlights the well documented advantages of GPS [6]. In the GPS/INS integration strategy used in this thesis, the GPS receiver outputs position solutions as latitude, longitude, and altitude while the velocity solutions are output in the North-East-Down (NED) navigation frame. The velocity signals are unbiased and contain no scale factor error. The errors present on the GPS positions and velocities can be modeled as uncorrelated Gaussian random noise, as seen in Equation (2.19). 2 66 66 4 ? h 3 77 77 5 meas = 2 66 66 4 ? h 3 77 77 5 + lla Vnmeas = Vn + v (2.19) In this equation, represents the latitude, ? represents the longitude, and h represents the altitude. Vnmeas is the measured velocity, Vn is the true velocity, and i is the noise vector for each solution. There do exist more detailed models of GPS errors, but this simpli ed model is representative of the error behavior at the position and velocity level and is su cient for this work. 19 Figure 2.1: Diagram of the Modi ed Modular Estimator. 2.2 GPS/INS: The Modi ed Modular Estimator 2.2.1 Estimation Strategy Overview The modi ed modular estimator is an extension of the estimator presented in [6]. The estimation strategy is largely directed by the sensors and corresponding measurements avail- able. This thesis assumes the following sensors to be available: a ve degree of freedom (DOF) IMU consisting of accelerometers mounted in the body x;y; and z axes along with roll rate and yaw rate gyroscopes, and a single antenna GPS receiver. While 5 DOF IMU clusters are not ubiquitous in the automotive world, they can be found in certain production vehicles with a roll stability control system or with a rollover curtain system. The following paragraph gives a broad, big picture outline of the estimation strategy. Figure 2.1 is a block diagram showing an overview of the process. First the IMU signals are resolved into the navigation frame using the current attitude angle estimates. This removes the biases resulting from gravity which are present in the x and y accelerometer measurements. Second, Kalman lters are used to estimate the vertical and longitudinal velocities as described in [4]. These velocities are then used to calculate an accurate estimate 20 of the road grade, which is used as a substitute for the true pitch angle. Next, a Kalman lter which estimates the vehicle heading and the yaw gyroscope bias is used, together with the GPS course measurement, to obtain an initial estimate of the sideslip. Finally, the initial sideslip estimate is combined with the GPS velocity measurement to produce a derived \measurement" of the lateral velocity for a lateral state Kalman lter. This Kalman lter estimates the vehicle?s lateral velocity, lateral accelerometer bias, roll angle, and roll rate gyroscope bias. The nal sideslip estimate is calculated in a straightforward manner from the lateral velocity estimate [8], [9]. It is available at the frequency of the IMU, while the initial sideslip estimate is only available at the frequency of the GPS. 2.2.2 Road Grade Estimation The slope of the road in the forward direction is generally referred to as the road grade. It can be presented in two forms: either as the actual slope of road (% grade) or as the angle that the road makes with the horizon; where conversion between the two is a matter of simple trigonometry. Determination of this angle can be accomplished using the ratio of the vertical and horizontal speeds, assuming that the vehicle is moving (so as not to have a zero in the denominator). GPS receivers output a vertical speed and a speed-over- ground velocity vector, where the speed over ground is the vehicle velocity vector in the local navigation frame. The magnitude of this vector can be taken to be the longitudinal speed of the vehicle in the navigation frame, assuming little to no sideslip. Therefore the arctangent of the two speeds can be taken to nd the grade angle. This method has been proven to produce high quality, unbiased estimates of the road grade, yet it should be noted that any bounce motions that the vehicle experiences will a ect the grade estimate. However, this has been shown not to signi cantly diminish the estimator?s performance [4]. Coupling the GPS system and the IMU together in a Kalman lter structure o ers many advantages. The following equations, which are simply scalar versions of the vector Equation (2.15) , show how this is possible: 21 az _Vz +gz + fz + accel (2.20) ax _Vx +gz sin + fx + accel (2.21) where az;ax represent the accelerometer measurements in the z and x directions, _Vz and _Vx are the true accelerations, gz is gravity, is the total Euler pitch, fz;x is the inherent sensor bias, and is the sensor noise. The Kalman lter now takes the following form, where the accelerometer measurement is the input and the GPS velocity is the measurement: 2 64 _^Vz _^fz 3 75 = 2 640 1 0 1Tm 3 75 2 64 ^Vz ^fz 3 75+ 2 641 0 3 75(a z gz) + 2 641 0 0 1Tm 3 75 (2.22) y = 1 0 26 4 ^Vz ^fz 3 75+ z (2.23) 2 64 _^Vx _^fx 3 75 = 2 640 1 0 1Tm 3 75 2 64 ^Vx ^fx 3 75+ 2 641 0 3 75(a x gsin ) 2 641 0 0 1Tm 3 75 (2.24) y = 1 0 2 64 ^Vx ^fx 3 75+ x (2.25) where and are the measurement noise and process noises, respectively, ^Vz;x represents the velocity estimates, and ^fz;x represent the bias estimates. Tm represents the bias time constants. Both of these estimator models are observable. The road grade estimate is the arctangent of the two speed estimates. ^ = arctan ^Vz^ Vx (2.26) 22 Since the input to the Kalman lter is an accelerometer, the process noise is taken to be noise from the accelerometer where the noise driving the bias Markov process is included as well. Both are taken to be Gaussian white noise, as shown below. = 2 64 accel 3 75 (2.27) accel N(0; 2accel) (2.28) N(0; 2m) (2.29) Ef Tg= Q = 2 64 2accel 0 0 2m 3 75 (2.30) The measurement noise is the noise on the GPS velocity signal, which is also assumed to be Gaussian white noise. Due to satellite orientations the noise is generally higher on the vertical speed measurement than on the speed over ground measurement. The measurement noise is taken to be x N(0; 2GPSx) (2.31) z N(0; 2GPSz) (2.32) Ef x Txg= Rx = 2GPSx (2.33) Ef z Tzg= Rz = 2GPSz (2.34) This lter structure combines the advantages of the high accuracy, unbiased GPS velocity measurements with the high update rate signals from the IMU, thereby o ering high quality velocity estimates (and therefore a high quality road grade estimate) at a frequency suitable for control signals. The GPS measurement enables correction for any accelerometer biases, while the accelerometer allows for continued velocity tracking during GPS outages over short time intervals [4]. 23 2.2.3 Heading Estimation Vehicle sideslip is de ned in two ways. It is the di erence between the vehicle course (the direction that the vehicle is traveling) and the vehicle heading (the direction that the vehicle is pointed). It is also the arctangent of the ratio of lateral speed to longitudinal speed (all references to course and velocities \of the vehicle" are with respect to the IMU location, which is near the center of gravity.) The vehicle course and longitudinal speed are readily available with a single antenna GPS receiver, but neither heading nor lateral velocity are measurable with only one antenna. This renders sideslip technically unobservable, but it does not make it impossible to produce useful sideslip estimates under certain conditions. A yaw rate gyro could be integrated to obtain a heading estimate which could be subtracted from the course measurement to obtain sideslip. However doing so rst requires overcoming several problems resulting from integration. The rst obstacle is any bias that may be present in the sensor. This can be estimated and removed using a Kalman lter under the condition that the vehicle is driving straight. When this is true, the course angle and the heading angle are the same. A notable exception occurs on a banked road, causing a small steady sideslip, although this would be a rare scenario for straight sections of roadway. Common road crowns will yield on a very slight steady sideslip angle. Since GPS measures course and the course is equal to heading when the vehicle is driving straight, the estimation can be \switched on" during this scenario. The Kalman lter could then estimate any biases in the gyroscope and remove them. When a turning maneuver is initiated the estimation is \switched o " and the yaw gyro is integrated to determine heading. The di erence in this heading estimate and the course measurement becomes the sideslip estimate. This process is seen the equations below, beginning with the gyroscope model, which is the scalar version of (2.16). !r _ + !r + gyro (2.35) 24 The previous equation, along with (2.17), now yields the Kalman lter. 2 64 _^ _^!r 3 75 = 2 640 1 0 1Tm 3 75 2 64 ^ ^!r 3 75+ 2 641 0 3 75(! r) + 2 641 0 0 1Tm 3 75 r (2.36) When driving straight: y = 1 0 2 64 ^ ^!r 3 75+ = GPS (2.37) Otherwise, the estimation is switched o : y = 0 0 2 64 ^ ^!r 3 75+ = GPS (2.38) When the vehicle is driving straight, the C matrix is set to [1 0] and measurement updates are performed. The C matrix is set to [0 0] when turning. In both cases the sideslip estimate is the di erence between the course measurement and the heading estimate: ^ 0 = GPS ^ (2.39) As in the case of the road grade estimation lters, the input to the system is an inertial sensor measurement. The process noise is the noise on the gyroscope signal and the noise driving the random walk. Both are assumed to be zero mean Gaussian white noise: r = 2 64 gyro 3 75 (2.40) gyro N(0; 2gyro) (2.41) N(0; 2m) (2.42) 25 Ef r Trg= Q = 2 64 2accel 0 0 2m 3 75 (2.43) The measurement noise is the noise on the GPS course measurement, and is assumed to be zero mean Gaussian white noise as well. However, the accuracy of this measurement increases with speed, giving a variance that is a function of speed. N(0; 2GPS ) (2.44) Ef T g= R = 2GPS (2.45) 2GPS = 2 V (2.46) The second problem lies in determining whether or not the vehicle is going straight. It is imperative to the estimator performance that the biases are estimated accurately prior to periods of integrating the gyroscope signal. This makes it necessary to include logic statements to determine whether or not the vehicle is driving straight, which is done using the the yaw gyro signal. The basic logic is that if the absolute value of the yaw rate is less than some threshold then the vehicle is deemed to be driving straight. Complications arise, however, from the noise on the signal. The resulting situation becomes a trade-o in the sensitivity to turning motion verses false alarms caused by the noise. If the thresholds are set too close to the noise oor the estimator will \believe" the vehicle is turning during many time instances in which it is not. This means that the heading estimation is turned o , and no gyro bias correction is done. In this work the GPS measurement only comes in at 1Hz, so losing that measurement can be very costly. The primary limitation of this sideslip estimator is its dependency on periods of straight driving to zero out the gyro bias, and every missed course measurement results in a lost second of straight driving. At moderate speeds on winding roads, for example, one second of straight driving can be rare. Therefore the tuning of these thresholds plays an important role in the overall performance of the estimator. 26 The other obvious problem with this method comes from integrating a noisy sensor signal. Some improvement could be found by low pass ltering the signal, but all of the noise cannot be removed. Therefore this estimation scheme is limited as well in terms of the amount of time that the gyro can be integrated before the error grows too large. 2.2.4 Lateral Velocity As previously noted, sideslip can be de ned as the arctangent of the ratio of the lateral speed of the vehicle to its longitudinal speed. The longitudinal speed can be estimated accurately by combining wheel speed sensors, longitudinal accelerometers, and GPS, but the lateral speed is not measurable with only a single antenna GPS system. Using the sideslip estimate from the heading Kalman lter described above, a lateral velocity \measurement" can be generated and therefore a lateral state estimator can be introduced in the form of a Kalman lter. The following equations form the foundation of this estimator. ay _Vy +Vx _ +gz sin + fy + accel (2.47) !p _ + !p + gyro (2.48) As was the case before, this sensor model assumes no scale factor error. The biases for both the lateral accelerometer and the roll rate gyro are taken to be Markov random process driven by Gaussian white noise as described previously. There are two sources of bias in the lateral accelerometer measurement, one being the roll component of gravity and the other being the sensor?s random walk. Both of these in uences have the exact same e ect on the velocity error, therefore the lter will not be able to distinguish the two. If a direct measurement of roll were available, from a double antenna GPS system for example, the two would be independently observable. Yet since roll is not measurable with a single antenna, the two states must be lumped together into one. The resulting simpli cation of the equation follows from using the small angle approximation and lumping the two terms together. 27 ay _Vy +Vx _ +gz ( + fy) + accel (2.49) This results in an estimator in the form: 2 66 66 4 _^V y _^ + _^f y _^!p 3 77 77 5 = 2 66 66 4 0 gz 0 0 0 1 0 0 1=Tm 3 77 77 5 2 66 66 4 ^Vy ^ + ^fy ^!p 3 77 77 5 + 2 66 66 4 1 0 0 1 0 0 3 77 77 5 2 64 ay ^Vx (!r ^!r) !p 3 75 + 2 66 66 4 1 0 0 0 1 0 0 0 1=Tm 3 77 77 5 2 66 66 4 accel gyro 3 77 77 5 (2.50) y = 1 0 0 2 66 66 4 ^Vy ^ + ^fy ^bp 3 77 77 5 + y = VGPS sin ^ 0 (2.51) ^ = arctan ^ Vy ^Vx ! (2.52) The process noise for the system arises from the IMU measurements. Since the measurement is constructed using the sideslip estimate from the heading lter, the measurement noise is approximated as the noise on the GPS course measurement. E !!T = Q = 2 66 66 4 2accel +V2 2gyro 0 0 0 2gyro 0 0 0 2m 3 77 77 5 (2.53) 28 Ef T g= R = 2GPS (2.54) It was stated above that the Kalman lter cannot separate the roll and the lateral accelerom- eter bias. The reason is because the two states look exactly the same in terms of velocity errors. However, they do not share the same frequency characteristics. That is, they do not change in the same way. This di erence can be exploited using complimentary low and high pass lters as shown in [6]. The following equations show the Laplace representation of the complimentary lters. ^ = Tms Tms+ 1 ^ + ^fy ^fy = 1T ms+ 1 ^ + ^fy (2.55) Since the roll angle changes much faster than the bias, this ltering approach yields an accurate estimate of both states independently. 2.3 GPS/INS: The Automotive Navigation (AUNAV) Estimator The AUNAV estimator is most easily understood as a modi ed version of the generic loosely coupled GPS/INS blending strategy. Therefore this section is divided into two sub- sections, the rst of which details the standard loosely coupled algorithm. After this, the modi cations which are made to complete the AUNAV lter are discussed. 2.3.1 The Loosely Coupled Algorithm The \loosely coupled" GPS/INS method of integration is a well documented technique for blending the GPS and INS navigation solutions [19]. Figure 2.2 gives a high level view of the integration. The INS and GPS position and velocity solutions are compared, and the di erence is input into the extended Kalman lter (EKF) as a measurement. This is 29 because the states of the lter are the di erences between the true states and INS estimates, as opposed to actual position and velocity states. The lter outputs estimated corrections to the INS solution, which are added to the INS solution. The corrected INS solution serves as the nal estimate. The EKF also estimates the inertial errors of the INS, which are fed back into the INS to continuously improve the estimation process. The system functions basically as two independent navigators (the GPS and INS) and one EKF. When using lower grade inertial navigation systems, the loosely coupled approach treats the GPS solutions as truth [19]. Therefore the states of the EKF practically become the di erences between the INS and GPS solutions. It should be noted that the inertial bias estimates are not error states, but rather they represent estimates of the actual biases themselves. Figure 2.2: Diagram of the Standard Loosely Coupled Integration Strategy. The details are of the loosely coupled approach are described as follows. The states of the Kalman lter, ^X = ^r; ^V; ^ ; ^f; ^! 0 , are the estimates of the errors in the INS position-velocity-attitude (PVA) solution and the biases of the inertial sensors. The rst step of the overall system is to calculate the INS solution. The inertial sensors are rotated into the navigation frame and then propagated forward in time via trapezoidal 30 integration to obtain the PVA estimates as in Equations (2.56-2.62), a process also known as IMU mechanization. _^ k = F (! INS ^!) (2.56) _^V k = R n b(a INS ^f) g (2.57) _^rk = T ^Vk (2.58) ^ k+1 = ^ k + 1 2 t _^ k + _^ k 1 (2.59) ^Vk+1 = ^Vk + 1 2 t _^ Vk + _^Vk 1 (2.60) ^rk+1 = ^rk + 12 t _ ^rk + _^rk 1 (2.61) T = 2 66 66 4 1 Rn+h 0 0 0 1(Re+h)cos( ) 0 0 0 1 3 77 77 5 (2.62) In Equations (2.56-2.57), Rnb represents the rotation matrix from the body frame to the NED frame, and F represents the mechanization matrix to align the gyroscopes with the NED frame. These are outlined in Section 2.1.2 in more detail. The matrix T transforms the velocities from the NED frame into rates of latitude, longitude, and altitude. Re and Rn are parameters of the reference ellipsoid. During this step the EKF also propagates the uncertainty of the error estimates forward using standard Kalman lter covariance update equations linearized about the current state estimates. This is shown in Equation (2.63), 31 where J is the Jacobian (de ned later), and Bw is simply a 15 15 matrix with the vector ([03x3;Rnb;Rnb;Rnb;Rnb]) on the diagonal and 0?s everywhere else. _Pk = JPk +PkJT +BwQBTw (2.63) Equation (2.63) is then integrated according to Equation (2.7) to complete the covariance propagation. Equations (2.56-2.63) are done at the update rate of the INS. The error esti- mates are not propagated during this time, because they are reset to zero each time they are added to the INS solutions. When GPS measurements arrive, the di erence between the GPS and INS position and velocity solutions are taken in Equation (2.64). Y = 2 64 r V 3 75 GPS 2 64 ^r ^V 3 75 (2.64) This di erence serves as measurement for the error state Kalman lter. At this point the Kalman lter calculates the gain L, error residuals, and the covariance matrix as per the stan- dard Kalman lter measurement equations outlined in Section 2.1.1; followed by updating the error states as in Equation (2.65). ^Xk = ^Xk +L Y C ^Xk (2.65) The term C in Equation (2.65) represents the measurement model. It comes from the fact that the GPS outputs position and velocity measurements. The C matrix is de ned below. C = I6x6 06x9 6x15 (2.66) The estimates of the INS velocity and attitude errors are then added to the current INS solution, as per Equations (2.67-2.69), and are subsequently reset to zero. ^r = ^r ^r (2.67) 32 ^V = ^V ^V (2.68) (Rnb)k = I + ^ (Rnb)k (2.69) The notation ( ^ ) indicates the skew symmetric form of the vector ^ . The Euler angles are computed from Rnb as in (2.70-2.72) [19]. k = arctan Rn b (3;2)k Rnb (3;3)k (2.70) k = arcsin (Rnb (3;1)k) (2.71) k = arctan Rn b (2;1)k Rnb (1;1)k (2.72) At each time step, whether during the INS estimate propagation or during a GPS update, the sideslip estimate is calculated in Equation (2.73) [19]. ^ = arctan ^ Veast ^Vnorth ! ^ yaw (2.73) Finally, the estimated inertial sensor errors ^f and ^! are fed back into the INS (the sub- tracted terms in Equations (2.56-2.57). This is not necessary for high grade inertial units, but it is absolutely imperative in this work due to the relatively large errors in the automo- tive grade sensors. This feedback is known as a closed loop implementation as opposed to open loop. The extended Kalman lter is based on the inertial sensor error models, Equations (2.15-2.18), and on the error propagation equations of the INS position, velocity, and attitude solutions which are Equations (2.74-2.76), which are simpli ed versions of those found in [19]. These are the equations used to form the Jacobian matrix which is required to update the covariances. signi es INS estimation error. 33 _r T V (2.74) _V Rnb aINS +Rnb f (2.75) _ Rnb ! (2.76) J = 2 66 66 66 66 66 4 03 T 03 03 03 03 03 Rnb aINS Rnb 03 03 03 03 03 Rnb 03 03 03 1 I3 03 03 03 03 03 1 I3 3 77 77 77 77 77 5 (2.77) 2.3.2 Modi cations to the Loosely Coupled Algorithm There are two primary changes that need to be made to the loosely coupled lter in order to accurately estimate sideslip and roll using only sensors available on current vehicles. First, the pitch rate gyroscope must be removed, as these are not currently available on commercial vehicles. Therefore the impact of this sensor reduction on the estimation performance of the loosely coupled estimator must be studied. It is hypothesized that the AUNAV lter will be able to accurately estimate the sideslip and roll despite this sensor reduction. It is also expected that even without the pitch rate gyroscope the AUNAV estimator will be able to estimate the low frequency component of pitch. This is important since the road grade is the low frequency component and it has much higher amplitudes than the higher frequency suspension pitch changes. Both of these expectations are con rmed in Chapter 4. Additionally, the observability of the new system requires evaluation. This is done in Chapter 3. It is also important to note that because the pitch rate gyroscope bias is one of the states of the EKF in the loosely coupled lter, the number of states of the AUNAV 34 estimator is reduced by 1. The AUNAV estimator has 14 states, therefore all of the EKF matrices must be resized accordingly. The second change that is made to the loosely coupled estimator is that a measurement of the vehicle?s course angle from the GPS is added when the vehicle is driving straight. The loosely coupled estimator su ers from observability issues when there is little excitation present [19,28]. Many authors seek to overcome the problem by adding velocity constraints [16, 18, 20]. They make the assumptions that the vehicle?s lateral and vertical velocities in the vehicle frame are both zero at all times. These constraints are then added in the form of a virtual measurement update. Practically speaking, the latter assumption simply means that the vehicle is constrained to the road. The former assumption, however, is tantamount to assuming that there is no sideslip. This is acceptable for navigation purposes, but it will obviously not do given the explicit goal of estimating sideslip. So in order to add constraints that do not violate the estimation of sideslip, a virtual measurement of the course angle is created when the vehicle is driving straight. Recall the de nition of vehicle sideslip given by Equation (1.1). If the vehicle is driving straight, and assuming that the side slope (bank) of the road is small, then the sideslip will be practically zero. If this is true, then from Equation (1.1) it is clear that the course and yaw angles are equivalent. The course angle can be calculated from the GPS north and east velocities as shown in Equation (2.78). GPS = arctan V east Vnorth GPS (2.78) When the vehicle is driving straight, the course angle measurement is used as a measurement of the yaw angle. The new measurement vector is shown below. Y = 2 66 66 4 r V 3 77 77 5 GPS 2 66 66 4 ^r ^V ^ yaw 3 77 77 5 (2.79) 35 It is important that the lter not use the course measurement when the vehicle is turning, because the sideslip will cause the necessary assumptions to be violated, resulting in the course angle being unequal to the yaw angle. Using the course measurement under these circumstances will result in the lter falsely attributing the inevitable error to other sources, thereby corrupting the other state estimates. Therefore a means of switching is required for the lter to toggle between using the course measurement and not using it. This is done simply by setting the term in C corresponding to the course measurement to 1 or 0, depending on whether or not the measurement is being used. When the vehicle is turning, the course measurement is not used and the C matrix is de ned as shown below. C = 2 64 I6x6 06x2 06x1 06x5 01x6 01x2 0 01x5 3 75 7x14 (2.80) When the vehicle is driving straight, the course measurement is used and the C matrix is de ned as shown below. C = 2 64 I6x6 06x2 06x1 06x5 01x6 01x2 1 01x5 3 75 7x14 (2.81) Note that the column dimension of the C matrix is now 14 instead of 15, because of the reduction of the pitch rate gyroscope bias state. Adding the course measurement keeps the attitude errors bounded during periods of straight driving, as there is no lateral excitation with which to relate the north and east velocities to the accelerations. It is not trivial to determine whether or not the vehicle is turning. At rst glance, it may seem simple enough to use the steering angle. A simple law could perhaps be used that says if the steer angle is above a certain threshold, then the vehicle is turning. The problem with this is that if the wheels are misaligned, or if there is a large enough road crown (a slight road bank that improves water drainage), then the steer angle will have some constant o set from zero. The lateral accelerometer could also potentially be used, but it is potentially 36 subject to gravitational e ects resulting from inaccurate roll angle compensation. In this thesis, the yaw rate gyroscope is used to determine whether or not the vehicle is driving straight. A ag is set if the yaw rate signal has been within 3 deg=s, consecutively, for a certain period of time. If this ag is \true", then the vehicle is assumed to be driving straight. If the absolute value of the yaw rate signal rises above the threshold even once, then the ag is reset to false. The detection logic requires that the absolute value yaw rate signal be below the threshold consecutively for a certain period of time in order to toggle the ag to true (driving straight), as opposed to simply setting the ag to true upon the rst yaw rate signal below the threshold. This is done to avoid the problem of zero crossings of the yaw rate during a turning maneuver. The yaw rate will be zero for a short period of time, for example, during a sinusoidal steering maneuver. The vehicle is still turning in this case, therefore it would be an error to assume straight driving conditions. The time requirement on the detection logic helps mitigate this e ect. There is no corresponding time requirement to toggle the ag back to false (turning). This is in order to be conservative, so as not to introduce error into the states by assuming that there is no sideslip when in fact there is. The threshold of 3 deg=s was chosen as a conservative value, because experimental tests showed that sideslip was generally extremely close to zero for yaw rates below this. There are several problems with the current method of turning detection. First, the e ects of the yaw rate bias need to be studied. For now, the threshold is simply set large enough to accommodate the bias. The bias on the gyroscope used is quite small (on the order of hundredths of a degree per second), so this is not a major problem. For lower quality gyroscopes, however, it could be. There are potential problems as well with using the yaw rate gyroscope bias estimate to try to mitigate the problem. For example, it could result in an unstable feedback type of situation, where a large initial error in the bias estimate causes yaw rate signal to always fail the test, thereby impeding the estimation of the bias. The other problem is the sensor noise. If the noise on the gyro is too high it becomes di cult to estimate slowly growing sideslip, because raising the detection threshold will mean that low rates of 37 turning will fall into the zero sideslip assumption. Setting the threshold correctly is also important. It makes sense to set it to some integer multiple of the noise standard deviation. The problem is that drivers rarely drive perfectly straight, and setting the threshold in this way would only trigger the yaw course measurement under perfectly straight conditions. In e ect, the threshold becomes a trade o between the sensitivity of the estimator and how often the course measurements are applied. Furthermore, the time requirement is a tunable parameter. Setting the value too low means that zero crossings which occur slowly will result in the estimator assuming zero sideslip in the middle of the turn. Setting the value too large limits the instances where the valuable course information is used. Therefore both the signal threshold and the time threshold are important parameters of the overall AUNAV system which must be tuned carefully. 2.3.3 Conclusion In this chapter some fundamentals of GPS systems, inertial measurement units, and Kalman ltering were discussed. Following this, the algorithm for the modi ed modular estimator was presented. In contrast to the AUNAV estimator, the MME algorithm consists of several distinct Kalman lters, each estimating states along di erent axes. Therefore there is qualitatively less coupling between the states of all of the di erent lters in the MME when compared with the coupling between the states of the AUNAV estimator. The MME is a modi ed version of the estimator presented in [6]. The modi cation consists of the removal of the pitch rate gyroscope. The pitch angle estimate in the MME comes from the road grade estimate, which is a function of the vertical and horizontal velocity estimates. Finally, the development of the algorithm for the AUNAV estimator was presented. The AUNAV estimator is developed by making two important modi cations to a loosely coupled GPS/INS lter. The classic loosely coupled algorithm is rst discussed, followed by a discussion of the modi cations. The rst modi cation is the removal of the pitch rate gyroscope (and its corresponding bias state in the EKF). This is necessary because pitch 38 rate gyroscopes are not currently available on commercial vehicles. The second modi cation is the GPS course measurement which is used when the vehicle is driving straight. This is necessary to improve the observability when the vehicle is driving straight, because there is no lateral acceleration with which to relate heading errors to velocity errors. Turn detection is done using the yaw rate gyroscope. 39 Chapter 3 Observability of the AUNAV Estimator 3.1 De nitions The notion of observability for a particular system is obviously of great importance for estimation algorithms. The observability of a system indicates whether or not it is possible to estimate the states of the system based on the given sensor con guration. As an overly simplistic example, consider the problem of determining the direction that a vehicle is facing (also known as vehicle heading). Certainly this state could not be determined if the only sensor available is a thermometer in the cabin of the vehicle. This is obvious, because the cabin temperature information is utterly unrelated to the vehicle?s heading. Yet what if GPS north and east velocity measurements were available? This information is much more related to heading, but is it enough, and under what conditions? These questions of relating the information from the sensors to knowledge of the desired states are at the heart of observability. For linear, time invariant, deterministic systems, observability is simply a function of the pair of the system dynamics matrix and the measurement matrix. Changing the measurement matrix, which implies changing the sensor con guration, or changing the system dynamics matrix, which de nes how the states are related to one another dynamically, will a ect the observability properties. This is intuitive. For linear time-varying or non- linear systems things become more complicated. Let?s begin the discussion by focusing rst on linear, deterministic systems which are not time invariant. The problems imposed by non-linearity and by stochastic in uences will be discussed afterward. A formal de nition of observability is given by [43]. 40 De nition 1. A system is said to be observable if the initial state x(t0) can be determined from the output y(t) over the nite time interval [t0;tf]. De nition 1 assumes that the input u(t) is known. This de nition serves as a general de nition of what is meant by \observability" for all types of systems, yet it is far from the only de nition in the literature. In fact the researcher cannot be too careful when considering the terminology of observability, as more speci c de nitions and conditions also abound. Silverman and Meadows o er three more discriminating de nitions of di erent types of observability for linear time variant systems [42]. They are complete, total, and uniform observability. It is worth noting, as an aside, an example of inconsistent terminology found in the literature. In [35] the authors, while citing Silverman and speaking of the exact same three observability de nitions, refer to them as complete, di erential, and instantaneous observability. In this work, the author adopts the terminology of Silverman. Here, complete observability carries the same de nition of observability as De nition 1. Total and uniform observability are de ned as follows [42]. De nition 2. A system is said to be totally observable on an interval [t0;tf] if it is completely observable on all subintervals of [t0;tf]. De nition 3. A system is said to be uniformly observable on an interval [t0;tf] if the matrix Q0(t) is full rank for all t on [t0;tf]. De nition 3 is the strongest of the three conditions. The matrix Q0(t) is de ned as follows. If a linear, continuous system is given by (3.1), then the matrix Q0(t) is de ned as in (3.2). _x(t) = A(t)x(t) +B(t)u(t) y(t) = C (t)x(t) (3.1) 41 Q0 (t) = S0 (t) S1 (t) Sn 1 (t) Sk+1 (t) = A0(t)Sk (t) + _Sk (t) S0 (t) = C0(t) (3.2) The general observability condition (De nition 1) is a necessary one for estimation purposes. For systems which are linear, deterministic, and time invariant, it is also su cient, but this is not true of the loosely coupled lter. It is important to remember moving forward that proofs of convergence are not in view, rather the focus is on veri cations that the system meets the minimum condition for estimation. It will be seen that many times, the loosely coupled lter does not. It will now be considered how these de nitions have been applied in the literature to the problem at hand, namely, the loosely coupled GPS/INS lter. 3.2 Applications in Literature In [21] Goshen-Meskin and Bar-Itzhack develop a theoretical method of analyzing the observability of linear time varying systems by considering them as piece-wise continuous systems. They rst show that often times it is valid to consider a time varying system as a sequence of consecutive time invariant systems if the time varying system meets certain conditions. This allows the authors to view the observability of the overall system as a simpler function of the observability of each time segment. This idea is then applied in [22] to the in ight alignment (IFA) problem for INS units in aircraft. Various maneuvers are reduced to distinct and consecutive time segments of constant systems, such as a constant acceleration or a constant radius turn, and the overall observability is studied. In this way it is possible to consider whether certain maneuvers will result in an observable system. Said another way, it makes it possible to theoretically decide what is an e ective sequence of motions for IFA. They conclude that, for the 12 state loosely coupled GPS/INS, any rst segment has an 42 observability matrix of rank 9. Adding another distinct segment increases the rank to 11, and any third increases it to full rank. The order of the segments does not matter, and repeating segments has no e ect. All segments in that work consist of distinct, constant accelerations in either the north, east, or down directions. This analysis is very similar in approach to most other works in the literature. That is, researchers are not evaluating the observability of the \system", but rather asking whether or not the system is observable along certain trajectories. The fact is, this system and those like it are neither linear nor time invariant, so the analysis is restricted to local observability analyses of individual trajectories. Yet this a very useful endeavor as an analysis or design tool, allowing the designer to draw conclusive and de ned operating boundaries outside of which the lter can be guaranteed to fail. Since the system is actually non-linear, the results for the local trajectory represent a necessary condition, not a su cient condition. This analysis of the observability of the nonlinear system along a certain trajectory will be referred to hereafter as the observability \of the trajectory" for simplicity. Another important work is that of Rhee et al. [35]. Here the authors look at the observability of several trajectories using the de nitions and conditions de ned in [42]. First they look at the case of constant linear acceleration, treating it as a time invariant system as in [22]. Using well known observability tests such as the Hautus test and the standard LTI observability matrix, they nd that the total system is unobservable and that the attitude angles are the unobservable modes. This brings up a very important discussion. In this case, Rhee et al. found that the observability matrix was rank de cient by 3. This means that there are 12 observable modes, which is not the same thing as 12 observable states. Modes may be states or linear functions of states. In this case, the 3 unobservable modes correspond directly to states (the 3 attitude angles). However, 6 of the 12 observable modes do not correspond to individual states, rather they are a function of other states (speci cally the attitudes and inertial biases). While there are 12 observable modes, there are only 7 observable states. These are position, velocity, and the vertical accelerometer bias. The remaining 5 observable 43 modes are functions of the other 8 unobservable states. This means that the combination of the remaining attitudes and inertial biases is observable, but that the lter is unable in this case to separate them individually. This is consistent with [22], and is basically equivalent to applying their test to just the one acceleration segment alone. In short, the lter cannot distinguish between the attitudes and sensor biases (except for the vertical accelerometer bias); however the combination of the attitude errors and the sensor biases is observable. Yet it is extremely important for the lter to be able to distinguish sensor biases from attitude, therefore knowing the circumstances under which the attitude and bias estimates cannot be independently observed is crucial. Rhee also considers the uniform observability of the case of non-constant linear acceleration or constant rotation. This is done by applying the de nition of uniform observability (De nition 3) found in [42]. In doing so they nd that maneuvers of either type increase the number of uniformly observable modes by two. For the case of non-constant axial acceleration, two attitude angles are made observable. The attitude angle around the jerk vector is the state which remains unobservable. For the constant turn case, the authors nd that the yaw angle remains unobservable. In each case, the attitude angles which are made observable by the maneuver might also cause the biases to become independently observable as well. Recall that for the case of no excitation (driving straight) there are functions of the attitudes and biases which are observable, even though the attitudes and biases aren?t observable independently. Making the attitude angles observable e ectively decreases the number of unknowns in the equations, making the biases observable also. This makes sense intuitively. Consider the lateral accelerometer model, Equation (2.47). If the only biasing e ects are from the roll angle and the sensor bias fy, and if the roll angle is observable or known, then it follows that the bias likewise is observable. Uniform observability (De nition 3) is the strongest condition of the three, and the authors do not address the general observability condition (De nition 1) for the time variant case. This makes it di cult to quantitatively compare these results with others in the 44 literature. Hong et al. [26] also examines the single antenna loosely coupled GPS/INS lter. They nd that the attitude and bias states are unobservable if the system can be represented as being time-invariant (i.e. when only undergoing constant axial acceleration). Furthermore they conclude that linear acceleration changes enhance the overall observability of these states. These ndings are qualitatively consistent with those previously mentioned, and they also align with the intuitive presentation of observability given in [19]. 3.3 Observability Simulations The above ndings in the literature paint a clear picture of the observability of the loosely coupled lter concerning most of the relevant trajectories and operating conditions. However these ndings are all for the standard algorithm, and the observability of the modi- ed algorithm in this work still needs to be considered. The modi cations to the lter include removing a state (the pitch rate gyro bias), an input (the pitch rate gyro), and adding a measurement (the yaw/course measurement). Adding the measurement of yaw will certainly improve the observability, and the degree to which it does so will be shown in this section. When considering the observability of the modi ed algorithm, the general de nition of observability given by De nition 1 is used. As a means of evaluating a system?s observability, Stengel [43] gives an equation for the observability matrix of a linear time-varying system. OLTV (tf;t0) = tfZ t0 T ( ;t0)HTH ( ;t0)d (3.3) The system is observable if OLTV (tf;t0) is non-singular, where OLTV (tf;t0) represents the observability matrix for the system on the interval [t0;tf], ( ;t0) represents the state tran- sition matrix from t0 to tf, and H represents the measurement matrix. The state transition matrix is obtained by multiplying in series the state transition matrices of each discrete time step. 45 (k;0) = Jd(k)Jd(k 1) Jd(0) (3.4) The state transition matrix of each time step is obtained by discretizing the Jacobian of the system dynamics. This is done according to (3.5) using the matrix exponential function provided by Matlab, expm() Jd = e(J Ts) (3.5) where Ts represents the sample time. The AUNAV estimator is a nonlinear system, therefore it bears restating that the observability results are local to the trajectory about which the system is linearized. It is also important to note that for the observability analysis the system is linearized not about state estimates, but about the true trajectory. This is because the problem is to determine whether or not a certain trajectory (maneuver) theoretically results in an observable time varying system. The modi ed algorithm is investigated by analyzing the rank of the matrix OLTV (tf;t0). If it is full rank, then it is also non-singular and the trajectory of interest is fully observable on the time interval [t0;tf]. If the trajectory is observable, then the lter will converge if it is tuned appropriately. The analysis from the literature provides further insight into the case where OLTV (tf;t0) is rank de cient. If we rst analyze the modi ed lter without including the yaw constraint (i.e. the standard loosely coupled lter without having the pitch rate gyro or the pitch rate bias state), we should expect to see results in accord with those discussed in Section 3.2. Several simulations were performed in Carsim to validate this expectation. A note here regarding the following results is necessary. The following plots show the rank of OLTV (tf;t0) over the course of the simulations. In general, the rank test of OLTV (tf;t0) shows whether or not the system is observable on the interval [t0;tf]. This means that if a maneuver is performed which increases the rank to full, during some time interval, then the rank will thereafter remain full and the system will be declared observable until the end of that interval. What is really 46 Figure 3.1: Observability of the Loosely Coupled and AUNAV Filters During Longitudinal Dynamics. of interest here is to show the impact of various maneuvers regarding observability. Because of this, small intervals of one second were chosen to evaluate OLTV . Two simulations were done in Carsim in order to test the observability of each trajectory. All values from the simulations are true values, no noise or other errors were added, because the analysis is done concerning the linearization about the true trajectory. The rst test involves the vehicle driving in a straight line with a period of forward acceleration followed by deceleration. Figure 3.1 shows the rank of OLTV during this test, with subplots showing the yaw rate and accelerations. Full rank is 15 for the standard loosely coupled lter and 14 for the modi ed lter. Figure 3.2 shows the results from a simulation in which the vehicle drives straight, enters a steady state turn, and resumes straight driving. The rank of OLTV during these tests con rms the ndings in the literature. Speci cally, the system is not fully observable under constant acceleration, but requires changes in acceleration to reach full observability. It can be seen in Figure 3.1 that as the vehicle begins to accelerate the system 47 Figure 3.2: Observability of the Loosely Coupled and AUNAV Filters During Lateral Dy- namics. becomes observable, but as the acceleration becomes steady the rank drops back to 11. The same is seen when the vehicle decelerates. Figure 3.2 con rms this also, showing the same behavior in regard to lateral acceleration. Under constant acceleration, the observability matrix is rank de cient by three. Constant acceleration in another direction increases the rank by two, con rming the results in [21]. The lter only reaches full rank when a change in acceleration occurs, con rming the results in [35]. These results show that the modi ed system, without the extra yaw constraint, behaves just like the standard system in regards to observability. Therefore the conclusions of Section 3.2 apply to the modi ed lter when the yaw constraint is not applied. This gives some insight into the anticipated performance of the lter during constant acceleration operation (straight steady driving). Rhee found that in this situation the attitude and bias states are unobservable [35]. This means that the estimates will likely be biased, yet because the combined e ect of the leveling angles and 48 Figure 3.3: Observability of the LC Filter Compared with the AUNAV Filter During Lon- gitudinal Dynamics when the Course Measurement is Conditionally Added to the AUNAV Filter. the accelerometer biases is observable, the errors will be bounded. This cannot be said of the yaw angle, which is known to exhibit drift during this time. Figures 3.3 and 3.4 show the results from the same two simulations, only this time the yaw constraint is imposed when the straight driving condition is met. There is no di erence in the rank of OLTV under dynamics, but the rank is closer to being full during straight driving. In this case the observability matrix is only de cient by one, as opposed to being de cient by three. The improvement of two more observable states is easily explained. During this time, the lter has direct yaw information available (because sideslip is assumed to be zero), which makes the yaw angle observable. Furthermore, since the integral of the yaw rate gyroscope is measurable, the bias of this gyro becomes observable. In this way the modi cation overcomes the problem of drifting yaw estimates. While the previous simulation results show when the lter does and does not meet the necessary conditions for estimation, the convergence of the lter still requires investigation. 49 Figure 3.4: Observability of the LC Filter Compared with the AUNAV Filter During Lateral Dynamics when the Course Measurement is Conditionally Added to the AUNAV Filter. If the system were in fact linear and deterministic, observability would imply convergence. However large initial errors or unmodeled disturbances could cause convergence to an in- correct local minimum. What?s more, it has been shown in [5] that the stochastic elements by themselves can cause the lter to diverge if the lter is improperly tuned. The authors there describe the distinction that results between standard observability and \stochastic" observability. The system can meet the necessary conditions for observability and yet diverge due to poor tuning of P;Q;R, due to too large values of P0, or due to large errors in the initial estimates. So lter convergence of the nonlinear estimator can only be expected if the trajectory is observable, the tuning is appropriate, and the initial error is not too large. The following simulations demonstrate the behavior of the lter under such conditions. These simulations consist of the combination of the two prior simulations. That is, the vehicle enters a steady state turn, disengages from the turn, and accelerates and decelerates while driving straight. The simulation is run rst without the yaw constraint, followed by 50 Figure 3.5: Speed and Yaw Rate Pro le of Convergence Test Simulation. Figure 3.6: Convergence of the AUNAV Accelerometer Bias Estimates During Simulation. 51 Figure 3.7: Convergence of the AUNAV Gyroscope Bias Estimates During Simulation. a run with the constraint. Figure 3.5 shows the speed and yaw rate for the simulations. Figures 3.6-3.11 show the estimates of the bias states and the leveling angles, where the blue signal represents the estimates without the yaw constraint and the cyan represents the estimates with the constraint. Let?s rst consider the case without the yaw constraint (blue). It can be seen in Figure 3.6 that the x and y accelerometer bias estimates do in fact converge toward the true value during periods where the observability test is full rank (during acceleration changes). The y accelerometer bias shows in particular that bias convergence is strongest when the accel- eration change is along the axis collinear with that bias. It is also observed that the z axis accelerometer bias converges regardless of the dynamics. Both of these results are in accord with the results in [22], [26] and [35]. Figure 3.7 shows the behavior of the gyroscope biases. The yaw rate gyro bias (!z bias) also behaves as expected, converging toward the true value during changes in acceleration. By contrast, the roll rate gyroscope bias (!x bias) converges rapidly, regardless of the dynamics. This result is in disagreement with the results in [35]. 52 There the authors nd that when there is no excitation (straight driving at a constant speed), an unobservable mode is given by x3x = !p z + y (3.6) where x3x is the particular unobservable mode in question (using the author?s notation), !p is the roll rate gyroscope bias, and and are the pitch and yaw errors respectively (using the notation of this thesis). The terms z and y relate to the Earth?s rotation rate, see [35] for details. It is observed that if the second two terms in Equation (3.6) are small, then x3x !p (3.7) Since x3x is an observable mode even with no excitation, then the roll rate gyroscope bias is observable under these conditions. While in reality it is the sum of the bias and the Earth rotation terms which is observable, the rotational terms are small enough compared with the bias that they can be disregarded. Carsim does not include any simulated e ects from the Earth?s rotation, which explains the convergence in simulation. In order to verify that the bias estimate converges using the true sensors, experimental data was analyzed. In this experiment, the vehicle is driven on a mostly straight road at close to a constant speed. Details of the sensors and vehicle are given in Chapter 4. Figure 3.8 shows the roll rate gyroscope bias estimates. Analysis of static data for this run reveals a bias of 0.139 deg=sec. This was found by taking the mean of the roll rate signal over 25 seconds of static data (collected at 100Hz). Is is observed that even during periods of very little excitation, the bias estimate converges to very near 0.14 deg=sec, con rming the simulation results and the hypothesis that the Earth rotational terms are small enough to neglect. Figure 3.9 is included to show the forward speed and yaw rate, showing that apart from the period 500s 84s. The roll angle error grows, creating an arti cial bias in the accelerometer, and the sawtooth shapes appear in the lateral velocity. As the roll angle begins to converge again, the magnitude of the saw teeth diminishes. The AUNAV estimator recovers more quickly from this problem than the MME does, and it produces in general a smoother estimate of sideslip than the MME. 5.3 Conclusion The same experimental tests which were performed to validate the performance of the AUNAV estimator were used to validate modular lter. The tests were divided into two phases as in the case of the AUNAV estimator. Simulations and experimental data were used to validate the initialization phase of the modular lter. The experimental data comes from the same run used to validate the AUNAV estimator initialization, and the simulation was designed in Carsim to mimic the real world experiment. The results from the simulations 95 and real data are in agreement and show that given dynamic conditions over time the modular lter is able to accurately separate the bias and level angles despite large initial errors in the bias state. The MME estimator performs well during the dynamic estimation phase of the experiment, except for the instance where the lever arm e ects come into play. The modular lter is found to be more susceptible to these e ects than the AUNAV estimator. In short, the modular lter provides good estimation performance for ESC applications. The AUNAV estimator performs better than the MME estimator by most comparisons. The roll angle estimates from the AUNAV estimator were found to be more accurate than the MME estimates in both tests. There is little performance di erence when looking at the forward and vertical accelerometer biases, but the AUNAV estimator converges faster when estimating the lateral accelerometer bias. The velocity estimates are almost qualitatively the same, although the AUNAV estimator is observed to be slightly more accurate. The pitch angle estimates are also comparable, with the main di erence being the suspension pitch. In this case the MME estimate is probably preferable over the AUNAV estimate, because it is always clear exactly what it is outputting. The MME, in steady state, always produces an unbiased estimate of the road grade. The AUNAV estimator, by contrast, may not have converged to the true pitch value. It was demonstrated in this thesis that the AUNAV pitch estimate is close to the true pitch value, and that correspondingly the forward bias estimate is close to the true value, but some ambiguity remains. This is of course due to the lack of the pitch rate gyroscope. There is no ambiguity in the road grade estimate from the MME estimator, making it the preferable pitch estimate on vehicles which have negligible suspension pitch. The AUNAV estimator strongly out performs the MME in regards to roll rate bias estimation, as shown in Section 5.1. On the other hand, the yaw rate bias is more accurately estimated by the MME estimator. This is because this estimate is only a function of the yaw rate signal and the course measurement. Therefore, misalignment errors in the yaw direction do not a ect the bias estimate. Unmodeled misalignment errors will a ect 96 the bias estimate of the AUNAV lter, as described in Section 4.1, although including these e ects in the AUNAV model could potentially mitigate these errors. It is no surprise that the AUNAV estimator outperforms the MME. The reason is simply that the AUNAV estimator is based on a more complete model of the underlying kinematic relationships. The more accurate the model is, the more accurate the estimator will be. This obvious conclusion actually draws a parallel with model based state estimation methods. The performance of those methods is also primarily a function of the model accuracy. The di erence, however, is that the model parameters for the the kinematic AUNAV and MME estimators are measurable and practically unchanging. This is the primary advantage of the AUNAV and MME estimators. Finally, although the MME estimator is capable of accurately estimating important vehicle states for ESC systems, it is outperformed by the AUNAV estimator. 97 Chapter 6 Other GPS Applications: Tire Radius Estimation, Tire Pressure Monitoring, and Steering Misalignment Detection 6.1 Introduction GPS information can be also be used to serve vehicle functions other than state esti- mation and navigation. In this chapter, GPS information is used to estimate the e ective radius of the tires, to detect pressure drops in the tires, and to detect steering misalignment. The state estimates from the AUNAV estimator can also be used to aid in these processes. For example, road grade information is important for the pressure change detection system, because large road grades a ect the tire loading and may impact the radius estimate. Fur- thermore, an accurate yaw rate signal is required for the steering misalignment detection algorithm presented here. The yaw rate gyroscope bias estimate produced from the state estimator can be used to remove the bias from the yaw rate signal in order to provide the needed level of accuracy. In short, this chapter will show how both direct GPS information and the state estimates produced from the AUNAV estimator can be used to aid in other vehicle safety functions beyond dynamic state estimation. 6.2 Tire Rolling Radius Combining GPS with on board ESC sensors can aid in estimating the tire?s rolling radius, which is an important parameter for various vehicle systems. An approach based on linear estimation techniques is presented in [31], where the authors combine GPS and wheel speed information with a vehicle model to obtain estimates of the rolling radius and the longitudinal sti ness of the driven wheels. In [11] the authors expand on the work done in [31] 98 by considering the adverse properties of linear estimation methods. Finding that noise in the measurement matrix (as opposed to merely having noise in the measurement vector) causes a bias in the linear least squares estimate, the authors propose using nonlinear optimization methods to solve the problem. Improvements to the nonlinear estimation strategy are given in [12], [13]. The approaches in all of these are for nding the longitudinal sti ness and rolling radius of the driven wheels. However, the rolling radius of the undriven wheels are estimated and discussed also. Equation (6.1) is the simple, important governing relationship which is exploited to produce the estimate of the undriven rolling radius. Vx = Reff! (6.1) Equation (6.1) describes the relationship between the forward velocity Vx, the rolling radius Reff, and the wheel speed ! under the important condition that there is no slip. In [13] the radius estimate is obtained by solving Equation (6.1) in matrix (batch) form. For reference, Equation (6.2) expands the relationship to deal with slip by including the forward slip ratio . Vx = Reff!1 (6.2) All of the above references focus on methods for estimating the driven wheel radius together with the longitudinal sti ness. These estimates are useful in themselves, however the authors also focus on how the sti ness in particular might be an indicator of the tire-road friction coe cient. This follows the hypothesis raised in [24], [25] that the longitudinal sti ness value could be an accurate predictor of the friction even at low slip values. In pursuing this investigation, the authors nd that many things in uence the sti ness estimate including tire pressure [13]. Therefore much discussion is given to how the sti ness might also be an indicator of tire pressure. In this thesis the focus is on how the estimate of the rolling radius might accurately indicate tire pressure or pressure changes, as opposed to the sti ness. While 99 minor consideration is given to this idea in [11], [12], and [13], this section of this thesis will explore how the radius estimate alone might be a successful predictor of tire pressure. If the rolling radius estimate of the undriven wheel can successfully predict tire pressure, the method might be expanded to the driven wheels using relative radius estimation methods. Therefore consider the case where there is no slip, then Equation (6.1) can be used to form a linear Kalman lter. In this case the state estimate is held constant in between the recurring measurements of Vx=!. For comparison, this is simply the recursive version of the approach shown in [13] for the undriven wheel radius. Equations (6.3) and (6.4) show the state vector and the measurement vector for the lter. ^X = Reff fl Reff fr Reff rl Reff rr T (6.3) Y = Vx fl !fl Vx fr !fr Vx rl !rl Vx rr !rr T + (6.4) In (6.4), represents the measurement noise vector. The lter operates as a normal Kalman lter according to Equations (2.1-2.3). Since the wheel speeds are by de nition the rotational speeds at the tire, the translational speeds also must be at the tire. These are denoted in (6.4) by Vx fl;Vx fr;Vx rl;Vx rr. Calculating these velocities is straightforward, and Equation (6.5) shows how this is done. Vx fl = Vx rl = Vx + 12ltw!z Vx fr = Vx rr = Vx 12ltw!z (6.5) In (6.5), !z is the yaw rate, and ltw represents the track width. Figure 6.1 shows the estimation performance in simulation. Carsim was used to simulate a simple scenario where a front wheel drive sedan drives in a straight line for 10 minutes. The estimate converges to the true value over time. Note that a small bias is present in the 100 Figure 6.1: Undriven Wheel E ective Radius Estimate Convergence in Simulation. Figure 6.2: Undriven Wheel E ective Radius Estimate Convergence with Faster Tuning in Simulation. 101 Figure 6.3: Driven Wheel E ective Radius Estimate Convergence in Simulation. estimator. This is because even for the undriven wheel there exists a very tiny steady state slip of around 0.04% (according to Carsim?s models). Yet the bias from this is miniscule, at only 0.1mm. The convergence speed is a function of the tuning, although there is a trade o between speed and smoothness. A faster tuning o ers quicker convergence. This is also seen in Figure 6.2, where the tuning has been \sped up" for the same run. That is, the process noise covariance matrix (Q) value is larger. The sensor noise covariance matrix (R) value is held constant. In this case the lter converges more quickly, but it is not as smooth. Later discussions will focus on the importance of smoothness. Figure 6.3 shows the radius estimate of a driven wheel. The transient performance is exactly the same, (since the slip is constant), but the bias is larger because the slip is larger for the driven wheel. It violates the no slip assumption stated previously, and the bias is around 1.5mm. Figure 6.4 shows the radius estimate of the undriven wheel during a simulation of the vehicle driving in a gure 8 pattern at much lower speeds, demonstrating that the lter is able to operate under turning conditions. 102 Figure 6.4: Undriven Wheel E ective Radius Estimate Convergence During Figure-8 Turning Maneuvers in Simulation. 6.3 Tire Pressure Monitoring It is hypothesized that the e ective rolling radius is a function of the tire pressure [11], [12], [13]. Since it has now been shown that GPS o ers a way to very accurately estimate the e ective radius, it is further hypothesized in this thesis that this estimate can be used to estimate the tire pressure and detect changes. In order to validate this hypothesis, four sets of data were collected on the loop at NCAT. The vehicle was driven at a nearly constant speed of 50mph for 45 minutes. Before and after each run, the pressure of all four tires was recorded. The G35 is a rear wheel drive vehicle, so the front tires are the undriven wheels. The front left and rear right tires were chosen as the variable tires, and they were run with a di erent pressure each time. While the other two tires were maintained at 33psi, the front left and rear right tires were set to 36, 32, 28, and 24 psi for each respective run. Figure 6.5 shows the estimates for the front left tire for each run. It should be noted that only the data from the straightaways is processed in the estimate. This is to limit any e ects of weight transfer 103 Figure 6.5: Front Left Tire Radius Estimates for Di erent Pressures. on the tires, as the steeply banked turns might result in small tire deformations. Recall that the radius is speci cally the e ective or rolling radius, which means it is extremely di cult measure the true value. Yet it is known that this value lies between the loaded and unloaded radii [34]. The radius estimates were all found to lie within the measured loaded and unloaded radii as expected. It can be seen that changes in tire pressure do result in changes in the rolling radius. The rst pressure drop from 36 to 32 psi results in a radius change of 0.5mm. This might seem too small to track, but the estimator is clearly able to do so given enough data to average out the noise. This is because of the accuracy of the GPS velocity measurements, which are unbiased and have noise characteristics of approximately 5 cm=s (1 ). The second drop from 32 to 28 psi results in a change of 0.5mm. The last drop from 28psi to 24 psi results in a change of 0.6mm. Figure 6.6 shows the estimates from the rear right tire. It is observed from this graph that the pressure drops correspond with decreases in the radius estimate of 0.5mm, 0.4mm, and 0.6mm. Recall that the method presented in this thesis 104 Figure 6.6: Rear Right Tire Radius Estimates for Di erent Pressures. Figure 6.7: Estimates for All Four Tires for All Four Experiments. 105 results in a biased estimate when it is applied to the driven wheels. However, this bias does not a ect the change in radius that results from the pressure. Therefore although the radius estimates of the driven wheels are not as accurate as those of the undriven wheels, they are still able to be used for pressure loss detection. Looking at Figures 6.5 and 6.6 alone would suggest that these pressure ranges constitute an easily dividable space for a lookup table or other classi er. Indeed the gaps between signals is far larger than the noise, which is a good sign. However the repeatability of the estimates must be considered. Speaking qualitatively, if there is much variation between the radius estimates of a tire with a constant pressure, then it will be very di cult to isolate radius changes which correspond to pressure changes. Figure 6.7 shows the estimates for all four tires for all runs. Recall that the front right and rear left tires were maintained near 33psi for all four runs. It appears that all of the estimates for the control tires lie within the range 331.1mm and 331.5mm. Furthermore, the estimates for the front left and rear right tires lies within this range for the 32psi run. This indicates a degree of consistency across all four tires. From Figure 6.7 it can be inferred that if the radius estimate is within this range, then the tire pressure lies within 30psi to 34psi. Similar regions can be constructed for the other pressure ranges. The space from 331.1mm to 330.7mm corresponds to a pressure between 30psi and 26psi. Below 330.7mm, the tire pressure can be considered to be less than 26psi. Clearly these conclusions only apply to this particular data, and much more data would be needed to construct a reliable tire pressure predictor. However, Figures 6.5 - 6.7 show that indirect tire pressure monitoring is possible with GPS. 6.4 Steering Misalignment Detection Steering misalignment is a problem because of the accelerated and irregular wear that it causes on the tires. Large misalignments are obvious and easily noticed by the driver, but smaller misalignment problems can be far more subtle. Even small misalignments have negative e ects on tire wear and gas mileage, therefore it would be advantageous if the 106 vehicle were able to detect small misalignments and inform the driver, and GPS provides the ability to do this. The phrase \steering misalignment" is used here to describe any condition which causes the steering system to produce an improper steer angle at the tires for a given steering wheel input. This may be caused by a variety of problems in the suspension or steering systems, but from a general standpoint the end e ect is the same. The premise of the misalignment detection strategy presented in this thesis is that if the vehicle is driving straight, then the steer angle ought to be zero degrees. This is not exactly true, however, because of the steering e ects of road crown. Most roads generally have a very small side slope in order to improve water drainage, and this slope will cause a very slight o set from zero in the steer angle. In this thesis, all tests were done on the same section of roadway, so the e ects of road crown will be the same for all experiments. Therefore the road crown e ects will be ignored, although the accurate estimate of the vehicle roll angle produced by the AUNAV estimator could be used to detect non standard road crowns. The steering misalignment detection algorithm functions as follows. It requires the steering wheel angle (SWA), an accurate yaw rate signal, noise characteristics for the yaw rate sensor, and a baseline steer angle. The baseline steer angle is the steady state steer angle for straight driving when the vehicle is perfectly aligned, which can be ascertained the rst time a production vehicle is driven. If there is no road crown, then the baseline steer angle will be zero. For the experiments in this thesis, the baseline steer angle was found to be approximately 2.25 degrees. The assumption is made that the In niti G35 was perfectly aligned before the experiments were made. Since road crown is assumed to be constant, the baseline steer angle is adjusted to be zero degrees, meaning that a constant 2.25 degrees is subtracted from all SWA measurements. This is because it is the relative steer angle compared to the baseline which is important, not necessarily the absolute steer angle. When it is determined that the vehicle is driving straight, as described in Section 2.3.2, a moving average (100 second window) of the SWA measurement is taken. The yaw rate gyroscope 107 bias estimate from the AUNAV lter is subtracted from the raw yaw rate signal during the process of determining straight driving conditions. If the average steer angle is o set from zero, when the vehicle is driving straight, then it is concluded that there is a misalignment problem. Four experiments were conducted to validate the steering misalignment detection strat- egy. These tests were conducted using the In niti G35, which is instrumented as described at the beginning of Chapter 4. Each test consisted of driving 10 minutes north on I-85 towards Atlanta while maintaining a steady speed and with minimal lane changes. The rst test was conducted without making any modi cations to the vehicle, assuming that the alignment at that time was very good. Thus the alignment for the rst test was taken as the baseline. For each subsequent test, misalignment error was manually introduced by adjusting the toe angle of the front right tire. The toe was adjusted by screwing the tie road in or out of the sleeve by some amount. In this experiment, the tie road was screwed into the sleeve, thereby shortening the overall length. Since this linkage is located behind the center of the tire, the change resulted in a toe out situation on the front right tire. The tie road was adjusted by making two turns on the screw for the rst experimental run, 1/2 turn for the second, and 1 full turn for the nal run. Figure 6.8 shows the raw SWA measurements and the corresponding moving averages for the experiments. The baseline steer angle of 2.25 degrees is also observed in this plot. Notice that the magnitude of the steer angle increases with the amount of alignment adjustments made. Figure 6.9 shows the moving averages of the steering wheel angles, the yaw rate signals for each run, and the yaw rate detection threshold for straight driving determination. It can be clearly seen that adjustments to the toe angle of the tire results in non zero steering angles when the vehicle is driving straight. Furthermore, the magnitude of the o set clearly corresponds with the degree to which the toe angle is adjusted. The yaw rate threshold for straight driving detection is shown as a reference. A threshold of 3 was used, where the assumed standard deviation of the noise on the yaw rate signal is = 0:5deg=s. The yaw rate signals have been improved by subtracting 108 Figure 6.8: Steer Angle Signals for All Four Misalignment Experiments. the estimates of the yaw rate bias provided by the AUNAV lter. In this case only a marginal improvement was obtained because of the high quality of the Crossbow gyroscopes. However less expensive sensors will have larger biases, making it necessary to use the bias estimate in order to be able to detect straight driving. Based on Figure 6.9 it is concluded that it is possible to detect even slight misalignments using GPS enhanced inertial sensor signals and the steering wheel angle. 6.5 Conclusions It has been shown that GPS information can be combined with wheel speeds to very accurately estimate the rolling radius of the undriven wheels. The algorithm operates on the assumption that there is no slip. This assumption is valid for the undriven wheels except in the case of braking. This does not present an obstacle for the algorithm however, because the braking signals are readily available on the CAN bus and can be used as a toggle on 109 Figure 6.9: Averaged Steer Angle Signals and Yaw Rate Signals For All Four Misalignment Experiments. and o switch for the estimator. It has also been shown that the rolling radius varies with tire pressure, presenting an opportunity for indirect tire pressure monitoring. That is, GPS o ers the possibility of detecting pressure losses without actually needing pressure sensors in the tire. The TREAD act requires tire pressure monitoring systems (TPMS) to notify the driver if the tire pressure has dropped by 25% of the recommended cold pressure [1]. The recommended pressure for the G35 is 33psi, which means that any TPMS must be able to detect pressure drops of 8.25 psi. The results from Figure 6.7 show that this is extremely possible with the rolling radius estimate. Such a system would require many more data sets for analysis to provide statistically signi cant detection and alarm thresholds. However, the operation is simple: if the radius estimate drops below a certain value, then the system interprets that to correspond with a drop of a certain pressure. Given the data presently available, the design would be as follows. If the radius for one of the front tires drops below 331.1mm, then a drop of 4psi (12%) can be safely inferred. This is well within the accuracy 110 requirements of the TREAD act. Again, more data needs to be collected to improve the statistics of the detection algorithm, but the concept is shown feasible here. Future work would also deal with studying and isolating the e ects of pressure on the radius. For instance, the e ects of vertical loading conditions on the radius estimate needs to be studied, because these e ects must be isolated from the pressure detection algorithm. Yet it has been shown that GPS can be used to accurately estimate the vehicle mass online [3]. If the mass is known, and more importantly if the change in mass is known, this information can be included in the process of inferring the pressure from the radius. Tire wear might also present a problem. If the change in radius due to tire wear is in the same range as the change in radius due to pressure, it may be very di cult to separate these e ects. Yet for now it is concluded that the GPS aided estimate of the undriven wheel rolling radius o ers great potential in determining pressure loss without needing tire pressure sensors. It has also been shown that the yaw rate signal, aided with the bias estimate from the AUNAV estimator, can be used with the steer angle sensor to detect steering misalignment. Data was collected on portions of I-85 north with various alignment adjustments to the steering linkage. The data con rmed the hypothesis that misalignment conditions result in steady state steer angle o sets when the vehicle is driving straight. Therefore if the vehicle is known to be driving straight via the yaw rate information and if there is a non zero steady state steering wheel angle, then it is accurately inferred that there is an alignment problem. 111 Chapter 7 Conclusions and Future Work This thesis has shown the advantages of combining the increasingly common single antenna GPS systems with sensors which are already present on board vehicles equipped with ESC and RSC systems. First, the Automotive Navigation (AUNAV) estimator was presented. This estimator integrates GPS information with information from a 5 degree of freedom inertial sensor cluster to produce accurate estimates of the vehicle sideslip, attitude, velocity, and position along with accurate estimates of the inertial sensor biases. An analysis of the observability of the AUNAV estimator was also presented, showing that this estimator is fully observable under certain dynamic conditions. The AUNAV estimator was then contrasted with the Modi ed Modular Estimator (MME), which is a di erent approach to integrating the same sensors. Finally, methods of estimating the e ective tire radius, detecting tire pressure changes, and detecting steering misalignment were presented. These methods all take advantage of GPS information and information provided by the AUNAV estimator. The AUNAV estimator is a specialized form of the generic loosely coupled GPS/INS integration lter that is shown in [19]. It consists of the GPS and IMU functioning as two independent navigation systems, with the GPS serving as truth, and one extended Kalman lter. The states of the EKF are the errors in the INS solution, so the measurements used by the lter are the di erences between the GPS and INS solutions. The estimates of the INS solution errors are added back to the original INS solutions and the corrected solution is used as the nal estimate. The EKF also produces estimates of the inertial sensor biases, and these are used in the INS processing stage to improve the solutions. The AUNAV estimator di ers in its design from the loosely coupled lter in two ways. First, the AUNAV estimator 112 does not use a pitch rate gyroscope because these are not available on modern commercial vehicles. Second, the AUNAV estimator uses the GPS course measurement as a measurement of heading (yaw) when the vehicle is driving straight. This is made possible because when the course and heading are equivalent when there is no sideslip. The AUNAV estimator also di ers from the loosely coupled lter in that the AUNAV estimator is primarily used to estimate sideslip, while the loosely coupled lter is primarily analyzed for its positioning capabilities. A study of the observability of the AUNAV estimator was conducted. Many authors have analytically determined the observability of the loosely coupled lter under various dynamic scenarios, making the important case that the observability is contingent on the dynamics. It was hypothesized in this work that the same conclusions apply for the AUNAV estimator, and this was tested using an observability test for linear time varying systems and simulations. The observability tests con rmed the hypothesis that the AUNAV estimator has the same observability characteristics as the loosely coupled lter. The AUNAV estimator requires acceleration changes in order to be made fully observable, but combinations of the attitude states and the inertial biases remain observable when the vehicle is not performing any acceleration. This means that dynamics are required in order for the estimator to be able to separate the attitude states from the bias states. Simulations were performed which con rmed the results of the observability checks. It was also found that the roll rate gyroscope bias converges when there are no dynamics. The yaw measurement gives the AUNAV estimator an observability advantage over the loosely coupled lter by making the yaw angle and the yaw rate gyroscope bias observable when the vehicle is driving straight. The performance of the AUNAV estimator was demonstrated using experimental data collected with an In niti G35 sedan which is instrumented with various GPS and inertial sensors, along with an attitude determination system. It was shown that the AUNAV es- timator is able accurately estimate the sideslip, roll, and pitch during dynamic maneuvers. It was also shown that the estimator is able to accurately separate the attitude states and 113 the biases when dynamic maneuvers are performed over time, con rming the observability analysis. Finally, it was shown that the AUNAV estimator is able to accurately estimate sideslip which is increasing a rates as slow a 1.5 deg=s. The performance of the AUNAV estimator was also compared with the performance of the MME estimator using the same data from the previously experiments. The MME estimator is based on the approach presented in [6]. It consists of several independent GPS/INS lters which estimate states along the di erent axes. The roll angle must be additionally high pass ltered from a lumped state in the lateral lter. The pitch angle is obtained by estimating the road grade as in [4]. The sideslip angle is computed from the lateral and forward velocity estimates. It was found that in general the AUNAV estimator outperforms the MME estimator in almost all cases. This is because the AUNAV estimator uses a more complete model of the kinematic relationships than the MME does. However, the MME does a better job of estimating the yaw rate gyroscope bias. This is because its simpli ed model is more isolated from the e ects of misalignment than the AUNAV estimator. In fact, the MME heading lter is not subject to yaw angle misalignment e ects at all. The AUNAV lter combines all yaw rate errors, including misalignment, into the bias estimate, making it a less accurate estimate of the pure sensor bias. A simple method of using GPS and wheel speed sensors to estimate the absolute e ective rolling radius of the wheels was also shown. The method was demonstrated in simulation to be unbiased if the no slip assumption holds. While the estimate for the driven wheels is biased due to wheel slip, the estimate for the undriven wheels can serve as an absolute radius for any relative calculations for the driven wheels. There is no reference method or \truth" measurement for the e ective radius with which to compare the estimate in real experiments, yet it is known that the e ective radius lies within the loaded and unloaded radii. Several runs were performed on the G35 on the NCAT loop, and the radius estimates for all four tires were within this range for all runs. This served as a sanity check, showing that while there may not be an exact truth for comparison, the estimates from the experiment are reasonable. 114 A method for inferring the tire pressure on the basis of the rolling radius estimate is also proposed. Tests were conducted showing that the rolling radius estimate does vary according to tire pressure, and that these changes are distinguishable from the noise on the estimates. This leads to the conclusion that a simple hypothesis tester can be used to detect pressure drops of half of the percentage required by the TREAD act [1]. Future work requires many more data runs to improve the statistics of such a detector and to investigate other sources of change in the radius estimate. This is because the radius is only a good indicator of pressure if the changes resulting from pressure changes can be isolated. A method of using the yaw rate bias estimate from the AUNAV estimator, the yaw rate signal, and the steering wheel angle to detect steering misalignment was presented. This method assumes that an aligned vehicle will have a zero steady state steer angle when driving straight if there is no road crown. For this thesis, the e ects of road crown are ignored, although they could potentially be incorporating using the roll angle estimate provided by the AUNAV estimator. Experiments were conducted by altering the toe angle of the front right tire of the G35 and collecting data along a 10 minute section of I-85 north. It was found that tire misalignment results in a non zero steady state steer angle when driving straight, and that the corrected yaw rate signal and the steer angle signal can be used to accurately detect even subtle steering misalignments. If a non zero steady state steering angle is present when the corrected yaw angle is zero, then a ag is set indicating a misalignment condition. There are many important avenues of future work stemming from this thesis. First, a more detailed analysis on the sensitivity of the AUNAV estimator to sensor quality needs to be done. The bias stability of the inertial sensors in particular plays an important role in the overall observability and operation of the lter. It was claimed in this thesis that once the biases are properly identi ed, the estimator can sustain accurate estimates of the level angles and the biases for a period of time even when the vehicle is not undergoing any dynamics. The bias stability is the main factor determining how long this period of time is. Furthermore, the length of this period of time is an extremely important system parameter 115 of the AUNAV estimator, and it would have to be accurately known in order to safely implement the estimator in commercial vehicles. Sensor quality also a ects the amount of time that the estimator can sustain accurate state estimates during a GPS outage. This is another very important system parameter because GPS outages are ultimately inevitable. Another avenue of future work would be to investigate other methods of determining when the vehicle is driving straight. One potential method would be to run a model switching algorithm. The switching would be between an instance of the AUNAV lter which assumes straight driving and one which does not. It currently remains to be seen whether there are any advantages in using such a method or any other method beyond the current detection strategy. The e ects of the bias stability on the straight driving detection method also requires additional study. It might be possible for the bias to cause the lter to never detect periods of straight driving, making it even more di cult to estimate the bias. This could potentially cause rapid degradation in the lter performance. More data is required to improve the tire pressure detection algorithm, in order to make the results statistically signi cant and to more accurately quantify the accuracy and resolution. Another current problem is that there is no way to establish a baseline radius for the tire. The absolute radius can be accurately estimated for the undriven wheels, but there is no way of knowing as a baseline what pressure a given radius corresponds to. A solution to this problem would be essential to real world implementation. The e ects of tire loading on the radius estimate also needs to be studied, as the pressure e ects have to be isolated from these. The radius estimates have a very ne resolution, and it is likely that large loading variations could cause true deformations in the tires within the order of magnitude of the estimation resolution. The steering misalignment algorithm would also bene t from more data for the same reasons as the tire radius estimation algorithm. That is, more data is required in order to statistically set detection thresholds. The e ects of road crown also need to be investigated. It is possible for non standard road crowns to cause the system to 116 falsely detect a steering misalignment. The roll angle estimate could be used to mitigate this problem, but this has not yet been studied. This thesis has shown the bene ts of combining single antenna GPS information with the signals from standard ESC/RSC sensor clusters. These results can be applied today, from the standpoint of sensor availability, to any vehicle equipped with a navigation pack- age and ESC/RSC. Navigation packages are becoming increasingly ubiquitous on passenger vehicles, making these solutions extremely cost e ective. No additional sensors are required. Furthermore these solutions o ers the advantage of robustness to vehicle parameters, since they does not use any such parameters. Neither changes in vehicle loading, new sets of tires, nor after market sway bar installations will cause any problems for the systems presented here. In conclusion, the solutions o ered here present great opportunity to improve vehicle safety without one cent of additional materials cost, and it is all made possible with the Global Positioning System. 117 Bibliography [1] National Highway Tra c Safety Administration. Federal motor vehicle safety standards; tire pressure monitoring systems; controls and displays. Technical report, Department of Transportation, http://www.nhtsa.gov/cars/rules/rulings/tirepres nal/index.html, 2000. [2] Rusty Anderson and David M. Bevly. Estimation of slip angles using a model based estimator and GPS. In Proceedings of the American Control Conference, volume 3, pages 2122{2127 vol.3, June-2 July 2004. [3] Hong S. Bae, Jihan Ryu, and J. Christian Gerdes. Road grade and vehicle parameter estimation for longitudinal control using GPS. In Proceedings of the IEEE Conference on Intelligent Transportation Systems, August 2001. [4] H.S. Bae and J. Christian Gerdes. Command modi cation using input shaping for automated highway systems with heavy trucks. In Proceedings of the American Control Conference, volume 1, pages 54{59 vol.1, June 2003. [5] V.L. Bageshwar, D. Gebre-Egziabher, W.L. Garrard, and T.T. Georgiou. A Stochastic Observability Test for Discrete-Time Kalman Filters. JOURNAL OF GUIDANCE, CONTROL, AND DYNAMICS, 32(4), 2009. [6] David M. Bevly. GPS: A low-cost velocity sensor for correcting inertial sensor errors on ground vehicles. Journal of Dynamic Systems, Measurement, and Control, 126:255{264, June 2004. [7] David M. Bevly, Robert Daily, and William Travis. Estimation of critical tire parameters using GPS based sideslip measurements. In Proceedings of the SAE Dynamics, Stability, and Controls Conference, number 2006-01-1965, pages 87{94, February 2006. [8] David M. Bevly, Robert Sheridan, and J. Christian Gerdes. Integrating INS sensors with GPS velocity measurements for continuous estimation of vehicle sideslip and tire cornering sti ness. In Proceedings of the American Control Conference, June 2001. [9] D.M. Bevly, J. Ryu, and J.C. Gerdes. Integrating INS sensors with GPS measurements for continuous estimation of vehicle sideslip, roll, and tire cornering sti ness. Intelligent Transportation Systems, IEEE Transactions on, 7(4):483{493, Dec. 2006. [10] F.X. Cao, D.K. Yang, A.G. Xu, J. Ma, W.D. Xiao, C.L. Law, K.V. Ling, and H.C. Chua. Low cost sins/gps integration for land vehicle navigation. In Intelligent Transportation Systems, 2002. Proceedings. The IEEE 5th International Conference on, pages 910{913, 2002. 118 [11] C.R. Carlson and J.C. Gerdes. Identifying tire pressure variation by nonlinear estima- tion of longitudinal sti ness and e ective radius. In Proceedings of AVEC 2002 6th International Symposium of Advanced Vehicle Control, 2002. [12] C.R. Carlson and J.C. Gerdes. Nonlinear estimation of longitudinal tire slip under several driving conditions. In American Control Conference, 2003. Proceedings of the 2003, volume 6, pages 4975{4980. IEEE, 2003. [13] C.R. Carlson and J.C. Gerdes. Consistent nonlinear estimation of longitudinal tire sti ness and e ective radius. Control Systems Technology, IEEE Transactions on, 13(6):1010{1020, 2005. [14] J.L. Crassidis and J.L. Junkins. Optimal Estimation of Dynamic Systems, volume 2. Chapman & Hall, 2004. [15] Robert Daily, William Travis, and David M. Bevly. Cascaded observers to improve lateral vehicle state and tire parameter estimates. International Journal of Vehicle Autonomous Systems, 5:230{255, 2007. [16] G. Dissanayake, S. Sukkarieh, E. Nebot, and H. Durrant-Whyte. The aiding of a low- cost strapdown inertial measurement unit usingvehicle model constraints for land vehicle applications. IEEE Transactions on Robotics and Automation, 17(5):731{747, 2001. [17] NHTSAs National Center for Statistics and Analysis. Tra c safety facts 2008 data. Technical report, National Highway Transportation Safety Administration, http://www-nrd.nhtsa.dot.gov/Pubs/811162.PDF, 2008. [18] J. Gao, MG Petovello, and ME Cannon. Development of precise GPS/INS/wheel speed sensor/yaw rate sensor integrated vehicular positioning system. In Proceedings of the National Technical Meeting of the Institute of Navigation (ION NTM06, volume 2, pages 780{792, 2006. [19] S. Gleason and D. Gebre-Egziabher, editors. GNSS Applications and Methods. Artech House Publishers, 685 Canton Street Norwood, MA 02062, 2009. [20] S. Godha and ME Cannon. Integration of DGPS with a low cost MEMSbased Inertial Measurement Unit (IMU) for land vehicle navigation application. In Proceedings of the 18th International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GNSS05), pages 333{345, 2005. [21] D. Goshen-Meskin and I.Y. Bar-Itzhack. Observability analysis of piece-wise constant systems. i. theory. Aerospace and Electronic Systems, IEEE Transactions on, 28(4):1056 {1067, October 1992. [22] D. Goshen-Meskin and I.Y. Bar-Itzhack. Observability analysis of piece-wise constant systems. ii. application to inertial navigation in- ight alignment [military applications]. Aerospace and Electronic Systems, IEEE Transactions on, 28(4):1068 {1075, October 1992. 119 [23] P.D. Groves and Ebooks Corporation. Principles of GNSS, inertial, and multisensor integrated navigation systems. Artech House, 2008. [24] F. Gustafsson. Slip-based tire-road friction estimation* 1. Automatica, 33(6):1087{1099, 1997. [25] F. Gustafsson, M. Drevo, M. Lofgren, N. Persson, and H. Quicklund. Virtual sensors of tire pressure and road friction. 2001. [26] Sinpyo Hong, Man Hyung Lee, Ho-Hwan Chun, Sun-Hong Kwon, and J.L. Speyer. Observability of error states in gps/ins integration. Vehicular Technology, IEEE Trans- actions on, 54(2):731{743, March 2005. [27] Sinpyo Hong, Man Hyung Lee, Ho-Hwan Chun, Sun-Hong Kwon, and J.L. Speyer. Experimental study on the estimation of lever arm in gps/ins. Vehicular Technology, IEEE Transactions on, 55(2):431{448, March 2006. [28] Sinpyo Hong, Man Hyung Lee, Sun Hong Kwon, and Ho Hwan Chun. A car test for the estimation of gps/ins alignment errors. Intelligent Transportation Systems, IEEE Transactions on, 5(3):208{218, Sept. 2004. [29] Henrik Jansson, Ermin Kozica, Per Sahlholm, and Karl Henrik Johansson. Improved road grade estimation using sensor fusion. In Proceedings of the 12th Reglerm ote in Stockholm, Sweden, 2006. [30] Peter Lingman and Bengt Schmidtbauer. Road slope and vehicle mass estimation using kalman ltering. In Proceedings of the 19th IAVSD Symposium, Copenhagen, Denmark, 2001. [31] S.L. Miller, B. Youngberg, A. Millie, P. Schweizer, and J.C. Gerdes. Calculating lon- gitudinal wheel slip and tire parameters using GPS velocity. In American Control Conference, 2001. Proceedings of the 2001, volume 3, pages 1800{1805. IEEE, 2001. [32] NASA. Robot coordinate frames and motion direction. Online, May 2009. [33] American Association of State Highway and Transportation O cials. A policy on ge- ometric design of highways and streets. American Association of State Highway and Transportation O cials, 444 North Capitol Street, N.W, Suite 249, Washington, DC 20001, 5 edition, 2004. [34] R. Rajamani. Vehicle dynamics and control. Springer, 2006. [35] Ihnsoek Rhee, Mamoun F. Abdel-Hafez, and Jason L. Speyer. Observability of an inte- grated gps/ins during maneuvers. Aerospace and Electronic Systems, IEEE Transactions on, 40(2):526{535, April 2004. read. [36] Jonathan Ryan, David Bevly, and Jianbo Lu. Robust sideslip estimation using GPS road grade sensing to replace a pitch rate sensor. In 2009 IEEE International Conference on Systems, Man, and Cybernetics, pages 2026 {2031, oct. 2009. 120 [37] Jonathan Ryan, Jianbo Lu, and David Bevly. State estimation for vehicle stability control: A kinematic approach using only gps and vsc sensors. In Proceedings of the ASME 2010 Dynamic Systems and Control Conference, Sept. 2010. [38] Jihan Ryu and J. Christian Gerdes. Estimation of vehicle roll and road bank angle. In American Control Conference, 2004. Proceedings of the 2004, volume 3, pages 2110{ 2115 vol.3, June-2 July 2004. [39] Jihan Ryu and J. Christian Gerdes. Integrating inertial sensors with global positioning system (GPS) for vehicle dynamics control. Journal of Dynamic Systems, Measurement, and Control, 126(2):243{254, 2004. [40] Jihan Ryu, Eric J. Rossetter, and J. Christian Gerdes. Vehicle sideslip and roll param- eter estimation using GPS. In Proceedings of the AVEC International Symposium on Advanced Vehicle Control, 2002. [41] Per Sahlholm and Karl Henrik Johansson. Road grade estimation for look-ahead vehicle control. In Proceedings of the 17th IFAC World Congress, Seoul, Korea, 2008. [42] L. M. Silverman and H. E. Meadows. Controllability and observability in time-variable linear systems. SIAM Journal on Control, 5(1):64{73, 1967. [43] R.F. Stengel. Optimal control and estimation. Dover Pubns, 1994. [44] H. Eric Tseng, Li Xu, and Davor Hrovat. Estimation of land vehicle roll and pitch angles. Vehicle System Dynamics, 45:433{443, May 2007. [45] Yunchun Yang and J.A. Farrell. Magnetometer and di erential carrier phase GPS- aided INS for advanced vehicle control. Robotics and Automation, IEEE Transactions on, 19(2):269{282, Apr 2003. 121