Function navtk::navutils::correct_dcm_with_tilt

Defined in File navigation.hpp

Function Documentation

Matrix3 navtk::navutils::correct_dcm_with_tilt(const Matrix3 &dcm, const Vector3 &tilt)

Corrects a DCM by applying tilt error corrections (usually generated via the Pinson Error model).

Consider a dcm that rotates a vector from frame B to frame A ( \(\textbf{C}_\text{B}^\text{A}\)). Next, consider tilt to be a rotation vector. When coordinate frame A is rotated by the rotation vector defined by tilt it becomes coordinate frame \(\text{A}'\).

The DCM that is returned by this function would then be

\(\textbf{C}_\text{B}^{\text{A}'} = \textbf{C}_\text{A}^{\text{A}'} \textbf{C}_\text{B}^\text{A}\).

For example, consider the case where the attitude is expressed as a dcm that rotates from a platform frame \(\text{P}\) to the \(\text{NED}\) frame \(\textbf{C}_\text{P}^\text{NED}\). However, this attitude is not quite correct, and when applied doesn’t rotate a vector from the platform frame to the true \(\text{NED}\) frame ( \(\text{NED}_{true}\)). The tilt corrections represent the relationship between the assumed \(\text{NED}\) frame ( \(\text{NED}\)) and the true \(\text{NED}\) frame ( \(\text{NED}_{true}\)). If the \(\text{NED}\) coordinate frame is rotated by the rotation vector described by tilt, the result is the \(\text{NED}_{true}\) coordinate frame. In this example, the function will return the DCM corresponding to \(\textbf{C}_\text{P}^{\text{NED}_{true}}\)

Note that this function is equivalent to (in pseudocode):

angle = magnitude(tilt)

axis = tilt / angle (axis vector is normalized rotation vector)

corrected_dcm = transpose(axis_angle_to_dcm(axis, angle)) * dcm

WARNING: The definition of the tilt rotation vector given here is opposite of that used in other functions (correct_quat_with_tilt(), for example), where the tilt rotation does a frame rotation from \(\text{A}'\) to \(\text{A}\). In other words, a sign change on the tilts is required between these functions. However, the definition of tilts here is consistent with those typically estimated by our navtk::filtering::Pinson15NedBlock model using the additive error state formulation. In other words, when one has an estimated \(\textbf{C}^{\text{NED}}_\text{P}\) matrix and is using the Pinson model to estimate error states, the following applications of tilts are all equivalent and will produce \(\textbf{C}^{\text{NED}_{true}}_\text{P}\):

correct_dcm_with_tilt( \(\textbf{C}^{\text{NED}}_\text{P}\), tilt)

quat_to_dcm(correct_quat_with_tilt(dcm_to_quat( \(\textbf{C}^{\text{NED}}_\text{P}\)), -tilt))

dot(rot_vec_to_dcm(-tilt), \(\textbf{C}^{\text{NED}}_\text{P}\))

dot(transpose(rot_vec_to_dcm(tilt)), \(\textbf{C}^{\text{NED}}_\text{P}\))