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