Roll and Bank Estimation Using GPS/INS and Suspension Deflections ^†

Abstract

This article presents a method that provides an estimate of road bank by decoupling the vehicle roll due to the dynamics and the roll due to the road bank. Suspension deflection measurements were used to provide a measurement of the relative roll between the vehicle body frame and the axle frame or between the sprung mass and the unsprung mass, respectively. A deflection scaling parameter was found via suspension geometry and dynamic analysis. The relative roll measurement was then incorporated into two different kinematic navigation models based on extended Kalman filter (EKF) architectures. Each algorithm was tested and then verified on the Prowler ATV experimental platform at the National Center for Asphalt Technology (NCAT). Experimental data showed that both the cascaded and coupled approach performed well in providing estimates of the current vehicle roll and instantaneous road bank.

1. Introduction

Vehicle rollover is a topic that has been widely researched by the vehicle community for some time. With the rise in popularity of high center of gravity vehicles, such as sports utility vehicles, rollover has become an important issue for vehicle safety.

In 2009, 2.954 trillion miles were traveled by motorists in the United States. Among those miles, motor vehicle crashes resulted in 30,797 fatalities [1]. Rollover crashes account for only 3% of vehicle crashes. However, they lead to approximately one third of all occupant deaths.

Some vehicle safety features recently introduced by manufacturers into motor vehicle fleets may be contributing to a reduction in rollover crashes and the harm they cause. These features include rollover sensors to trigger inflatable side curtain airbags (SCABs, known as rollover protection) to mitigate occupant injury, electronic stability control (ESC) to reduce loss of yaw control and roll stability control (RSC) to minimize the number of rollover crashes that occur. These safety features are typically installed in light trucks and vans (LTVs) [2].

A review of the empirical evidence shows that ESC has the potential to reduce the number of rollover accidents and several other fatal types of accidents, as shown in [3]. In fact, the National Center for Highway Safety (NTSHA) has issued a final rule in the Federal Register that requires manufacturers to implement ESC systems for passenger cars, multipurpose passenger vehicles, trucks and buses with a gross vehicle weight rating of 4,536 kg (10,000 pounds). According to NHTSA research, preventing single-vehicle loss-of-control crashes is the most effective way to reduce deaths resulting from rollover crashes. The majority of loss-of-control crashes involve the vehicle entering the road shoulder or median, where the bank is often more extreme and the terrain is more unforgiving. NHTSA concludes that requiring ESC will significantly reduce single-vehicle crashes by over 30 percent and reduce vehicle rollovers by over 70 percent [4].

1.1. Rollover Algorithms

The rollover issue has led to the development of several metrics and methodologies to predict and prevent vehicle roll over. Although, typically, the motivation for these studies has been passenger vehicle safety, the use of these metrics can also be used to aid in the navigation and control of unmanned ground vehicles (UGV). In order for the UGV to be effective at performing its task, it must remain in a usable and controllable state. Thus, the vehicle must avoid rolling over. Additionally, the appropriate control effort required to keep the vehicle from rolling is dependent on the bank on which the vehicle is operating.

Work has been done to develop metrics and algorithms for rollover indication and prediction. The most basic is the static stability factor (SSF). The SSF is a function of the track width and the vertical height of the center of gravity (CG) of the vehicle. Increasing the CG height or decreasing the track width will cause an increase in rollover propensity [5]. Other metrics, including the lateral load transfer ratio (LLT, LTR) and the roll stability factor (RSF), use the difference in normal forces on the left and right sides of the vehicle to indicate rollover [5,6]. Two-wheel lift off can be determined through the observation of the RSF. A two-wheel lift-off velocity (TWLV) value can then be assigned and used to characterize rollover propensity [5]. To predict a time-to-rollover (TTR), Gáspár et al. [7] used a simple model to calculate a TTR, assuming that the speed and steer angle remain constant. In that work, a neural network is used to make up for the lack of complexity of the simple vehicle model, which needs to run faster than real time.

Rajamani et al. [8] determined that if the roll motion of the sprung mass is caused entirely by the lateral acceleration, ignoring the road and other external inputs, the rollover index can be approximated as a function of lateral acceleration and CG height. Thus, the roll angle estimates and CG height estimates need to be accurate. They then proposed a dynamic observer and a CG height estimator to estimate the real-time roll angle and CG height in order to calculate an accurate rollover index value.

These rollover metrics and algorithms work well with the assumption that the road is flat and level. This assumption will hold for small bank angles. However, in the more extreme cases, where bank angles exceed the threshold of the small angle approximation, rollover characteristics change. In order to improve the accuracy of the roll indicator, it is beneficial to have an understanding of the bank on which the vehicle is traveling. A vehicle operating on a large bank will have an increased tendency to roll compared to a vehicle operating on a flat surface. Peters and Iagnemma [9] developed a stability metric based on the distribution of wheel terrain contact forces. They showed that the metric can successfully predict rollover on surfaces with arbitrary geometries. Ryu and Gerdes investigated the problem of estimating vehicle and road bank [10]. They modeled the road bank as a disturbance and used a disturbance observer to estimate the road bank. Using a two-antenna GPS system, they were able to accurately estimate roll and road bank.

1.2. Outline

The outline of this article is as follows: The relationship of roll and bank is discussed in Section 2. The concept of relative roll is explained, and a geometry-based method that uses suspension deflections to acquire a measurement of relative roll is presented. A scaling parameter, η, is shown to be necessary for the use of the relative roll measurement. An overview of the formulation of the extended Kalman filter for navigation comprises most of Section 3. The roll estimate is then combined with the relative roll measurement in a cascaded approach for road bank estimation at the end of Section 3.

In Section 4, the navigation filter is augmented with a road bank state. This coupled EKF approach to the bank estimation is then simulated and shown to provide the advantage of noise filtering on the bank state. The experimental test setup is described in Section 5. The experimental results of both the cascaded and coupled versions of the navigation EKF are discussed in Section 6. Both navigation filter approaches are validated with the experimental data. Finally, Section 7 consolidates the conclusions from preceding sections.

2. Roll, Relative Roll and Road Bank

Figure 1a represents the roll model of the vehicle. In this model, the sprung mass rotates about the roll center. The road bank is described by the angle ϕ_r that exists between the road surface and the horizontal. The angle ϕ_s describes the suspension roll, or relative roll, that is due to the suspension deflections caused by the dynamics of the vehicle. The sum of the road bank and the relative roll is the total roll indicated by ϕ_v. The center of gravity is denoted by CG, and the gravity vector points downward and is denoted by g. The distance from the ground surface to the roll center is h_rc; the distance from the roll center to the center of gravity is h_rg; and the distance between the two wheels is the track width, denoted as t_w.

2.1. Relative Roll

The vehicle roll can be estimated using suspension deflections [11]. This measurement of roll can be determined based on the geometry of the suspension and can be estimated using Equation (1),

$${\overline{\varphi }}_s={\sin }^{-1}\left(\frac{\mathrm{\Delta }{L}_{LF}-\mathrm{\Delta }{L}_{RF}+\mathrm{\Delta }{L}_{LR}-\mathrm{\Delta }{L}_{RR}}{2t_w}\right)$$

(1)

where ${\overline{\varphi }}_s$ is the relative roll of the suspension. It should be noted that the suspension roll is the roll of the body relative to the surface on which it is driving. The deflections ΔL_ij are the suspension deflections at the respective corners of the vehicle. The suspension roll for the front axle is the inverse sine of the left deflection minus the right deflection divided by the vehicle track width (t_w), as observed from Figure 1b.

The suspension roll for the rear axle can be determined similarly, but the j subscript is denoted by an R for rear axle. The front and rear suspension roll values can be averaged for a suspension roll calculation of the whole vehicle. It should also be noted that this method captures the dynamics of the suspension relative to the sample time of the deflection sensors.

2.2. The Eta (η) Parameter for Roll Scaling

Based on the vehicle suspension geometry, Equation (1) will underestimate the true suspension roll by a factor that can be determined experimentally [11]. Thus, Equation (1) must be multiplied by the scale factor η to represent the true suspension roll.

$${\varphi }_s=\eta {\overline{\varphi }}_s$$

(2)

Note that η is a parameter associated with the static geometry of the vehicle’s suspension and therefore is vehicle suspension dependent. Initial results suggest that η is a constant value unique to the vehicle and will not vary based on bank angle or other road inputs. This scale factor can be determined by taking the ratio of the unscaled suspension roll to the difference of the true body roll and the bank angle, as shown in Equation (3).

$$\eta =\frac{{\varphi }_v-{\varphi }_r}{{\overline{\varphi }}_s}$$

(3)

In practice, calculating η is most easily performed on a flat surface with no bank angle. When the bank angle (ϕ_r) is zero, the suspension roll (ϕ_s) becomes equal to the total roll (ϕ_v). Note that the total roll (ϕ_v) should be a direct measurement or an estimate with suitable accuracy.

To demonstrate the procedure for calculating η, CarSim was used. The software has been accepted by the industry as accurately representing vehicle dynamics. The vehicle used in the simulations was the CarSim “small SUV”, which has vehicle parameters typically associated with the small sports utility vehicle (SUV) class of vehicles.

Figure 2 shows the CarSim results of the vehicle performing a double-lane change (DLC) on a flat surface. The DLC maneuver is the industry standard for testing dynamic roll properties for various vehicles [12]. The effect of the scaling factor on the suspension roll estimate can be seen in Figure 2. The scaling value η of Equation (3) was calculated at each time step for the duration of the maneuver in Figure 2 and then averaged. The averaged η was then applied to the raw suspension measurement to yield the corrected suspension roll. Although the unscaled suspension roll captures the shape of the vehicle roll, it under-predicts the magnitude. After the scaling is applied, the suspension roll closely matches the total roll, as expected.

The η parameter is a peculiar characteristic of the relative roll equation. The dynamic characteristics of the η parameter were studied by altering the vehicle mass, location of the CG and inertial properties of a simulated vehicle. It is unusual that a constant parameter can correct for the errors from the suspension deflections.

2.3. The Effect of Speed on η

Individual double-lane change runs were created in CarSim at increments of 10 km/h from 10 km/h to 250 km/h on a flat surface see Figure 3. Equation (3) was then used to calculate the average η value for each run. For the η calculation, only the peaks of the Euler roll and suspension roll were used. The average η value across all speeds was then calculated to be 1.736 $\raisebox{1ex}{$\mathrm{rad}$}\!\left/ \!\raisebox{-1ex}{$\mathrm{rad}$}\right.$. The slower speeds seem to generate lower η values, but the values still remain grouped close to the average.

The average η was then applied to each of the runs for 10–250 km/h. The root mean squared (RMS) error of the bank estimate was then calculated at each speed, as shown in Figure 4. The plot of the RMS error shows a trend of increasing RMS as speed increases, but the RMS remains small relative to the roll angles, which are near eight and nine degrees at the higher speeds.

2.4. Eta (η) as the Kinematic Suspension Ratio

In practice, both an accurate measurement of total roll, ϕ_v, and a flat surface may not be available. These issues present a challenge to finding the η factor. However, an alternate method is available, which uses measurements from the suspension links. Known primarily as the kinematic suspension ratio, the method relates the vertical deflection of the suspension spring and damper to the vertical deflection that is experienced at the tire through Equation (4). In [13], the relationship is termed the installation ratio, and elsewhere, it is known as the motion ratio. Note that some suspensions are designed such that the ratio is not constant. This work assumes that the ratio is constant. For information on a significantly varying ratio, refer to [13].

Figure 5 shows the geometry of the vertical deflections of the suspension component and the vertical deflection at the tire. The vertical displacements of the wheel center ΔZ and the suspension component ΔL are related, such that:

$$\cos \left(\frac{\mathrm{\Delta }Z}{a+b}\right)=\cos \left(\frac{\mathrm{\Delta }L}{a}\right)$$

(4)

$$\frac{\mathrm{\Delta }Z}{a+b}\approx \frac{\mathrm{\Delta }L}{a}$$

(5)

After assuming small angles, the expression for the vertical deflection of the tire becomes:

$$\mathrm{\Delta }Z=\left(a+b\right)\frac{\mathrm{\Delta }L}{a}$$

(6)

$$\mathrm{\Delta }Z={\eta }_r\mathrm{\Delta }L$$

(7)

The η_r scaling factor is thus the ratio of the upper A-arm length, (a + b), to the length from the upper A-arm link and the vehicle frame joint to the point where the suspension component attaches to the upper A-arm link joint, a, as seen in Equation (8).

$${\eta }_r=\frac{\left(a+b\right)}{a}$$

(8)

The scale factor η_r should be applied directly to the suspension deflections prior to the relative roll calculations, as shown in Equation (9). However, suspension roll on vehicles does not exceed the small angle approximation. Thus, the scaling method from Section 2.2 and the kinematic suspension ratio method are roughly equivalent for small angles. As such, η_r is equivalent to η from Equation (2) and Equation (3).

$${\varphi }_s={\sin }^{-1}\left(\frac{\mathrm{\Delta }{L}_{LF}-\mathrm{\Delta }{L}_{RF}+\mathrm{\Delta }{L}_{LR}-\mathrm{\Delta }{L}_{RR}}{2t_w}\right)$$

(9)

Therefore, the scaling method in Section 2.2 was used for the CarSim simulations in which no physical measurements of the suspension links exist to be measured. The kinematic suspension ratio method was used to find η for the experimental data with the Prowler ATV, because the suspension could be directly measured and a sufficiently flat surface could not be found.

2.5. Eta (η) and Road Disturbances

Using the suspension deflections as the sole estimation of roll becomes problematic when there are additional inputs into the suspension, such as bumps, or if the vehicle is traveling on a banked road. For a banked road, a method of measuring or estimating the total roll of the vehicle must be used to decouple the roll due to bank and the roll due to dynamics.

Thus, if the road surface is a assumed to be flat, a low fidelity roll model can be used in conjunction with suspension deflection measurements to calculate the roll of a vehicle. The roll scaling parameter eta (η) is needed to account for the effects of the suspension geometry on the deflection measurements. It is possible to calculate the parameter η experimentally if a perfectly flat surface is available. For banked road surfaces, the roll calculated from suspension deflections will be corrupted due to the bank disturbance input to the suspension. A method for measuring or estimating the total roll is needed to decouple the roll from the bank and the roll due to dynamics of the vehicle.

In lieu of using an attitude measurement from a multi-antenna GPS system, the vehicle roll can be estimated using a navigation filter. This method provides an estimate of the attitude of the vehicle body in an absolute sense based on the kinematic relationships between positions and velocities measured from a single antenna GPS and the accelerations and roll rates measured from an inertial measurement unit (IMU). The navigation filter is used for sensor fusion of the GPS and IMU for the purpose of obtaining an estimate/observation of total roll ϕ_v, which is not measured.

3. Road Bank Estimation Using Suspension Deflections and a Cascaded Navigation EKF

The EKFs used in this work utilize the methods from [14] with supplemental references to [15]. The navigation filters are error state filters, which means that the error states are added to the actual states during each measurement update. The filter described below is used to blend Global Navigation Satellite System (GNSS) solutions with inertial navigation system (INS) solutions. Specifically, the filter is constructed with a six degrees of freedom inertial measurement unit (IMU) time update and a Global Positioning System (GPS) measurement update.

3.1. Coordinate Frames

Inertial navigation algorithms use a few coordinate frames. The Earth-centered inertia (ECI), used in navigation applications, is an inertial reference frame that does not accelerate or rotate with respect to the rest of the Universe [15]. The ECI frame is denoted by the symbol i as a superscript or a subscript. The axes of the IMU form the body fixed frame. The assumption is made that the x-, y- and z-axes align with the front, right and down axes of the vehicle [14]. The body frame is denoted by the symbol b. The Earth frame or Earth-centered Earth-fixed (ECEF) frame is similar to the ECI frame. However, the ECEF frame remains fixed to the Earth, and thus, spins as the Earth spins. The ECEF frame is denoted with the symbol e. The local navigation frame is a geodetic or geographic frame and is denoted with the symbol n. The origin of the body frame is termed the “point of interest”, which is typically the center of mass of the vehicle. The z-axis is referred to as the down axis and is normal to the surface of the reference ellipsoid and points towards the center of the Earth. The x-axis is the axis that points north in the plane orthogonal to the z-axis and is also called the north axis. The y-axis or east axis always points east [15].

3.2. Inertial Navigation

The velocities and angular rate measurements of an IMU can be used for stand-alone navigation for short durations of time. In brief, integration of the angular rates yields attitude updates; the first integration of the accelerations produces velocities; and positions are acquired from the integration of these velocities or the second integration of the accelerations. The integrated noise and bias degrades the solution with each subsequent integration. However, an integrated navigation system can provide corrections to the IMU outputs and the inertial navigation solutions from estimates of IMU bias, which can be determined with the GNSS/INS integration algorithm [14].

3.3. Mechanization Equations

The equations of motion expressed in the north-east-down (NED) local navigation frame are:

$$\dot{\mathbf{p}}=\mathbf{T}{\mathbf{v}}^n$$

(10)

$$\dot{\mathbf{v}}={\mathbf{C}}_b^n{\mathbf{f}}^b-\left[{\left(2{\mathit{\omega }}_{ie}^n+2{\mathit{\omega }}_{en}^n\right)}^{\wedge }\right]{\mathbf{v}}^n+{\mathbf{g}}^n$$

(11)

$${\dot{\mathbf{C}}}_b^n={\mathbf{C}}_b^n\left(\left[{\left({\mathit{\omega }}_{ib}^b\right)}^{\wedge }\right]-\left[{\left({\mathit{\omega }}_{in}^b\right)}^{\wedge }\right]\right)$$

(12)

Note that ${\mathit{\omega }}_{ba}^c$ denotes the rotation rate of frame b relative to frame a parameterized in frame c; thus, ${\mathit{\omega }}_{ib}^n$ is the rotation rate of the inertial frame (the ECI frame) relative to the body frame (the vehicle/IMU frame) parametrized in the local navigation frame. The vector p is the position of interest, which is the user, expressed in the geodetic coordinates of latitude (L), longitude (λ) and altitude (h).

$$\mathbf{p}=\left[\begin{array}{c}L\\ \lambda \\ h\end{array}\right]$$

The vector v represents the NED components of the users velocity v_N, v_E and v_D.

$$\mathbf{v}=\left[\begin{array}{c}{v}_N\\ v_E\\ v_D\end{array}\right]$$

Local gravity is represented by the vector g. The matrix T is the transformation matrix that converts the linear velocities of the NED frame to angular changes in latitude and longitude of the local navigation frame.

$$\mathbf{T}=\left[\begin{array}{ccc}\frac{1}{R_N+h}& 0& 0\\ 0& \frac{1}{(R_E+h)\cos L}& 0\\ 0& 0& -1\end{array}\right]$$

(13)

The values of R_N and R_E are the radii of curvature of the reference ellipsoid for north/south (meridian) and the east/west (prime vertical), respectively [15].

The matrix ${\mathbf{C}}_b^n$ is the rotation matrix. It is of the form ${\mathbf{C}}_a^b$, such that the matrix rotates from frame a to frame b. Thus, ${\mathbf{C}}_b^n$ completes the angular rotation from the body frame to the local navigation frame. The notation $[{({\mathit{\omega }}_{ba}^c)}^{^}]$, used in [14], is the skew symmetric matrix of the vector ${\mathit{\omega }}_{ba}^c={\left[{\dot{\varphi }}_{ba}^c{\dot{\theta }}_{ba}^c{\dot{\psi }}_{ba}^c\right]}^T$ and is defined as:

$$[{({\mathit{\omega }}_{ba}^c)}^{^}]=\left[\begin{array}{ccc}0& -{\omega }_{ba,3}^c& {\omega }_{ba,2}^c\\ {\omega }_{ba,3}^c& 0& -{\omega }_{ba,1}^c\\ -{\omega }_{ba,2}^c& {\omega }_{ba,1}^c& 0\end{array}\right]=\left[\begin{array}{ccc}0& -{\dot{\psi }}_{ba}^c& {\dot{\theta }}_{ba}^c\\ {\dot{\psi }}_{ba}^c& 0& -{\dot{\varphi }}_{ba}^c\\ -{\dot{\theta }}_{ba}^c& {\dot{\varphi }}_{ba}^c& 0\end{array}\right]$$

(14)

In this work, attitude is parametrized using Euler angles. Specifically, the aerospace sequence of yaw, pitch and roll (ψ, θ and ϕ) is used. Accordingly, the attitude vector is:

$${\mathit{\psi }}_{nb}=\left[\begin{array}{c}{\varphi }_{nb}\\ {\theta }_{nb}\\ {\psi }_{nb}\end{array}\right]$$

(15)

3.4. Update Equations

Assuming the initial position is known, three steps are needed to implement and solve the mechanization equations. The steps include an attitude update, a velocity update and a position update. The attitude update uses the gyro measurements from the IMU. These gyro measurements form an angular velocity vector of the body frame relative to an inertial frame of reference, ${\mathit{\omega }}_{ib}^b$. For attitude determination, the attitude of the body frame relative to the navigation frame and expressed in the body frame, ${\mathit{\omega }}_{nb}^b$, is needed. To obtain ${\mathit{\omega }}_{nb}^b$, the rotation rate of the navigation frame relative to the inertial frame expressed in the body frame, ${\mathit{\omega }}_{in}^b$, is subtracted from ${\mathit{\omega }}_{ib}^b$, the angular rate of the body frame relative to the inertial frame.

$${\mathit{\omega }}_{nb}^b={\mathit{\omega }}_{ib}^b-{\mathit{\omega }}_{in}^b$$

(16)

Note that ${\mathit{\omega }}_{in}^b$ is essentially the rotation of the Earth. When using low-cost gyros, the rotation rates from Earth are often less than the noise sensitivity of the gyros, such that the following approximation can be made.

$${\mathit{\omega }}_{nb}^b\approx {\mathit{\omega }}_{ib}^b$$

(17)

The Euler rates are then computed from the angular velocity vector as:

$${\dot{\mathit{\psi }}}_{nb}=\mathrm{F}({\mathit{\psi }}_{nb}){\mathit{\omega }}_{nb}^b$$

(18)

where:

$$\mathrm{F}({\mathit{\psi }}_{nb})=\frac{1}{\cos {\theta }_{nb}}\left[\begin{array}{ccc}1& \sin {\varphi }_{nb}\sin {\theta }_{nb}& \cos {\varphi }_{nb}\sin {\theta }_{nb}\\ 0& \cos {\varphi }_{nb}\cos {\theta }_{nb}& -\sin {\varphi }_{nb}\cos {\theta }_{nb}\\ 0& \sin {\varphi }_{nb}& \cos {\varphi }_{nb}\end{array}\right]$$

(19)

For consumer-grade and automotive gyros, Euler integration of Equation (18) is sufficient to generate an estimate of attitude.

$${\left[\begin{array}{c}{\varphi }_{nb}\\ {\theta }_{nb}\\ {\psi }_{nb}\end{array}\right]}_{k+1}={\left[\begin{array}{c}{\varphi }_{nb}\\ {\theta }_{nb}\\ {\psi }_{nb}\end{array}\right]}_k+\tau \mathrm{F}({\mathit{\psi }}_{nb}(t_k)){\mathit{\omega }}_{nb}^b(t_k)$$

(20)

In Equation (20)t_k is an instance in time at step k, and τ is the change in time, such that τ = t_k+1−t_k. The attitude estimate ${\mathit{\psi }}_{nb}(t_k)={\left[{\varphi }_{nb}{\theta }_{nb}{\psi }_{nb}\right]}_k^T$ is used for the rotation matrix ${\mathbf{C}}_b^n$ that rotates vectors from the body frame to the navigation frame in the proceeding velocity and position updates. The ${\mathbf{C}}_b^n$ transformation matrix is populated as:

$${\mathbf{C}}_b^n={\left[\begin{array}{ccc}\cos {\psi }_{nb}& \sin {\psi }_{nb}& 0\\ -\sin {\psi }_{nb}& \cos {\psi }_{nb}& 0\\ 0& 0& 1\end{array}\right]}_k{\left[\begin{array}{ccc}\cos {\theta }_{nb}& 0& -\sin {\theta }_{nb}\\ 0& 1& 0\\ \sin {\theta }_{nb}& 0& \cos {\theta }_{nb}\end{array}\right]}_k{\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\varphi }_{nb}& \sin {\varphi }_{nb}\\ 0& -\sin {\varphi }_{nb}& \cos {\varphi }_{nb}\end{array}\right]}_k$$

(21)

The acceleration of the vehicle in the navigation frame is a function of the specific force measurement vector from the IMU, f^b = [f_x f_y f_z]^T, the angular rate of the Earth, ${\mathit{\omega }}_{ie}^n$, the transport rate, ${\mathit{\omega }}_{en}^n$, and the gravity vector, gⁿ, and is given by:

$${\dot{\mathbf{v}}}^n={\mathbf{C}}_b^n{\mathbf{f}}^b-[(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n){\mathbf{v}}^n+{\mathbf{g}}^n]$$

(22)

For low-cost IMUs, the measurement errors overpower the Coriolis effect, such that the acceleration can be simplified to:

$${\dot{\mathbf{v}}}^n\approx {\mathbf{C}}_b^n{\mathbf{f}}^b+{\mathbf{g}}^n$$

(23)

Applying Euler integration to Equation (22) yields:

$${\left[\begin{array}{c}{v}_N\\ v_E\\ v_D\end{array}\right]}_{k+1}={\left[\begin{array}{c}{v}_N\\ v_E\\ v_D\end{array}\right]}_k+\tau {\dot{\mathbf{v}}}^n$$

(24)

The position update requires applying Euler integration to Equation (10), which yields:

$$L_{k+1}=L_k+\tau \left(\frac{v_n}{R_N+h}\right){|}_k$$

(25)

$${\lambda }_{k+1}={\lambda }_k+\tau \left(\frac{v_E}{(R_E+h)\cos L}\right){|}_k$$

(26)

$$h_{k+1}=h_k+\tau (-v_D){|}_k$$

(27)

It is important to remember that the precision of latitude and longitude at the level of meters represented in the local navigation frame will require seven or more significant digits (1 m ≈ 1.6e⁻⁷ rad).

3.5. Error Equations

The manner in which errors propagate through an INS is useful for navigation, because the mechanization equations, Equations (10)–(12), represent the ideal case. Perturbation analysis of the mechanization equations yields the first-order Taylor series expansion of the mechanization equations:

$$\delta \dot{\mathbf{p}}={\mathbf{T}}^{\prime }\delta {\mathbf{p}}^n+\mathbf{T}\delta {\mathbf{v}}^n$$

(28)

$$\delta {\dot{\mathbf{v}}}^n=\left[{({\mathbf{C}}_b^n{\mathbf{f}}^b)}^{^}\right]\delta {\mathit{\psi }}_{nb}^n+{\mathbf{C}}_b^n\delta {\mathbf{f}}^b-[2{({\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n)}^{^}]\delta {\mathbf{v}}^n-[{(2\delta {\mathit{\omega }}_{ie}^n+\delta {\mathit{\omega }}_{en}^n)}^{^}]{\mathbf{v}}^n+\delta {\mathbf{g}}^n$$

(29)

$$\delta {\dot{\mathit{\psi }}}_{nb}^n\approx -[{({\mathit{\omega }}_{in}^n)}^{^}]\delta {\mathit{\psi }}_{nb}^n+\delta {\mathit{\omega }}_{in}^n-{\mathbf{C}}_b^n\delta {\mathit{\omega }}_{ib}^b$$

(30)

Error is represented by δ, and the matrix T′ is the time derivative that relates positional errors into corresponding velocities. In practice, the negative skew of the transport rate, $-[{({\mathit{\omega }}_{en}^n)}^{^}]$, can be used as T′. The error in gravity is often approximated as a function of the magnitude of gravity at the user’s latitude and the error in altitude.

$$\delta {\mathbf{g}}^n=\left[\begin{array}{c}0\\ 0\\ -\frac{2g}{R_o}\delta h\end{array}\right]$$

(31)

Note that $\delta {\mathit{\psi }}_{nb}^n$, the attitude errors, are the errors resolved about the NED frame, not errors in the Euler angles themselves. Therefore, note that ${\mathit{\omega }}_{in}^n$ is equal to the sum of the Earth rate and transport rate.

$${\mathit{\omega }}_{in}^n={\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n$$

(32)

The variables $\delta {\mathit{\omega }}_{ib}^b$ and δf^b are the errors from the accelerometer and rate gyro output errors. These errors ultimately determine the performance of inertial navigation. Simple models of the accelerometer and gyro are:

$${\mathbf{f}}^b={\mathbf{f}}_t^b+{\mathrm{M}}_a{\mathbf{f}}_t^b+{\mathbf{b}}_a^b+{\mathbf{w}}_a^b$$

(33)

$${\mathit{\omega }}_{ib}^b={\mathit{\omega }}_{t{b}_t}^b+{\mathrm{M}}_g{\omega }_{i{b}_t}^b+{\mathbf{b}}_g^b+{\mathbf{w}}_g^b$$

(34)

The subscripts a and g denote terms from the accelerometer and gyro, respectively. The subscript t represents the true value that is being measured, and the superscript b means that the term is expressed in the body frame. The matrices M_a and M_g account for the effects of scale factors and any non-orthogonality errors. The b term is the bias vector, and the w term is the uncorrelated output noise vector. The bias vector and the uncorrelated noise form the error model for the accelerometer and gyro, δf^b and $\delta {\mathit{\omega }}_{ib}^b$, used in Equations (28)–(30).

$$\delta {\mathbf{f}}^b={\mathbf{b}}_a^b+{\mathbf{w}}_a^b$$

(35)

$$\delta {\mathit{\omega }}_{ib}^b={\mathbf{b}}_g^b+{\mathbf{w}}_g^b$$

(36)

These errors limit the performance of inertial navigation. Because they are integrated twice, accelerometer errors cause position errors to grow as a function of the time squared. Position errors from angular velocity will grow as a function of the time cubed. Including accurate position and velocity updates will help to mitigate the error growth problem [14].

3.6. Integration of Inertial Navigation and GPS

In this work, an automotive-grade INS is used. Therefore, the loosely-coupled filter with feedback is utilized. Loose GPS/INS integration occurs at the PVT (position, velocity and time) level. This means that the PVT measurements from the GPS are blended with the PVA (position, velocity and attitude) estimates of the INS. In the closed loop configuration, the inertial sensor errors estimated by the filter are fed back to the INS equations to compensate for the accelerometer and gyro errors. The most common fusing filter used for loosely-coupled GPS/INS is the extended Kalman filter (EKF) [14] and is the filter of choice for this work. Refer to [16,17] for further specific details on the Kalman filter.

3.6.1. Navigation Error State EKF with Closed Loop Feedback

The system model for navigation states consists of the mechanization equations, Equations (10)–(12), which are nonlinear, and the models for the IMU biases. The linearized error state formulation of the mechanization equations was discussed as an item of interest of the standalone inertial navigation algorithm and formed Equations (28)–(30). While the INS states are error states, the remaining states are the full bias states of the accelerometer and gyro from the IMU. Accordingly for the cascaded navigation EKF, the error state vector is defined as:

$$\mathbf{x}=\left[\begin{array}{c}\delta \mathbf{p}\\ \delta {\mathbf{v}}^n\\ \delta {\mathit{\psi }}_{nb}\\ {\mathbf{b}}_a^b\\ {\mathbf{b}}_g^b\end{array}\right]$$

(37)

Remember that the position vector p is a vector of latitude, longitude and altitude, vⁿ is a velocity vector of north, east and down velocities and ψ_nb is an attitude vector of roll, pitch and yaw. Note that the position states are not necessarily required to estimate the attitude, but are included in the formulation, because the typical user is also interested in the position information. The two bias vectors are included to account for the biases present in the IMU, where ${\mathbf{b}}_a^b$ is comprised of the three bias states corresponding to the accelerometer biases, and similarly, ${\mathbf{b}}_g^b$ is comprised of the gyro biases.

$${\mathbf{b}}_a^b=\left[\begin{array}{c}{\mathrm{b}}_{a_x}^b\\ {\mathrm{b}}_{a_y}^b\\ {\mathrm{b}}_{a_z}^b\end{array}\right]$$

$${\mathbf{b}}_g^b=\left[\begin{array}{c}{\mathrm{b}}_{g_{\varphi }}^b\\ {\mathrm{b}}_{g_{\theta }}^b\\ {\mathrm{b}}_{g_{\psi }}^b\end{array}\right]$$

The IMU model consists of random noise and a bias added to the accelerations and angular velocities. The biases are modeled as a random walk. Specifically, the bias models for a low-cost inertial IMU are:

$${\mathbf{b}}_a^b(t)={\mathbf{b}}_{as}^b+{\mathbf{b}}_{ad}^b+{\mathbf{w}}_a^b$$

(38)

$${\mathbf{b}}_g^b(t)={\mathbf{b}}_{gs}^b+{\mathbf{b}}_{gd}^b+{\mathbf{w}}_g^b$$

(39)

Note that the biases are a function of time, with some components that are modeled as stochastic processes. Both the accelerometer biases and the gyro biases are the sum of three random processes. The terms ${\mathbf{b}}_{as}^b$ and ${\mathbf{b}}_{gs}^b$ represent the constant null shifts of the accelerometer and gyro. They are essentially static biases that are modeled as random constants. The terms ${\mathbf{b}}_{ad}^b$ and ${\mathbf{b}}_{gd}^b$ are the dynamic “in-run” biases. These two time-varying components are modeled as an exponentially correlated random process, i.e., a first order Gauss–Markov process specifically with a standard deviation σ_ad for the accelerometer and σ_gd for the gyro. The Gauss–Markov processes for the accelerometer and gyro bias models are:

$$\frac{d{\mathbf{b}}_{ad}^b}{dt}={\dot{\mathbf{b}}}_{ad}^b=\frac{-1}{{\tau }_a}{\mathbf{I}}_{3\times 3}{\mathbf{b}}_{ad}^b+{\mathbf{I}}_{3\times 3}{\mathit{\mu }}_a^b$$

(40)

$$\frac{d{\mathbf{b}}_{gd}^b}{dt}={\dot{\mathbf{b}}}_{gd}^b=\frac{-1}{{\tau }_g}{\mathbf{I}}_{3\times 3}{\mathbf{b}}_{gd}^b+{\mathbf{I}}_{3\times 3}{\mathit{\mu }}_g^b$$

(41)

The linearized system matrix F is the Jacobian of the system model equations, which are Equations (28)–(30) and (40)–(41). The Jacobian of the system model equations yields the system model matrix F:

$$\mathbf{F}=\left[\begin{array}{ccccc}-\left[{\left({\mathit{\omega }}_{en}^n\right)}^{^}\right]& \mathbf{T}& 0_3& 0_3& 0_3\\ {\mathbf{G}}_g& -\left[2{\left({\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right)}^{^}\right]& \left[{\left({\mathbf{C}}_b^n{\mathbf{f}}^b\right)}^{^}\right]& {\mathbf{C}}_b^n& 0_3\\ 0_3& 0_3& -\left[{\left({\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right)}^{^}\right]& 0_3& -{\mathbf{C}}_b^n\\ 0_3& 0_3& 0_3& \frac{-1}{{\tau }_a}{\mathbf{I}}_3& 0_3\\ 0_3& 0_3& 0_3& 0_3& \frac{-1}{{\tau }_g}{\mathbf{I}}_3\end{array}\right]$$

(42)

where 0₃ is a three by three null matrix, I₃ is a three by three identity matrix and:

$${\mathbf{G}}_g=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& -\frac{2g}{R_o}\end{array}\right]$$

(43)

For small propagation times, the F matrix can be simplified to:

$$\mathbf{F}=\left[\begin{array}{ccccc}{0}_3& \mathbf{T}& 0_3& 0_3& 0_3\\ {\mathbf{G}}_g& 0_3& \left[{\left({\mathbf{C}}_b^n{\mathbf{f}}^b\right)}^{^}\right]& {\mathbf{C}}_b^n& 0_3\\ 0_3& 0_3& 0_3& 0_3& -{\mathbf{C}}_b^n\\ 0_3& 0_3& 0_3& \frac{-1}{{\tau }_a}{\mathbf{I}}_3& 0_3\\ 0_3& 0_3& 0_3& 0_3& \frac{-1}{{\tau }_g}{\mathbf{I}}_3\end{array}\right]$$

(44)

The corresponding process noise, w_s, of the system is:

$${\mathbf{w}}_s=\left[\begin{array}{c}{0}_{3\times 1}\\ {\mathbf{C}}_b^n{\mathbf{w}}_a\\ {\mathbf{C}}_b^n{\mathbf{w}}_g\\ {\mathbf{C}}_b^n{\mathit{\mu }}_a\\ {\mathbf{C}}_b^n{\mathit{\mu }}_g\end{array}\right]$$

(45)

The process noise is used to define the process covariance matrix, Q. Taking the expectation of the process noise defines the diagonal terms of Q as power spectral densities. The expectations, $\mathrm{E}\left\{{\mathbf{w}}_a{\mathbf{w}}_a^T\right\}$ and $\mathrm{E}\left\{{\mathbf{w}}_g{\mathbf{w}}_g^T\right\}$, are the accelerometer and gyro noise power spectral densities or standard deviations. According to [15], the process noise of the bias states, µ_a and µ_g are $2{\sigma }_{ad/{\tau }_a}^2$ and $2{\sigma }_{gd/{\tau }_g}^2$, respectively. The terms τ_a and τ_g are the time constants for the Markov process error model for acceleration and gyro errors, respectively. The discrete process noise matrix, Q_k, is calculated from the continuous process noise matrix, Q, using the time interval τ_s = t_k+1 − t_k.

$${\mathbf{Q}}_k={\tau }_s\mathbf{Q}$$

(46)

Neglecting any lever arm effects (i.e., the kinematic displacement between the center of gravity and the sensor location), the measurement matrix H is:

$$\mathbf{H}={\left[\begin{array}{ccccc}{\mathbf{I}}_3& 0_3& 0_3& 0_3& 0_3\\ 0_3& {\mathbf{I}}_3& 0_3& 0_3& 0_3\end{array}\right]}_{6\times 15}$$

(47)

For the closed-loop error state navigation EKF, all of the states, except the attitude states of the state vector, x, need to be fed back following the measurement update prior to the mechanization phase. For example, the update to the position state is given in Equation (48):

$${\widehat{\mathbf{p}}}^{+}={\widehat{\mathbf{p}}}^{-}-\delta {\widehat{\mathbf{p}}}_{k+1}$$

(48)

where the superscripts + and − represent before and after the error state corrections are applied and $\delta {\widehat{\mathbf{p}}}_{k+1}$ represents the position error state estimate from the measurement update. The attitude errors, $\delta {\widehat{\mathit{\psi }}}_{nb}^n$, are corrections to the rotation matrix, ${\widehat{\mathbf{C}}}_b^n$, and therefore cannot simply be added to the previous Euler angle estimates. The attitude states are updated by executing the following equations:

$${\widehat{\mathbf{C}}}_b^{n+}=\left({\mathbf{I}}_3+\left[\delta {\widehat{\mathit{\psi }}}_{nb}^n^\right]\right){\widehat{\mathbf{C}}}_b^{n-}$$

(49)

The Euler angles are then extracted from ${\widehat{\mathbf{C}}}_b^{n+}$ by:

$$\varphi ={\tan }^{-1}\left(\frac{c_{32}}{c_{33}}\right)$$

(50)

$$\theta =-{\sin }^{-1}{c}_{31}$$

(51)

$$\psi ={\tan }^{-1}\left(\frac{c_{21}}{c_{11}}\right)$$

(52)

where c_ij represents the element in the i-th row and j-th column of the rotation matrix ${\mathbf{C}}_b^{n+}$. Once the update is complete, the Kalman filter state vector, ${\widehat{\mathbf{x}}}_{k+1}^{+}$, is reset to zero and ${\mathbf{P}}_{k+1}^{+}$ is used to initiate another time update cycle [14].

3.7. NAV EKF Cascaded with Relative Roll

The difference between the total roll (ϕ_v) and the relative roll (ϕ_s) of the suspension deflections is the road bank.

$${\varphi }_r={\varphi }_v-{\varphi }_s$$

(53)

Since the suspension roll can be determined from Equation (2), we can subtract it from the total body roll estimated by the EKF to yield an estimate of road bank:

$${\varphi }_{r,est}={\varphi }_{v,EKF}-{\varphi }_s$$

(54)

Assuming the tires remain in contact with the ground, the dynamics of the vehicle body can be correctly removed via the measurement relationship in Equation (54). Note that errors in the estimate of road bank will be related to the accuracy of the total roll estimate, ϕ_v, and how well the suspension scaling parameter, η, scales the relative roll measurement, ϕ_s.

4. Road Bank Estimation Using Suspension Deflections and a Coupled Navigation EKF

The goal of the coupled approach is to see if any useful relationship between the relative roll angle given from the suspension deflections and the total roll state of the navigation filter could be exploited. Since the deflections are measured from potentiometers that operate at 100 Hz or better, it seemed logical to have the suspension deflections implemented inside the INS navigation portion of the EKF filter, the time propagation phase.

The coupled or augmented approach uses the same principle equations and structure as the cascaded method described in Section 3.6.1. However, there are some details that are important to know for the coupled approach. First, the error state vector is augmented with the road bank as the 16th state.

$$\mathbf{x}=\left[\begin{array}{c}\delta \mathbf{p}\\ \delta {\mathbf{v}}^n\\ \delta {\mathit{\psi }}_{nb}^n\\ {\mathbf{b}}_a^b\\ {\mathbf{b}}_g^b\\ {\varphi }_r^n\end{array}\right]$$

(55)

The system model, the F matrix, must be adjusted, as well. Ideally, there would exist an equation that propagates the road bank rate as a function of the states in the state vector. No such function exists, so the road bank rate is modeled as a disturbance with the time constant τ_s.

$${\dot{\varphi }}_r^n=\frac{-1}{{\tau }_s}{\varphi }_r^n$$

(56)

The new model matrix is:

$$\mathbf{F}=\left(\begin{array}{cccccc}-\left[{\left({\mathit{\omega }}_{en}^n\right)}^{^}\right]& \mathbf{T}& 0_3& 0_3& 0_3& 0_{3\times 1}\\ {\mathbf{G}}_g& -\left[2{\left({\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right)}^{^}\right]& \left[{\left({\mathbf{C}}_b^n{\mathbf{f}}^b\right)}^{^}\right]& {\mathbf{C}}_b^n& 0_3& 0_{3\times 1}\\ 0_3& 0_3& -\left[{\left({\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right)}^{^}\right]& 0_3& -{\mathbf{C}}_b^n& 0_{3\times 1}\\ 0_3& 0_3& 0_3& \frac{-1}{{\tau }_a}{\mathbf{I}}_3& 0_3& 0_{3\times 1}\\ 0_3& 0_3& 0_3& 0_3& \frac{-1}{{\tau }_g}{\mathbf{I}}_3& 0_{3\times 1}\\ 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& \frac{-1}{{\tau }_s}\end{array}\right)$$

(57)

where 0₃ is a three by three null matrix, 0_3×1 is a three by one null matrix, 0_1×3 is a one by three null matrix, I₃ is a three by three identity matrix and:

$${\mathbf{G}}_g=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& -\frac{2g}{R_o}\end{array}\right]$$

For small propagation times, the F matrix can be simplified to:

$$\mathbf{F}=\left(\begin{array}{cccccc}{0}_3& \mathbf{T}& 0_3& 0_3& 0_3& 0_{3\times 1}\\ {\mathbf{G}}_g& 0_3& \left[{\left({\mathbf{C}}_b^n{\mathbf{f}}^b\right)}^{^}\right]& {\mathbf{C}}_b^n& 0_3& 0_{3\times 1}\\ 0_3& 0_3& 0_3& 0_3& -{\mathbf{C}}_b^n& 0_{3\times 1}\\ 0_3& 0_3& 0_3& \frac{-1}{{\tau }_a}{\mathbf{I}}_3& 0_3& 0_{3\times 1}\\ 0_3& 0_3& 0_3& 0_3& \frac{-1}{{\tau }_g}{\mathbf{I}}_3& 0_{3\times 1}\\ 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& \frac{-1}{{\tau }_s}\end{array}\right)$$

(58)

The measurement matrix for the GPS measurement is:

$$\mathbf{H}={\left[\begin{array}{ccccc}{\mathbf{I}}_3& 0_3& 0_3& 0_3& 0\\ 0_3& {\mathbf{I}}_3& 0_3& 0_3& 0\end{array}\right]}_{6\times 16}$$

(59)

The suspension deflection measurement is incorporated through a second measurement update. Like the GPS update, the second measurement update will occur whenever a new deflection measurement exists. The measurement matrix for the second update is:

$${\mathbf{H}}_2={\left[0_{1\times 3}{0}_{1\times 3}1000_{1\times 3}{0}_{1\times 3}-1\right]}_{1\times 16}$$

(60)

In practice, coupling the navigation filter with the road bank state did not provide any noticeable benefits to the estimation of the other states in the filter. However, the experimental results show that adding the road bank as a state to the navigation EKF provided some filtering of noise on the road bank state estimate.

5. Experimental Setup

The Prowler ATV, a light, tactical, all-terrain vehicle for the military designed by ATV Corporation, was used to test the road bank algorithms. The Prowler has been automated and is outfitted with various sensors. To test the road bank algorithm, the following sensors were used: single-antenna GPS receiver, six degrees of freedom IMU and four linear potentiometers to measure suspension deflections. In addition, a three-antenna GPS attitude system was utilized to obtain accurate attitude truth measurements for comparison with the EKF attitude estimates.

The area selected for testing of the vehicle occurred at the National Center for Asphalt Technology (NCAT) track. The facility has a closed 1.7-mile oval track and a skid pad area, which allows for safe testing of autonomous vehicle operation with sufficient GPS accessibility for various testing scenarios.

5.1. Sensor Mounting

The Prowler vehicle has a range of sensors to provide information about the vehicle states. The six DOF Crossbow 440 IMU provides accelerations and gyroscopic rates about all three vehicle axes. As seen in Figure 6, the Crossbow is mounted directly behind the driver seat and is centered with respect to the lateral axis. Various GPS sensors have been implemented on the vehicle, including a Novatel system with integrated real-time-kinematic (RTK) corrections and a Septentrio system, which provides GPS-based roll, pitch and yaw measurements. The Novatel antenna is located on top of the Prowler roll cage and is laterally and longitudinally aligned with the CrossBow IMU. Celesco potentiometers mounted on the suspension components produce measurements of suspension deflections. Brackets were created that position the potentiometer, such that it is collinear with the shock and spring assembly, as seen in Figure 6. A Celesco string potentiometer is mounted on the steering rack and measures the turns of the steering column. The string potentiometer provides a measurement of the steering angle at the wheels with the assumption that steer angles at the left and right wheels are equal.

The black case mounted on the front of the Prowler holds almost all of the sensor electronics, as shown in Figure 7a. The Septentrio and Novatel receivers are stacked on top of one another and connect via a serial port to the Advantech fan-less computer, the silver brick furthest from the viewer in Figure 7a. The Crossbow IMU communicates via a serial connection, as well. The steering potentiometer and suspension potentiometers are both read by an A/D converter on a Microchip PIC and custom board. Each board is housed in an aluminum enclosure shown in the center of Figure 7a. The PIC sends the converted reading to the CAN Bus, which is read via a CAN toUSB adapter by the Advantech computer.

The Advantech computer runs Ubuntu, which is a Linux operating system. The MOOS (Mission Oriented Operating Suite) architecture is the C++ platform used to log the data. Sensor interfaces that inherit from a base sensor class were written to read and log each sensor.

5.2. Test Procedures

For maneuvers on flat terrain, the NCAT skid pad and the straights on the the NCAT oval track were used. Note that the flat surface of the skidpad appears to have roughly a two-degree slope, with the high portion in the northwest corner and the low portion in the southeast corner. The straights of the NCAT oval track have a 1–2 degree crown, with the peak running between the two lanes.

Maneuvers involving the bank were performed on turns of the NCAT oval track. Both turning sections transition from the 1–2-degree crown of the straights to roughly eight degrees of the bank and then back to 1–2 degrees of the crown. Figure 7b via a chase vehicle shows the Prowler driving through the turn on the west end of the track.

Specific maneuvers of interest were the lane changes, sinusoidal steer inputs, constant radius steady-state turns and straight driving at various speeds. Lane changes were performed on the straights and the banked turns of the NCAT oval track, but they were not executed with any measured spacings or lengths. The runs involving sinusoidal steer inputs were intended to provide insight into the effects on bank estimation with respect to the high- and low-frequency steering inputs.

6. Results and Discussion

From Equation (8) and suspension component measurements, η was found to be 2.3. The eight-degree banked turns on the NCAT oval track were used to verify the η scale factor for the suspension deflection measurements.

The standard deviation values used for the process covariance matrix Q are listed in

Table 1. Process noise standard deviations for Q.

Name	Value	Units
σa	0.3	m/s2
σg	0.3	degrees/s
${\sigma }_{b_a}$	0.01	m/s2
${\sigma }_{b_g}$	0.035	degrees/s
${\sigma }_{{\varphi }_r}$	1	degrees

. Note that the variance for states with process noise modeled via the Markov process error model are functions of the standard deviation and the corresponding time constant or $2\sigma \raisebox{1ex}{$2$}\!\left/ \!\raisebox{-1ex}{$\tau $}\right.$. The time constants for the IMU bias in

Table 2. Time constants.

Name	Value	Units
τa	500	s
τg	1,300	s
τs	1,000	s

are based on the findings in [18]. The standard deviation values for the measurement covariance matrix R and scalar R₂, to be used with the second measurement update H₂ in the coupled EKF, are presented in

Table 3. Tuned measurement standard deviations for R and R₂.

Name	Value	Units
σPos. Horz.	0.0000052	degrees
σPos. Z	1	m
σVel. Horz.	0.05	m/s
σPos. Z	0.1	ms
σDef	0.0002	m
${\sigma }_{{\varphi }_s}$	${\sin }^{-1}\left(\frac{4{\sigma }_{Def}}{2t_w}\right)$	rad

. Finally, the initial variance values for the covariance matrix P are found in

Table 4. Error covariances for P.

Name	Value	Units
${\sigma }_{L_{\mathrm{P}},{\lambda }_{\mathrm{P}}}^2$	0.001	rad
${\sigma }_{h_{\mathrm{P}}}^2$	2.8	m
${\sigma }_{{\mathrm{Vel}}_{\mathrm{P}}}^2$	0.1	m/s
${\sigma }_{\varphi ,{\theta }_{\mathrm{P}}}^2$	0.01	rad
${\sigma }_{{\psi }_{\mathrm{P}}}^2$	0.05	rad
${\sigma }_{b_{a_{\mathrm{P}}}}^2$	0.000001	m/s2
${\sigma }_{b_{g_{\mathrm{P}}}}^2$	0.000001	rad/s
${\sigma }_{{\varphi }_{r_{\mathrm{P}}}}^2$	0.0001	degrees

.

The initial state vector values were assumed to be known. Position and velocities states were initialized with values from the Novatel GPS receiver. Euler angle states were initialized with values from the Septentrio receiver. Acceleration and angular rates from the CrossBow IMU were each averaged from static data taken at the begging of the dataset. These averaged values were then used to initialize the bias states of the state vector. High dynamic maneuvers at the beginning of the run served to provide sufficient excitation, so that the bias could be estimated.

No difference in time to convergence of the error covariances was observed between the cascaded and coupled approach. Note, that if the initial states, especially the gyro bias and accelerometer bias states, are significantly different from truth, then the attitude estimate states will be less accurate until the error covariances converge and the biases are estimated. In this work, the initial position velocity and attitude states are known, and the bias states are initialized from averaged static IMU measurements. For a deeper look into the convergence of the bias states and the importance of excitation for attitude estimates, refer to [19]. In [19], the observability of a similar EKF is explored, and experimental results using similar sensors are provided.

6.1. Comparison of Cascaded and Suspension-Coupled Navigation EKF

Both architectures presented in Sections 3 and 4 were adjusted to handle the timing and structure of real data. The filter states are very similar in comparison. The bank estimates of each architecture match the expected measurements of bank on the NCAT oval track. For reference, Figure 8 represents a plot of the measured and estimated positions of the Prowler on the NCAT oval track. The estimates are from the 15-state navigation EKF.

6.1.1. Navigation EKF Cascaded

Recall that a three-antenna Septentrio unit was mounted on the Prowler to provide truth measurements of the roll, pitch and yaw of the vehicle. To demonstrate that the navigation EKF filter is estimating the states correctly, the attitude measurements from the Septentrio are compared with the attitude estimates of the 15-state navigation EKF. Figure 9 represents the roll and pitch comparison, while Figure 10 represents the comparison of yaw. The data in both figures are from a single lap around the NCAT oval track. The banked turns are marked by the sections where the roll angle is roughly eight degrees. The sinusoids are purposefully induced to excite the vehicle states for the estimation of biases. Note for the data shown in Figures 9 and 10 that the bias states have been previously estimated and were used as the initial biases for this dataset. Accordingly, the covariance matrix was set to small initial values, indicating that the biases were known.

Observe in Figure 9 that the pitch estimate is roughly one degree different from the Septentrio measurement. The difference in the pitch data from the Septentrio is due to slight misalignment of the Septentrio antenna frame and the IMU frame. The IMU mounting location behind the seat of the Prowler is not completely coplanar in the direction of pitch with the vehicle body. The mounting surface pitches downward roughly one to two degrees. Additionally, observe in Figure 9 that the Septentrio has difficulties maintaining both a “no fix solution” and a “fixed solution” for attitude during the high-frequency vehicle body roll that begins prior to 520 s.

Figure 10 represents the yaw estimate and gives further confidence that the EKF filter is estimating properly. The estimates of yaw match quite well with the measurements from the Septentrio.

The estimate of bank is shown in Figure 11. For reference, the estimate of total roll, the Euler roll and the measured roll from the suspension deflections are displayed. The figure shows a section of straight driving followed by sinusoidal motion. Then, straight driving is resumed during the banked turn, after which sinusoidal motion is resumed. While in the straight section, the one to two degrees of road crown are observed in the estimate of the bank from time 460 s to 465 s. During the sinusoids, the bank estimate remains within reasonable bounds. Note that due to the orientation of the vehicle as it oscillates from left to right, the bank estimate is a combination of the crown and vehicle orientation. The vehicle oscillates in the right side lane from 460–475 s. This is evident from the average bank estimate of two degrees. Similarly, from 475 s till the bank begins at 485 s, the vehicle oscillates about the center of the crown and displays an average bank estimate of zero degrees. Recall that the NCAT oval track has banks of eight degrees through the turns. The estimate of bank during the steady-state turn occurring over the period of 492–506 s is eight degrees. Note that roll measured by suspension deflections is not zero during the steady-state turn on the banked surface.

6.1.2. Navigation EKF Couple

The measurements and estimates of roll, pitch and yaw are again presented to validate that the 16-state coupled Navigation EKF is functioning as expected. The estimates of roll pitch and yaw in Figures 12 and 13 match very closely with the estimates of the 15-state navigation filter estimates shown in Figures 9 and 10.

The root mean square values of the difference between attitude measurements from the Septentrio and attitude estimates from both filters are provided in

Table 5. Root mean square error for cascaded EKF attitude estimates.

Name	Value	Units
ϕv	0.401	degrees
θ	0.570	degrees
ψ	0.577	degrees

and

Table 6. Root mean square error for coupled EKF attitude estimates.

Name	Value	Units
ϕv	0.422	degrees
θ	0.526	degrees
ψ	0.696	degrees

, respectively. Note, there maybe some minor alignment discrepancies between the coordinate frame of the filter sensors and the Septentrio frame due to mounting of sensor equipment. Thus, some deviation from the reference Septentrio measurements is to be expected. Nonetheless, the RMS error of the attitude from both filter methods confirms that the attitude is being estimated properly.

Figure 14 represents a section from the NCAT oval track. The same time interval of 460 s to 520 s is used to compare the result with Figure 11. The estimate of the bank for the navigation EKF coupled with the bank state looks very similar to the cascaded bank estimate approach with standard navigation EKF. However, as mentioned previously, the coupled EKF filters the noise from the bank estimate. The noise filtering is the primary advantage of the coupled approach. For this reason, the remaining experimental result analysis is presented with the coupled 16-state navigation filter.

6.1.3. Additional Validation of the Coupled Navigation EKF

A circle with a radius of 10 ft was marked on the skid pad area at NCAT. Laps were driven around the circle. The velocity was increased about every two laps, until the tires exceeded peak force generation and were unable to provide enough lateral force to hold the turn for the ATV. The idea is to demonstrate the steady-state cornering capabilities of a vehicle in both the linear and the nonlinear section of the tire curve. As the speed increases more, the steer angle is required to hold the turn. In the nonlinear portion of the tire curve, the required steer angle is no longer linearly proportional to the step increase in velocity. Figure 15 shows Novatel measurements of position, as well as the horizontal velocity profile for the duration of the 10-ft circle run. Figure 16 shows the bank and total roll estimates and the relative roll measurement from the suspension potentiometers. The peaks of the bank are represented by triangles. In this run, the Prowler is turning right, which means the lateral acceleration pushes the vehicle body towards the outside of the circle, and the roll due to dynamics ϕ_s is always negative. At 121.4 s, the vehicle was pointed roughly east and experienced positive 1.25 degrees of bank. At 124.6 s, when the vehicle was pointed roughly west, the vehicle experienced −1.5 degrees of bank. Thus, this area of the skidpad appears to have roughly a bank of one to two degrees. Static measurements observed while testing confirm that the skidpad area does tend to slope about 1–2 degrees in the direction running from the southeast to the northwest.

The prowler suspension is nonlinear in that at extreme turn angles, the outer wheel turns in more than the inner wheel, as seen in Figure 17. Also present in Figure 17 is tire deformation in the outer front tire. These properties could effect the bank angle estimate. However, Figure 16 shows almost no noticeable growth in peak bank estimates as the vehicle increases speed towards the conditions where asymmetrical steering and tire relaxation are prevalent.

Revisiting the NCAT oval track run, the coupled EKF bank estimate was tested under highly dynamic steering input excitations. Figure 18 shows the position and velocity for a banked turn on the NCAT oval track. On this particular turn, the roll axis is excited through sinusoidal steering inputs. The sinusoids start during the transition from the crowned straight to the banked left turn and end just before the transition back to the crowned straight. The bank estimate through the turn shown in Figure 19 closely resembles the bank estimate in Figure 14. Notice that the bank estimate for the increasing bank angle is unaffected by the vehicle body roll excitation.

In Figure 19, the vehicle returns to the straight section of track around 710 s. At 713.5 s, the vehicle is centered over the road crown straddling the two lanes, and the bank is roughly zero. The vehicle then performs a lane change maneuver to the outer edge of the track, and at 716.9 s, the estimate of the crown for this section of straight is observed to be about two degrees, which corresponds to the 1–2 degrees of crown purposefully designed into the track construction.

7. Conclusions

This work presents a method that provides an estimate of road bank by decoupling the vehicle roll due to dynamics and roll due to the road bank. Suspension deflection measurements were used to provide a measurement of the relative roll between the vehicle body frame and the axle frame or between the sprung mass and the unsprung mass, respectively. A method of scaling the suspension deflection measurements to vertical wheel motion was explored. A deflection scaling parameter was found by both a dynamics-based method and a suspension geometry-based method. The parameter was determined to effectively scale the suspension deflection measurements with minimum error variances over varying vehicle speeds. The relative roll measurement was then incorporated into two different estimation architectures. Two kinematic navigation model-based extended Kalman filters (EKF) were developed. The first EKF used a cascaded approach to incorporate the relative roll measurement. The second EKF, a coupled approach, augmented the state vector with a state for the road bank. The road bank rate was modeled as a time-varying disturbance, and a measurement update for the relative roll measurement was developed. The estimators were tested on the Prowler ATV experimental platform at the National Center for Asphalt Technology (NCAT). Both the cascaded and coupled approach performed well for both simulation and experimental data. The EKFs correctly estimated the road crown and banked turns of the NCAT oval track. The coupled EKF displayed the added benefit of filtering the noise on the bank estimate. Both of the kinematic-based approaches performed well across all ranges of dynamics and road bank disturbances. However, the coupled approach filtered the noise on the bank estimate, which was determined to be advantageous. Furthermore, it should be noted that the methodology presented can be modified and applied to vehicle pitch and road grade estimation.