Function navtk::navutils::platform_to_sensor

Function Documentation

std::pair<Vector3, Matrix3> navtk::navutils::platform_to_sensor(const std::pair<Vector3, Matrix3> &platform_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 a sensor frame given the pose of the platform frame and the relationship (pose/lever arm and relative orientation) between the platform frame and the sensor frame.

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

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

Parameters

  • platform_pose – A std::pair consisting of the position of the platform frame in some 3D Cartesian frame \(J\) (such as the ECEF frame) \( \mathbf{p}^\text{J}_{\text{J}\to \text{P}}\), and the DCM that rotates from some (potentially) other Cartesian frame \(K\) (such as the NED frame) to the platform frame \(\mathbf{C}^\text{P}_\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 platform_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{S}}\) and \(\mathbf{C}^\text{S}_\text{K}\).