Transforming a Mobile Coordinate System to a Stationary Coordinate System

64 Views Asked by At

Image

The above image describes the problem I am facing with the 'prime' system being the mobile one. I am only working in a 2D coordinate system and I am looking to get the angle between the x-axes of each coordinate system. The point is stationary at $(1,0)$ in the stationary coordinate frame, which gives the vector $r$, and the vectors $r'$ and $r_0$ are also known.

I have currently been using a rotation matrix in the form of $v' = Rv + t$ where $v'$ is the $r$ vector, $v$ is the $r'$ vector, and $t$ is the $r_0$ vector in the image. For the following equations, I am usually an $s$ subscript for the stationary system and an $m$ subscript for the mobile one.

$\left\lgroup\matrix{1 \cr 0}\right\rgroup = \left\lgroup\matrix{\cos(\theta) & -\sin(\theta)\cr \sin(\theta) & \cos(\theta)}\right\rgroup \left\lgroup\matrix{x_m \cr y_m}\right\rgroup + \left\lgroup\matrix{x_s \cr y_s}\right\rgroup$

In testing, this has seemed to be an issue when the mobile coordinate system is close (within 2-5 meters) to the origin of the stationary coordinate system. I am calculating the angle from

$\theta = \cos^{-1}(\frac{x_m - y_s*y_m - x_s*x_m}{x_m^2 + y_m^2})$.

This seems to be accurate for the angle magnitude, but in some cases ends up off from the correct angle but with some increment of 90 degrees. I am not certain that this is the correct angle calculation and I would encourage feedback on if this is the best way to approach getting the rotation angle between two coordinate systems.