bluerov_nmpc_6dof.m

clc;
clear all;
close all;

Ts = 0.1;
EXPORT = true;
% return the time derivative of the state vector
% input the state of the vehicle and all external force

DifferentialState velocity(6) position(6);
Control tau(6);


u = position1; v = position2; w = position3;
p = position4; q = position5; r = position6;
phi = velocity4; theta = velocity5; psi = velocity6;

% Rigid body mass parameters
m = 11.5;
xG = 0;    yG = 0;    zG = 0.02;
Ix = 0.16; Ixy = 0;   Ixz = 0;
Iyx = 0;   Iy = 0.16; Iyz = 0;
Ixz = 0;   Izy = 0;   Iz = 0.16;

% Added mass parameters
Xudot = 5.5;  % Abir 2.6
Yvdot = 12.7; %      18.5
Zwdot = 14.57;
Kpdot = 0.12;
Mqdot = 0.12;
Nrdot = 0.12;

% Linear and quadratic damping parameters
Xu = 4.03; Xuu = 18.18;  % Abir Xu = 0;    Xuu = 34.96;
Yv = 6.22; Yvv = 21.66;  %      Yv = 0.26; Yvv = 103.25;
Zw = 5.18; Zww = 36.99;
Kp = 0.07; Kpp = 1.55;
Mq = 0.07; Mqq = 1.55;
Nr = 0.07; Nrr = 1.55;   %      Nr = 4.64; Nrr = 0.43;

% Restoring force parameters

W = 112.8;
B = 114.8;


Mrb = [m      0       0        0        m * zG  -m * yG;
       0      m       0       -m * zG   0        m * xG;
       0      0       m        m * yG  -m * xG   0;
       0     -m * zG  m * yG   Ix      -Ixy     -Ixz;
       m * zG 0      -m * xG  -Iyx      Iy      -Iyz;
      -m * yG m * xG  0       -Ixz     -Izy      Iz; ];

Ma = diag([Xudot, Yvdot, Zwdot, Kpdot, Mqdot, Nrdot]);


M = Mrb + Ma;
inM = inv(M);

Crb = [ 0      0      0      0       m * w   -m * v;
        0      0      0     -m * w   0        m * u;
        0      0      0      m * v  -m * u    0;
        0      m * w -m * v  0       Iz * r  -Iy * q;
       -m * v  0      m * u -Iz * r  0        Ix * p;
        m * v -m * u  0      Iy * q  -Ix*p    0];


Ca  = [0         0         0         0          Zwdot * w      0;
       0         0         0        -Zwdot * w  0             -Xudot * u;
       0         0         0        -Yvdot * v  Xudot * u      0; 
       0        -Zwdot * w Yvdot*v   0         -Nrdot * r      Mqdot * q;
       Zwdot*w   0        -Xudot*u   Nrdot * r  0             -Kpdot * p;
       -Yvdot*v  Xudot*u   0        -Mqdot * q  Kpdot * p      0; ];

Dnu = diag([Xu * u Yv * v Zw * w Kp * p Mq * q Nr *r]);
Dnl = diag([Xuu, Yvv, Zww, Kpp, Mqq, Nrr]);


geta = [(W-B) * sin(theta);
       -(W-B) * cos(theta) * sin(phi);
       -(W-B) * cos(theta) * sin(phi);
       -zG * W * cos(theta) * sin(phi);
       -zG * W * sin(theta);
        0];

nu = [u v w p q r]';

nudot = inM * (tau - (Crb+Ca) * nu - (Dnu+Dnl) * nu - geta);

f = dot([velocity; position]) == ...
    [inM * (tau - (Crb+Ca) * nu - (Dnu+Dnl) * nu - geta);...
    velocity;...
    ];

h = [position;...
    velocity;...
    tau];

hN = [position;...
    velocity];

acadoSet('problemname', 'bluerov_nmpc');

N = 20;
ocp = acado.OCP( 0.0, N*Ts, N );

W_mat = eye(length(h));
WN_mat = eye(length(hN));
W = acado.BMatrix(W_mat);
WN = acado.BMatrix(WN_mat);

ocp.minimizeLSQ( W, h );
ocp.minimizeLSQEndTerm( WN, hN );
ocp.subjectTo(-3 <= [tau1; tau2] <= 3);
ocp.setModel(f);


mpc = acado.OCPexport( ocp );
mpc.set( 'HESSIAN_APPROXIMATION',       'GAUSS_NEWTON'      );
mpc.set( 'DISCRETIZATION_TYPE',         'MULTIPLE_SHOOTING' );
mpc.set( 'SPARSE_QP_SOLUTION',        'FULL_CONDENSING_N2'  ); %FULL_CONDENsinG_N2
mpc.set( 'INTEGRATOR_TYPE',             'INT_IRK_GL2'       );
mpc.set( 'NUM_INTEGRATOR_STEPS',         N                  );
mpc.set( 'QP_SOLVER',                   'QP_QPOASES'    	);
mpc.set( 'HOTSTART_QP',                 'NO'             	);
mpc.set( 'LEVENBERG_MARQUARDT',          1e-10				);
mpc.set( 'LINEAR_ALGEBRA_SOLVER',        'GAUSS_LU'         );
mpc.set( 'IMPLICIT_INTEGRATOR_NUM_ITS',  2                  );
mpc.set( 'CG_USE_OPENMP',                'YES'              );
mpc.set( 'CG_HARDCODE_CONSTRAINT_VALUES', 'NO'              );
mpc.set( 'CG_USE_VARIABLE_WEIGHTING_MATRIX', 'NO'           );


if EXPORT
    cd('./acado_model')
    mpc.exportCode('.');
end