Skip to content

Commit ca2ed41

Browse files
lucasrm25lucasrm25
committed
fixing minor bugs
1 parent 6651d02 commit ca2ed41

File tree

4 files changed

+67
-100
lines changed

4 files changed

+67
-100
lines changed

‎GP.m

Lines changed: 51 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -18,15 +18,15 @@
1818
%------------------------------------------------------------------
1919

2020
properties
21-
% kernel parameters (one for each output dimension)
22-
var_f % <p> signal/output covariance
23-
var_n % <p> evaluation noise covariance
24-
M % <n,n,p> length scale covariance
25-
2621
isActive = true
2722
end
2823

2924
properties(SetAccess=private)
25+
% kernel parameters (one for each output dimension)
26+
M % <n,n,p> length scale covariance
27+
var_f % <p> signal/output covariance
28+
var_n % <p> evaluation noise covariance
29+
3030
% dictionary: [X,Y]
3131
X % <n,N> input dataset
3232
Y % <N,p> output dataset
@@ -62,15 +62,8 @@
6262
obj.p = p;
6363
obj.X = [];
6464
obj.Y = [];
65-
obj.var_f = var_f;
66-
obj.var_n = var_n;
67-
obj.M = M;
68-
obj.Nmax = maxsize;
69-
70-
% validade model parameters
71-
assert( n == size(M,1), 'Matrix M has wrong dimension or parameters n/p are wrong. Expected dim(M)=<n,n,p>=<%d,%d,%d>',n,n,p);
72-
assert( p == size(var_f,1), 'Matrix var_f has wrong dimension or parameter p is wrong. Expected dim(var_f)=<p>=<%d>, got <%d>.',p,size(var_f,1));
73-
assert( p == size(var_n,1), 'Matrix var_n has wrong dimension or parameter p is wrong. Expected dim(var_n)=<p>=<%d>, got <%d>.',p,size(var_n,1));
65+
obj.Nmax = maxsize;
66+
obj.setHyperParameters( M, var_f, var_n )
7467
end
7568

7669

@@ -88,36 +81,18 @@
8881
N = size(obj.X,2);
8982
end
9083

91-
% function set.M(obj,M)
92-
% assert( obj.n == size(M,1), 'Matrix M has wrong dimension or parameters n/p are wrong. Expected dim(M)=<n,n,p>=<%d,%d,%d>',obj.n,obj.n,obj.p);
93-
% obj.M = M;
94-
% obj.isOutdated = true;
95-
% end
96-
%
97-
% function set.var_f(obj,var_f)
98-
% assert( obj.p == size(var_f,1), 'Matrix var_f has wrong dimension or parameter p is wrong. Expected dim(var_f)=<p>=<%d>, got <%d>.',obj.p,size(var_f,1));
99-
% obj.var_f = var_f;
100-
% obj.isOutdated = true;
101-
% end
102-
%
103-
% function set.var_n(obj,var_n)
104-
% assert( obj.p == size(var_n,1), 'Matrix var_n has wrong dimension or parameter p is wrong. Expected dim(var_n)=<p>=<%d>, got <%d>.',obj.p,size(var_n,1));
105-
% obj.var_n = var_n;
106-
% obj.isOutdated = true;
107-
% end
108-
%
109-
% function set.X(obj,X)
110-
% obj.X = X;
111-
% % data has been added. GP is outdated. Please call obj.updateModel
112-
% obj.isOutdated = true;
113-
% end
114-
%
115-
% function set.Y(obj,Y)
116-
% obj.Y = Y;
117-
% % data has been added. GP is outdated. Please call obj.updateModel
118-
% obj.isOutdated = true;
119-
% end
120-
84+
function setHyperParameters(obj, M, var_f, var_n)
85+
%------------------------------------------------------------------
86+
% set new values to the hyperparameter
87+
%------------------------------------------------------------------
88+
assert( obj.n == size(M,1), 'Matrix M has wrong dimension or parameters n/p are wrong. Expected dim(M)=<n,n,p>=<%d,%d,%d>',obj.n,obj.n,obj.p);
89+
assert( obj.p == size(var_f,1), 'Matrix var_f has wrong dimension or parameter p is wrong. Expected dim(var_f)=<p>=<%d>, got <%d>.',obj.p,size(var_f,1));
90+
assert( obj.p == size(var_n,1), 'Matrix var_n has wrong dimension or parameter p is wrong. Expected dim(var_n)=<p>=<%d>, got <%d>.',obj.p,size(var_n,1));
91+
obj.M = M;
92+
obj.var_f = var_f;
93+
obj.var_n = var_n;
94+
obj.updateModel();
95+
end
12196

12297
function mean = mu(~,x)
12398
%------------------------------------------------------------------
@@ -131,7 +106,7 @@
131106
end
132107

133108

134-
function kernel = K(obj,x1,x2)
109+
function kernel = K(obj, x1, x2)
135110
%------------------------------------------------------------------
136111
% SEQ kernel function: cov[f(x1),f(x2)]
137112
% k(x1,x2) = var_f * exp( 0.5 * ||x1-x2||^2_M )
@@ -142,10 +117,11 @@
142117
% out:
143118
% kernel: <N1,N2,p>
144119
%------------------------------------------------------------------
120+
kernel = zeros(size(x1,2),size(x2,2),obj.p);
145121
for pi = 1:obj.p
146-
D(:,:,pi) = pdist2(x1',x2','mahalanobis',obj.M(:,:,pi)).^2;
122+
D = pdist2(x1',x2','mahalanobis',obj.M(:,:,pi)).^2;
147123
%D = pdist2(x1',x2','seuclidean',diag((obj.M).^0.5)).^2;
148-
kernel(:,:,pi) = obj.var_f(pi) * exp( -0.5 * D(:,:,pi) );
124+
kernel(:,:,pi) = obj.var_f(pi) * exp( -0.5 * D );
149125
end
150126
end
151127

@@ -155,6 +131,10 @@ function updateModel(obj)
155131
% Update precomputed matrices L and alpha, that will be used when
156132
% evaluating new points. See [Rasmussen, pg19].
157133
% -----------------------------------------------------------------
134+
% nothing to update... return
135+
if obj.N == 0
136+
return;
137+
end
158138
% store cholesky L and alpha matrices
159139
I = eye(obj.N);
160140

@@ -165,7 +145,6 @@ function updateModel(obj)
165145
for pi=1:obj.p
166146
obj.L(:,:,pi) = chol(K(:,:,pi)+ obj.var_n(pi) * I ,'lower');
167147
% sanity check: norm( L*L' - (obj.K(obj.X,obj.X) + obj.var_n*I) ) < 1e-12
168-
169148
obj.alpha(:,pi) = obj.L(:,:,pi)'\(obj.L(:,:,pi)\(obj.Y(:,pi)-obj.mu(obj.X)));
170149
end
171150

@@ -178,15 +157,15 @@ function updateModel(obj)
178157
end
179158

180159

181-
function add(obj,X,Y)
160+
function add(obj, X, Y)
182161
%------------------------------------------------------------------
183162
% Add new data points [X,Y] to the dictionary
184163
%
185164
% args:
186165
% X: <n,N>
187166
% Y: <N,p>
188167
%------------------------------------------------------------------
189-
OPTION = 'A'; % {'A','B'}
168+
OPTION = 'B'; % {'A','B'}
190169

191170
assert(size(Y,2) == obj.p, ...
192171
sprintf('Y should have %d columns, but has %d. Dimension does not agree with the specified kernel parameters',obj.p,size(Y,2)));
@@ -204,7 +183,6 @@ function add(obj,X,Y)
204183
% data overflow: dictionary will be full. we need to select
205184
% relevant points
206185
else
207-
208186
Nthatfit = obj.Nmax - obj.N;
209187

210188
% make dictionary full
@@ -230,14 +208,17 @@ function add(obj,X,Y)
230208

231209
% OPTION B)
232210
% the point with lowest variance will be removed
233-
else
211+
elseif strcmp(OPTION,'B')
234212
X_all = [obj.X,X];
235213
Y_all = [obj.Y;Y];
214+
236215
[~, var_y] = obj.eval( X_all, true);
237216
[~,idx_keep] = maxk(sum(reshape(var_y, obj.p^2, obj.N+Nextra )),obj.Nmax);
238217

239218
obj.X = X_all(:,idx_keep);
240219
obj.Y = Y_all(idx_keep,:);
220+
else
221+
error('Option not implemented');
241222
end
242223
end
243224
% update pre-computed matrices
@@ -321,7 +302,12 @@ function optimizeHyperParams(obj, method)
321302

322303
% set optimization problem
323304
nvars = obj.n + 1 + 1; % M, var_f, var_n
324-
fun = @(vars) loglikelihood(obj,ip,vars);
305+
%fun = @(vars) loglikelihood(obj,ip,vars);
306+
307+
fun = @(vars) loglikelihood(obj,ip,... % output dim
308+
diag(10.^vars(1:end-2)),... % M
309+
10.^vars(end-1),... % var_f
310+
10.^vars(end)); % var_n
325311

326312
ub = [ 1e+5 * ones(obj.n,1);
327313
1e+5;
@@ -366,24 +352,18 @@ function optimizeHyperParams(obj, method)
366352
warning('on', 'MATLAB:singularMatrix')
367353
end
368354

369-
function logL = loglikelihood(obj,outdim,vars)
355+
function logL = loglikelihood(obj, outdim, M, var_f, var_n)
370356
%------------------------------------------------------------------
371-
% calculate the log likelihood: p(Y|X,theta),
372-
% where theta are the hyperparameters and (X,Y) the dictionary
373-
%------------------------------------------------------------------
374-
% variables in log10 space
375-
M = diag(10.^vars(1:end-2));
376-
var_f = 10.^vars(end-1);
377-
var_n = 10.^vars(end);
378-
357+
% calculate the log likelihood: log(p(Y|X,theta)),
358+
% where theta are the hyperparameters and (X,Y) the training data
359+
%------------------------------------------------------------------
379360
Y = obj.Y(:,outdim);
380361
K = var_f * exp( -0.5 * pdist2(obj.X',obj.X','mahalanobis',M).^2 );
381362
Ky = K + var_n*eye(obj.N);
382-
363+
% calculate log(p(Y|X,theta))
383364
logL = -(-0.5*Y'/Ky*Y -0.5*logdet(Ky) -obj.n/2*log(2*pi));
384365
end
385366

386-
387367
function plot2d(obj, truefun, varargin)
388368
%------------------------------------------------------------------
389369
% Make analysis of the GP quality (only for the first output dimension.
@@ -455,10 +435,10 @@ function plot2d(obj, truefun, varargin)
455435
figure('Color','w', 'Position', [-1827 27 550 420])
456436
%figure('Color','white','Position',[513 440 560 420]);
457437
hold on; grid on;
458-
s1 = surf(X1, X2, Ymean,'edgecolor',0.8*[1 1 1], 'EdgeAlpha', 0.3 ,'FaceColor', [153, 51, 255]/255)
438+
s1 = surf(X1, X2, Ymean,'edgecolor',0.8*[1 1 1], 'EdgeAlpha', 0.3 ,'FaceColor', [153, 51, 255]/255);
459439
s2 = surf(X1, X2, Ymean+sigmalevel*Ystd, Ystd, 'FaceAlpha',0.2,'EdgeAlpha',0.2, 'EdgeColor',0.4*[1 1 1]); %, 'FaceColor',0*[1 1 1])
460440
s3 = surf(X1, X2, Ymean-sigmalevel*Ystd, Ystd, 'FaceAlpha',0.2,'EdgeAlpha',0.2, 'EdgeColor',0.4*[1 1 1]); %, 'FaceColor',0*[1 1 1])
461-
p1 = scatter3(obj.X(1,:),obj.X(2,:),obj.Y(:,outdim),'filled','MarkerFaceColor','red')
441+
p1 = scatter3(obj.X(1,:),obj.X(2,:),obj.Y(:,outdim),'filled','MarkerFaceColor','red');
462442
title('mean\pm2*stddev Prediction Curves')
463443
xlabel('X1'); ylabel('X2');
464444
view(70,10)
@@ -510,17 +490,13 @@ function plot1d(obj, truthfun, varargin)
510490
% This function can only be called when the GP input is 1D
511491
%
512492
% args:
513-
% truthfun: anonymous function @(x) which returns the true function
514-
% varargin{1} = rangeX1:
515-
% varargin{2} = rangeX2: <1,2> range of X1 and X2 where the data
516-
% will be evaluated and ploted
493+
% see inputParser parameters
517494
%------------------------------------------------------------------
518495

519496
assert(obj.N>0, 'Dataset is empty. Aborting...')
520497
% we can not plot more than in 3D
521498
assert(obj.n==1, 'This function can only be used when dim(X)=1. Aborting...');
522499

523-
524500
%--------------------------------------------------------------
525501
% parse inputs
526502
%--------------------------------------------------------------
@@ -555,6 +531,10 @@ function plot1d(obj, truthfun, varargin)
555531
Ymean = mu';
556532
Ystd = sqrt(squeeze(var));
557533

534+
% prior
535+
%Ymean = 0*mu';
536+
%Ystd = sqrt(diag(obj.K(X,X)));
537+
558538
%--------------------------------------------------------------
559539
% Generate plots
560540
%--------------------------------------------------------------

‎README.md

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ and it is trained (hyperparameter optimization) by maximizing the log Likelihood
3636
### Results
3737

3838

39-
| MPC controller with Gaussian Process **deactivated** | MPC controller with **TRAINED** and **ACTIVATED** Gaussian Process |
39+
| NMPC controller with unmodelled dynamics | Adaptive NMPC controller (with trained Gaussian Process) |
4040
| ------------- |-------------|
4141
| <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"/> |
4242

‎main_invertedPendulum.m

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -236,10 +236,7 @@
236236

237237
%% Optimize GP hyperparameters ??? (Offline procedure, after simulation)
238238

239-
d_GP.M = M
240-
d_GP.var_f = var_f;
241-
d_GP.var_n = var_n;
242-
239+
d_GP.setHyperParameters( M, var_f, var_n )
243240
% d_GP.optimizeHyperParams('ga');
244241
d_GP.optimizeHyperParams('fmincon');
245242

‎test-files/test_GP.m

Lines changed: 14 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -26,52 +26,42 @@
2626
close all;
2727

2828
% test 1
29-
gp.M = 1e-2;
30-
gp.var_f = 2;
31-
gp.var_n = var_w;
32-
gp.updateModel();
29+
gp.setHyperParameters(1e-2, 2, var_w)
3330
gp.plot1d(truefun)
3431
xlim([-6 5]); ylim([-4 4]);
3532

3633
% test 2
37-
gp.M = 50;
38-
gp.var_f = 100;
39-
gp.var_n = var_w;
40-
gp.updateModel();
34+
gp.setHyperParameters(50, 100, var_w)
4135
gp.plot1d(truefun)
4236
xlim([-6 5]); ylim([-4 4]);
4337

4438
% test 3
45-
gp.M = 1e-3;
46-
gp.var_f = 1e-2;
47-
gp.var_n = var_w;
48-
gp.updateModel();
39+
gp.setHyperParameters(1e-3, 1e-2, var_w)
4940
gp.plot1d(truefun)
5041
xlim([-6 5]); ylim([-4 4]);
5142

5243
% test 4
53-
gp.M = 50;
54-
gp.var_f = 1e7;
55-
gp.var_n = var_w;
56-
gp.updateModel();
44+
gp.setHyperParameters(50, 1e7, var_w)
5745
gp.plot1d(truefun)
5846
xlim([-6 5]); ylim([-4 4]);
5947

6048
% test 4
61-
gp.M = 100^2;
62-
gp.var_f = 100^2;
63-
gp.var_n = 1^2;
64-
gp.updateModel();
49+
gp.setHyperParameters(100^2, 100^2, 1^2)
6550
gp.plot1d(truefun)
6651
xlim([-6 5]); ylim([-4 4]);
6752

6853
%% optimize and plot
6954

70-
gp.M = 0.1^2;
71-
gp.var_f = 0.1^2;
72-
gp.var_n = 1e-8;
73-
55+
gp.setHyperParameters(0.1^2, 0.1^2, 1e-8)
7456
% gp.optimizeHyperParams('ga');
7557
gp.optimizeHyperParams('fmincon');
58+
59+
close all
7660
gp.plot1d(truefun)
7761
xlim([-6 5]); ylim([-4 4]);
62+
63+
%%
64+
close all
65+
gp.setHyperParameters(1.5^2, 2^2, 4.0228e-09)
66+
gp.plot1d(truefun)
67+
xlim([-6 5]); ylim([-5 5]);

0 commit comments

Comments
 (0)