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

% Note that the transformation matrix must be recalculated every time
% the orientation, heading, pitch or roll changes.

% Transformation matrix for beam to xyz coordinates,
% the values can be found from the header file that is generated in the
% conversion of data to ASCII format
% This example shows the transformation matrix for a standard Aquadopp head
T =[ 2896  2896    0 ;...
    -2896  2896    0 ;...
    -2896 -2896 5792  ];

T = T/4096;
% Scale the transformation matrix correctly to floating point numbers
%!Only necessary if matrix contains high valus as shown in example!

T_org = T;

% If instrument is pointing down (bit 0 in status equal to 1)
% rows 2 and 3 must change sign
% NOTE: For the Vector the instrument is defined to be in
%       the UP orientation when the communication cable
%       end of the canister is on the top of the canister, ie when
%       the probe is pointing down.
%       For the other instruments that are used vertically, the UP
%       orientation is defined to be when the head is on the upper
%       side of the canister.


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

% 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 --------
% -----------------------------------------------

% beam is beam coordinates, for example beam = [0.23 ; -0.52 ; 0.12]

% 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