MATLAB/Simulink code update

Matlab_scripts/MPCforSonja/MPCcasadi_v1_0.m

@@ -0,0 +1,225 @@
+classdef MPCcasadi_v1_0 < matlab.System
+
+    % Public, tunable properties
+    properties(Nontunable)
+        TimeStep = 0; % Time step MPC
+        N        = 0; % Planning and control horizon N
+        R        = 1; % Weights for control cost R 
+        T        = 1; % Weights for slack variable for output constraints T 
+        nState   = 0; % Number of states X
+        nOut     = 0; % Number of outputs Y
+        nIn      = 0; % Number of controlled inputs U
+        nDst     = 0; % Number of disturbance inputs
+        A        = 0; % A 
+        Bd       = 0; % Bd (disturbance) 
+        Bu       = 0; % Bu (control) 
+        C        = 0; % C 
+        D        = 0; % D 
+        uMin     = 0; % Lower control constraints uMin
+        uMax     = 0; % Upper control constraints uMax
+        yMin     = 0; % Lower output constraints yMin
+        yMax     = 0; % Upper output constraints yMax
+    end
+
+    properties(DiscreteState)
+
+    end
+
+    % Pre-computed constants
+    properties(Access = private)
+      casadi_solver
+      lbg  
+      ubg   
+    end
+
+    methods(Access = protected)
+        function sts = getSampleTimeImpl(obj)
+            sts = createSampleTime(obj, 'Type', 'Controllable', 'TickTime', obj.TimeStep); % Time step
+        end
+        
+        function num = getNumInputsImpl(~) % Number of inputs
+            num = 4;
+        end
+        function num = getNumOutputsImpl(~) % Number of outputs
+            num = 5;
+        end
+        function [dt1, dt2, dt3, dt4, dt5] = getOutputDataTypeImpl(~) % Output data type
+        	dt1 = 'double';
+            dt2 = 'double';
+            dt3 = 'double';
+            dt4 = 'double';
+            dt5 = 'double';
+        end
+        function dt1 = getInputDataTypeImpl(~) % Input data type
+        	dt1 = 'double';
+        end
+        function [sz1, sz2, sz3, sz4, sz5] = getOutputSizeImpl(obj) % OUtput dimensions
+        	sz1 = [1,        obj.nIn];     % mv
+            sz2 = [obj.N+1,  obj.nState];  % xStar               
+            sz3 = [obj.N,    obj.nOut];    % sStar
+            sz4 = [obj.N,    obj.nIn];     % uStar
+            sz5 = [1,        obj.nOut];    % yStarOut
+        end
+        function [sz1, sz2, sz3, sz4] = getInputSizeImpl(obj) % Input dimensions
+        	sz1  = [obj.nState,  1];                 % xHat
+            sz2  = [obj.N,       obj.nDst];          % disturbances
+            sz3  = [obj.N,       1];                 % elec price
+            sz4  = [1,           1];                 % on
+        end
+        function cp1 = isInputComplexImpl(~) % Inputs are complex numbers?
+        	cp1 = false;
+        end
+        function [cp1, cp2, cp3, cp4, cp5] = isOutputComplexImpl(~) % Outputs are complex numbers?
+        	cp1 = false;
+            cp2 = false;
+            cp3 = false;
+            cp4 = false;            
+            cp5 = false;
+        end
+        function fz1 = isInputFixedSizeImpl(~) % Input fixed size?
+        	fz1 = true;
+        end
+        function [fz1, fz2, fz3, fz4, fz5] = isOutputFixedSizeImpl(~) % Output fixed size?
+        	fz1 = true;
+            fz2 = true;
+            fz3 = true;
+            fz4 = true;
+            fz5 = true;
+        end
+        
+        function setupImpl(obj)
+            % Perform one-time calculations, such as computing constants
+            import casadi.*
+
+            %% Parameters
+            nState  = obj.nState;
+            nIn     = obj.nIn;
+            nOut    = obj.nOut;
+            nDst    = obj.nDst;
+            N       = obj.N;
+            R       = obj.R;
+            T       = obj.T;
+            A       = obj.A;
+            Bd      = obj.Bd;
+            Bu      = obj.Bu;
+            C       = obj.C;
+            D       = obj.D;
+
+            %% Prepare variables
+            U = MX.sym('U', nIn,     N); 
+            P = MX.sym('P', nState + N + nDst*N); % Initial values, costElec, disturbances
+            X = MX.sym('X', nState, (N+1));
+            S = MX.sym('S', nOut,    N); % First state free
+
+            J   = 0; % Objective function
+            g   = [];  % constraints vector
+            
+            %% P indices
+            iX0     = [1:nState];
+            iCoEl   = [nState+1:nState+N];
+            iDist   = [nState+N+1:nState+N+nDst*N];
+            
+            %% Disassemble P
+            pX0     = P(iX0);
+            pCoEl   = P(iCoEl);
+            pDist   = reshape(P(iDist), [nDst N]); % Prone to shaping error
+
+            %% Define variables
+            states       = MX.sym('states',       nState);
+            controls     = MX.sym('controls',     nIn);
+            disturbances = MX.sym('disturbances', nDst);
+
+            %% Dynamics
+            f = Function('f',{P, states, controls, disturbances},{A*states + Bu*controls + Bd*disturbances});
+
+            %% Compile all constraints
+            g = [g; X(:,1) - pX0]; 
+
+            for i = 1:N    
+                g = [g; C*X(:,i+1) - S(:,i)];                           % State/output constraints, first state free
+                g = [g; U(:,i)];                                        % Control constraints
+                g = [g; X(:,i+1) - f(P, X(:,i), U(:,i), pDist(:,i))];   % System dynamics
+                    
+                % Cost function, first state given -> not punished
+                J = J + R * U(:,i) * pCoEl(i) + S(:,i)'*T*S(:,i);
+            end
+
+            %% Reshape variables
+            OPT_variables = veccat(X, S, U);
+
+            %% Optimization
+            nlp_mhe = struct('f', J,             ...
+                             'x', OPT_variables, ...
+                             'g', g,             ...
+                             'p', P); 
+
+            opts                   = struct;
+            opts.ipopt.print_level = 0; %5;
+            solver                 = nlpsol('solver', 'ipopt', nlp_mhe, opts);
+            
+            %% Pack opj
+            obj.casadi_solver = solver;
+            
+        end
+
+        function [mv, xStar, sStar, uStar, yStarOut] = stepImpl(obj, xHat, dist, cE, on) 
+            % Implement algorithm. Calculate y as a function of input u and
+            % discrete states.
+            if on > 0.5
+                %% Parameters
+                nState  = obj.nState;
+                N       = obj.N;
+                nOut    = obj.nOut;
+                nDst    = obj.nDst;
+                nIn     = obj.nIn;
+                yMin    = obj.yMin;
+                yMax    = obj.yMax;
+                uMin    = obj.uMin;
+                uMax    = obj.uMax;
+                C       = obj.C;
+                solver  = obj.casadi_solver;
+                Pdata   = [xHat; cE; reshape(dist', [nDst*N, 1])];   % Prone to shaping error!!!
+
+                %% Constraints
+                lbg = zeros(nState,1); % x0 constraints
+                ubg = zeros(nState,1);
+                
+                % Output, control and dynamics constraints
+                for i = 1:N
+                    lbg = [lbg; yMin];
+                    lbg = [lbg; uMin];
+                    lbg = [lbg; zeros(nState,1)];
+
+                    ubg = [ubg; yMax];
+                    ubg = [ubg; uMax];
+                    ubg = [ubg; zeros(nState,1)];
+                end
+
+                %% Solver
+                sol    = solver('x0',  0,     ... % x0 = x* from before, shift one time step, double last time step
+                                'lbg', lbg,   ...
+                                'ubg', ubg,   ...
+                                'p',   Pdata);
+
+                %% Outputs
+                xStar    = reshape(full(sol.x(1                    :nState*(N+1))),        [nState, (N+1)])';
+                sStar    = reshape(full(sol.x(nState*(N+1)+1       :nState*(N+1)+nOut*N)), [nOut,    N])';
+                uStar    = reshape(full(sol.x(nState*(N+1)+nOut*N+1:end)),                 [nIn,     N])';
+                
+                mv       =         full(sol.x(nState*(N+1)+nOut*N+1:nState*(N+1)+nOut*N+nIn))';
+                yStarOut = C*xStar(2,:)'; % Second value is the target
+
+            else % Zero output if MPC is disabled
+                mv       = zeros(1,       obj.nIn);
+                xStar    = zeros(obj.N+1, obj.nState);
+                uStar    = zeros(obj.N,   obj.nIn);
+                sStar    = zeros(obj.N,   obj.nOut);
+                yStarOut = zeros(1,       obj.nOut);   
+            end % \if on
+        end % \stepImpl
+
+        function resetImpl(obj)
+            % Initialize / reset discrete-state properties
+        end
+    end
+end

Matlab_scripts/MPCforSonja/setupMPCcasadi_v1_0.m

@@ -0,0 +1,30 @@
+
+
+%% Settings
+TimeStep = 900; % Step time
+nHor     = 4*24; % Length of ontrol and planning horizon
+%tSmp     = 0:TimeStep:nHor*TimeStep-1;
+
+nStt     = 1; % Number of states
+chY      = 1; % Number of observed variables
+nDst     = 1; % Number of disturbance variables
+nMV      = 1; % Number of controlled variables
+
+%% System matrices
+A  = 1;
+B  = [-1, 1]/(3000*4182/TimeStep);
+Bd = B(:, 1:nDst);
+Bu = B(:, nDst+1:end);
+C  = 1;
+D  = 0;
+
+%% Constraints and normalization
+uMin = 0; 
+uMax = 7500; 
+yMin = 40;
+yMax = 50;
+
+%% Weights
+R   = 1/uMax/0.1; 
+T   = 1e5*eye(chY); 
+

Matlab_scripts/casadi_gp_mpc.m

@@ -0,0 +1,86 @@
+clear all
+close all
+clc
+
+%% Import CasADi
+addpath('/home/radu/Media/MATLAB/casadi-linux-matlabR2014b-v3.5.5')
+import casadi.*
+
+load("gpr_model.mat")
+%% Initialize casadi callback
+cs_model = gpCallback('model');
+
+T_set = 20;
+N_horizon = 5;
+n_states = 7;
+
+
+COP = 5; %cooling
+EER = 5; %heating
+Pel = 6300; % Electric Power Consumption of the HVAC
+
+u_min = - COP * Pel;
+u_max =   EER * Pel;
+
+J = 0; % optimization objective
+g = []; % constraints vector
+
+% Set up the symbolic variables
+U = MX.sym('U', N_horizon, 1);
+W = MX.sym('W', N_horizon, 2);
+x0 = MX.sym('x0', 1, n_states - 3);
+
+% setup the first state
+wk = W(1, :);
+uk = U(1); % scaled input
+xk = [wk, Pel*uk, x0];
+yk = cs_model(xk);
+J = J + (yk - T_set).^2;
+
+% Setup the rest of the states
+for idx = 2:N_horizon
+    wk = W(idx, :); 
+    uk_1 = uk; uk = U(idx);
+    xk = [wk, Pel*uk, Pel*uk_1, yk, xk(5:6)];
+    yk = cs_model(xk);
+    J = J + (yk - T_set).^2;
+end
+    
+p = [vec(W); vec(x0)];
+
+nlp_prob = struct('f', J, 'x', vec(U), 'g', g, 'p', p);
+
+opts = struct;
+%opts.ipopt.max_iter = 5000;
+opts.ipopt.max_cpu_time = 15*60;
+opts.ipopt.hessian_approximation = 'limited-memory';
+%opts.ipopt.print_level =1;%0,3
+opts.print_time = 0;
+opts.ipopt.acceptable_tol =1e-8;
+opts.ipopt.acceptable_obj_change_tol = 1e-6;
+
+solver = nlpsol('solver', 'ipopt', nlp_prob,opts);
+
+real_x0 = [0,  23,  23,  23];
+real_W = [[57.9261000000000;54.9020333333334;73.8607000000000;76.0425333333333;64.9819666666667], [22; 22; 22; 22; 22]];
+
+real_p = vertcat(vec(DM(real_W)), vec(DM(real_x0)));
+
+
+res = solver('p', real_p, 'ubx', EER, 'lbx', -COP);
+%% Interpret the optimization result
+x = Pel * full(res.x);
+X = [real_W, x, [real_x0; zeros(N_horizon -1, size(real_x0, 2))]];
+
+X(2:end, 4) = X(1:end-1, 3);
+
+for idx=2:N_horizon
+    X(idx, 5) = full(cs_model(X(idx - 1, :)));
+    X(idx, 6:7) = X(idx - 1, 5:6);
+end
+T_horizon = cs_model(X');
+
+
+figure; hold on;
+plot(1:N_horizon, full(T_horizon));
+plot(1:N_horizon, T_set*ones(1, N_horizon));

Matlab_scripts/gpCallback.m

@@ -0,0 +1,39 @@
+classdef gpCallback < casadi.Callback
+  properties
+    model
+  end
+  methods
+    function self = gpCallback(name)
+      self@casadi.Callback();
+      construct(self, name, struct('enable_fd', true));
+    end
+
+    % Number of inputs and outputs
+    function v=get_n_in(self)
+      v=1;
+    end
+    function v=get_n_out(self)
+      v=1;
+    end
+    % Function sparsity
+    function v=get_sparsity_in(self, i)
+      v=casadi.Sparsity.dense(7, 1);
+    end
+
+    % Initialize the object
+    function init(self)
+      disp('initializing gpCallback')
+      gpr = load('gpr_model.mat', 'model');
+      self.model = gpr.model;
+    end
+
+    % Evaluate numerically
+    function arg = eval(self, arg)
+      x = full(arg{1});
+      % Transpose x since gp predictor takes row by row, and casadi gives
+      % colum by column
+      [mean, ~] = predict(self.model, x');
+      arg = {mean};
+    end
+  end
+end

Matlab_scripts/gp_casadi_test.m

@@ -0,0 +1,83 @@
+clear all
+close all
+clc
+%%%%%%%%%%%%
+addpath('/home/radu/Media/MATLAB/casadi-linux-matlabR2014b-v3.5.5')
+import casadi.*
+
+%% Generate GP data
+size = 500; n_samples = 15;
+X = linspace(-2, 2, size);
+Y = 3 * X .^2;
+
+% Add noise to the output
+mean = 0; std = 0.5;
+noise = mean + std.*randn(1, n_samples);
+
+idx_samples = randperm(size, n_samples);
+
+X_sampled = X(idx_samples);
+Y_sampled = Y(idx_samples);
+
+Y_sampled = Y_sampled + noise;
+
+figure; hold on; 
+plot(X, Y);
+scatter(X_sampled, Y_sampled);
+
+tbl_gpr_in = array2table([X_sampled', Y_sampled']);
+tbl_gpr_in.Properties.VariableNames = {'X', 'Y'};
+
+tic;
+model = fitrgp(tbl_gpr_in, 'Y', 'KernelFunction', 'ardsquaredexponential', ...
+                'FitMethod', 'sr', 'PredictMethod', 'fic', 'Standardize', 1);
+toc;
+
+%% Predict stuff
+[yhat_test, sigma_test] = predict(model, X');
+std_test = sqrt(sigma_test);
+
+% prepare it for the fill function
+x_ax    = X';
+X_plot  = [x_ax; flip(x_ax)];
+Y_plot  = [yhat_test-1.96.*std_test; flip(yhat_test+1.96.*std_test)];
+
+% plot a line + confidence bands
+figure(); hold on;
+title("GP performance on test data");
+plot(x_ax, Y, 'red', 'LineWidth', 1.2);
+plot(x_ax, yhat_test, 'blue', 'LineWidth', 1.2)
+fill(X_plot, Y_plot , 1,....
+        'facecolor','blue', ...
+        'edgecolor','none', ...
+        'facealpha', 0.3);
+legend({'data','prediction_mean', '95% confidence'},'Location','Best');
+hold off
+
+%% Save the model
+save('test_gpr_model.mat', 'model')
+
+%% CasADi optimization problem
+cs_model = test_gpCallback('model');
+cs_x = MX.sym('x');
+cs_y = 2 * cs_model(cs_x) + 5;
+f = Function('f', {cs_x}, {cs_y});
+
+
+nlp_prob = struct('f', f(cs_x), 'x', cs_x);
+
+
+opts = struct;
+opts.ipopt.max_iter = 2000;
+opts.ipopt.hessian_approximation = 'limited-memory';
+%opts.ipopt.print_level =1;%0,3
+opts.print_time = 0;
+opts.ipopt.acceptable_tol =1e-8;
+opts.ipopt.acceptable_obj_change_tol = 1e-6;
+
+solver = nlpsol('solver', 'ipopt', nlp_prob,opts);
+
+res = solver('lbx', -2, 'ubx', 2);
+
+res
+

Matlab_scripts/gp_identification.m

@@ -0,0 +1,71 @@
+clear all
+close all
+clc
+%%%%%%%%%%%
+
+load("gpr_carnot.mat");
+
+%% Format the train/test data arrays
+tbl_gpr_train = array2table(gpr_train);
+tbl_gpr_train.Properties.VariableNames = cellstr(table_cols);
+tbl_gpr_train = removevars(tbl_gpr_train,{'u'});
+tbl_gpr_train_x = removevars(tbl_gpr_train, {'y'});
+
+tbl_gpr_test = array2table(gpr_test);
+tbl_gpr_test.Properties.VariableNames = cellstr(table_cols);
+tbl_gpr_test = removevars(tbl_gpr_test,{'u'});
+tbl_gpr_test_x = removevars(tbl_gpr_test, {'y'});
+
+
+
+%% Train the GP model
+OutputName = 'y';
+
+tic;
+model = fitrgp(tbl_gpr_train, OutputName, 'KernelFunction', 'ardsquaredexponential', ...
+                'FitMethod', 'sr', 'PredictMethod', 'fic', 'Standardize', 1);
+toc;
+%% Validate the model using training data
+[yhat_train, sigma_train] = predict(model, tbl_gpr_train_x);
+std_train = sqrt(sigma_train);
+
+% prepare it for the fill function
+x_ax    = (1:size(tbl_gpr_train, 1))';
+X_plot  = [x_ax; flip(x_ax)];
+Y_plot  = [yhat_train-1.96.*std_train; flip(yhat_train+1.96.*std_train)];
+
+% plot a line + confidence bands
+figure(); hold on;
+title("GP performance on training data");
+plot(x_ax, tbl_gpr_train.y, 'red', 'LineWidth', 1.2);
+plot(x_ax, yhat_train, 'blue', 'LineWidth', 1.2)
+fill(X_plot, Y_plot , 1,....
+        'facecolor','blue', ...
+        'edgecolor','none', ...
+        'facealpha', 0.3);
+legend({'data','prediction_mean', '95% confidence'},'Location','Best');
+hold off 
+
+%% Validate the model using test data
+[yhat_test, sigma_test] = predict(model, tbl_gpr_test_x);
+std_test = sqrt(sigma_test);
+
+% prepare it for the fill function
+x_ax    = (1:size(tbl_gpr_test, 1))';
+X_plot  = [x_ax; flip(x_ax)];
+Y_plot  = [yhat_test-1.96.*std_test; flip(yhat_test+1.96.*std_test)];
+
+% plot a line + confidence bands
+figure(); hold on;
+title("GP performance on test data");
+plot(x_ax, tbl_gpr_test.y, 'red', 'LineWidth', 1.2);
+plot(x_ax, yhat_test, 'blue', 'LineWidth', 1.2)
+fill(X_plot, Y_plot , 1,....
+        'facecolor','blue', ...
+        'edgecolor','none', ...
+        'facealpha', 0.3);
+legend({'data','prediction_mean', '95% confidence'},'Location','Best');
+hold off 
+
+%% Export the final GP model
+save('gpr_model.mat', 'model')

Matlab_scripts/test_gpCallback.m

@@ -0,0 +1,39 @@
+classdef test_gpCallback < casadi.Callback
+  properties
+    model
+  end
+  methods
+    function self = test_gpCallback(name)
+      self@casadi.Callback();
+      construct(self, name, struct('enable_fd', true));
+    end
+
+    % Number of inputs and outputs
+    function v=get_n_in(self)
+      v=1;
+    end
+    function v=get_n_out(self)
+      v=1;
+    end
+    % Function sparsity
+    function v=get_sparsity_in(self, i)
+      v=casadi.Sparsity.dense(1, 1);
+    end
+
+    % Initialize the object
+    function init(self)
+      disp('initializing gpCallback')
+      gpr = load('test_gpr_model.mat', 'model');
+      self.model = gpr.model;
+    end
+
+    % Evaluate numerically
+    function arg = eval(self, arg)
+      x = full(arg{1});
+      % Transpose x since gp predictor takes row inputs, and casadi gives
+      % colum by column
+      [mean, ~] = predict(self.model, x');
+      arg = {mean};
+    end
+  end
+end