Performing the proper coordinate system transformation

Question

I could use some guidance regarding a coordinate system transform problem. My situation is this: my system begins at some unknown location, which I initialize the location (x y) and orientation (roll, pitch, and yaw) all to zero. I establish a frame of reference at this point, which I call the "local" coordinate frame. It is fixed in the world and does not move. At system startup, the body frame is perfectly aligned with the local frame, where body +x points forward, +y to the right and +z down. The body frame is fixed to my system, and travels with the system as it moves.

I have an estimation routine that provides me with the x and y position, as well as the roll, pitch, and yaw of the system. Yaw is rotation about the z axis of the local frame. Pitch and roll are with respect to the body frame (I.e.,if the robot pitches up, I always get a positive value. If it rolls right, I get a positive value.)

How can I take the known roll and pitch values and transform them to be with respect to the local (fixed) frame?

Answer 1

I introduced a different frame of reference, which I called $\mathbf{R}_p$, or position of the robot on the ground plane, instead of the "body" frame that I asked about.

$\mathbf{R}_p$ moves with the robot, but only in yaw and X and Y position. The pitch and roll (provided by the estimator) are relative to this frame, so the roll ($\phi$) and pitch ($\theta$) are relative to the frame $\mathbf{R}_p$. To transform a point from $\mathbf{R}_p$, to the local frame $\mathbf{L}$, we just rotate $\psi$, (yaw) about the Z axis of $\mathbf{R}_p$ of e.g.:

$$ \mathbf{T}_R^L = \begin{vmatrix} cos(\psi) & -sin(\psi) & 0 & 0 \\ sin(\psi) & cos(\psi) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{vmatrix} $$

$$ P_L = \mathbf{T}_R^L * \begin{vmatrix} \phi \\ \theta \\ 0 \\ 0 \end{vmatrix} $$

This gives me the attitude with respect to the local frame that I established when I initialized my system.