% initial conditions
ang = psiIn(1);
eta0 = [ northIn(1) eastIn(1) psiIn(1) ]';
nu0 = [ dNorthIn(1) dEastIn(1) r(1) ]';
nu_dot0 = [ddNorthIn(1) ddEastIn(1) ra(1)]';
u0 = M*nu_dot0+Damp*nu0;
tau0 = -M*nu_dot0 + Damp*nu0;
dtau0 = A_thr*(tau0-u0);
x0 = [ eta0' nu0' tau0']';
%x0 = [ northIn(1) eastIn(1) ang 0 0 0]';
rot = [ cos(ang) -sin(ang) 0; 
        sin(ang) cos(ang) 0; 
        0 0 1];
m = zeros(3, length(psiIn));
for i = 1:length(psiIn)
    rot = [ cos(psiIn(i)) -sin(psiIn(i)) 0; 
            sin(psiIn(i)) cos(psiIn(i)) 0; 
            0 0 1];
    m(:,i) = rot*[northIn(i); eastIn(i); psiIn(i)];
end
% rotate to vessel parallel coordinates
x0(1:3) = rot'*x0(1:3);

% initial additional dynamics (controller must be designed first)
xa0 = L2'*inv(L2*L2')*(L1*x0+u0);
xo0 = x0;
inverse_IC