Added MATLAB rewrite of the CARNOT optimisation function

Simulink/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

Simulink/gp_mpc_system.m

@@ -0,0 +1,150 @@
+classdef gp_mpc_system < matlab.System & matlab.system.mixin.Propagates
+    % untitled Add summary here
+    %
+    % This template includes the minimum set of functions required
+    % to define a System object with discrete state.
+
+    properties
+        % Control horizon
+        N = 0; 
+        % Time Step
+        TimeStep = 0;
+        % Max Electrical Power Consumption
+        Pel = 6300;
+
+    end
+
+    properties (DiscreteState)
+    end
+
+    properties (Access = private)
+        % Pre-computed constants.
+        casadi_solver
+        u_lags
+        y_lags
+        lbx
+        ubx
+    end
+
+    methods (Access = protected)
+        function num = getNumInputsImpl(~)
+            num = 2;
+        end
+        function num = getNumOutputsImpl(~)
+            num = 1;
+        end
+        function dt1 = getOutputDataTypeImpl(~)
+        	dt1 = 'double';
+        end
+        function [dt1, dt2] = getInputDataTypeImpl(~)
+        	dt1 = 'double';
+            dt2 = 'double';
+        end
+        function sz1 = getOutputSizeImpl(~)
+        	sz1 = 1;
+        end
+        function sz1 = getInputSizeImpl(~)
+        	sz1 = 1;
+        end
+        function cp1 = isInputComplexImpl(~)
+        	cp1 = false;
+        end
+        function cp1 = isOutputComplexImpl(~)
+        	cp1 = false;
+        end
+        function fz1 = isInputFixedSizeImpl(~)
+        	fz1 = true;
+        end
+        function fz1 = isOutputFixedSizeImpl(~)
+        	fz1 = true;
+        end
+        function setupImpl(obj,~,~)
+            % Implement tasks that need to be performed only once, 
+            % such as pre-computed constants.
+            addpath('/home/radu/Media/MATLAB/casadi-linux-matlabR2014b-v3.5.5')
+            import casadi.*
+            
+            % Initialize CasADi callback
+            cs_model = gpCallback('model');
+            
+            % Set up problem variables
+            T_set = 20;
+            n_states = 7;
+            
+            COP = 5; %cooling
+            EER = 5; %heating
+            
+            
+            obj.u_lags = [0];
+            obj.y_lags = [23 23 23];
+            
+            % Formulate the optimization problem
+            J = 0; % optimization objective
+            g = []; % constraints vector
+
+            % Set up the symbolic variables
+            U = MX.sym('U', obj.N, 1);
+            W = MX.sym('W', obj.N, 2);
+            x0 = MX.sym('x0', 1, n_states - 3);
+
+            % setup the first state
+            wk = W(1, :);
+            uk = U(1); % scaled input
+            xk = [wk, obj.Pel*uk, x0];
+            yk = cs_model(xk);
+            J = J + (yk - T_set).^2;
+            
+            % Setup the rest of the states
+            for idx = 2:obj.N
+                wk = W(idx, :); 
+                uk_1 = uk; uk = U(idx);
+                xk = [wk, obj.Pel*uk, obj.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 =0;%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);
+            
+            obj.casadi_solver = solver;
+            obj.lbx = -COP;
+            obj.ubx = EER;
+        end
+
+        function u = stepImpl(obj,x,w)        
+            import casadi.*
+            
+            %Update the y lags
+            obj.y_lags = [x, obj.y_lags(1:end-1)];
+            
+            
+            real_p = vertcat(vec(DM(w)), vec(DM([obj.u_lags obj.y_lags])));
+            disp("Starting optimization")
+            tic
+            %res = obj.casadi_solver('p', real_p, 'ubx', obj.ubx, 'lbx', obj.lbx);
+            t = toc;
+            disp(t)
+            u = obj.Pel * full(res.x(1));
+            
+            % Update the u lags
+            obj.u_lags = [u, obj.u_lags(2:end-1)];
+            
+        end
+
+        function resetImpl(obj)
+            % Initialize discrete-state properties.
+        end
+    end
+end

Simulink/mpc_simulink/casadi_block.m

@@ -0,0 +1,177 @@
+classdef casadi_block < matlab.System & matlab.system.mixin.Propagates
+    % untitled Add summary here
+    %
+    % This template includes the minimum set of functions required
+    % to define a System object with discrete state.
+
+    properties
+        % Public, tunable properties.
+
+    end
+
+    properties (DiscreteState)
+    end
+
+    properties (Access = private)
+        % Pre-computed constants.
+        casadi_solver
+        x0
+        lbx
+        ubx
+        lbg
+        ubg
+    end
+
+    methods (Access = protected)
+        function num = getNumInputsImpl(~)
+            num = 2;
+        end
+        function num = getNumOutputsImpl(~)
+            num = 1;
+        end
+        function dt1 = getOutputDataTypeImpl(~)
+        	dt1 = 'double';
+        end
+        function dt1 = getInputDataTypeImpl(~)
+        	dt1 = 'double';
+        end
+        function sz1 = getOutputSizeImpl(~)
+        	sz1 = [1,1];
+        end
+        function sz1 = getInputSizeImpl(~)
+        	sz1 = [1,1];
+        end
+        function cp1 = isInputComplexImpl(~)
+        	cp1 = false;
+        end
+        function cp1 = isOutputComplexImpl(~)
+        	cp1 = false;
+        end
+        function fz1 = isInputFixedSizeImpl(~)
+        	fz1 = true;
+        end
+        function fz1 = isOutputFixedSizeImpl(~)
+        	fz1 = true;
+        end
+        function setupImpl(obj,~,~)
+            % Implement tasks that need to be performed only once, 
+            % such as pre-computed constants.
+            
+            import casadi.*
+
+            T = 10; % Time horizon
+            N = 20; % number of control intervals
+
+            % Declare model variables
+            x1 = SX.sym('x1');
+            x2 = SX.sym('x2');
+            x = [x1; x2];
+            u = SX.sym('u');
+
+            % Model equations
+            xdot = [(1-x2^2)*x1 - x2 + u; x1];
+
+            % Objective term
+            L = x1^2 + x2^2 + u^2;
+
+            % Continuous time dynamics
+            f = casadi.Function('f', {x, u}, {xdot, L});
+
+            % Formulate discrete time dynamics
+            % Fixed step Runge-Kutta 4 integrator
+            M = 4; % RK4 steps per interval
+            DT = T/N/M;
+            f = Function('f', {x, u}, {xdot, L});
+            X0 = MX.sym('X0', 2);
+            U = MX.sym('U');
+            X = X0;
+            Q = 0;
+            for j=1:M
+               [k1, k1_q] = f(X, U);
+               [k2, k2_q] = f(X + DT/2 * k1, U);
+               [k3, k3_q] = f(X + DT/2 * k2, U);
+               [k4, k4_q] = f(X + DT * k3, U);
+               X=X+DT/6*(k1 +2*k2 +2*k3 +k4);
+               Q = Q + DT/6*(k1_q + 2*k2_q + 2*k3_q + k4_q);
+            end
+            F = Function('F', {X0, U}, {X, Q}, {'x0','p'}, {'xf', 'qf'});
+
+            % Start with an empty NLP
+            w={};
+            w0 = [];
+            lbw = [];
+            ubw = [];
+            J = 0;
+            g={};
+            lbg = [];
+            ubg = [];
+
+            % "Lift" initial conditions
+            X0 = MX.sym('X0', 2);
+            w = {w{:}, X0};
+            lbw = [lbw; 0; 1];
+            ubw = [ubw; 0; 1];
+            w0 = [w0; 0; 1];
+
+            % Formulate the NLP
+            Xk = X0;
+            for k=0:N-1
+                % New NLP variable for the control
+                Uk = MX.sym(['U_' num2str(k)]);
+                w = {w{:}, Uk};
+                lbw = [lbw; -1];
+                ubw = [ubw;  1];
+                w0 = [w0;  0];
+
+                % Integrate till the end of the interval
+                Fk = F('x0', Xk, 'p', Uk);
+                Xk_end = Fk.xf;
+                J=J+Fk.qf;
+
+                % New NLP variable for state at end of interval
+                Xk = MX.sym(['X_' num2str(k+1)], 2);
+                w = {w{:}, Xk};
+                lbw = [lbw; -0.25; -inf];
+                ubw = [ubw;  inf;  inf];
+                w0 = [w0; 0; 0];
+
+                % Add equality constraint
+                g = {g{:}, Xk_end-Xk};
+                lbg = [lbg; 0; 0];
+                ubg = [ubg; 0; 0];
+            end
+
+            % Create an NLP solver
+            prob = struct('f', J, 'x', vertcat(w{:}), 'g', vertcat(g{:}));
+            options = struct('ipopt',struct('print_level',0),'print_time',false);
+            solver = nlpsol('solver', 'ipopt', prob, options);
+
+            obj.casadi_solver = solver;
+            obj.x0 = w0;
+            obj.lbx = lbw;
+            obj.ubx = ubw;
+            obj.lbg = lbg;
+            obj.ubg = ubg;
+        end
+
+        function u = stepImpl(obj,x,t)
+            disp(t)
+            tic
+            w0 = obj.x0;
+            lbw = obj.lbx;
+            ubw = obj.ubx;
+            solver = obj.casadi_solver;
+            lbw(1:2) = x;
+            ubw(1:2) = x;
+            sol = solver('x0', w0, 'lbx', lbw, 'ubx', ubw,...
+                        'lbg', obj.lbg, 'ubg', obj.ubg);
+  
+            u = full(sol.x(3));
+            toc
+        end
+
+        function resetImpl(obj)
+            % Initialize discrete-state properties.
+        end
+    end
+end

Simulink/test_casadi_mpc.m

@@ -0,0 +1,10 @@
+clear all
+close all
+clc
+%%%%%%%%%%%%%%%%
+
+
+%% Load the existing GP
+addpath("../../Gaussiandome/Identification/Computation results/")
+load("Identification_Validation.mat")
+load("Gaussian_Process_models.mat")

Simulink/weather_predictor.m

@@ -0,0 +1,79 @@
+classdef weather_predictor < matlab.System
+    % untitled Add summary here
+    %
+    % This template includes the minimum set of functions required
+    % to define a System object with discrete state.
+
+    % Public, tunable properties
+    properties
+        
+    end
+    
+    % Public, tunable properties
+    properties(Nontunable)
+        TimeStep = 0;
+        N = 0;
+    end
+
+    properties(DiscreteState)
+
+    end
+
+    % Pre-computed constants
+    properties(Access = private)
+        
+    end
+
+    methods(Access = protected)      
+        function num = getNumInputsImpl(~)
+            num = 2;
+        end
+        function num = getNumOutputsImpl(~)
+            num = 1;
+        end
+        function dt1 = getOutputDataTypeImpl(~)
+        	dt1 = 'double';
+        end
+        function [dt1, dt2] = getInputDataTypeImpl(~)
+        	dt1 = 'double';
+            dt2 = 'double';
+        end
+        function sz1 = getOutputSizeImpl(obj)
+        	sz1 = [obj.N 2];
+        end
+        function sz1 = getInputSizeImpl(~)
+        	sz1 = 1;
+        end
+        function cp1 = isInputComplexImpl(~)
+        	cp1 = false;
+        end
+        function cp1 = isOutputComplexImpl(~)
+        	cp1 = false;
+        end
+        function fz1 = isInputFixedSizeImpl(~)
+        	fz1 = true;
+        end
+        function fz1 = isOutputFixedSizeImpl(~)
+        	fz1 = true;
+        end
+        
+        
+        function setupImpl(~, ~, ~)
+            disp('Hello World')
+            % Perform one-time calculations, such as computing constants
+        end
+
+        function w = stepImpl(obj,wdb_mat,timestamp)
+            disp(timestamp)
+            % Implement algorithm. Calculate y as a function of input u and
+            % discrete states.
+            curr_idx = find(wdb_mat(:, 1) == timestamp);
+            N_idx = (1:obj.N) + curr_idx;
+            w = [wdb_mat(N_idx, 18) + wdb_mat(N_idx, 19), wdb_mat(N_idx, 7)];
+        end
+
+        function resetImpl(obj)
+            % Initialize / reset discrete-state properties
+        end
+    end
+end

Simulink/weather_predictor2.m

@@ -0,0 +1,8 @@
+function w = weather_predictor2(wdb_mat,timestamp, N)
+%WEATHER_PREDICTOR2 Summary of this function goes here
+%   Detailed explanation goes here
+curr_idx = find(wdb_mat(:, 1) == timestamp);
+N_idx = (1:N) + curr_idx;
+w = [wdb_mat(N_idx, 18) + wdb_mat(N_idx, 19), wdb_mat(N_idx, 7)];
+
+end