# Determine a point, given another point and roll/pitch/yaw

Let's assume we have a plane (or a ship, does not matter). We have a GPS-receiver installed on our plane, e.g on a wing. So, how do we find position of a point (with known shifts from our receiver), if we know exact position (x, y, z) and roll/pitch/yaw at some time?

It's obvious, that rotation matrices are not applicable. I tried to solve this problem rotating the whole body around an axis, but it turned out, that this way I can take in account only two angles. However, I can be wrong.

Scilab solution:

pi = 3.15159265358979323;
trk = 45;
ptc = 0;
rll = 0;
track = (-trk)*pi/180; // minus because track is measured clockwize
pitch = (ptc)*pi/180; //
roll  = (rll)*pi/180;

v0 = [1;1;1];

// 1
rot_ax = [0;0;1];
fi = track;
cofi = cos(fi);
sifi = sin(fi);
x = rot_ax(1);
y = rot_ax(2);
z = rot_ax(3);
M1 = [
cofi + (1 - cofi)*x*x   , (1 - cofi)*x*y - sifi*z , (1 - cofi)*x*z + sifi*y     ;
(1 - cofi)*y*x + sifi*z , cofi + (1 - cofi)*y*y   , (1 - cofi)*y*z - sifi*x     ;
(1 - cofi)*z*x - sifi*y , (1 - cofi)*z*y + sifi*x , cofi + (1 - cofi)*z*z
];
v1 = M1*v0;

// 2
rot_ax = [v1(2),-v1(1),0];
fi = pitch;
cofi = cos(fi);
sifi = sin(fi);
x = rot_ax(1);
y = rot_ax(2);
z = rot_ax(3);
M2 = [
cofi + (1 - cofi)*x*x   , (1 - cofi)*x*y - sifi*z , (1 - cofi)*x*z + sifi*y     ;
(1 - cofi)*y*x + sifi*z , cofi + (1 - cofi)*y*y   , (1 - cofi)*y*z - sifi*x     ;
(1 - cofi)*z*x - sifi*y , (1 - cofi)*z*y + sifi*x , cofi + (1 - cofi)*z*z
];
v2 = M2*v1;

// 3
rot_ax = [v2(1),v2(2),v2(3)];
fi = roll;
cofi = cos(fi);
sifi = sin(fi);
x = rot_ax(1);
y = rot_ax(2);
z = rot_ax(3);
M3 = [
cofi + (1 - cofi)*x*x   , (1 - cofi)*x*y - sifi*z , (1 - cofi)*x*z + sifi*y     ;
(1 - cofi)*y*x + sifi*z , cofi + (1 - cofi)*y*y   , (1 - cofi)*y*z - sifi*x     ;
(1 - cofi)*z*x - sifi*y , (1 - cofi)*z*y + sifi*x , cofi + (1 - cofi)*z*z
];
v3 = M3*v2;
• can you take the exact $(x,y,z)$ coordinate of the receiver and add the $(x,y,z)$ translation to the point of interest? You are looking at translation matrices then – gt6989b May 31 '16 at 21:19
• Only when you are heading nort. When you are heading west, you shoud add the x translation to your y coordinate. – Alexander Dmitriev Jun 1 '16 at 4:36
• GPS gives you a position, not a rotational orientation (attitude). If you are talking about the attitude of the aircraft somehow affecting the GPS reading, just rotate the GPS reading into the comoving frame and apply the shift to get the GPS position of the point you want. I did not think that attitude affected a GPS reading however... – Mortified Through Math Jun 1 '16 at 4:56
• Actually, I have other sensors, which give orientation data. So I really know 3 coordinates and 3 angles. – Alexander Dmitriev Jun 1 '16 at 6:19

We have 3 deltas $[\Delta x,\Delta y,\Delta z]$ and three angles $\psi,\sigma,\theta$ for track, roll and pitch respectively. The idea is to carefully rotate 3 times.
1. For the first rotation we set $u_1=[0,0,1]$ and $\psi$ and then calculate the matrix $R_1$. We can directly apply it to our vector of shifts: $v_1 = R_1*u$.
2. For the second rotation we set $u_2=[v_{1y},-v_{1x},0]$ and $\theta$, calculate the rotation matrix $R_2$ and apply it: $v_2 = R_2*u_2$. The $u_2$ is orthogonal to $v_1$.
3. For the third rotation we set $u_3 = v_2$ and $\sigma$: $v_3 = R_3*u_3$.