1. IMU state vector
- All elements are column vectors. We ignore their transpose symbol.
-
is the quaternion from the global frame to the IMU frame.
-
is the pose with respect to global frame.
-
is the velocity with respect to global frame.
-
and
are biases of gyroscope and accelerometer, respectively.
-
: MSCKF uses JPL quaternion, which is left-hand. Some operations is different from the right-hand quaternion, Hamilton quaternion.
2. The continuous-time motion dynamics of the IMU
- The equation(1) resembles Possion equation where
- The relationship between equation (1) and Possion's equation is noted in the ordinary differential equation about rotation.
- (2) and (3) represents the derivatives of pose is velocity and that of velocity is acceleration, where all variable are respected to globel frame.
- (4) and (5) means the derivative of bias is gaussian noise.
3. Error state
- The error represent the different between true value and estimated value, such as
;
- The orientation error
with respect to
frame satisifies the following equation:
whereis because of JPL quaternions.
4. Error Propagation
4.1 Orientation
From time step to
, the estimated rotation matrix is
where
-
.
-
is the IMU frame at time
.
We define the error state by
where ,
From the above functions, (7) and (8), we obtain the following expression for the linearized error propagation:
The function is represented by equation (15) in msckf2.0 and its detail is shown in The Detail of The Error State in MSCKF.
4.2 Velocity
The velocity propation equation is
where represent the integral of time and
is the acceleration with respect to global frame.
where is imu measurement in body frame at time
.
compute the measurement with repsect to global frame.
add gravity, where the imu measurement contains a negative gravity (
) as following:
where is bias and
is noise.
We define and rebuild the velocity propagation as following:
Finally the error state of velocity is represented as following:
where only depends on the IMU noise. The detail of function is shown in The Detail of The Error State in MSCKF.
4.3 Pose
The position of IMU propagation is:
where . And proceeding to error-state model, we obtain:
By combining (9), (14) and (16), the error propagation function in MSCKF is writen as following:
and
can be writen by:
网友评论