function [xd,yd,zd]=rpy_correction(x,y,z,ya,p,r)
% correct position coordinates in ENU coordinate system due to
% roll/pitch/yaw. 
%    Definitions: (rotation direction by staring in direction of axis)
%       roll is positive for CW rotation about N axis
%       pitch is positive for CW rotation about E axis
%       yaw is positive for CW rotation about U axis

[na,nr]=size(x);
for ja=1:na
    yaw=ya(ja,1);
    pitch=p(ja,1);
    roll=r(ja,1);
    yawrot=[cosd(yaw) sind(yaw) 0
           -sind(yaw) cosd(yaw) 0
               0         0      1];
    pitchrot=[1    0           0
              0 cosd(pitch) sind(pitch)
              0 -sind(pitch) cosd(pitch)];
    rollrot=[cosd(roll) 0 -sind(roll)
                0       1    0       
            +sind(roll) 0 cosd(roll)];
    for jr=1:nr
        rotm=yawrot*pitchrot*rollrot;
        X=[x(ja,jr)
            y(ja,jr)
            z(ja,jr)];
        Y=rotm'*X;
        xd(ja,jr)=Y(1);
        yd(ja,jr)=Y(2);
        zd(ja,jr)=Y(3);
    end
end
return

               