3DM-CX5-GNSS/INS
User Manual
39
The 3DM-CX5-GNSS/INS has a full-state dynamics model. The state propagation utilizes Newton’s
and Euler’s equations of motion with the acceleration and angular rate treated as control inputs. In
addition to the GNSS measurement, the IMU magnetometer is available to correct heading mis-
alignments which occur during periods of low dynamics. The magnetometer corrections can be disabled
for applications where large, non-constant magnetic interference sources exist, which would impair
their use (such as when mounting the 3DM-CX5-GNSS/INS on a gimbal close to the frame of a ground
vehicle).
The Kalman filter estimates the full states of position, velocity, attitude, and sensor parameters for a
total of 34 states: 3 position, 3 velocity, 4 attitude (quaternion), 3 accel bias, 3 gyro bias, 3 accel scale
factor, 3 gyro scale factor, 9 magnetometer, and 3 GNSS antenna offset error states. states. The gyro
bias states are estimated to compensate for the time-varying biases inherent in MEMS gyros, which
are the largest error sources for these devices. The magnetometer bias and scale-factor states are
estimated to compensate for magnetic field distortions (hard and soft- iron errors). Note that only
distortions that are fixed with respect to the sensor body coordinate frame of reference can be tracked.
Tracking these errors automatically enhances overall accuracy, making the sensor easier to deploy.
The Kalman filter also provides statistical information about the quality of the estimated states described
in a covariance matrix. The diagonal terms of the matrix are the variance of each state, thus the square
root of these values are 1- sigma standard deviations. These values give the filter’s estimation of how
well it knows the individual states, given what it knows about the statistical properties of the noise sources
of the various sensors and also provide feedback to the user as uncertainty values. The GNSS position
and velocity noise are not white but are treated as such in a loosely-coupled filter. This approximation
results are not optimal, but the advantages of this type of filter outweigh that disadvantage.
5.12
Estimation Filter Convergence
5.12.1
Initial Convergence
After a successful initialization, a period of convergence for the Kalman filter states occurs. Position,
velocity, roll and pitch angle typically converge very quickly. Heading, accelerometer bias, and gyro
bias take more time to converge. If the initial attitude estimate provided to the filter is well outside of
the true attitude, the filter may diverge and never recover. This is most likely to occur for the heading
estimate when a poor value is used for initialization and when the vibration environment is strong.
Should this occur, it is recommended that the filter be reset and new attitude estimate provided.
Refer to the 3DM-CX5-GNSS/INS
Data Communications Protocol Manual
for the various ways of
providing an initial attitude estimate through a user-designed interface.
