The second problem in tracking and navigation is concerned with estimating the location and orientation of a body for which we have onboard kinematic measurements. Inertial measurement units (IMUs) consist of a set of three accelerometers placed to make acceleration-related measurements and a set of three rate gyroscopes that sense angular velocities in three mutually perpendicular directions. Here, we discuss the manner in which these signals can be used to determine the location and orientation of the body to which the IMU is rigidly attached. This process is known as strapdown inertial navigation [1, 2].
Signals from an inertial measurement unit
Referring to Figure 1, suppose the motion of a rigid body is characterized by a rotation tensor and the position vector of a point on the body where an IMU is attached. The point is sometimes called a landmark. The rotation tensor relates the fixed, right-handed Cartesian basis to the body’s corotational basis , which we assume is aligned with the accelerometers’ and rate gyroscopes’ axes.
The IMU’s rate gyroscopes measure the components of the rigid body’s absolute angular velocity in the corotational basis. That is, the rate gyroscopes provide the signals
The IMU’s accelerometers sense the corotational components of the so-called specific force exerted at the landmark . Specific force is simply the difference between the absolute acceleration of , , and gravitational acceleration, and thus the output of the accelerometers, , can be identified with the measurements
Suppose a 3-2-1 set of Euler angles is used to parameterize . In vehicle dynamics, the Euler angles , , and are identified as the yaw, pitch, and roll angles, respectively. For this set of Euler angles, provided rate gyroscope data , the orientation of the rigid body is determined by integrating the following set of differential equations:
for which we must specify the body’s initial orientation: , , and . You may have noticed that these equations relating the rates of change of the Euler angles and can be obtained using the dual Euler basis vectors: , , and . Further, these equations are singular when rad.
To solve for how the position vector of the landmark varies over time, we first transform the corotational components of specific force, , measured by the accelerometers into the fixed Cartesian frame and then correct for gravitational acceleration to obtain the fixed Cartesian components of the absolute acceleration of , . If we consider motion near the earth’s surface, then the gravitational acceleration vector . Consequently,
We then twice integrate the system of differential equations (4), with initial position and velocity , to obtain the trajectory of . Expressions (3) and (4) are often referred to as the navigation equations. The process we described here for computing the displacement of point and the body’s orientation over time using the IMU accelerometer and gyroscope data is summarized diagrammatically in Figure 2.
In principle, numerically integrating (3) and (4) is not difficult. However, because of the inherent errors in numerical integration, the predicted orientation and location of the IMU will not correspond to the actual attitude and location. In fact, the discrepancies between the computed and actual orientation and position will increase over time. This phenomenon is known as integration drift, and it is unavoidable in inertial navigation systems. Unfortunately, the presence of noise in the IMU data, and , further amplifies errors in the estimated orientation and location. The use of accurate, but very expensive, IMUs slows the effect of integration drift and alleviates the issue of sensor noise, but a more effective strategy for managing integration drift would involve using other navigation aides such as GPS signals, magnetometers, and speedometer signals to supplement the IMU data and periodically correct the computed attitude and position.
To illustrate the effect of integration drift, consider an IMU attached to the center of a vehicle that travels at a constant speed in a circle of radius in the horizontal plane, as shown in Figure 3, where denotes the vehicle’s yaw angle.
If the initial conditions for the vehicle are such that , , and , then the exact solutions for the position of and the vehicle’s orientation are given by
Now, suppose the IMU’s sampling rate is 100 Hz. If we solve the navigation equations (6) and integrate to compute the trajectory of the vehicle’s center , we find, as shown in Figure 5, that the estimated trajectory (drawn in green) noticeably deviates from the actual circular path (drawn in red) as time progresses. This integration drift is unavoidable, but its effect can be slowed if the IMU were to collect data at a higher sampling rate.
As expected, we see in Figure 7 that the presence of noise in the data exacerbates the effect of integration drift, quickly resulting in a significant discrepancy between the estimated trajectory of (draw in green) and the actual circular path (drawn in red).
The MATLAB code used to create Figures 4–7 is available here. Please note that the noise added to the ideal IMU signals shown in Figure 4 is generated randomly, so executing this code on your machine will yield results that are similar, but not necessarily identical, to those depicted in Figures 6 and 7.