# 3D Angular velocity to 3D velocity to predict next state

I have a sensor that gives R, Theta, Phi (Range, Azimuth and Elevation) As such: http://imgur.com/HpSQc50 I need to predict the next state of the object given the roll, pitch yaw angular velocities given the above information. But the math is really confusing me. So far all I've gotten is this:

Xvel = (R * AngularYVel * cos(Theta))
YVel = (R * AngularXVel * cos(Phi))
ZVel = (R * AngularYVel * -sin(Theta)) + (R * AngularXVel * -sin(Phi))


i worked this out by trigonometry, so far this seems to predict the pitching about the x axis and yawing about my y axis (sorry i have to use camera axis) But i dont know how to involve the roll (AngularZVel)

• "Given the roll, pitch, yaw angular velocities given the above information" - but you haven't told us any roll, pitch, or yaw information. Can you provide a more detailed graphic? What states are you trying to predict? Does the sensor only give you $R$, $\theta$, and $\phi$, or do you actually get roll, pitch, and yaw as well? Do you care about predicting future rotational pose or only translation? – Chuck Oct 10 '15 at 18:42
• only translation. i mean..i dont need to maintain the pose. – raaj Oct 11 '15 at 3:19

Assuming that you mean to say your frame of reference is rotating with those roll-pitch-yaw rates and you are tracking a stationary object with position defined by $R$, $\theta$, and $\phi$ in the rotating frame, then you need to do this:

1. Compute the new roll, pitch, and yaw angles given the previous values and the measured rates.

2. Compute the rotation matrix, $A$, for the new reference frame.

3. Compute the previous position vector, $p$, for the object, where $p = (R \cos \theta \cos \psi, R \sin \theta \cos \psi, R \sin \psi)$

4. Compute the new predicted position vector by rotating the previous one into the new frame (i.e., $p_{new} = A^{-1} p$)

5. Compute the new predicted $R$, $\theta$, and $\psi$ from the elements of the new predicted position vector ($R_{new} = \sqrt{p_{new,1}^2 + p_{new,2}^2 + p_{new,3}^2}$, etc.)

Note that I am not paying attention to your axes and sign conventions, so you will have to take that into account for your own solution.