enu2ecef(V_enu, O_ecef)

function [P_ecef] = enu2ecef(V_enu, O_ecef) 
% ENU2ECEF conversion
% [Penu] = ecef2enu(V_enu, O_ecef)
% Convert to Earth Centered Eart Fixed
% from local East, North, Up (ENU) coordinates
%
% Input:
%   V_enu:  [3 N] matrix with column vectors to be converted, N >= 1
%   O_ecef:  [3 1] column vector with ecef coordinates of ref point O
%           (the one used for origin in the ENU reference)
% Output:
%   P_ecef: [3 N] matrix with colum vectors in ECEF coordinates
%

%% Input Checks
if nargin ~= 2
    error('Two arguments required in input')
end
sz=size(O_ecef);
if (sz(1) ~= 3 && sz(2) ~= 1)
    error('O_ecef should be a [3,1] column vector')
end
sz=size(V_enu);
if (sz(1) ~= 3 )
    error('V_ecef should be a [3,N] matrix')
end
N=sz(2);

%% Compute rotation matrix: from ecef 2 enu
B = [ -sin(lm) cos(lm) 0; 
    -sin(ph)*cos(lm) -sin(ph)*sin(lm) cos(ph);...
    cos(ph)*cos(lm) cos(ph)*sin(lm) sin(ph)];
  
%% Conversion to ENU is done first by roation into ECEF then Translation
% Rotation
V_ecef = B'*V_enu;

% Translation
P_ecef = V_ecef + O_ecef*ones(1,N);