# Transform this matrix of distances into a more general one

I need to make the navigation and guidance of a vehicle (a quadcopter) in a platform. This platform can be seen like this:

where the blue dots are the center of each square, and the $$x$$ distances are all the same, and the $$y$$ distances are all the same.

I need the distance between each blue dot to the center (the blue dot of the $$(2;2)$$), but that distance depends on the $$yaw$$ angle. For example, if $$yaw=0^\circ$$, the situation is like this:

and the distances are:

$$d_{1;1} = (-d_x; -d_y)$$ $$d_{1;2} = (-d_x; 0)$$ $$d_{1;3} = (-d_x; d_y)$$

$$d_{2;1} = (0; -d_y)$$ $$d_{2;2} = (0; 0)$$ $$d_{2;3} = (0; d_y)$$

$$d_{3;1} = (d_x; -d_y)$$ $$d_{3;2} = (d_x; 0)$$ $$d_{3;3} = (d_x; d_y)$$

If the situation is with $$yaw=180^\circ$$:

the distances are the same but with the opposite sign, i.e,

$$d_{1;1} = (d_x; d_y)$$ $$d_{1;2} = (d_x; 0)$$ $$d_{1;3} = (d_x; -d_y)$$

$$d_{2;1} = (0; d_y)$$ $$d_{2;2} = (0; 0)$$ $$d_{2;3} = (0; -d_y)$$

$$d_{3;1} = (-d_x; d_y)$$ $$d_{3;2} = (-d_x; 0)$$ $$d_{3;3} = (-d_x; -d_y)$$

If $$yaw=90^\circ$$, the situation is like this:

and the distances (see the difference between $$d_x$$ and $$d_y$$) would be:

$$d_{1;1} = (-d_y; d_x)$$ $$d_{1;2} = (-d_y; 0)$$ $$d_{1;3} = (-d_y; d_x)$$

$$d_{2;1} = (0; -d_x)$$ $$d_{2;2} = (0; 0)$$ $$d_{2;3} = (0; d_x)$$

$$d_{3;1} = (d_y; -d_x)$$ $$d_{3;2} = (d_y; 0)$$ $$d_{3;3} = (d_y; d_x)$$

If $$yaw = -90^\circ$$:

the distances would be:

$$d_{1;1} = (d_y; d_x)$$ $$d_{1;2} = (d_y; 0)$$ $$d_{1;3} = (d_y; -d_x)$$

$$d_{2;1} = (0; d_x)$$ $$d_{2;2} = (0; 0)$$ $$d_{2;3} = (0; -d_x)$$

$$d_{3;1} = (-d_y; d_x)$$ $$d_{3;2} = (-d_y; 0)$$ $$d_{3;3} = (-d_y; -d_x)$$

I need to write a matrix that uses the information of the $$yaw$$ angle and returns the distances from each angle (not just 0, 90, -90 and 180, but also 1, 2, 3, ...)

I tried to write it but I couldn't find the solution.

Thank you very much. I really need this help

Edit: please note that the coordinate frame moves with the quadcopter, like in this image:

Edit 2: for example, if $$yaw=45^\circ$$, then the distance from $$(3;3)$$ to $$(2;2)$$ is $$\sqrt{d_x^2+d_y^2}$$ in $$x$$ and $$0$$ in $$y$$.

• Maybe I'm confused a bit...the distance between all points from the centre {2,2) is always $\sqrt{d^2{}_{x,i}+d^2{}_{y,i}}$ ...and they won't change regardless of what yaw is...what is there to calculate? Or are you asking how to know the velocity vector depending on yaw...as in, which direction should you fly to get to a different coordinate depending on yaw as a function...like $f(\text{yaw})\text{:=}\left( \begin{array}{c} x_{\text{yaw}} \\ y_{\text{yaw}} \\ \end{array} \right)$ broken up into vector components...? – morbo Jul 16 '19 at 21:55
• Exactly, I'm asking that – Unnamed Jul 16 '19 at 23:00

Place a fixed coordinate system at the upper left corner of your grid, with the x axis pointing down and y to the right. Call this the 0 coordinate system.

Imagine the origin of the moving coordinate system to be located at the origin of the 0 coordinate system. Unfortunately you have defined that coordinate system to have the x axis pointing up, and the y axis pointing to the left. We’ll handle that, but the math would be a bit simpler if the coordinate systems were aligned when $$yaw = 0$$. Call the moving coordinate system the 1 system.

Define a rotation matrix $$R_{01}$$, where $$\vec {p_1} = R_{01} \vec {p_0}$$

The above equation, then, can be used to map vectors from the 0 coordinate system (your grid) to vectors in the rotating coordinate system.

Beginning kinematics can be used to find that

$$R_{01} = \begin{bmatrix} - \cos(yaw) & \sin(yaw) \\ - \sin(yaw) & - \cos(yaw) \\ \end{bmatrix}$$

Note that each element’s sign would be the opposite of what it is above had the two coordinate systems aligned when $$yaw = 0$$.

Vector math gives:

$$x_1 = - \cos(yaw) x_0 + \sin(yaw) y_0$$ $$y_1 = - \sin(yaw) x_0 - \cos(yaw) y_0$$

where $$x_0$$ is what you call $$d_x$$, and $$y_0$$ is your $$d_y$$.

• The origin is the centre of the matrix. I didn't define the coordinates, they are like that because of the sensors that I'm using. – Unnamed Jul 16 '19 at 23:27
• The final two equations in my answer will map any delta x and delta y in the grid coordinates to the x and y values for the rotated coordinates. You can select the center of the grid, or really any location, to be the origin. It will still work. Use these equations for any two points and then use the distance formula. – SteveO Jul 16 '19 at 23:31
• Thanks! But I think that the signs of the matrix are the opposite, is that possible? – Unnamed Jul 17 '19 at 0:05
• They would be opposite what I wrote if your coordinates aligned when yaw = 0. – SteveO Jul 17 '19 at 0:07
• I don't understand what you say. In the situation that the coordinates are like in the first image in yaw=0, they're opposite – Unnamed Jul 17 '19 at 0:17