clearvars;close all;clc;

rng(1)

N = 1200; % Number of time step
M = 200; % Number of samples

t = 1:N;
u = bsxfun(@plus,2*randn(M,N),[20+5.*randn(1,M)]'); % target along-wind velocity
v = 0.75.*2*randn(M,N); % target across-wind velocity
w = 0.5.*2*randn(M,N); % target vertical-wind velocity

yaw  = 20; % yaw angle (horizontal plane, between u and v)
elev = 5; % elevation angle (vertical plane, between w and u)
incl = 5; % inclination angle (vertical plane, between w and v)



RotZ = [cosd(yaw),-sind(yaw),0;sind(yaw),cosd(yaw),0;0,0,1]; % matrixe rotation around axis z
RotX = [1,0,0; 0,cosd(incl),-sind(incl);0,sind(incl),cosd(incl)]; % matrixe rotation around axis x
RotY = [cosd(elev),0,sind(elev);0,1,0;-sind(elev),0,cosd(elev)]; % matrixe rotation around axis zy

A = RotZ*RotY*RotX;  % 3D rotation matrix

% Construction of tilted velocity component
u_tilted = zeros(size(u));
v_tilted = zeros(size(u));
w_tilted = zeros(size(u));
for ii=1:M,
    dummy = A\[u(ii,:);v(ii,:);w(ii,:)];
    u_tilted(ii,:) = dummy(1,:);
    v_tilted(ii,:) = dummy(2,:);
    w_tilted(ii,:) = dummy(3,:);
end

tic
[u3,v3,w3,b] = tiltCorrection(u_tilted,v_tilted,w_tilted,'method','fsolve');
toc

0.0019545    0.092308   -0.074834
Elapsed time is 0.121705 seconds.

clf;close all;
figure
plot3(nanmean(u_tilted,2),nanmean(v_tilted,2),nanmean(w_tilted,2),'ko','markerfacecolor','b')
hold on;box on;

[X,Y] = meshgrid(nanmean(u_tilted,2),nanmean(v_tilted,2));

Z= b(1)+ b(2).*X + b(3).*Y;
surf(X,Y,Z,'facecolor','g','facealpha',0.5)
shading flat
colormap([0,1,0])
legend('measured','planar fit')
grid on
xlabel('u (m/s)');
ylabel('v (m/s)');
zlabel('w (m/s)');
xlim([0,30])
set(gcf,'color','w')
view([-24,24])

u_star0 = zeros(1,M);
u_star3 = zeros(1,M);
for ii = 1:M,
    [u_star0(ii)] = frictionVelocity(u(ii,:),v(ii,:),w(ii,:));
    [u_star3(ii)] = frictionVelocity(u3(ii,:),v3(ii,:),w3(ii,:));
end


figure
plot(1:M,u_star0,'r*',1:M,u_star3,'ko');
ylabel('friction velocity (m/s)')
xlabel(' sample number')
legend('Target','Corrected','location','best')
set(gcf,'color','w')
ylim([0,1])

clf;close all;
figure
plot(abs(mean(w3,2)),'k');
hold on;
plot(abs(mean(w,2)),'r--')
plot(abs(mean(w_tilted,2)),'b-.')
legend('Corrected','Target','original','location','best')
ylim([-5,5])
xlabel('sample number')
ylabel('$\overline{w}$ (m/s)','interpreter','latex')
set(gcf,'color','w')

figure
plot(abs(std(w3,0,2)),'k');
hold on;
plot(abs(std(w,0,2)),'r--')
plot(abs(std(w_tilted,0,2)),'b-.')
legend('Corrected','Target','original','location','best')
ylim([0,2])
xlabel('sample number')
ylabel('\sigma_w (m/s)')
set(gcf,'color','w')

clf;close all;
figure
plot(abs(mean(v3,2)),'k');
hold on;
plot(abs(mean(v,2)),'r--')
plot(abs(mean(v_tilted,2)),'b-.')
legend('Corrected','Target','original','location','best')
ylim([-1,10])
ylabel('$\overline{v}$ (m/s)','interpreter','latex')
set(gcf,'color','w')

figure
plot(std(v3,0,2),'k');
hold on;
plot(std(v,0,2),'r--')
plot(std(v_tilted,0,2),'b-.')
legend('Corrected','Target','original','location','best')
ylim([0,2])
ylabel('\sigma_v(m/s)')
set(gcf,'color','w')

clf;close all;
figure

plot(abs(mean(u3,2)),'k');
hold on;
plot(abs(mean(u,2)),'r--')
plot(abs(mean(u_tilted,2)),'b-.')
legend('Corrected','Target','original','location','best')
xlabel('sample number')
ylabel('$\overline{u}$ (m/s)','interpreter','latex')
set(gcf,'color','w')

figure
plot((std(u3,0,2)),'k');
hold on;
plot((std(u,0,2)),'r--')
plot(std(v_tilted,0,2),'b-.')
legend('Corrected','Target','original','location','best')
xlabel('sample number')
ylabel('\sigma_u (m/s)')
set(gcf,'color','w')
ylim([0,3])