Merge branch 'test_ex_singletrack'

Author: lucasrm25

GP.m

@@ -36,7 +36,7 @@ and it is trained (hyperparameter optimization) by maximizing the log Likelihood
 ### Results
 
 
-| MPC controller with Gaussian Process **deactivated**        | MPC controller with **TRAINED** and **ACTIVATED** Gaussian Process         |
+| NMPC controller with unmodelled dynamics        | Adaptive NMPC controller (with trained Gaussian Process)         |
 | ------------- |-------------|
 | <img src="./simresults/trackAnimVideo-16-Jan-2020-without-GP.gif" alt="drawing" width="400"/>      | <img src="./simresults/trackAnimVideo-16-Jan-2020-with-GP-optimized.gif" alt="drawing" width="400"/>    |
 

Papers/kabzan2019learning.pdf

@@ -236,10 +236,7 @@
 
 %% Optimize GP hyperparameters ??? (Offline procedure, after simulation)
 
-d_GP.M = M
-d_GP.var_f = var_f;
-d_GP.var_n = var_n;
-
+d_GP.setHyperParameters( M, var_f, var_n )
 % d_GP.optimizeHyperParams('ga');
 d_GP.optimizeHyperParams('fmincon');
 

Papers/kocijan2004gaussian.pdf

@@ -487,16 +487,6 @@
     gamma = 1000;
     lambda = -0.1;
     offroad_error = 5*(sqrt((4+gamma*(lambda-offroad_error).^2)/gamma) - (lambda-offroad_error));
-
-    % % CHECK SMOOTH TRANSITION
-    % x = -0.5:0.01:0.5
-    % % Relaxied barrier function for (x<=lambda)
-    % gamma = 10000;
-    % lambda = -0.2;
-    % y = 0.5*(sqrt((4+gamma*(lambda-x).^2)/gamma) - (lambda-x)); % y = -log(lambda-x)
-    % figure; hold on; grid on;
-    % plot(x,y)
-    
     cost_outside = q_r * offroad_error^2;
 
     % ---------------------------------------------------------------------

Papers/ostafew2016learning.pdf

@@ -33,4 +33,31 @@
 cellfun( @(h) delete(h), trackAnim.h_var_x_pred_opt )
 legend('off')
 title('')
-fp.savefig(imgname,'format','epsc')
+fp.savefig(imgname,'format','epsc')
+
+
+
+
+
+%% CHECK RELAXED BARRIER FUNCTION PLOT
+
+
+x = -0.5:0.001:0.5;
+% Relaxied barrier function for (x<=lambda)
+gamma = 10000;
+lambda = -0.2;
+y = 0.5*(sqrt((4+gamma*(lambda-x).^2)/gamma) - (lambda-x)); 
+figure('Color','w'); hold on; %grid on;
+plot(x,y,'LineWidth',2)
+ylim([0,0.1])
+xlim([-0.4,0])
+set(gca,'YTickLabel',[]);
+set(gca,'XTickLabel',[]);
+%%%%
+lambda = -0.2;
+x = -0.6:0.001:(lambda-eps);
+y = 0.1*-log(lambda-x);
+figure('Color','w'); hold on; %grid on;
+plot(x,y,'LineWidth',2)
+xlim([-0.6,0])
+

README.md

@@ -0,0 +1,67 @@
+clear all; close all; clc;
+
+var_w   = 1e-8;
+var_f   = 1e4;              % output variance
+M       = diag(1e0);     % length scale
+var_n   = var_w;           % measurement noise variance
+maxsize = 100;             % maximum number of points in the dictionary
+
+% create GP object
+gp = GP(1, 1, var_f, var_n, M, maxsize);
+
+%% true function
+Xt = [-4 -3 -2 -1 0 1 2 3 4];
+Yt = [-1 0.5 1.5 1.5 1 0.8 1 1.5 2]';
+pp = spline(Xt,Yt');
+truefun = @(x) ppval(pp, x)';
+
+%% measure data
+X = Xt([1 2 3 5 7 8]);
+Y = truefun(X) + sqrt(var_n)*randn(length(X),1);
+
+gp.add(X,Y);
+
+
+%% plot
+close all;
+
+% test 1
+gp.setHyperParameters(1e-2, 2, var_w)
+gp.plot1d(truefun)
+xlim([-6 5]); ylim([-4 4]);
+
+% test 2
+gp.setHyperParameters(50, 100, var_w)
+gp.plot1d(truefun)
+xlim([-6 5]); ylim([-4 4]);
+
+% test 3
+gp.setHyperParameters(1e-3, 1e-2, var_w)
+gp.plot1d(truefun)
+xlim([-6 5]); ylim([-4 4]);
+
+% test 4
+gp.setHyperParameters(50, 1e7, var_w)
+gp.plot1d(truefun)
+xlim([-6 5]); ylim([-4 4]);
+
+% test 4
+gp.setHyperParameters(100^2, 100^2, 1^2)
+gp.plot1d(truefun)
+xlim([-6 5]); ylim([-4 4]);
+
+%% optimize and plot
+
+gp.setHyperParameters(0.1^2, 0.1^2, 1e-8)
+% gp.optimizeHyperParams('ga');
+gp.optimizeHyperParams('fmincon');
+
+close all
+gp.plot1d(truefun)
+xlim([-6 5]); ylim([-4 4]);
+
+%%
+close all
+gp.setHyperParameters(1.5^2, 2^2, 4.0228e-09)
+gp.plot1d(truefun)
+xlim([-6 5]); ylim([-5 5]);