We use a 6-axis inertial measurement unit (IMU) to propagate the inertial navigation system (INS), which provides measurements of the local rotational velocity (angular rate) and local translational acceleration :
where and are the true rotational velocity and translational acceleration in the IMU local frame , and are the gyroscope and accelerometer biases, and are white Gaussian noise, is the gravity expressed in the global frame (noting that the gravity is slightly different on different locations of the globe), and is the rotation matrix from global to IMU local frame.
where is the unit quaternion representing the rotation global to IMU frame, is the position of IMU in global frame, and is the velocity of IMU in global frame. We will often write time as a subscript of describing the state of IMU at the time for notation clarity (e.g., ). In order to define the IMU error state, the standard additive error definition is employed for the position, velocity, and biases, while we use the quaternion error state with a left quaternion multiplicative error :
where is the rotation axis and is the rotation angle. For small rotation, the error angle vector is approximated by as the error vector about the three orientation axes. The total IMU error state thus is defined as the following 15x1 (not 16x1) vector:
where we have modeled the gyroscope and accelerometer biases as random walk and thus their time derivatives are white Gaussian. Note that the above kinematics have been defined in terms of the true acceleration and angular velocities.
Given the continuous-time measurements and in the time interval , and their estimates, i.e. after taking the expectation, and , we can define the solutions to the above IMU kinematics differential equation. The solution to the quaternion evolution has the following general form:
Differentiating and reordering the terms yields the governing equation for as
where the approximation assumes small . We can formulate the quaternion propagation from to using the estimated rotational velocity as:
Having defined the integration of the orientation, we can integrate the velocity and position over the measurement interval:
Propagation of each bias and is given by:
The biases will not evolve since our random walk noises and are zero-mean white Gaussian. All of the above integrals could be analytically or numerically solved if one wishes to use the continuous-time measurement evolution model.
A simpler method is to model the measurements as discrete-time over the integration period. To do this, the measurements can be assumed to be constant during the sampling period. We employ this assumption and approximate that the measurement at time remains the same until we get the next measurement at . For the quaternion propagation, it is the same as continuous-time propagation with constant measurement assumption . We use subscript to denote it is the measurement we get at time . Therefore the propagation of quaternion can be written as:
For the velocity and position propagation we have constant over . We can therefore directly solve for the new states as:
The propagation of each bias is likewise the continuous system:
In order to propagate the covariance matrix, we should derive the error-state propagation, i.e., computing the system Jacobian and noise Jacobian . In particular, when the covariance matrix of the continuous-time measurement noises is given by , then the discrete-time noise covariance can be computed as (see [Indirect Kalman Filter for 3D Attitude Estimation] ):
The method of computing Jacobians is to "perturb" each variable in the system and see how the old error "perturbation" relates to the new error state. That is, and can be found by perturbing each variable as:
where are the IMU sensor noises.
For the orientation error propagation, we start with the perturbation using :
where handles both the perturbation to the bias and measurement noise. is the right Jacobian of that maps the variation of rotation angle in the parameter vector space into the variation in the tangent vector space to the manifold [see ov_core::Jr_so3()]. By neglecting the second order terms from above, we obtain the following orientation error propagation:
Now we can do error propagation of position and velocity using the same scheme:
where . By neglecting the second order error terms, we obtain the following position and velocity error propagation:
The propagation of two biases can be derived in the same way:
By collecting all the perturbation results, we can build and matrices as:
Now, with the computed and matrices, we can propagate the covariance from to :
Tab / T to search, Esc to close
Search for symbols, directories, files, pages or
modules. You can omit any prefix from the symbol or file path; adding a
: or / suffix lists all members of given symbol or
/ ↑ to navigate through the list,
Enter to go.
Tab autocompletes common prefix, you can
copy a link to the result using ⌘L while ⌘M produces a Markdown link.