Skip to content

Commit e9a7461

Browse files
LuziaKnLuziaKn
committed
changing back to original disturbance
1 parent e8a8d39 commit e9a7461

6 files changed

+295
-24
lines changed

‎GP.m

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -503,6 +503,104 @@ function plot2d(obj, truthfun, varargin)
503503
colormap(gcf,parula);
504504
end
505505

506+
function plotNd(obj, truthfun, varargin)
507+
%------------------------------------------------------------------
508+
% Make analysis of the GP quality (only for the first output dimension.
509+
% This function can only be called when the GP input is 2D
510+
%
511+
% args:
512+
% truthfun: anonymous function @(x) which returns the true function
513+
% varargin{1} = rangeX1:
514+
% varargin{2} = rangeX2: <1,2> range of X1 and X2 where the data
515+
% will be evaluated and ploted
516+
%------------------------------------------------------------------
517+
% output dimension to be analyzed
518+
pi = 1;
519+
520+
assert(obj.N>0, 'Dataset is empty. Aborting...')
521+
% we can not plot more than in 3D
522+
assert(obj.n==2, 'This function can only be used when dim(X)=2. Aborting...');
523+
524+
% Generate grid where the mean and variance will be calculated
525+
if numel(varargin) ~= 2
526+
factor = 0.3;
527+
rangeX1 = [ min(obj.X(1,:)) - factor*range(obj.X(1,:)), ...
528+
max(obj.X(1,:)) + factor*range(obj.X(1,:)) ];
529+
rangeX2 = [ min(obj.X(2,:)) - factor*range(obj.X(2,:)), ...
530+
max(obj.X(2,:)) + factor*range(obj.X(2,:)) ];
531+
else
532+
rangeX1 = varargin{1};
533+
rangeX2 = varargin{2};
534+
end
535+
536+
% generate grid
537+
[X1,X2] = meshgrid(linspace(rangeX1(1),rangeX1(2),100),...
538+
linspace(rangeX2(1),rangeX2(2),100));
539+
Ytrue = zeros('like',X1);
540+
Ystd = zeros('like',X1);
541+
Ymean = zeros('like',X1);
542+
for i=1:size(X1,1)
543+
for j=1:size(X1,2)
544+
% evaluate true function
545+
mutrue = truthfun([X1(i,j);X2(i,j)]);
546+
Ytrue(i,j) = mutrue(pi); % select desired output dim
547+
% evaluate GP model
548+
[mu,var] = obj.eval([X1(i,j);X2(i,j)],true);
549+
if var < 0
550+
error('GP obtained a negative variance... aborting');
551+
end
552+
Ystd(i,j) = sqrt(var);
553+
Ymean(i,j) = mu(:,pi); % select desired output dim
554+
end
555+
end
556+
557+
% plot data points, and +-2*stddev surfaces
558+
figure('Color','w')
559+
hold on; grid on;
560+
% surf(X1,X2,Y, 'FaceAlpha',0.3)
561+
surf(X1,X2,Ymean+2*Ystd ,Ystd, 'FaceAlpha',0.3)
562+
surf(X1,X2,Ymean-2*Ystd,Ystd, 'FaceAlpha',0.3)
563+
scatter3(obj.X(1,:),obj.X(2,:),obj.Y(:,pi),'filled','MarkerFaceColor','red')
564+
title('mean\pm2*stddev Prediction Curves')
565+
shading interp;
566+
colormap(gcf,jet);
567+
view(30,30)
568+
569+
% Comparison between true and prediction mean
570+
figure('Color','w')
571+
subplot(1,2,1); hold on; grid on;
572+
surf(X1,X2,Ytrue, 'FaceAlpha',.8, 'EdgeColor', 'none', 'DisplayName', 'True function');
573+
% surf(X1,X2,Ymean, 'FaceAlpha',.5, 'FaceColor','g', 'EdgeColor', 'none', 'DisplayName', 'Prediction mean');
574+
scatter3(obj.X(1,:),obj.X(2,:),obj.Y(:,pi),'filled','MarkerFaceColor','red', 'DisplayName', 'Sample points')
575+
zlim([ min(obj.Y(:,pi))-range(obj.Y(:,pi)),max(obj.Y(:,pi))+range(obj.Y(:,pi)) ]);
576+
legend;
577+
xlabel('X1'); ylabel('X2');
578+
title('True Function')
579+
view(24,12)
580+
subplot(1,2,2); hold on; grid on;
581+
% surf(X1,X2,Y, 'FaceAlpha',.5, 'FaceColor','b', 'EdgeColor', 'none', 'DisplayName', 'True function');
582+
surf(X1,X2,Ymean, 'FaceAlpha',.8, 'EdgeColor', 'none', 'DisplayName', 'Prediction mean');
583+
scatter3(obj.X(1,:),obj.X(2,:),obj.Y(:,pi),'filled','MarkerFaceColor','red', 'DisplayName', 'Sample points')
584+
zlim([ min(obj.Y(:,pi))-range(obj.Y(:,pi)),max(obj.Y(:,pi))+range(obj.Y(:,pi)) ]);
585+
legend;
586+
xlabel('X1'); ylabel('X2');
587+
title('Prediction Mean')
588+
view(24,12)
589+
590+
% plot bias and variance
591+
figure('Color','w')
592+
subplot(1,2,1); hold on; grid on;
593+
contourf(X1,X2, abs(Ymean-Ytrue), 50,'LineColor','none')
594+
title('Absolute Prediction Bias')
595+
colorbar;
596+
scatter(obj.X(1,:),obj.X(2,:),'filled','MarkerFaceColor','red')
597+
subplot(1,2,2); hold on; grid on;
598+
contourf(X1,X2, Ystd.^2, 50 ,'LineColor','none')
599+
title('Prediction Variance')
600+
colorbar;
601+
scatter(obj.X(1,:),obj.X(2,:),'filled','MarkerFaceColor','red')
602+
colormap(gcf,parula);
603+
end
506604

507605
function plot1d(obj, truthfun, varargin)
508606
%------------------------------------------------------------------

‎MotionModelGP_InvPendulum_deffect.m

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@
4343
xdot = f @ MotionModelGP_InvPendulum_nominal(obj,x,u);
4444

4545
% add deffect
46-
%xdot(3) = xdot(3) + (0.1 * x(3) - 0.01*x(4) + deg2rad(3)) *10;
46+
xdot(3) = xdot(3) + (0.1 * x(3) - 0.01*x(4) + deg2rad(3)) *10;
4747

4848
% xdot(2) = xdot(2) + ( -0.1 * x(1) + deg2rad(3));
4949
end

‎MotionModelGP_InvPendulum_nominal.asv

Lines changed: 173 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,173 @@
1+
%------------------------------------------------------------------
2+
% Programed by:
3+
% - Lucas Rath ([email protected])
4+
% -
5+
% -
6+
%------------------------------------------------------------------
7+
8+
9+
classdef MotionModelGP_InvPendulum_nominal < MotionModelGP
10+
%--------------------------------------------------------------------------
11+
% xk+1 = fd(xk,uk) + Bd * ( d(zk) + w ),
12+
%
13+
% where: zk = [Bz_x*xk ; Bz_u*uk],
14+
% d ~ N(mean_d(zk),var_d(zk))
15+
% w ~ N(0,sigmaw)
16+
%
17+
%
18+
% x = [s, ds, th, dth]' carriage position and pole angle (and derivatives)
19+
% u = [F]' force on the carriage and torque on the pole joint
20+
%
21+
%--------------------------------------------------------------------------
22+
23+
properties
24+
Mc % mass of the carriage
25+
Mp % mass of the pole
26+
b % friction coefficient between the carriage and the floor
27+
I % inertia matrix of the pole CG
28+
l % pole length
29+
g = 9.8
30+
end
31+
32+
properties(Constant)
33+
% keep in mind the dimensions: xk+1 = fd(xk,uk) + Bd*(d(z)+w)),
34+
% where z = [Bz_x*x;Bz_u*u] and x = [s, ds, th, dth]'
35+
36+
Bz_x = [0 0 1 0
37+
0 0 0 1]
38+
Bz_u = [];
39+
Bd = [0; % xk+1 = fd(xk,uk) + Bd*d(zk)
40+
0;
41+
1;
42+
0]
43+
44+
n = 4 % number of outputs x(t)
45+
m = 1 % number of inputs u(t)
46+
nz = size(Bz_x,1) % dimension of z(t)
47+
nd = size(Bd,2) % output dimension of d(z)
48+
end
49+
50+
51+
methods
52+
53+
function obj = MotionModelGP_InvPendulum_nominal (Mc, Mp, b, I, l, d, sigmaw)
54+
%------------------------------------------------------------------
55+
% object constructor
56+
%------------------------------------------------------------------
57+
% call superclass constructor
58+
obj = obj @ MotionModelGP(d,sigmaw);
59+
% store parameters
60+
obj.Mc = Mc;
61+
obj.Mp = Mp;
62+
obj.b = b;
63+
obj.I = I;
64+
obj.l = l;
65+
66+
% add folder CODEGEN to path. Here there will be some functions
67+
% generated with the method generate_invertedPendulum_functions()
68+
addpath(fullfile(pwd,'CODEGEN'))
69+
end
70+
71+
function xdot = f (obj, x, u)
72+
%------------------------------------------------------------------
73+
% Continuous time dynamics.
74+
% out:
75+
% xdot: <n,1> time derivative of x given x and u
76+
%------------------------------------------------------------------
77+
params = [obj.Mc obj.Mp obj.I obj.g obj.l obj.b]';
78+
xdot = invertedPendulum_f(x, u, params );
79+
end
80+
81+
function gradx = gradx_f(obj, x, u)
82+
%------------------------------------------------------------------
83+
% Continuous time dynamics.
84+
% out:
85+
% gradx: <n,n> gradient of xdot w.r.t. x
86+
%------------------------------------------------------------------
87+
params = [obj.Mc obj.Mp obj.I obj.g obj.l obj.b]';
88+
gradx = invertedPendulum_gradx_f(x, u, params );
89+
end
90+
91+
function gradu = gradu_f(obj, x, u)
92+
%------------------------------------------------------------------
93+
% Continuous time dynamics.
94+
% out:
95+
% gradu: <m,n> gradient of xdot w.r.t. u
96+
%------------------------------------------------------------------
97+
params = [obj.Mc obj.Mp obj.I obj.g obj.l obj.b]';
98+
gradu = invertedPendulum_gradu_f(x, u, params );
99+
end
100+
101+
function [A,B] = linearize (obj)
102+
%------------------------------------------------------------------
103+
% Return continuous time linearized model parameters A,B
104+
% xdot = A*x + B*u
105+
%------------------------------------------------------------------
106+
Mc=obj.Mc; Mp=obj.Mp; b=obj.b; I=obj.I; l=obj.l; g=obj.g;
107+
p = I*(Mc+Mp)+Mc*Mp*l^2;
108+
A = [0 1 0 0;
109+
0 -(I+Mp*l^2)*b/p (Mp^2*g*l^2)/p 0;
110+
0 0 0 1;
111+
0 -(Mp*l*b)/p Mp*g*l*(Mc+Mp)/p 0];
112+
B = [ 0;
113+
(I+Mp*l^2)/p;
114+
0;
115+
Mp*l/p];
116+
end
117+
end
118+
119+
methods(Static)
120+
function generate_invertedPendulum_functions()
121+
%------------------------------------------------------------------
122+
% Generate continuous time dynamics equations of the inverted
123+
% pendulum:. This function generates three functions:
124+
% xdot = f(x,u) - dynamics
125+
% gradx_xdot(x,u) - gradient of xdot w.r.t. x
126+
% gradu_xdot(x,u) - gradient of xdot w.r.t. u
127+
%
128+
% (Mc+Mp)*dds + b*ds + Mp*l/2*ddth*cos(th) - Mp*l/2*dth^2*sin(th) = F
129+
% (I+Mp*(l/2)^2)*ddth + Mp*g*l/2*sin(th) + Mp*l*dds*cos(th) = T
130+
%
131+
% x = [s, ds, th, dth]'
132+
% u = [F]'
133+
%
134+
%
135+
% Example how to run this function:
136+
% ip = MotionModelGP_InvertedPendulum(5, 2, 0.1, 0.6, 3, @(z)deal(0,0), 0);
137+
% ip.generate_invertedPendulum_functions();
138+
%------------------------------------------------------------------
139+
syms g Mc Mp b I l F T s ds dds th dth ddth real
140+
T = 0; % we are not using this input for now
141+
fzero = [(Mc+Mp)*dds + b*ds + Mp*l/2*ddth*cos(th) - Mp*l/2*dth^2*sin(th) - F ;
142+
(I+Mp*(l/2)^2)*ddth + Mp*g*l/2*sin(th) + Mp*l*dds*cos(th) - T ];
143+
sol = solve(fzero,[dds,ddth]);
144+
dds = simplify(sol.dds);
145+
ddth = simplify(sol.ddth);
146+
147+
u = F;
148+
x = [s, ds, th, dth]';
149+
xdot = [ds, dds, dth, ddth]';
150+
params = [Mc Mp I g l b ]';
151+
152+
153+
folder = fullfile(pwd,'CODEGEN');
154+
if ~exist(folder,'dir')
155+
mkdir(folder);
156+
end
157+
addpath(folder)
158+
159+
160+
matlabFunction( xdot, 'Vars', {x;u;params} ,'File', fullfile('CODEGEN','invertedPendulum_f') );
161+
162+
gradx_f = simplify(jacobian(xdot,x)');
163+
matlabFunction( gradx_f, 'Vars', {x;u;params} ,'File', fullfile('CODEGEN','invertedPendulum_gradx_f') );
164+
165+
gradu_f = simplify(jacobian(xdot,u)');
166+
matlabFunction( gradu_f, 'Vars', {x;u;params} ,'File', fullfile('CODEGEN','invertedPendulum_gradu_f') );
167+
168+
disp('FINISHED! functions invertedPendulum_f, invertedPendulum_gradx_f and invertedPendulum_gradu_f generated!!')
169+
170+
end
171+
end
172+
173+
end

‎MotionModelGP_InvPendulum_nominal.m

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -33,18 +33,18 @@
3333
% keep in mind the dimensions: xk+1 = fd(xk,uk) + Bd*(d(z)+w)),
3434
% where z = [Bz_x*x;Bz_u*u] and x = [s, ds, th, dth]'
3535

36-
Bz_x = [0 1 0 0
36+
Bz_x = [0 0 1 0
3737
0 0 0 1]
3838
Bz_u = [];
39-
Bd = [0 0 ; % xk+1 = fd(xk,uk) + Bd*d(zk)
40-
1 0;
41-
0 0;
42-
0 1]
39+
Bd = [0; % xk+1 = fd(xk,uk) + Bd*d(zk)
40+
0;
41+
1;
42+
0]
4343

4444
n = 4 % number of outputs x(t)
4545
m = 1 % number of inputs u(t)
46-
nz = 2 % dimension of z(t)
47-
nd = 2 % output dimension of d(z)
46+
nz = 2 % dimension of z(t)
47+
nd = 1 % output dimension of d(z)
4848
end
4949

5050

‎main_invertedPendulum.asv

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -24,12 +24,12 @@ maxiter = 15; % max NMPC iterations per time step
2424
N = 10; % NMPC prediction horizon
2525

2626

27-
loadPreTrainedGP = false;
27+
loadPreTrainedGP = true;
2828
GPfile = fullfile(pwd,'/simresults/20-01-17-out-GP-inverted-pendulum.mat');
29-
useGP = false;
29+
useGP = true;
3030
trainGPonline = true;
3131
useParallel = false;
32-
optimizeHyperparameters = true;
32+
optimizeHyperparameters = false;
3333

3434

3535
lookahead = dt*N;
@@ -58,10 +58,10 @@ clearvars d_GP
5858
%------------------------------------------------------------------
5959

6060
% define noise for true disturbance
61-
var_w = diag([0,0]);
61+
var_w = diag([0 0 0 0]);
6262

6363
% create true dynamics model
64-
trueModel = MotionModelGP_InvPendulum_deffect(Mc*0.9, Mp, b*5 , I*0.5, l*0.5, [], var_w);
64+
trueModel = MotionModelGP_InvPendulum_deffect(Mc, Mp, b , I, l*2, [], var_w);
6565

6666
%% Create Estimation Model and Nominal Model
6767

@@ -72,7 +72,7 @@ trueModel = MotionModelGP_InvPendulum_deffect(Mc*0.9, Mp, b*5 , I*0.5, l*0.5, []
7272

7373
% create nominal dynamics model (no disturbance)
7474
nomModel = MotionModelGP_InvPendulum_nominal(Mc, Mp, b, I, l, [], []);
75-
nomModel = trueModel;
75+
%nomModel = trueModel;
7676

7777
% -------------------------------------------------------------------------
7878
% Create adaptive dynamics model
@@ -86,9 +86,9 @@ gp_n = MotionModelGP_InvPendulum_nominal.nz;
8686
gp_p = MotionModelGP_InvPendulum_nominal.nd;
8787

8888
% GP hyperparameters
89-
var_f = [0.01,0.01]'; % output variance
90-
M = repmat(diag([1e0,1e0].^2),[1,1,gp_p]); % length scale
91-
var_n = [1e-8,1e-8]'; % measurement noise variance
89+
var_f = repmat(0.01,[gp_p,1]); % output variance
90+
M = repmat(diag(repmat(1e0,[1,gp_n]).^2),[1,1,gp_p]); % length scale
91+
var_n = repmat(1e-8,[gp_p,1]); % measurement noise variance
9292
maxsize = 100; % maximum number of points in the dictionary
9393

9494
% create GP object
@@ -100,7 +100,7 @@ end
100100

101101
% create estimation dynamics model (disturbance is the Gaussian Process GP)
102102
estModel = MotionModelGP_InvPendulum_nominal(Mc, Mp, b, I, l, @d_GP.eval, var_w);
103-
% estModel = trueModel;
103+
%estModel = trueModel;
104104

105105

106106
%% Controller
@@ -258,7 +258,7 @@ d_GP.M = M
258258
d_GP.var_f = var_f;
259259
d_GP.var_n = var_n;
260260

261-
% d_GP.optimizeHyperParams('ga');
261+
%d_GP.optimizeHyperParams('ga');
262262
d_GP.optimizeHyperParams('fmincon');
263263

264264
d_GP.M

0 commit comments

Comments
 (0)