problem with simulated sensor in Matlab?

I'm simulating a sensor in 3D. The sensor should determine ($p, \theta, \phi$) from the origin where $\theta$ is the rotation about z-axis and $\phi$ is the rotation about x-axis. The sensor is given position of a point($x, y, z$). This is what I did

p = sqrt(x^2 + y^2 + z^2);
theta = acos(z/p);   <---- I'm guessing the problem here
phi = atan2(y,x);

Now I need to get the Cartesian coordinates ($x',y',z'$). This is what I did

[p theta phi] = getmeasurement(x, y, z);
x' = p*cos(theta)*sin(phi);
y' = p*sin(theta)*sin(phi);
z' = p*cos(phi);

The sensor is working fine at the beginning but at a particular point it behaves strangely. I have the state vector to compare it with the measurement. I'm guessing that $\theta$ might be the problem.

Edit:

I'm sorry for this mistake. The aforementioned calculations based on the following picture So, the point will rotate first about z-axis ($\theta$) and then rotate about x-axis ($\phi$)

Your first set of formulas are calculating the spherical coordinates for the sensor which are different from the angles of rotation around the x-axis (which you have called $\phi$) and the y-axis (which you have called $\theta$)

When using the formulas for converting cartesian coordinates to spherical coordinates you end up with $\phi$ as the rotation about the x-axis and $\theta$ as the angle between the vector to the x,y,z location and the z-axis (the polar angle)

So the calculation with $\theta$ is correct it just isn't calculating what you thought.

The problem you have isn't with any of that though, it is with the calculation of z', the formula should be

z' = p*cos(theta);
• @Mathew Cordon, thanks for the reply. I've edited my post. $\theta$ is the rotation about z-axis and $\phi$ is the rotation about x-axis. Jan 18 '14 at 23:38
• The formula above should have resolved the issue, I tried a couple of simple test cases and the outputs looked right. I think you are still misinterpreting what $/phi$ is though. It is the angle between the vector to the 'sensor' and the axis, not the rotation around the axis. Jan 19 '14 at 16:24
• I think you're right. I've solved the problem by switching Phi with Theta. Phi = acos(pz/d); Theta = atan2(py,px); Jan 20 '14 at 8:45

I found the problem by switching the following

p = sqrt(x^2 + y^2 + z^2);
Phi = acos(z/p);   <---- Phi not Theta
Theta = atan2(y,x);  <---- Theta not Phi