% Transform.m is a Matlab script that shows how velocity data can be
% transformed between BEAM, XY and EN coordinates. Beam
% coordinates are defined as the velocity measured along the two
% beams of the instrument.
% EN coordinates are defined in an earth coordinate system, where
% E represents the East-West component and N represents the North-South
% component.

% Transformation matrix T that is unique for the instrument
% found in the '_Beam2xyz' variable (in "String Data.csv" / .mat file)
T =[ 0.5518  0.5518    ;...
    1.1831 -1.1831 ];


T_org = T;

% If instrument is pointing down (can be determined from status)
% rows 2 must change sign!
status=0

if status == 1,
   T(2,:) = -T(2,:);
end

%Input of example datatpoint:
heading = 320.5;      % instrument heading (deg, 0=N, 90=E from compass)
pitch = 42.13;
roll = 0;

beam = [0.217 ; -0.521] %velocity in beam coordinates


% heading, pitch and roll are the angles output in the data in degrees
% Transform attitude data to radians

% Subtract 90 from the heading so that instrument x becomes more compareable to
% the earth x direction (i.e. East).

hdg = pi*(heading-90)/180;
pch = pi*pitch/180;
rll = pi*roll/180;

% Make heading matrix
H = [cos(hdg) sin(hdg) 0; -sin(hdg) cos(hdg) 0; 0 0 1]

% Make tilt matrix (Pitch and Roll combined)
P = [cos(pch) -sin(pch)*sin(rll) -cos(rll)*sin(pch);...
      0             cos(rll)          -sin(rll);  ...
      sin(pch) sin(rll)*cos(pch)  cos(pp)*cos(rll)]

% Make resulting transformation matrix
R = H*P;

%combine with T
F=R*T

% -----------------------------------------------
% -------- APPLY TRANSFORMATION MATRICES --------
% -----------------------------------------------


% Given BEAM velocities, ENU coordinates are calculated as
enu = F*beam
% Given ENU velocities, BEAM coordinates are calculated as
beam = inv(F)*enu


% Transformation between BEAM and XYZ coordinates are done using
% the original T matrix since the instruments orientation is irrelevant

% Given BEAM velocities, XYZ coordinates are calculated as
xyz = T_org*beam
% Given XYZ velocities, BEAM coordinates are calculated as
beam = inv(T_org)*xyz


% Given XYZ velocities, ENU coordinates are calculated as
enu = F* inv(T_org)*xyz
% Given ENU velocities, XYZ coordinates are calculated as
xyz = T_org*inv(F)*enu