% Motion correction used with NAV420 in Ikawa&Oechel(2015JGR)
% Hiroki Ikawa
% The coding was originally developed for C-sat (Campbell Sci.) and NAV420 (Xbow).
% If a diffrent anemometer is to be used, please check the coordinate carefully.
% Options for Gill and Kaijo sonics are prepared but they are not tested.
% If you have any question & suggestion, please contact: Hiroki Ikawa
% (hikawa.biomet@gmail.com)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% INPUT with examples
%ux (north to south), uy (west to east), uz (down to top) (ms-1) without motion.
%roll: (deg) portside (left) rolling up
%pitch: (deg) bow (front) pitching up % Note that Edson98 used pitch-down
%yaw: (deg) clockwise from north
%rollang, pitchang, yawang: (deg/s) angular velocity. They tell how fast the object rotating.
%xv (south to north), yv (west to east), zv (top to down): (ms-1): translational velocity on the earth coordinate.
% Followings are vector position from the gyro sensor to the sonic on the platform coordinate
% I measured them with a convex ruler.
% For example
%posx = -1; % (m) front to back (north to south when the platform coordinate matches the earth coordinate)
%posy = -0.12; % (m) left to write (west to east when the platform coordinate matches the earth coordinate)
%posz = 0.01; % (m) down to top
% sonic type 1: Gill wind master (not tested), 2 Campbell Sci C-sat (tested), 3 Kaijo SAT-600 (not tested)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% OUTPUT
% [ux_corr, uy_corr, uz_corr]: corrected xyz wind speed (m s-1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [ux_corr,uy_corr,uz_corr]=...
motion_NAV420(ux,uy,uz,roll,pitch,yaw,rollang,pitchang,yawang,xv,yv,zv,posx,posy,posz,sonictype)
if sonictype==1 % Gill Windmaster untested
ux=-ux; uy=-uy;
elseif sonictype==2 % Campbell C-sat tested
elseif sonictype==3 % Kaijo SAT-600 untested
ux=uy; uy=-ux;
end
% degree to radian
roll = roll/360*2*pi;pitch = pitch/360*2*pi;yaw = yaw/360*2*pi;
rollang = rollang/360*2*pi;pitchang = pitchang/360*2*pi;yawang = yawang/360*2*pi;
% Convert the coordinate from NAV to C-sat
xv = -xv; zv = -zv;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
posx(1:length(ux),1) = posx;
posy(1:length(ux),1) = posy;
posz(1:length(ux),1) = posz;
%% Alternative calculation without taking advantage of Matlab's matrix calculation.
% ux1 = ux; uy1 = uy.*cos(roll) + uz.*sin(roll); uz1 = -uy.*sin(roll) + uz.*cos(roll);
% ux2 = ux1.*cos(pitch) + uz1.*sin(pitch); uy2 = uy1; uz2 = -ux1.*sin(pitch) + uz1.*cos(pitch);
% ux3 = ux2.*cos(yaw) + uy2.*sin(yaw); uy3 = -ux2.*sin(yaw) + uy2.*cos(yaw); uz3 = uz2;
%
% omtposx = pitchang.*posz + posy.*yawang; omtposy = -yawang.*posx + posz.*rollang; omtposz = -rollang.*posy - posx.*pitchang;
%
% tposx1 = omtposx; tposy1 = omtposy.*cos(roll) + omtposz.*sin(roll); tposz1 = -omtposy.*sin(roll) + omtposz.*cos(roll);
% tposx2 = tposx1.*cos(pitch) + tposz1.*sin(pitch); tposy2 = tposy1; tposz2 = -tposx1.*sin(pitch) + tposz1.*cos(pitch);
% tposx3 = tposx2.*cos(yaw) + tposy2.*sin(yaw); tposy3 = -tposx2.*sin(yaw) + tposy2.*cos(yaw); tposz3 = tposz2;
% ux_corr_backup = ux3 + tposx3 + xv; uy_corr_backup = uy3 + tposy3 + yv; uz_corr_backup = uz3 + tposz3 + zv;
%% Transform coordinate from C-sat to Edson98
posx=-posx;
posy=-posy;
ux=-ux;
uy=-uy;
xv=-xv;
yv=-yv;
pitchang=-pitchang;
pitch=-pitch;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Equation (4) in Edson 98
[U_corr]=matrixT(ux,uy,uz,yaw,roll,pitch); % Correct the coordinate
ompos_corr=cross([rollang pitchang -yawang],[posx posy posz]); % Cross vector of the sensor position and motion on the sensor's coordinate.
[tompos_corr]=matrixT(ompos_corr(:,1),ompos_corr(:,2),ompos_corr(:,3),yaw,roll,pitch); % Correct the coordinate
ux_corr = U_corr(:,1) + tompos_corr(:,1) + xv;
uy_corr = U_corr(:,2) + tompos_corr(:,2) + yv;
uz_corr = U_corr(:,3) + tompos_corr(:,3) + zv;
% Change signs back to C-sat
ux_corr = -ux_corr; uy_corr = -uy_corr;
function [vcorr]=matrixT(x,y,z,yaw,roll,pitch)
vcorr(1:length(x),1:3)=0;
for i=1:length(x)
T=[cos(yaw(i))*cos((pitch(i))) sin(yaw(i))*cos(roll(i))+cos(yaw(i))*sin(pitch(i))*sin(roll(i)) -sin(yaw(i))*sin(roll(i))+cos(yaw(i))*sin(pitch(i))*cos(roll(i));...
-sin(yaw(i))*cos(pitch(i)) cos(yaw(i))*cos(roll(i))-sin(yaw(i))*sin(pitch(i))*sin(roll(i)) -sin(pitch(i))*cos(roll(i))*sin(yaw(i))-sin(roll(i))*cos(yaw(i));...
-sin(pitch(i)) cos(pitch(i))*sin(roll(i)) cos(pitch(i))*cos(roll(i)) ];
vcorr(i,:)=T*[x(i);y(i);z(i)];
end
return;