Function navtk::navutils::sensor_to_platform

Function Documentation

std::pair<Vector3, Matrix3> navtk::navutils::sensor_to_platform(const std::pair<Vector3, Matrix3> &sensor_pose, const Vector3 &platform_to_sensor_in_platform, const Matrix3 &C_platform_to_sensor, const Matrix3 &C_k_to_j = eye(3))

Calculate the pose (position and orientation) of the platform frame given the pose of a sensor frame and the relationship (pose/lever arm and relative orientation) between the platform frame and the sensor frame.

The pose of the sensor frame can be expressed relative to two different coordinate frames,

\( \mathbf{C}^\text{P}_\text{K} = \mathbf{C}^\text{P}_\text{S} \mathbf{C}^\text{S}_\text{K} \)

\( \mathbf{p}^\text{J}_{\text{J}\to \text{P}} = \mathbf{p}^\text{J}_{\text{J}\to \text{S}} - \mathbf{C}^\text{J}_\text{K} \mathbf{C}^\text{K}_\text{P} \mathbf{p}^\text{P}_{\text{P}\to \text{S}} \)

Parameters

  • sensor_pose – A std::pair consisting of the position of the sensor frame in some 3D Cartesian frame \(J\)(such as the ECEF frame) \( \mathbf{p}^\text{J}_{\text{J}\to \text{S}}\), and the DCM that rotates from some (potentially) other Cartesian frame \(K\)(such as the NED frame) to the sensor frame \(\mathbf{C}^\text{S}_\text{K} \). Frames \(J\)and \(K\)can be the same frame.

  • platform_to_sensor_in_platform – A ‘lever arm’ pointing from the origin of the platform frame to the origin of the sensor frame, coordinatized in the platform frame, \(\mathbf{p}^\text{P}_{\text{P}\to \text{S}}\). Units should be the same as those used with sensor_pose .

  • C_platform_to_sensor – The DCM that rotates from the platform frame to the sensor frame, \(\mathbf{C}^\text{S}_\text{P}\).

  • C_k_to_j – The DCM that rotates from the \(K\)frame to the \(J\)frame, \(\mathbf{C}^\text{J}_\text{K}\). Defaults to identity matrix.

Returns

A std::pair consisting of \(\mathbf{p}^\text{J}_{\text{J}\to \text{P}}\) and \(\mathbf{C}^\text{P}_\text{K}\).