Class CoarseDynamicAlignment
Defined in File CoarseDynamicAlignment.hpp
Inheritance Relationships
Base Type
public navtk::inertial::AlignBase(Class AlignBase)
Class Documentation
Class that performs a dynamic alignment using IMU ( \(\delta v\) and \(\delta \theta\)) and position measurements.
Performance is affected by inertial biases, the level of position noise with respect to the platform velocity, and the accuracy of the initial position.
As the algorithm is fairly complicated, a high-level description follows.
First a period of stationary IMU data is collected to roughly estimate gyro biases and minimize error in the integrated orientation. Then position and IMU data are collected. With two sequential position measurements defining a measurement interval, the delta rotations within the interval are integrated to provide the change in rotation over the interval. These rotations are tracked over the entire alignment period to provide a total estimate of orientation with respect to the initial frame. Then the 2 position measurements are joined with the immediate prior position measurement and double-differenced to provide a reference value of the average specific force in the initial NED frame at the time of the middle position measurement. The delta velocities over the measurement interval are averaged and rotated back into the initial IMU sensor frame and scaled to provide specific forces. These 2 force vectors (NED frame from positions, initial IMU frame from IMU data) are then used to estimate the solution to Wahba’s problem, which concerns finding the rotation between 2 vector observations taken in different frames that differ by a fixed rotation. Once the solution has settled (based on the covariance of the last N solutions) this stage is considered complete.
However, the method used to solve Wahba’s problem may provide more than one solution (theoretically 4, typically 2), so a second stage is used to discriminate between these. Each solution is used to seed a BufferedImu and a position/INS filter estimating Pinson states. These are continuously updated with inertial and position data, and once one judged to be better than the rest (based on absolute and relative measures) the winning inertial and filter solutions are used to supply the alignment values.
Public Functions
Constructor.
- Parameters
model – Model of the inertial being aligned.
static_time – Minimum number of seconds of stationary data that is expected at the start of the run.
reset_time – How long, in seconds, to continue trying the current stage of alignment, with current data before reinitializing and trying again.
dcm_integration_method – Method to use when integrating DCMs in various places.
Add a measurement to be used in the alignment.
- Parameters
message – Measurement to use. Currently, only aspn_xtensor::MeasurementPosition and aspn_xtensor::MeasurementImu are recognized. Imu data must be ASPN_MEASUREMENT_IMU_IMU_TYPE_INTEGRATED type. All others will be ignored.
- Throws
std::invalid_argument – if error mode is DIE and an imu measurement of type other than ASPN_MEASUREMENT_IMU_IMU_TYPE_INTEGRATED is received.
- Returns
Status of alignment after incorporating the input.
Get computed covariance.
- Parameters
format – Format for the covariance block.
- Returns
A pair with a bool representing validity of the attached matrix, and the NxN covariance matrix (see AlignBase::get_computed_covariance for units).
Get IMU errors.
- Returns
A bool representing the validity of the attached errors, and the estimated accel (m/s^2) and gyro bias (rad/s) parameters; no other ImuErrors fields are currently estimated.
- Returns
Indicates whether the alignment strategy requires stationary data, motion, or doesn’t care.