Moog Crossbow NAV440 Series - Manual (Page 25)

User Manual
7430‐0131‐01 Rev. F
Page 25 NAV440
NOTE: If the coordinate system is configured to a non‐standard or custom configuration, apply the appropriate
rotation and configure the filter settings accordingly.
VG440 (Vertical Gyroscope)Function
The VG440 provides dynamic roll and pitch measurements, as well as allIMU440 data functions. The dynamic roll
and pitch measurements are stabilized by the using the accelerometers as a long‐term gravity reference. The VG440
can also output a free integrating yaw angle measurement that is not stabilized by a magnetometer or compass
heading).
At a fixed 100Hz rate, the VG440 continuously maintains both the digital IMU440 data as well as the dynamic roll
and pitch data. As shown in the software block diagram(Figure 2 on page 21), after the Sensor Calibration block, the
IMU440 data is passed into an Integration to Orientation block. (If using external GPS, refer to Figure 4on
page22.)The Integration to Orientation block integrates body frame sensed angular rate to orientation at a fixed 100
nd to av es when dealing with the ix, a
t to p e pro
times per second. For improved accuracy a oid singulariti cosine rotation matr
quaternion formulation is used in the algori hm rovide attitud pagation.
Also shown in the software block diagram (Figure 2, page 21) the Integration to Orientation block receives drift
corrections from the Extended Kalman Filter or Drift Correction Module. In general, rate sensors and accelerometers
suffer from bias drift, misalignment errors, acceleration errors (g‐sensitivity), nonlinearity (square terms), and scale
factor errors.
The largest error in the orientation propagation is associated with the rate sensor bias terms. The Extended Kalman
Filter (EKF) module provides an on‐the‐fly calibration for drift errors, including the rate sensor bias, by providing
corrections to the Integration to Orientation block and a characterization of the gyroscope bias state. In the VG440,
the internally computed gravity reference vector provides a reference measurement for the EKF when the unit is in
quasi‐static motion to correct roll and pitch angle drift and to estimate the X and Y gyroscope rate bias. Because the
gravity vector has no horizontal component, the EKF has no ability to estimate either the yaw angle error or the Z
gyroscope rate bias.
VG440 adaptively tunes the EKF feedback in order to best balance the bias estimation and attitude correction with
distortion free performance during dynamics when the object is accelerating either linearly (speed changes) or
centripetally (false gravity forces from turns). Because centripetal and other dynamic accelerations are often
associated with yaw rate, the unit maintains a low‐pass filtered yaw rate signal and compares it to the turnSwitch
threshold field (user adjustable).
When the platform the unit is attached to exceeds the turnSwitch threshold yaw rate, the unit lowers the feedback
gains from the accelerometers to allow the attitude estimate to coast through the dynamic situation with primary
reliance on angular rate sensors. This situation is indicated by the softwareStatus→turnSwitch status flag. Using the
turnSwitch maintains better attitude accuracy during short‐term dynamic situations, but care must be taken to
ensure that the duty cycle of the turnSwitch generally stays below 10% during the vehicle mission. A high
turnSwitch duty cycle does not allow the system to apply enough rate sensor bias correction and could allow the
attitude estimate to become unstable.
The VG440 algorithm has two major phases of operation. The first phase of operation is the initialization phase.
During the initialization phase, the unit is expected to be stationary or quasi‐static so the EKF weights the
accelerometer gravity reference heavily in order to rapidly estimate the roll and pitch angles, and X, Y rate sensor
bias. The initialization phase lasts approximately 60 seconds, and the initialization phase can be monitored in the
software Status BIT transmitted by default in each measurement packet. After the initialization phase, the unit
operates with lower levels of feedback (also referred to as EKF gain) from the accelerometers to continuously
estimate and correct for roll and pitch errors, as well as to estimate X and Y rate sensor bias. To reset the algorithm
or re‐enter the initialization phase, sending the algorithm reset command, AR, will force the algorithm into the reset
phase.