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

enter image description here

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:

enter image description here

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

enter image description here

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:

enter image description here

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

enter image description here

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:

enter image description here

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

  • 1
    $\begingroup$ 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...? $\endgroup$ Jul 16, 2019 at 21:55
  • $\begingroup$ Exactly, I'm asking that $\endgroup$
    – Unnamed
    Jul 16, 2019 at 23:00

1 Answer 1


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

  • $\begingroup$ 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. $\endgroup$
    – Unnamed
    Jul 16, 2019 at 23:27
  • $\begingroup$ 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. $\endgroup$
    – SteveO
    Jul 16, 2019 at 23:31
  • $\begingroup$ Thanks! But I think that the signs of the matrix are the opposite, is that possible? $\endgroup$
    – Unnamed
    Jul 17, 2019 at 0:05
  • $\begingroup$ They would be opposite what I wrote if your coordinates aligned when yaw = 0. $\endgroup$
    – SteveO
    Jul 17, 2019 at 0:07
  • $\begingroup$ 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 $\endgroup$
    – Unnamed
    Jul 17, 2019 at 0:17

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.