chenyutao36 / matmpc Goto Github PK

%% Dimensions

nx=2;  % No. of differential states
nu=1;  % No. of controls
nz=0;  % No. of algebraic states
ny=0;  % No. of outputs

nyN=2; % No. of outputs at the terminal point
np=0; % No. of model parameters
nc=0; % No. of general inequality constraints
ncN=0; % No. of general inequality constraints
nbx = 0; % No. of bounds on states
nbu = 1; % No. of bounds on controls

% state and control bounds-+
nbx_idx = [];  % indexs of states which are bounded
nbu_idx = 1;  % indexs of controls which are bounded

%% create variables

import casadi.*

states   = SX.sym('states',nx,1);   % differential states
controls = SX.sym('controls',nu,1); % control input
alg      = SX.sym('alg',nz,1);      % algebraic states
params   = SX.sym('paras',np,1);    % parameters
refs     = SX.sym('refs',ny,1);     % references of the first N stages
refN     = SX.sym('refs',nyN,1);    % reference of the last stage
Q        = SX.sym('Q',ny,1);        % weighting matrix of the first N stages
QN       = SX.sym('QN',nyN,1);      % weighting matrix of the last stage
aux      = SX.sym('aux',ny,1);      % auxilary variable
auxN     = SX.sym('auxN',nyN,1);    % auxilary variable


%% Dynamics

%Constant
mVal = 2;

p=states(1); 
pdot = states(2); 

u1=controls(1);% f1

% explicit ODE RHS
x_dot= [pdot; 
 u1/mVal];

% algebraic function
z_fun = [];

% implicit ODE: impl_f = 0
xdot = SX.sym('xdot',nx,1);
impl_f = xdot - x_dot;

%% Objectives and constraints


% inner objectives
h=[];
hN=[p pdot]';


% outer objectives
obji =0;% 0.5*(h-refs)'*diag(Q)*(h-refs);
objN = 0.5*(hN-refN)'*diag(QN)*(hN-refN);

obji_GGN =0;% 0.5*(aux-refs)'*(aux-refs);
objN_GGN = 0.5*(auxN-refN)'*(auxN-refN);

% general inequality path constraints
general_con = [];
general_con_N = []; 

%% NMPC discretizing time length [s]

Ts_st = 0.01; % shooting interval time

        x_ini = zeros(nx,1);
        input.x0=x_ini;
        input.u0=zeros(nu,1);
        input.z0 = zeros(nz,1);
        para0 = 0;
        q =0;
        qN = [1, 1];     % pz pzdot
        Q = 0;
        QN = qN';
        % upper and lower bounds for states (=nbx) 
        lb_x = [];
        ub_x = [];
        % upper and lower bounds for controls (=nbu)           
        lb_u = -1;
        ub_u = 1;
        % upper and lower bounds for general constraints (=nc)
        lb_g = [];
        ub_g = [];
        lb_gN = [];
        ub_gN = [];

        refss=zeros(1,nx+nu);
        refss(1)=0.1;
        data.REF=refss;