problem with simulated sensor in Matlab?

Question

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$)

Answer 1

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);    

Answer 2

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