/
Rotate.asv
38 lines (31 loc) · 980 Bytes
/
Rotate.asv
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
phi = -0.076613;
theta = 0.048582;
psi = 0.784805;
sqrt(54^2 + (-72)^2 + 954^2)
x = 54;
y = -72;
z = 954;
phi = atan(y / z ) ;
theta = atan( -x / z) ;
psi = atan(y / x ) ;
x = -990;
y = -90;
z = -90;
% North-East-Down reference frame (see Figure)
% Roll(phi) Pitch(theta) Yaw(psi), angles in radians
% derotation matrix coefficients (rotation matrix is transposed)
m11= cos(theta)*cos(psi);
m21= cos(theta)*sin(psi);
m31=-sin(theta);
m12=sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi);
m22=sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi);
m32=sin(phi)*cos(theta);
m13=cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi);
m23=cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi);
m33=cos(phi)*cos(theta);
% derotate vector (product matrix by vector)
xr = m11*x + m12*y + m13*z;
yr = m21*x + m22*y + m23*z;
zr = m31*x + m32*y + m33*z;
phi2 = atan(yr / zr) * 57,3;
theta2 = atan( -xr / sqrt(yr^2 + zr^2)) *57,3 ;