I have a 3D point in space with it's XYZ Coordinates about some Frame A. I need to calculate the new XYZ coordinates, given the angular velocities of each axis at that instant of time about Frame A

I was referring to my notes, but I'm a little confused. This is what my notes say: https://i.sstatic.net/hoREn.jpg

As you can see, i can calculate the angular velocity vector w given my angular velocities. But I'm not sure how this translates to how to calculate my new XYZ position! How can i calculate the RPY values this equation seems to need from my XYZ, and how can i calculate my new position from there

  • $\begingroup$ it's basically a 3D point cloud that i need to "predict" it's new position based on my IMU velocity data. $\endgroup$
    – raaj
    Commented Nov 15, 2015 at 14:34
  • $\begingroup$ Is the object (3D point) constrained to perform only rotational motion? $\endgroup$
    – 50k4
    Commented Nov 15, 2015 at 20:08

1 Answer 1


Overall, you simply need to rotate each point in your cloud using the rotation matrix $R$. That rotation matrix will be the latest rotation matrix for your vehicle/sensor. You can compute the rotation matrix based on the current values for roll, pitch, and yaw according to the following:

$R = \begin{bmatrix} c_\psi c_\theta & c_\psi s_\theta s_\phi - s_\psi c_\phi & c_\psi s_\theta c_\phi + s_\psi s_\phi \\ s_\psi c_\theta & s_\psi s_\theta s_\phi + c_\psi c_\phi & s_\psi s_\theta c_\phi - c_\psi s_\phi \\ -s_\theta & c_\theta s_\phi & c_\theta c_\phi \end{bmatrix}$

Where $\psi$ is yaw, $\theta$ is pitch, and $\phi$ is roll.

To get the latest roll, pitch, yaw angles you will need to integrate the current roll, pitch, yaw angle rates. Typically, you will be measuring the body angular rates using an IMU, but these rates are (a) not in the world frame and (b) not in roll, pitch, yaw form.

First, you need to transform the body-frame angular rates into world-frame angular rates. To do that, simply pre-multiply the body rates by the current rotation matrix:

$\omega_o = R \omega_b$

Where $\omega_o = \begin{bmatrix} \omega_{o,x} \\ \omega_{o,y} \\ \omega_{o,z}\end{bmatrix}$ are the world-frame angular rates and $\omega_b = \begin{bmatrix} \omega_{b,x} \\ \omega_{b,y} \\ \omega_{b,z}\end{bmatrix}$ are the body-frame angular rates.

Now that you have the angular rates about each axis in the world frame, you can use that equation you linked to in your question to convert to roll, pitch, yaw rates:

$\begin{bmatrix} \omega_{o,x} \\ \omega_{o,y} \\ \omega_{o,z}\end{bmatrix} = \begin{bmatrix} c_\psi c_\theta & -s_\psi & 0 \\ s_\psi c_\theta & c_\psi & 0 \\ -s_\theta & 0 & 1 \end{bmatrix} \begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix} $

$\omega_o = E \omega_{rpy}$

Note that you are starting with $\omega_o$ and finding $\omega_{rpy}$, so that means you need the inverse of $E$, and you'll need to watch for singularities (when $\cos \theta = 0$).

$\omega_{rpy} = E^{-1} \omega_o$

Then integrate the rates to get new roll, pitch, yaw angles, where $\tau$ is your time step:

$\phi_{k+1} = \phi_k + \dot{\phi}_k \tau$

$\theta_{k+1} = \theta_k + \dot{\theta}_k \tau$

$\psi_{k+1} = \psi_k + \dot{\psi}_k \tau$

Those new roll, pitch, yaw values give you a new rotation matrix. Then just rotate the points in your point cloud accordingly. Keep in mind that you want to predict the new point cloud in the body frame as your question states. This rotation matrix takes vectors in the body frame and defines them in the world frame, so you need to do the opposite, which just means using the inverse of the rotation matrix.

So, for a point $p_{o,i}$ in the world frame, its coordinates in the body frame $p_{b,i}$ are defined by:

$p_{b,i} = R^{T} p_{o,i}$

Where the transpose $R^T$ is the same as the inverse $R^{-1}$.

Also note that my equations might look a bit confusing since I am only including the time step index $k$ when showing the forward integration of roll, pitch, yaw angles. You'll be using the rotation matrix from the previous time step to convert the angular rates in the current time step to the world frame, then updating the roll, pitch, yaw rates and angles, and then computing the rotation matrix for the current time step in order to transform your point cloud (unless this is part of a graph-based solution).

  • $\begingroup$ Wait..but my system gives the RPY velocities to begin with (I apologize it was confusing as I said angular velocities). If thats the case..I just integrate then rotate it? $\endgroup$
    – raaj
    Commented Nov 17, 2015 at 3:06
  • $\begingroup$ Yes, in that case you can skip the conversion from $\omega_b$ to $\omega_o$ and then $\omega_{rpy}$. If you are starting with $\omega_{rpy}$ then just integrate to get a new set of roll-pitch-yaw angles, then use those angles to get a new rotation matrix $R$, then use that rotation matrix to rotate all of your points (actually, the inverse of $R$). $\endgroup$ Commented Nov 17, 2015 at 3:08
  • $\begingroup$ I'm showing the most expanded solution but you can just use the parts you need if you are starting with $\omega_b$, $\omega_{rpy}$, or even the latest roll-pitch-yaw estimates themselves. The key thing is $p_{b,i} = R^T p_{o,i}$, and then how you get the latest $R$ depends on what you get for sensor data. $\endgroup$ Commented Nov 17, 2015 at 3:10

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.