Class ManualHeadingAlignment
Defined in File ManualHeadingAlignment.hpp
Inheritance Relationships
Base Type
public navtk::inertial::StaticAlignment(Class StaticAlignment)
Class Documentation
Static alignment, but override the computed heading with a user-provided one for use when gyrocompassing isn’t possible.
Should only be used with approximately NED (z axis down) aligned IMUs to ensure heading is translated accurately.
Public Functions
Initialize with user-provided heading, using a leveling procedure to determine a roll and pitch that minimizes horizontal acceleration.
- Parameters
heading – Heading to assign to the alignment, radians (right handed rotation from north, about down axis).
heading_sigma – One sigma uncertainty in heading, radians. Defaults to the equivalent of 1 deg.
model – The model of the inertial being aligned. Used in approximating the uncertainty of the orientation in the alignment solution.
align_time – The amount of stationary IMU data that must be collected before calculating an alignment, in seconds. Once this threshold has been reached an alignment and covariance are calculated and status will change to AlignmentStatus::ALIGNED_GOOD.
vel_cov – Initial covariance to attach to the stationary NED velocity.
Get the covariance associated with the computed alignment.
- Parameters
format – Format for the covariance block.
- Returns
A pair consisting of a boolean indicating usability of solution, and the solution itself. The covariance is the same as that provided by StaticAlignment::get_computed_covariance, aside from the variance of the down tilt set according to the heading sigma provided by the user, and NE tilt covariances being calculated as a function of the filtering::ImuModel::accel_bias_initial_sigma
values provided by the IMU model. Cross-terms between the accel bias and tilts are also included, as follows.
With tilt errors means defined as
\( \underline{\epsilon} = \begin{bmatrix} \epsilon_x \\ \epsilon_y \\ \epsilon_z\end{bmatrix}\) and the measured, NED frame specific force vector as \( f^n = \begin{bmatrix} f_x \\ f_y \\ f_z \end{bmatrix}\) The relation between the tilts and measured horizontal specific force components is \( f_e = g\sin{\epsilon_x} \) \( \epsilon_x \approx \frac{1}{g}f_e \) \( -f_n = g\sin{\epsilon_y} \) \( \epsilon_y \approx -\frac{1}{g}f_n \) Defining \( W = \begin{bmatrix} 0 & \frac{1}{g} & 0 \\ -\frac{1}{g} & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} \) we have \( \underline{\epsilon} \approx W\begin{bmatrix} f_n \\ f_e \\f_d \end{bmatrix} \) the covariance of these is \( P_{\epsilon\epsilon} = E[\delta\underline{\epsilon}\delta\underline{\epsilon}^T] \) \( = WP_{f^nf^n}W^T \) \( = WC_s^nP_{f^sf^s}C^s_nW^T \)This, along with the user-provided heading/down tilt uncertainty is used to initialize the tilt covariance.
As for the cross terms, given the provided variance of the accel bias errors (from the sensor model)
\( E[\delta f_e^2] = \sigma^2_e; E[\delta f_n^2] = \sigma^2_n \) then \( E[\delta f_e\delta\epsilon_x] = E[\delta f_e\frac{1}{g}f_e] = \frac{1}{g}\sigma^2_e \) \( E[\delta f_n\delta\epsilon_y] = E[\delta f_e\frac{1}{g}f_e] = -\frac{1}{g}\sigma^2_n \) and \( P_{f^n}\epsilon = E[\delta f^n\delta \epsilon] = \begin{bmatrix}0 & -\frac{1}{g}\sigma^2_n & 0 \\ \frac{1}{g}\sigma^2_e & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} \)
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.
Add a measurement to be used in the alignment.
- Parameters
message – Measurement to use. Currently, only aspn_xtensor::MeasurementPosition and aspn_xtensor::MeasurementImu from the are recognized. All others will be ignored. For imu measurements only ASPN_MEASUREMENT_IMU_IMU_TYPE_INTEGRATED are supported. Measurements provided after status has changed to AlignmentStatus::ALIGNED_GOOD are discarded and a warning logged; the alignment will not incorporate new information.
- 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.