[Math] Determine a point, given another point and roll/pitch/yaw

geometryrotations

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;

Best Answer

So, it looks like I've finally found the solution and it's based on rotation matrix from axis and angle (wikipedia).

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

I've included Scilab code in the question, though it's not very neat and commented.

Related Question