State conversion from cartesian to spherical

41 Views Asked by At

I am looking for a general transformation matrix to convert a state vector from spherical coordinates to cartesian.

$$\begin{bmatrix}r \\ \theta \\ \phi \\ \dot{r} \\ \dot{\theta} \\ \dot{\phi} \end{bmatrix} = A \cdot \begin{bmatrix} x \\ y \\ z \\ \dot{x} \\ \dot{y} \\ \dot{z} \end{bmatrix}$$

This link shows the transformation from $ (x, y, z)\rightarrow (r,\theta,\phi)$

and this link (equation 97) shows the transformation from $ (\dot{x}, \dot{y}, \dot{z})\rightarrow (\dot{r}, \dot{\theta}, \dot{\phi})$

lets say $$\begin{bmatrix}r \\ \theta \\ \phi \end{bmatrix} = B \cdot \begin{bmatrix} x \\ y \\ z \end{bmatrix}$$ and $$\begin{bmatrix}\dot{r} \\ \dot{\theta} \\ \dot{\phi} \end{bmatrix} = C \cdot \begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{z} \end{bmatrix}$$

could I also say this?

$$\begin{bmatrix}r \\ \theta \\ \phi \\ \dot{r} \\ \dot{\theta} \\ \dot{\phi} \end{bmatrix} = \begin{Bmatrix} B & 0 \\ 0 & C\end{Bmatrix} \cdot \begin{bmatrix} x \\ y \\ z \\ \dot{x} \\ \dot{y} \\ \dot{z} \end{bmatrix}$$

I'm currently trying to create a transformation matrix for a Kalman filter that takes spherical sensor data to best estimate the position and velocity of an object and I'm asking this to create an H matrix. Currently using this source as a guide.

Thanks in advance for any help!