length scale covariance
27 | var_f % signal/output covariance
28 | var_n %
evaluation noise covariance
29 |
30 | % dictionary: [X,Y]
31 | X % input dataset
32 | Y % output dataset
33 |
34 | N % <1> dictionary size
35 | Nmax % <1> max dictionary size
36 |
37 | n % <1> input dimension
38 | p % <1> output dimension
39 |
40 | % aux variables
41 | L % : chol(obj.KXX + sigman^2 * I,'lower');
42 | alpha % : L'\(L\(Y-muX));
43 | % (DEPRECATED) inv_KXX_sn %
44 |
45 | % isOutdated = false % model is outdated if data has been added withouth updating L and alpha matrices
46 | % isOptimized = false % true if model had its kernel parameters optimized and no new data has been added
47 | end
48 |
49 | methods
50 | function obj = GP(n, p, var_f, var_n, M, maxsize)
51 | %------------------------------------------------------------------
52 | % GP constructor
53 | % args:
54 | % n: <1> input dimension
55 | % p: <1> output dimension
56 | % var_f: signal/output covariance
57 | % var_n:
evaluation noise covariance
58 | % M: length scale covariance matrix
59 | % maxsize: <1> maximum dictionary size
60 | %------------------------------------------------------------------
61 | obj.n = n;
62 | obj.p = p;
63 | obj.X = [];
64 | obj.Y = [];
65 | obj.Nmax = maxsize;
66 | obj.setHyperParameters( M, var_f, var_n )
67 | end
68 |
69 |
70 | function bool = isfull(obj)
71 | %------------------------------------------------------------------
72 | % is dictionary full?
73 | %------------------------------------------------------------------
74 | bool = obj.N >= obj.Nmax;
75 | end
76 |
77 | function N = get.N(obj)
78 | %------------------------------------------------------------------
79 | % return dictionary size = N
80 | %------------------------------------------------------------------
81 | N = size(obj.X,2);
82 | end
83 |
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)==<%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)==<%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)=
=<%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
96 |
97 | function mean = mu(~,x)
98 | %------------------------------------------------------------------
99 | % zero mean function: mean[f(x)] = 0
100 | % args:
101 | % x:
102 | % out:
103 | % mean:
104 | %------------------------------------------------------------------
105 | mean = zeros(size(x,2),1);
106 | end
107 |
108 |
109 | function kernel = K(obj, x1, x2)
110 | %------------------------------------------------------------------
111 | % SEQ kernel function: cov[f(x1),f(x2)]
112 | % k(x1,x2) = var_f * exp( 0.5 * ||x1-x2||^2_M )
113 | %
114 | % args:
115 | % x1:
116 | % x2:
117 | % out:
118 | % kernel:
119 | %------------------------------------------------------------------
120 | kernel = zeros(size(x1,2),size(x2,2),obj.p);
121 | for pi = 1:obj.p
122 | D = pdist2(x1',x2','mahalanobis',obj.M(:,:,pi)).^2;
123 | %D = pdist2(x1',x2','seuclidean',diag((obj.M).^0.5)).^2;
124 | kernel(:,:,pi) = obj.var_f(pi) * exp( -0.5 * D );
125 | end
126 | end
127 |
128 |
129 | function updateModel(obj)
130 | % -----------------------------------------------------------------
131 | % Update precomputed matrices L and alpha, that will be used when
132 | % evaluating new points. See [Rasmussen, pg19].
133 | % -----------------------------------------------------------------
134 | % nothing to update... return
135 | if obj.N == 0
136 | return;
137 | end
138 | % store cholesky L and alpha matrices
139 | I = eye(obj.N);
140 |
141 | % for each output dimension
142 | obj.alpha = zeros(obj.N,obj.p);
143 | obj.L = zeros(obj.N,obj.N);
144 | K = obj.K(obj.X,obj.X);
145 | for pi=1:obj.p
146 | obj.L(:,:,pi) = chol(K(:,:,pi)+ obj.var_n(pi) * I ,'lower');
147 | % sanity check: norm( L*L' - (obj.K(obj.X,obj.X) + obj.var_n*I) ) < 1e-12
148 | obj.alpha(:,pi) = obj.L(:,:,pi)'\(obj.L(:,:,pi)\(obj.Y(:,pi)-obj.mu(obj.X)));
149 | end
150 |
151 | %-------------------- (DEPRECATED) ------------------------
152 | % % SLOW BUT RETURNS THE FULL COVARIANCE MATRIX INSTEAD OF ONLY THE DIAGONAL (VAR)
153 | % % precompute inv(K(X,X) + sigman^2*I)
154 | % I = eye(obj.N);
155 | % obj.inv_KXX_sn = inv( obj.K(obj.X,obj.X) + obj.var_n * I );
156 | %-------------------- (DEPRECATED) ------------------------
157 | end
158 |
159 |
160 | function add(obj, X, Y)
161 | %------------------------------------------------------------------
162 | % Add new data points [X,Y] to the dictionary
163 | %
164 | % args:
165 | % X:
166 | % Y:
167 | %------------------------------------------------------------------
168 | OPTION = 'B'; % {'A','B'}
169 |
170 | assert(size(Y,2) == obj.p, ...
171 | sprintf('Y should have %d columns, but has %d. Dimension does not agree with the specified kernel parameters',obj.p,size(Y,2)));
172 | assert(size(X,1) == obj.n, ...
173 | sprintf('X should have %d rows, but has %d. Dimension does not agree with the specified kernel parameters',obj.n,size(X,1)));
174 |
175 | Ntoadd = size(X,2);
176 | Nextra = obj.N + Ntoadd - obj.Nmax;
177 |
178 | % if there is space enough to append the new data points, then
179 | if Nextra <= 0
180 | obj.X = [obj.X, X];
181 | obj.Y = [obj.Y; Y];
182 |
183 | % data overflow: dictionary will be full. we need to select
184 | % relevant points
185 | else
186 | Nthatfit = obj.Nmax - obj.N;
187 |
188 | % make dictionary full
189 | obj.X = [obj.X, X(:,1:Nthatfit) ];
190 | obj.Y = [obj.Y; Y(1:Nthatfit,:) ];
191 | obj.updateModel();
192 |
193 | % points left to be added
194 | X = X(:,Nthatfit+1:end);
195 | Y = Y(Nthatfit+1:end,:);
196 |
197 | % OPTION A)
198 | % The closest (euclidian dist.) points will be iteratively removed
199 | if strcmp(OPTION,'A')
200 | for i=1:Nextra
201 | D = pdist2(obj.X',X(:,i)','euclidean').^2;
202 | [~,idx_rm] = min(D);
203 | idx_keep = 1:obj.N ~= idx_rm;
204 |
205 | obj.X = [obj.X(:,idx_keep), X(:,i)];
206 | obj.Y = [obj.Y(idx_keep,:); Y(i,:)];
207 | end
208 |
209 | % OPTION B)
210 | % the point with lowest variance will be removed
211 | elseif strcmp(OPTION,'B')
212 | X_all = [obj.X,X];
213 | Y_all = [obj.Y;Y];
214 |
215 | [~, var_y] = obj.eval( X_all, true);
216 | [~,idx_keep] = maxk(sum(reshape(var_y, obj.p^2, obj.N+Nextra )),obj.Nmax);
217 |
218 | obj.X = X_all(:,idx_keep);
219 | obj.Y = Y_all(idx_keep,:);
220 | else
221 | error('Option not implemented');
222 | end
223 | end
224 | % update pre-computed matrices
225 | obj.updateModel();
226 | end
227 |
228 |
229 | function [mu_y, var_y] = eval(obj, x, varargin)
230 | %------------------------------------------------------------------
231 | % Evaluate GP at the points x
232 | % This is a fast implementation of [Rasmussen, pg19]
233 | %
234 | % args:
235 | % x: point coordinates
236 | % varargin:
237 | % true: force calculation of mean and variance even if GP is inactive
238 | % out:
239 | % muy: E[Y] = E[gp(x)]
240 | % vary:
Var[Y] = Var[gp(x)]
241 | %------------------------------------------------------------------
242 | assert(size(x,1)==obj.n, sprintf('Input vector has %d columns but should have %d !!!',size(x,1),obj.n));
243 |
244 | % calculate mean and variance even if GP is inactive
245 | forceActive = length(varargin)>=1 && varargin{1}==true;
246 |
247 | Nx = size(x,2); % size of dataset to be evaluated
248 |
249 | % if there is no data in the dictionary or GP is not active
250 | % then return prior (for now returning zero variance)
251 | if obj.N == 0 || (~obj.isActive && ~forceActive)
252 | mu_y = repmat(obj.mu(x),[1,obj.p])';
253 | var_y = zeros(obj.p,obj.p,Nx);
254 | return;
255 | end
256 |
257 | % in case the matrices alpha and L are empty we need to update the model
258 | if isempty(obj.alpha) || isempty(obj.L)
259 | obj.updateModel();
260 | end
261 |
262 | % Calculate posterior mean mu_y for each output dimension
263 | KxX = obj.K(x,obj.X);
264 | mu_y = zeros(obj.p,Nx);
265 | for pi=1:obj.p
266 | mu_y(pi,:) = obj.mu(x) + KxX(:,:,pi) * obj.alpha(:,pi);
267 | end
268 |
269 | % Calculate posterior covariance var_y
270 | var_y = zeros(obj.p,obj.p,Nx);
271 | for pi=1:obj.p
272 | for i=1:Nx
273 | % (less efficient) v = obj.L\obj.K(x(:,i),obj.X)';
274 | v = obj.L(:,:,pi)\KxX(i,:,pi)';
275 | K = obj.K(x(:,i),x(:,i));
276 | var_y(pi,pi,i) = K(:,:,pi) - v'*v;
277 | end
278 | end
279 |
280 | % --------------------- (DEPRECATED) -------------------------
281 | % % SLOW BUT RETURNS THE FULL COVARIANCE MATRIX INSTEAD OF ONLY THE DIAGONAL (VAR)
282 | % KxX = obj.K(x,obj.X);
283 | % muy = obj.mu(x) + KxX * obj.inv_KXX_sn * (obj.Y-obj.mu(obj.X));
284 | % vary = obj.K(x,x) - KxX * obj.inv_KXX_sn * KxX';
285 | % --------------------- (DEPRECATED) -------------------------
286 | end
287 |
288 |
289 | function optimizeHyperParams(obj, method)
290 | %------------------------------------------------------------------
291 | % Optimize kernel hyper-parameters based on the current dictionary
292 | % Method: maximum log likelihood (See Rasmussen's book Sec. 5.4.1)
293 | %------------------------------------------------------------------
294 |
295 | warning('off', 'MATLAB:nearlySingularMatrix')
296 | warning('off', 'MATLAB:singularMatrix')
297 |
298 | obj.updateModel();
299 |
300 | % error('not yet implemented!!!');
301 | for ip = 1:obj.p
302 |
303 | % set optimization problem
304 | nvars = obj.n + 1 + 1; % M, var_f, var_n
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
311 |
312 | ub = [ 1e+5 * ones(obj.n,1);
313 | 1e+5;
314 | 1e+5 ];
315 | lb = [ 1e-8 * ones(obj.n,1);
316 | 0*1e-8;
317 | 1e-10 ];
318 | x0 = [ diag(obj.M(:,:,ip)); obj.var_f(ip); obj.var_n(ip); ];
319 |
320 | % convert to log10 space
321 | ub = log10(ub);
322 | lb = log10(lb);
323 | x0 = log10(x0);
324 |
325 | % use genetic algorithm or interior-point-method
326 | if strcmp(method, 'ga')
327 | options = optimoptions('ga',...
328 | 'ConstraintTolerance',1e-6,...
329 | 'PlotFcn', @gaplotbestf,...
330 | 'Display','iter',...
331 | 'UseParallel',false);
332 | opt_vars = ga(fun,nvars,[],[],[],[],lb,ub,[],[],options);
333 | elseif strcmp(method,'fmincon')
334 | options = optimoptions('fmincon', ...
335 | 'PlotFcn','optimplotfval',...
336 | 'Display','iter');
337 | [opt_vars,~] = fmincon(fun,x0,[],[],[],[],lb,ub,[],options);
338 | else
339 | error('Method %s not implemented, please choose an existing method',method);
340 | end
341 |
342 | % retrieve optimal results to absolute scale
343 | obj.M(:,:,ip) = diag( 10.^opt_vars(1:end-2) );
344 | obj.var_f(ip) = 10.^opt_vars(end-1);
345 | obj.var_n(ip) = 10.^opt_vars(end);
346 | end
347 |
348 | % update matrices alpha and L
349 | obj.updateModel();
350 |
351 | warning('on', 'MATLAB:nearlySingularMatrix')
352 | warning('on', 'MATLAB:singularMatrix')
353 | end
354 |
355 | function logL = loglikelihood(obj, outdim, M, var_f, var_n)
356 | %------------------------------------------------------------------
357 | % calculate the negative log likelihood: -log(p(Y|X,theta)),
358 | % where theta are the hyperparameters and (X,Y) the training data
359 | %------------------------------------------------------------------
360 | Y = obj.Y(:,outdim);
361 | K = var_f * exp( -0.5 * pdist2(obj.X',obj.X','mahalanobis',M).^2 );
362 | Ky = K + var_n*eye(obj.N);
363 | % calculate log(p(Y|X,theta))
364 | logL = -(-0.5*Y'/Ky*Y -0.5*logdet(Ky) -obj.n/2*log(2*pi));
365 | end
366 |
367 | function plot2d(obj, truefun, varargin)
368 | %------------------------------------------------------------------
369 | % Make analysis of the GP quality (only for the first output dimension.
370 | % This function can only be called when the GP input is 2D
371 | %
372 | % args:
373 | % truthfun: anonymous function @(x) which returns the true function
374 | % varargin{1} = rangeX1:
375 | % varargin{2} = rangeX2: <1,2> range of X1 and X2 where the data
376 | % will be evaluated and ploted
377 | %------------------------------------------------------------------
378 |
379 | assert(obj.N>0, 'Dataset is empty. Aborting...')
380 | assert(obj.n==2, 'This function can only be used when dim(X)=2. Aborting...');
381 |
382 | %--------------------------------------------------------------
383 | % parse inputs
384 | %--------------------------------------------------------------
385 | p = inputParser;
386 |
387 | addParameter(p,'factor',0.3);
388 | addParameter(p,'outdim',1);
389 | addParameter(p,'npoints',50);
390 | addParameter(p,'sigmalevel',2);
391 | parse(p,varargin{:});
392 |
393 | factor = p.Results.factor;
394 | outdim = p.Results.outdim;
395 | npoints = p.Results.npoints;
396 | sigmalevel = p.Results.sigmalevel;
397 |
398 | addParameter(p,'rangeX1', minmax(obj.X(1,:)) + [-1 1]*factor*range(obj.X(1,:)) );
399 | addParameter(p,'rangeX2', minmax(obj.X(2,:)) + [-1 1]*factor*range(obj.X(2,:)) );
400 | parse(p,varargin{:});
401 |
402 | rangeX1 = p.Results.rangeX1;
403 | rangeX2 = p.Results.rangeX2;
404 |
405 | %--------------------------------------------------------------
406 | % Evaluate Ytrue, Ymean and Ystd
407 | %--------------------------------------------------------------
408 |
409 | % generate grid
410 | [X1,X2] = meshgrid(linspace(rangeX1(1),rangeX1(2),npoints),...
411 | linspace(rangeX2(1),rangeX2(2),npoints));
412 | Ytrue = zeros('like',X1);
413 | Ystd = zeros('like',X1);
414 | Ymean = zeros('like',X1);
415 | for i=1:size(X1,1)
416 | for j=1:size(X1,2)
417 | % evaluate true function
418 | mutrue = truefun([X1(i,j);X2(i,j)]);
419 | Ytrue(i,j) = mutrue(outdim); % select desired output dim
420 | % evaluate GP model
421 | [mu,var] = obj.eval([X1(i,j);X2(i,j)],true);
422 | if var < 0
423 | error('GP obtained a negative variance... aborting');
424 | end
425 | Ystd(i,j) = sqrt(var);
426 | Ymean(i,j) = mu(:,outdim); % select desired output dim
427 | end
428 | end
429 |
430 | %--------------------------------------------------------------
431 | % Generate plots
432 | %--------------------------------------------------------------
433 |
434 | % plot data points, and +-2*stddev surfaces
435 | figure('Color','w', 'Position', [-1827 27 550 420])
436 | %figure('Color','white','Position',[513 440 560 420]);
437 | hold on; grid on;
438 | s1 = surf(X1, X2, Ymean,'edgecolor',0.8*[1 1 1], 'EdgeAlpha', 0.3 ,'FaceColor', [153, 51, 255]/255);
439 | 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])
440 | 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])
441 | p1 = scatter3(obj.X(1,:),obj.X(2,:),obj.Y(:,outdim),'filled','MarkerFaceColor','red');
442 | title('mean\pm2*stddev Prediction Curves')
443 | xlabel('X1'); ylabel('X2');
444 | view(70,10)
445 | colormap(gcf,jet);
446 |
447 |
448 | % Comparison between true and prediction mean
449 | figure('Color','w', 'Position',[-1269 32 1148 423])
450 | subplot(1,2,1); hold on; grid on;
451 | surf(X1,X2,Ytrue, 'FaceAlpha',.8, 'EdgeColor', 'none', 'DisplayName', 'True function');
452 | % surf(X1,X2,Ymean, 'FaceAlpha',.5, 'FaceColor','g', 'EdgeColor', 'none', 'DisplayName', 'Prediction mean');
453 | scatter3(obj.X(1,:),obj.X(2,:),obj.Y(:,outdim),'filled','MarkerFaceColor','red', 'DisplayName', 'Sample points')
454 | zlim( minmax(obj.Y(:,outdim)') +[-1 1]*range(obj.Y(:,outdim)) );
455 | legend;
456 | title('True Function')
457 | xlabel('X1'); ylabel('X2');
458 | view(-60,17)
459 | subplot(1,2,2); hold on; grid on;
460 | % surf(X1,X2,Y, 'FaceAlpha',.5, 'FaceColor','b', 'EdgeColor', 'none', 'DisplayName', 'True function');
461 | surf(X1,X2,Ymean, 'FaceAlpha',.8, 'EdgeColor', 'none', 'DisplayName', 'Prediction mean');
462 | scatter3(obj.X(1,:),obj.X(2,:),obj.Y(:,outdim),'filled','MarkerFaceColor','red', 'DisplayName', 'Sample points')
463 | zlim( minmax(obj.Y(:,outdim)') +[-1 1]*range(obj.Y(:,outdim)) );
464 | legend;
465 | title('Prediction Mean')
466 | xlabel('X1'); ylabel('X2');
467 | view(-60,17)
468 |
469 | % plot bias and variance
470 | figure('Color','w', 'Position',[-1260 547 894 264])
471 | subplot(1,2,1); hold on; grid on;
472 | contourf(X1,X2, abs(Ymean-Ytrue), 50,'LineColor','none')
473 | title('Absolute Prediction Bias')
474 | xlabel('X1'); ylabel('X2');
475 | colorbar;
476 | scatter(obj.X(1,:),obj.X(2,:),'filled','MarkerFaceColor','red')
477 | subplot(1,2,2); hold on; grid on;
478 | contourf(X1,X2, Ystd.^2, 50 ,'LineColor','none')
479 | title('Prediction Variance')
480 | xlabel('X1'); ylabel('X2');
481 | colorbar;
482 | scatter(obj.X(1,:),obj.X(2,:),'filled','MarkerFaceColor','red')
483 | colormap(gcf,parula);
484 | end
485 |
486 |
487 | function plot1d(obj, truthfun, varargin)
488 | %------------------------------------------------------------------
489 | % Make analysis of the GP quality (only for the first output dimension.
490 | % This function can only be called when the GP input is 1D
491 | %
492 | % args:
493 | % see inputParser parameters
494 | %------------------------------------------------------------------
495 |
496 | assert(obj.N>0, 'Dataset is empty. Aborting...')
497 | % we can not plot more than in 3D
498 | assert(obj.n==1, 'This function can only be used when dim(X)=1. Aborting...');
499 |
500 | %--------------------------------------------------------------
501 | % parse inputs
502 | %--------------------------------------------------------------
503 | p = inputParser;
504 |
505 | addParameter(p,'factor',0.3);
506 | addParameter(p,'outdim',1);
507 | addParameter(p,'npoints',300);
508 | addParameter(p,'sigmalevel',2);
509 | parse(p,varargin{:});
510 |
511 | factor = p.Results.factor;
512 | outdim = p.Results.outdim;
513 | npoints = p.Results.npoints;
514 | sigmalevel = p.Results.sigmalevel;
515 |
516 | addParameter(p,'rangeX', minmax(obj.X) + [-1 1]*factor*range(obj.X) );
517 | parse(p,varargin{:});
518 |
519 | rangeX = p.Results.rangeX;
520 |
521 |
522 | %--------------------------------------------------------------
523 | % Evaluate Ytrue, Ymean and Ystd
524 | %--------------------------------------------------------------
525 |
526 | % generate grid
527 | X = linspace(rangeX(1),rangeX(2),npoints);
528 | % evaluate and calculate prediction mean+-2*std
529 | [mu,var] = obj.eval(X,true);
530 | Ytrue = truthfun(X);
531 | Ymean = mu';
532 | Ystd = sqrt(squeeze(var));
533 |
534 | % prior
535 | %Ymean = 0*mu';
536 | %Ystd = sqrt(diag(obj.K(X,X)));
537 |
538 | %--------------------------------------------------------------
539 | % Generate plots
540 | %--------------------------------------------------------------
541 |
542 | figure('Color','w'); hold on; grid on;
543 | p0 = plot(X,Ytrue, 'LineWidth',2);
544 | p1 = plot(X,Ymean, 'LineWidth',0.5,'Color', [77, 0, 153]/255);
545 | p2 = plot(X,Ymean+sigmalevel*Ystd, 'LineWidth',0.5,'Color', [77, 0, 153]/255);
546 | p3 = plot(X,Ymean-sigmalevel*Ystd, 'LineWidth',0.5,'Color', [77, 0, 153]/255);
547 | p4 = patch([X fliplr(X)], [Ymean'+sigmalevel*Ystd' fliplr(Ymean'-sigmalevel*Ystd')], [153, 51, 255]/255, ...
548 | 'FaceAlpha',0.2, 'EdgeColor','none');
549 | p5 = scatter( obj.X, obj.Y, 'MarkerFaceColor','r','MarkerEdgeColor','r');
550 | % title('mean \pm 2*std curves');
551 | xlabel('X'); ylabel('Y');
552 | end
553 | end
554 | end
555 |
--------------------------------------------------------------------------------
/classes/MotionModelGP.m:
--------------------------------------------------------------------------------
1 | %------------------------------------------------------------------
2 | % Programed by:
3 | % - Lucas Rath (lucasrm25@gmail.com)
4 | % -
5 | % -
6 | %------------------------------------------------------------------
7 |
8 | classdef (Abstract) MotionModelGP < handle
9 | %--------------------------------------------------------------------------
10 | % Abstract class for implementing Motion Model of Plant
11 | % Intended to be used with GP and NMPC classes
12 | %
13 | % Please inherit this class and implement all the (Abstract) methods and
14 | % variables
15 | %
16 | % xk+1 = fd(xk,uk) + Bd * ( d(zk) + w ),
17 | %
18 | % where: zk = [Bz_x*xk ; Bz_u*uk],
19 | % d ~ N(mean_d(zk),var_d(zk))
20 | % w ~ N(0,var_w)
21 | %
22 | %--------------------------------------------------------------------------
23 |
24 | properties (Abstract, Constant)
25 | Bd % xk+1 = fd(xk,uk) + Bd*d(zk)
26 | Bz_x % z = [Bz_x*x;Bz_u*u]
27 | Bz_u %
28 | n % <1> number of outputs x(t)
29 | m % <1> number of inputs u(t)
30 | nd % <1> output dimension of d(z)
31 | nz % <1> dimension of z(t)
32 | end
33 |
34 | properties (SetAccess=private)
35 | discretization = 'ode2';
36 |
37 | d % [E[d(z)] , Var[d(z)]] = d(z): disturbace model
38 | var_w % measurement noise covariance matrix. w ~ N(0,var_w)
39 | end
40 |
41 | properties (SetAccess=public)
42 | % is_d_active = false
43 | end
44 |
45 | methods (Abstract)
46 | xdot = f (obj, x, u)
47 | %------------------------------------------------------------------
48 | % Continuous time dynamics.
49 | % out:
50 | % xdot: time derivative of x given x and u
51 | %------------------------------------------------------------------
52 |
53 | gradx = gradx_f(obj, x, u)
54 | %------------------------------------------------------------------
55 | % Continuous time dynamics.
56 | % out:
57 | % gradx: gradient of xdot w.r.t. x
58 | %------------------------------------------------------------------
59 |
60 | gradu = gradu_f(obj, x, u)
61 | %------------------------------------------------------------------
62 | % Continuous time dynamics.
63 | % out:
64 | % gradu: gradient of xdot w.r.t. u
65 | %------------------------------------------------------------------
66 | end
67 |
68 | methods
69 | function obj = MotionModelGP (d, var_w)
70 | %------------------------------------------------------------------
71 | % object constructor
72 | % args:
73 | % d: evaluates nonlinear motion model mean and covariance
74 | % function [mean_d, var_d] = d(z), with z = Bz*x
75 | % var_w: <1> measurement noise covariance
76 | % obs: if d or var_w are empty, then this function will set them
77 | % to zero with the correct dimensions
78 | %------------------------------------------------------------------
79 | obj.d = d;
80 | if isempty(obj.d)
81 | mu_d = @(z) zeros(obj.nd,1);
82 | var_d = @(z) zeros(obj.nd);
83 | obj.d = @(z) deal( mu_d(z), var_d(z) );
84 | end
85 |
86 | obj.var_w = var_w;
87 | if isempty(obj.var_w)
88 | obj.var_w = zeros(obj.nd);
89 | end
90 |
91 | %--------------------------------------------------------------
92 | % assert model
93 | %--------------------------------------------------------------
94 | assert(size(obj.Bz_x,1) + size(obj.Bz_u,1) == obj.nz, ...
95 | sprintf('obj.Bz_x and obj.Bz_u matrices should have %d columns in total, but have %d',obj.nz,size(obj.Bz_x,1) + size(obj.Bz_u,1)))
96 | assert(size(obj.Bd,2) == obj.nd, ...
97 | sprintf('obj.Bd matrix should have %d columns, but has %d',obj.nd,size(obj.Bd,2)))
98 |
99 | assert( all(size(obj.var_w)==[obj.nd,obj.nd]), ...
100 | sprintf('Variable var_w should have dimension %d, but has %d',obj.nd,size(obj.var_w,1)))
101 | assert(size(obj.Bd,1) == obj.n, ...
102 | sprintf('obj.Bd matrix should have %d rows, but has %d',obj.n,size(obj.Bd,1)))
103 | assert(size(obj.Bz_x,2) == obj.n || isempty(obj.Bz_x), ...
104 | sprintf('obj.Bz_x matrix should have %d columns, but has %d',obj.n,size(obj.Bz_x,1)))
105 | assert(size(obj.Bz_u,2) == obj.m || isempty(obj.Bz_u), ...
106 | sprintf('obj.Bz_u matrix should have %d columns, but has %d',obj.m,size(obj.Bz_u,1)))
107 |
108 | % validate given disturbance model
109 | ztest = [obj.Bz_x*zeros(obj.n,1) ; obj.Bz_u*zeros(obj.m,1)];
110 | [muy,vary] = obj.d(ztest);
111 | assert( size(muy,1)==obj.nd, ...
112 | sprintf('Disturbance model d evaluates to a mean value with wrong dimension. Got %d, expected %d',size(muy,1),obj.nd))
113 | assert( all(size(vary)==[obj.nd,obj.nd]), ...
114 | sprintf('Disturbance model d evaluates to a variance value with wrong dimension. Got %d, expected %d',size(vary,1),obj.nd))
115 | end
116 |
117 |
118 | function zk = z(obj, xk, uk)
119 | %------------------------------------------------------------------
120 | % select variables (xk,uk) -> z
121 | %------------------------------------------------------------------
122 | if ~isempty(obj.Bz_x)
123 | z_xk = obj.Bz_x * xk; else, z_xk=[];
124 | end
125 | if ~isempty(obj.Bz_u)
126 | z_uk = obj.Bz_u * uk; else, z_uk = [];
127 | end
128 | zk = [ z_xk ; z_uk ];
129 | end
130 |
131 |
132 | function [xkp1, gradx_xkp1] = fd (obj, xk, uk, dt)
133 | %------------------------------------------------------------------
134 | % Discrete time dynamics (ODE1 Euler approximation)
135 | % args:
136 | % xkp1: state prediction (without disturbance model)
137 | % grad_xkp1: gradient of xkp1 w.r.t. xk
138 | %------------------------------------------------------------------
139 | if strcmp(obj.discretization,'ode1')
140 | %-----------------
141 | % Fixed step ODE1
142 | %-----------------
143 | % calculate continous time dynamics
144 | xkp1 = xk + dt * obj.f(xk,uk);
145 |
146 | elseif strcmp(obj.discretization,'ode2')
147 | %-----------------
148 | % Fixed step ODE2 (developed by myself)
149 | %-----------------
150 | [~,xkp1] = ODE.ode2(@(t,x) obj.f(x,uk), xk, dt, dt);
151 |
152 | elseif strcmp(obj.discretization,'ode4')
153 | %-----------------
154 | % Fixed step ODE4 (developed by myself)
155 | %-----------------
156 | [~,xkp1] = ODE.ode4(@(t,x) obj.f(x,uk), xk, dt, dt);
157 |
158 | elseif strcmp(obj.discretization,'ode23')
159 | %-----------------
160 | % Variable step ODE23
161 | %-----------------
162 | opts_1 = odeset('Stats','off','RelTol',1e-1,'AbsTol',1e-1);
163 | [~,xkp1_23] = ode23( @(t,x) obj.f(x,uk), [0 dt], xk, opts_1);
164 | xkp1 = xkp1_23(end,:)';
165 |
166 | elseif strcmp(obj.discretization,'ode23')
167 | %-----------------
168 | % Variable step ODE23
169 | %-----------------
170 | opts_1 = odeset('Stats','off','RelTol',1e-1,'AbsTol',1e-1);
171 | [~,xkp1_23] = ode23( @(t,x) obj.f(x,uk), [0 dt], xk, opts_1);
172 | xkp1 = xkp1_23(end,:)';
173 |
174 | else
175 | error('Chosen discretization: %s is not yet implemented',obj.discretization);
176 | end
177 |
178 | % for now, gradient is being discretized using a simple ode1
179 | gradx_xdot = obj.gradx_f(xk,uk);
180 | gradx_xkp1 = eye(obj.n) + dt * gradx_xdot;
181 | end
182 |
183 |
184 | function [mu_xkp1,var_xkp1] = xkp1 (obj, mu_xk, var_xk, uk, dt)
185 | %------------------------------------------------------------------
186 | % State prediction (motion model) using Extended Kalman Filter
187 | % approach
188 | %
189 | % xk+1 = fd(xk,uk) + Bd * ( d(zk) + w ), zk=Bz*xk
190 | %
191 | %------------------------------------------------------------------
192 | % calculate discrete time dynamics
193 | [fd, gradx_fd] = obj.fd(mu_xk,uk,dt);
194 |
195 | % calculate grad_{x,d,w} xk+1
196 | grad_xkp1 = [gradx_fd; obj.Bd'; obj.Bd'];
197 |
198 | % select variables (xk,uk) -> z
199 | z = obj.z(mu_xk,uk);
200 |
201 | % evaluate disturbance
202 | [mu_d, var_d] = obj.d(z);
203 |
204 | % A) Mean Equivalent Approximation:
205 | var_x_d_w = blkdiag(var_xk, var_d, obj.var_w);
206 |
207 | % B) Taylor Approximation
208 | %--------------------------------------------------------------
209 | % TODO
210 | %--------------------------------------------------------------
211 |
212 | % predict mean and variance (Extended Kalman Filter)
213 | mu_xkp1 = fd + obj.Bd * ( mu_d );
214 | var_xkp1 = grad_xkp1' * var_x_d_w * grad_xkp1; % zeros(obj.n);
215 | end
216 |
217 | end
218 | end
219 |
--------------------------------------------------------------------------------
/classes/MotionModelGP_InvPendulum_deffect.m:
--------------------------------------------------------------------------------
1 | %------------------------------------------------------------------
2 | % Programed by:
3 | % - Lucas Rath (lucasrm25@gmail.com)
4 | % -
5 | % -
6 | %------------------------------------------------------------------
7 |
8 |
9 | classdef MotionModelGP_InvPendulum_deffect < MotionModelGP_InvPendulum_nominal
10 | %--------------------------------------------------------------------------
11 | % Inverted pendulum dynamics based on the nominal model but with some
12 | % deffect in the actuators
13 | %
14 | % xk+1 = fd_deffect(xk,uk) + Bd * ( d(zk) + w ),
15 | %
16 | % where: zk = [Bz_x*xk ; Bz_u*uk],
17 | % d ~ N(mean_d(zk),var_d(zk))
18 | % w ~ N(0,sigmaw)
19 | %
20 | %
21 | % x = [s, ds, th, dth]' carriage position and pole angle (and derivatives)
22 | % u = [F]' force on the carriage and torque on the pole joint
23 | %
24 | %--------------------------------------------------------------------------
25 |
26 | methods
27 | function obj = MotionModelGP_InvPendulum_deffect(Mc, Mp, b, I, l, d, sigmaw)
28 | obj = obj @ MotionModelGP_InvPendulum_nominal(Mc, Mp, b, I, l, d, sigmaw);
29 | end
30 |
31 |
32 | function xdot = f (obj, x, u)
33 | %------------------------------------------------------------------
34 | % Continuous time dynamics of the inverted pendulum but with some
35 | % deffect (additional disturbance)
36 | % args:
37 | % x:
38 | % u:
39 | % out:
40 | % xdot: time derivative of x given x and u
41 | %------------------------------------------------------------------
42 | % get dynamics from nominal model
43 | xdot = f @ MotionModelGP_InvPendulum_nominal(obj,x,u);
44 |
45 | % add deffect
46 | xdot(3) = xdot(3) + (0.1 * x(3) - 0.01*x(4) + deg2rad(3)) *10;
47 |
48 | % xdot(2) = xdot(2) + ( -0.1 * x(1) + deg2rad(3));
49 | end
50 |
51 | % function [xkp1, gradx_xkp1] = fd (obj, xk, uk, dt)
52 | % % get dynamics from nominal model
53 | % [xkp1, gradx_xkp1] = fd @ MotionModelGP_InvPendulum_nominal(obj, xk, uk, dt);
54 | % % deffect
55 | % xkp1(3) = xkp1(3) + (0.1 * xk(3) - 0.01*xk(4) + deg2rad(3)) *1;
56 | % end
57 |
58 | end
59 |
60 | end
--------------------------------------------------------------------------------
/classes/MotionModelGP_InvPendulum_nominal.m:
--------------------------------------------------------------------------------
1 | %------------------------------------------------------------------
2 | % Programed by:
3 | % - Lucas Rath (lucasrm25@gmail.com)
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]
35 | Bz_x = [0 0 1 0
36 | 0 0 0 1]
37 | Bz_u = [];
38 | Bd = [0; % xk+1 = fd(xk,uk) + Bd*d(zk)
39 | 0;
40 | 1;
41 | 0]
42 |
43 | n = 4 % number of outputs x(t)
44 | m = 1 % number of inputs u(t)
45 | nz = 2 % dimension of z(t)
46 | nd = 1 % output dimension of d(z)
47 | end
48 |
49 |
50 | methods
51 |
52 | function obj = MotionModelGP_InvPendulum_nominal (Mc, Mp, b, I, l, d, sigmaw)
53 | %------------------------------------------------------------------
54 | % object constructor
55 | %------------------------------------------------------------------
56 | % call superclass constructor
57 | obj = obj @ MotionModelGP(d,sigmaw);
58 | % store parameters
59 | obj.Mc = Mc;
60 | obj.Mp = Mp;
61 | obj.b = b;
62 | obj.I = I;
63 | obj.l = l;
64 |
65 | % add folder CODEGEN to path. Here there will be some functions
66 | % generated with the method generate_invertedPendulum_functions()
67 | addpath(fullfile(pwd,'CODEGEN'))
68 | end
69 |
70 | function xdot = f (obj, x, u)
71 | %------------------------------------------------------------------
72 | % Continuous time dynamics.
73 | % out:
74 | % xdot: time derivative of x given x and u
75 | %------------------------------------------------------------------
76 | params = [obj.Mc obj.Mp obj.I obj.g obj.l obj.b]';
77 | xdot = invertedPendulum_f(x, u, params );
78 | end
79 |
80 | function gradx = gradx_f(obj, x, u)
81 | %------------------------------------------------------------------
82 | % Continuous time dynamics.
83 | % out:
84 | % gradx: gradient of xdot w.r.t. x
85 | %------------------------------------------------------------------
86 | params = [obj.Mc obj.Mp obj.I obj.g obj.l obj.b]';
87 | gradx = invertedPendulum_gradx_f(x, u, params );
88 | end
89 |
90 | function gradu = gradu_f(obj, x, u)
91 | %------------------------------------------------------------------
92 | % Continuous time dynamics.
93 | % out:
94 | % gradu: gradient of xdot w.r.t. u
95 | %------------------------------------------------------------------
96 | params = [obj.Mc obj.Mp obj.I obj.g obj.l obj.b]';
97 | gradu = invertedPendulum_gradu_f(x, u, params );
98 | end
99 |
100 | function [A,B] = linearize (obj)
101 | %------------------------------------------------------------------
102 | % Return continuous time linearized model parameters A,B
103 | % xdot = A*x + B*u
104 | %------------------------------------------------------------------
105 | Mc=obj.Mc; Mp=obj.Mp; b=obj.b; I=obj.I; l=obj.l; g=obj.g;
106 | p = I*(Mc+Mp)+Mc*Mp*l^2;
107 | A = [0 1 0 0;
108 | 0 -(I+Mp*l^2)*b/p (Mp^2*g*l^2)/p 0;
109 | 0 0 0 1;
110 | 0 -(Mp*l*b)/p Mp*g*l*(Mc+Mp)/p 0];
111 | B = [ 0;
112 | (I+Mp*l^2)/p;
113 | 0;
114 | Mp*l/p];
115 | end
116 | end
117 |
118 | methods(Static)
119 | function generate_invertedPendulum_functions()
120 | %------------------------------------------------------------------
121 | % Generate continuous time dynamics equations of the inverted
122 | % pendulum:. This function generates three functions:
123 | % xdot = f(x,u) - dynamics
124 | % gradx_xdot(x,u) - gradient of xdot w.r.t. x
125 | % gradu_xdot(x,u) - gradient of xdot w.r.t. u
126 | %
127 | % (Mc+Mp)*dds + b*ds + Mp*l/2*ddth*cos(th) - Mp*l/2*dth^2*sin(th) = F
128 | % (I+Mp*(l/2)^2)*ddth + Mp*g*l/2*sin(th) + Mp*l*dds*cos(th) = T
129 | %
130 | % x = [s, ds, th, dth]'
131 | % u = [F]'
132 | %
133 | %
134 | % Example how to run this function:
135 | % ip = MotionModelGP_InvertedPendulum(5, 2, 0.1, 0.6, 3, @(z)deal(0,0), 0);
136 | % ip.generate_invertedPendulum_functions();
137 | %------------------------------------------------------------------
138 | syms g Mc Mp b I l F T s ds dds th dth ddth real
139 | T = 0; % we are not using this input for now
140 | fzero = [(Mc+Mp)*dds + b*ds + Mp*l/2*ddth*cos(th) - Mp*l/2*dth^2*sin(th) - F ;
141 | (I+Mp*(l/2)^2)*ddth + Mp*g*l/2*sin(th) + Mp*l*dds*cos(th) - T ];
142 | sol = solve(fzero,[dds,ddth]);
143 | dds = simplify(sol.dds);
144 | ddth = simplify(sol.ddth);
145 |
146 | u = F;
147 | x = [s, ds, th, dth]';
148 | xdot = [ds, dds, dth, ddth]';
149 | params = [Mc Mp I g l b ]';
150 |
151 |
152 | folder = fullfile(pwd,'CODEGEN');
153 | if ~exist(folder,'dir')
154 | mkdir(folder);
155 | end
156 | addpath(folder)
157 |
158 |
159 | matlabFunction( xdot, 'Vars', {x;u;params} ,'File', fullfile('CODEGEN','invertedPendulum_f') );
160 |
161 | gradx_f = simplify(jacobian(xdot,x)');
162 | matlabFunction( gradx_f, 'Vars', {x;u;params} ,'File', fullfile('CODEGEN','invertedPendulum_gradx_f') );
163 |
164 | gradu_f = simplify(jacobian(xdot,u)');
165 | matlabFunction( gradu_f, 'Vars', {x;u;params} ,'File', fullfile('CODEGEN','invertedPendulum_gradu_f') );
166 |
167 | disp('FINISHED! functions invertedPendulum_f, invertedPendulum_gradx_f and invertedPendulum_gradu_f generated!!')
168 |
169 | end
170 | end
171 |
172 | end
173 |
--------------------------------------------------------------------------------
/classes/MotionModelGP_SingleTrack_nominal.m:
--------------------------------------------------------------------------------
1 | %--------------------------------------------------------------------------
2 | % Programed by:
3 | % - Lucas Rath (lucasrm25@gmail.com)
4 | % -
5 | % -
6 | %--------------------------------------------------------------------------
7 |
8 | classdef MotionModelGP_SingleTrack_nominal < MotionModelGP
9 | %--------------------------------------------------------------------------
10 | % xk+1 = fd(xk,uk) + Bd * ( d(zk) + w ),
11 | %
12 | % where: zk = [Bz_x*xk ; Bz_u*uk],
13 | % d ~ N(mean_d(zk),var_d(zk))
14 | % w ~ N(0,sigmaw)
15 | %
16 | %
17 | % x = [I_x (x position in global coordinates),
18 | % I_y (y position in global coordinates),
19 | % psi (yaw angle),
20 | % V_vx (longitudinal velocity in vehicle coordinates)
21 | % V_vy (lateral velocity in vehicle coordinates)
22 | % psi_dot (yaw rate),
23 | % track_dist (distance traveled in the track centerline)
24 | % ]
25 | %
26 | % u = [delta (steering angle),
27 | % T (wheel torque gain), -1=max.braking, 1=max acc.
28 | % track_vel (velocity in the track centerline)
29 | % ]
30 | %
31 | %--------------------------------------------------------------------------
32 |
33 | properties(Constant)
34 | M = 500 *1.5 % vehicle mass
35 | I_z = 600 *1.5 % vehicle moment of inertia (yaw axis)
36 | g = 9.81 % gravitation
37 | l_f = 0.9 % distance of the front wheel to the center of mass
38 | l_r = 1.5 % distance of the rear wheel to the center of mass
39 |
40 | deltamax = deg2rad(30) % maximum steering amplitude
41 | % deltadotmax = deg2rad(20) % maximum steering velocity amplitude
42 |
43 | maxbrakeWForce = 8000 % = 2*g*M; % allow ~ 2g brake
44 | maxmotorWForce = 4000 % = 1*g*M; % allow ~ 1g acc
45 |
46 |
47 | % Pacejka lateral dynamics parameters
48 | c_f = 14000 * 2.5 % = 1*g*M/deltamax % front coornering stiffness (C*delta=Fy~M*a)
49 | c_r = 14000 * 2.5 % = 2*g*M/deltamax % rear coornering stiffness
50 | end
51 |
52 | properties(Constant)
53 | % keep in mind the dimensions: xk+1 = fd(xk,uk) + Bd*(d(z)+w)),
54 | % where z = [Bz_x*x;Bz_u*u]
55 | Bz_x = [zeros(3), eye(3), zeros(3,1)]
56 | Bz_u = [1 0 0;
57 | 0 1 0]
58 | Bd = [zeros(3);
59 | eye(3);
60 | zeros(1,3)]
61 | n = 7 % number of outputs x(t)
62 | m = 3 % number of inputs u(t)
63 | nz = 5 % dimension of z(t)
64 | nd = 3 % output dimension of d(z)
65 | end
66 |
67 | methods
68 | function obj = MotionModelGP_SingleTrack_nominal(d,sigmaw)
69 | %------------------------------------------------------------------
70 | % object constructor. Create model and report model stability
71 | % analysis
72 | %------------------------------------------------------------------
73 | % call superclass constructor
74 | obj = obj@MotionModelGP(d,sigmaw);
75 | % report single track dynamics analysis
76 | % obj.analyseSingleTrack();
77 |
78 | % add folder CODEGEN to path. Here there will be some functions
79 | % generated with the method generate_grad_functions()
80 | addpath(fullfile(pwd,'CODEGEN'))
81 | end
82 |
83 | function xdot = f (obj, x, u)
84 | %------------------------------------------------------------------
85 | % Continuous time dynamics of the single track (including
86 | % disturbance):
87 | %------------------------------------------------------------------
88 |
89 | %--------------------------------------------------------------
90 | % Model parameters
91 | %--------------------------------------------------------------
92 | g = obj.g;
93 | % switch to symbolic variables if input is symbolic
94 | % (to be used with automatic gradient code generation)
95 | if isa(x,'sym')
96 | syms M I_z l_f l_r deltamax maxbrakeWForce maxmotorWForce c_f c_r real
97 | else
98 | M = obj.M;
99 | I_z = obj.I_z;
100 | l_f = obj.l_f;
101 | l_r = obj.l_r;
102 | deltamax = obj.deltamax;
103 | maxbrakeWForce = obj.maxbrakeWForce;
104 | maxmotorWForce = obj.maxmotorWForce;
105 | c_f = obj.c_f;
106 | c_r = obj.c_r;
107 | end
108 |
109 | %--------------------------------------------------------------
110 | % State Vector
111 | %--------------------------------------------------------------
112 | I_x = x(1);
113 | I_y = x(2);
114 | psi = x(3);
115 | V_vx = x(4);
116 | V_vy = x(5);
117 | psi_dot = x(6);
118 | track_dist = x(7);
119 | beta = atan2(V_vy,V_vx);
120 |
121 | %--------------------------------------------------------------
122 | % Inputs
123 | %--------------------------------------------------------------
124 | delta = u(1); % steering angle
125 | T = u(2); % wheel torque gain, -1=max.braking, 1=max acc.
126 | track_vel = u(3); % track centerline velocity
127 |
128 | %--------------------------------------------------------------
129 | % Saturate inputs and
130 | %--------------------------------------------------------------
131 | % saturate steering angle
132 | if isa(x,'sym') % switch to differentiable function
133 | delta = obj.sclip(delta, -deltamax, deltamax);
134 | else
135 | delta = obj.clip(delta, -deltamax, deltamax); % (NOT DIFFERENTIABLE)
136 | end
137 |
138 | % saturate pedal input
139 | if isa(x,'sym') % switch to differentiable function
140 | T = obj.sclip( T, -1, 1);
141 | else
142 | T = obj.clip( T, -1, 1); % % (NOT DIFFERENTIABLE)
143 | end
144 |
145 | %--------------------------------------------------------------
146 | % Wheel slip angles (slip ration not being used for now)
147 | %--------------------------------------------------------------
148 | a_r = atan2(V_vy-l_r*psi_dot,V_vx);
149 | a_f = atan2(V_vy+l_f*psi_dot,V_vx) - delta;
150 |
151 | %--------------------------------------------------------------
152 | % Tyre forces
153 | %--------------------------------------------------------------
154 | % desired total wheel torque to be applied
155 | if isa(x,'sym') % switch to differentiable function
156 | totalWForce = T*( (obj.gez(T)).*maxmotorWForce ...
157 | +(obj.lez(T)).*maxbrakeWForce.*obj.ssign(V_vx));
158 | else
159 | totalWForce = T * ( (T>0)*maxmotorWForce+(T<0)*maxbrakeWForce*sign(V_vx) ); % % (NOT DIFFERENTIABLE)
160 | end
161 | % longitudinal wheel torque distribution
162 | zeta = 0.5;
163 |
164 | % Wheel forces in wheel coordinates (z-axis points down, x-axis front)
165 | % This means positive W_Fy_f turns vehicle to the right
166 | W_Fx_r = zeta * totalWForce;
167 | W_Fx_f = (1-zeta) * totalWForce;
168 |
169 | W_Fy_r = c_r * a_r;
170 | W_Fy_f = c_f * a_f;
171 | % W_Fy_r = D_r*sin(C_r*atan(B_r*a_r-E_r*(B_r*a_r -atan(B_r*a_r)))); % rear lateral force
172 | % W_Fy_f = D_f*sin(C_f*atan(B_f*a_f-E_f*(B_f*a_f -atan(B_f*a_f)))); % front lateral force
173 |
174 | % Wheel forces in vehicle coordinates (z-axis points up, x-axis front)
175 | V_Fx_r = W_Fx_r;
176 | V_Fx_f = W_Fx_f;
177 | V_Fy_r = - W_Fy_r;
178 | V_Fy_f = - W_Fy_f;
179 |
180 | %--------------------------------------------------------------
181 | % Calculate state space time derivatives
182 | %--------------------------------------------------------------
183 | % vector field (right-hand side of differential equation)
184 | I_x_dot = V_vx*cos(psi) - V_vy*sin(psi); % longitudinal velocity
185 | I_y_dot = V_vx*sin(psi) + V_vy*cos(psi); % lateral velocity
186 | V_vx_dot = 1/M * (V_Fx_r + V_Fx_f*cos(delta) - V_Fy_f*sin(delta) + V_vy*psi_dot);
187 | V_vy_dot = 1/M * (V_Fy_r + V_Fx_f*sin(delta) + V_Fy_f*cos(delta) - V_vy*psi_dot);
188 | psi_dot_dot = 1/I_z * (V_Fy_f*l_f*cos(delta) + V_Fx_f*l_f*sin(delta) - V_Fy_r*l_r);
189 | track_dist_dot = track_vel; % Traveled distance in the track centerline
190 |
191 | %--------------------------------------------------------------
192 | % write outputs
193 | %--------------------------------------------------------------
194 | xdot = [I_x_dot; I_y_dot; psi_dot; V_vx_dot; V_vy_dot; psi_dot_dot; track_dist_dot];
195 |
196 | %--------------------------------------------------------------
197 | % check model integrity
198 | %--------------------------------------------------------------
199 | if ~isa(x,'sym') % sym input used for code generation
200 | if any(isnan(xdot)) || any(isinf(xdot)) || any(imag(xdot)~=0)
201 | error('Single Track Model evaluated to Inf of NaN... CHECK MODEL!!!')
202 | end
203 | end
204 | end
205 |
206 |
207 | function gradx = gradx_f(obj, x, u)
208 | %------------------------------------------------------------------
209 | % Continuous time dynamics.
210 | % out:
211 | % gradx: gradient of xdot w.r.t. x
212 | %------------------------------------------------------------------
213 | % gradx = zeros(obj.n);
214 | params = [obj.M obj.I_z obj.l_f obj.l_r obj.deltamax ...
215 | obj.maxbrakeWForce obj.maxmotorWForce obj.c_f obj.c_r]';
216 | gradx = singletrack_gradx_f(x,u,params);
217 | end
218 |
219 |
220 | function gradu = gradu_f(obj, x, u)
221 | %------------------------------------------------------------------
222 | % Continuous time dynamics.
223 | % out:
224 | % gradu: gradient of xdot w.r.t. u
225 | %------------------------------------------------------------------
226 | % gradu = zeros(obj.m,obj.n);
227 | params = [obj.M obj.I_z obj.l_f obj.l_r obj.deltamax ...
228 | obj.maxbrakeWForce obj.maxmotorWForce obj.c_f obj.c_r]';
229 | gradu = singletrack_gradu_f(x,u,params);
230 | end
231 |
232 |
233 | function generate_grad_functions(obj)
234 | %------------------------------------------------------------------
235 | % Generate external files for the evaluation of the gradient of
236 | % the continuous time dynamics. (Make use of symbolic toolbox)
237 | % Please ensure that your dynamics only contains smooth
238 | % diferentiable functions.
239 | %
240 | % To generate files, simply call:
241 | % nomModel = MotionModelGP_SingleTrack_nominal(@(z)deal(zeros(3,1),zeros(3)), zeros(3));
242 | % nomModel.generate_grad_functions()
243 | %------------------------------------------------------------------
244 | syms I_x I_y vpsi V_vx V_vy psi_dot track_dist real
245 | x = [I_x I_y vpsi V_vx V_vy psi_dot track_dist]';
246 | syms delta T track_vel real
247 | u = [delta T track_vel]';
248 | syms M I_z l_f l_r deltamax maxbrakeWForce maxmotorWForce c_f c_r real
249 | params = [M I_z l_f l_r deltamax maxbrakeWForce maxmotorWForce c_f c_r]';
250 |
251 | xdot = obj.f(x,u);
252 | gradx = jacobian(xdot,x)';
253 | gradu = jacobian(xdot,u)';
254 | % gradx = simplify(expand(gradx)); % does not work. eqs are too complex
255 | % gradu = simplify(expand(gradu)); % does not work. eqs are too complex
256 |
257 | % Save model
258 | folder = fullfile(pwd,'CODEGEN');
259 | if ~exist(folder,'dir')
260 | mkdir(folder);
261 | end
262 | addpath(folder)
263 |
264 | matlabFunction(gradx,'Vars',{x,u,params},'File',fullfile('CODEGEN','singletrack_gradx_f'),'Optimize',true);
265 | matlabFunction(gradu,'Vars',{x,u,params},'File',fullfile('CODEGEN','singletrack_gradu_f'),'Optimize',true);
266 |
267 | disp('FINISHED! functions CODEGEN_singletrack_gradx_f and CODEGEN_singletrack_gradu_f generated!!')
268 | end
269 |
270 |
271 | function analyseSingleTrack(obj)
272 | %------------------------------------------------------------------
273 | % report single track model dynamics analysis
274 | %------------------------------------------------------------------
275 | fprintf('Single track model created!!! Summary:\n');
276 | % vehicle size
277 | l = obj.l_f + obj.l_r;
278 | % Eigenlenkgradient = Yaw gradient (d_delta/d_ay)
279 | EG = obj.M/l*(obj.l_r/obj.c_f - obj.l_f/obj.c_r);
280 | fprintf(2,'\tYaw gradient EG=(d_delta/d_ay) ~ %.1f [deg/g]\n',rad2deg(EG)*obj.g);
281 | if rad2deg(EG)*obj.g < 10
282 | fprintf(2,'\tBE CAREFULL... EG is too low (< 10 [deg/g])... increase l_r,c_r or decrease l_f,c_f\n');
283 | end
284 | if EG < 0
285 | v_cr = sqrt( obj.c_f*obj.c_r*l^2 / (obj.M*(obj.c_f*obj.l_f - obj.c_r*obj.l_r)));
286 | fprintf(2,'\tVehicle is intrinsically OVERSTEER (c_r*l_r < c_f*l_f)\n');
287 | fprintf('\tCritical velocity v_cr ~ %.1f [m/s]\n', v_cr);
288 | fprintf('\tMax ss. Yaw (at delta_max) at v= 5[m/s] ~ %.1f [deg/s]\n',rad2deg(obj.deltamax)* 5/(l+EG* 5^2));
289 | fprintf('\tMax ss. Yaw (at delta_max) at v=10[m/s] ~ %.1f [deg/s]\n',rad2deg(obj.deltamax)*10/(l+EG*10^2));
290 | fprintf('\tMax ss. Yaw (at delta_max) at v=15[m/s] ~ %.1f [deg/s]\n',rad2deg(obj.deltamax)*15/(l+EG*15^2));
291 | else
292 | v_ch = sqrt(l/EG);
293 | fprintf(2,'\tVehicle is intrinsically UNDERSTEER (c_r*l_r > c_f*l_f)\n');
294 | fprintf('\tCharacteristic velocity v_ch ~ %.1f [m/s]\n',v_ch);
295 | fprintf('\tMax ss. Yaw rate gain (at v_ch) (psidot/delta)ss,max ~ %.1f [1/s]\n',v_ch/(l+EG*v_ch^2));
296 | fprintf('\tMax ss. Yaw rate (at v_ch and delta_max) (psidot)ss,max~ %.1f [deg/s]\n',rad2deg(obj.deltamax)*v_ch/(l+EG*v_ch^2));
297 | end
298 | fprintf('\tax_{max} ~ %.1f [g]\n',obj.maxmotorWForce/obj.M/obj.g);
299 | fprintf('\tax_{min} ~ -%.1f [g]\n',obj.maxbrakeWForce/obj.M/obj.g);
300 | end
301 | end
302 |
303 |
304 | %----------------------------------------------------------------------
305 | % Smooth alternatives for nonmooth functions
306 | %----------------------------------------------------------------------
307 | methods
308 | function x = sclip(obj,x,lb,ub)
309 | % Smooth (differentiable) clip (saturation) function
310 | x = x.*obj.gez(x-lb).*obj.lez(x-ub) + ub*obj.gez(x-ub) + lb*obj.lez(x-lb);
311 | end
312 | end
313 | methods(Static)
314 | function x = clip(x,lb,ub)
315 | % standard nonsmooth clip (saturation) function
316 | x = min(max(x,lb),ub);
317 | end
318 | function x = srec(x,lb,ub)
319 | % Smooth rectangular function
320 | alpha = 50; % the larger the sharper the rectangular function
321 | x = 0.5*(tanh((x-lb)*alpha)-tanh((x-ub)*alpha));
322 | end
323 | function x = gez(x)
324 | % Smooth >=0 boolean function
325 | alpha = 50; % the larger the sharper the clip function
326 | x = (1+exp(-alpha*x)).^-1;
327 | end
328 | function x = lez(x)
329 | % Smooth <=0 boolean function
330 | alpha = 50; % the larger the sharper the clip function
331 | x = 1-(1+exp(-alpha*x)).^-1;
332 | end
333 | function x = ssign(x)
334 | % Smooth sign(x) boolean function
335 | alpha = 100; % the larger the sharper the clip function
336 | x = tanh(alpha*x);
337 | end
338 | end
339 | end
340 |
--------------------------------------------------------------------------------
/classes/MotionModelGP_SingleTrack_true.m:
--------------------------------------------------------------------------------
1 | %--------------------------------------------------------------------------
2 | % Programed by:
3 | % - Lucas Rath (lucasrm25@gmail.com)
4 | % -
5 | % -
6 | %--------------------------------------------------------------------------
7 |
8 | classdef MotionModelGP_SingleTrack_true < MotionModelGP
9 | %--------------------------------------------------------------------------
10 | % xk+1 = fd(xk,uk) + Bd * ( d(zk) + w ),
11 | %
12 | % where: zk = [Bz_x*xk ; Bz_u*uk],
13 | % d ~ N(mean_d(zk),var_d(zk))
14 | % w ~ N(0,sigmaw)
15 | %
16 | %
17 | % x = [I_x (x position in global coordinates),
18 | % I_y (y position in global coordinates),
19 | % psi (yaw angle),
20 | % V_vx (longitudinal velocity in vehicle coordinates)
21 | % V_vy (lateral velocity in vehicle coordinates)
22 | % psi_dot (yaw rate),
23 | % track_dist (distance traveled in the track centerline)
24 | % ]
25 | %
26 | % u = [delta (steering angle),
27 | % T (wheel torque gain), -1=max.braking, 1=max acc.
28 | % track_vel (velocity in the track centerline)
29 | % ]
30 | %
31 | %--------------------------------------------------------------------------
32 |
33 | properties(Constant)
34 | M = 500 % vehicle mass
35 | I_z = 600 % vehicle moment of inertia (yaw axis)
36 | g = 9.81 % gravitation
37 | l_f = 0.9 % distance of the front wheel to the center of mass
38 | l_r = 1.5 % distance of the rear wheel to the center of mass
39 |
40 | deltamax = deg2rad(30) % maximum steering amplitude
41 | % deltadotmax = deg2rad(20) % maximum steering velocity amplitude
42 |
43 | maxbrakeWForce = 8000 % = 2*g*M; % allow ~ 2g brake
44 | maxmotorWForce = 4000 % = 1*g*M; % allow ~ 1g acc
45 |
46 | % Pacejka lateral dynamics parameters
47 | B_f = 0.4; % stiffnes factor (Pacejka) (front wheel)
48 | C_f = 8; % shape factor (Pacejka) (front wheel)
49 | D_f = 4560.4; % peak value (Pacejka) (front wheel)
50 | E_f = -0.5; % curvature factor (Pacejka) (front wheel)
51 | B_r = 0.45; % stiffnes factor (Pacejka) (rear wheel)
52 | C_r = 8; % shape factor (Pacejka) (rear wheel)
53 | D_r = 4000; % peak value (Pacejka) (rear wheel)
54 | E_r = -0.5; % curvature factor (Pacejka) (rear wheel)
55 | end
56 |
57 | properties(Constant)
58 | % keep in mind the dimensions: xk+1 = fd(xk,uk) + Bd*(d(z)+w)),
59 | % where z = [Bz_x*x;Bz_u*u]
60 | Bz_x = [zeros(3), eye(3), zeros(3,1)]
61 | Bz_u = [1 0 0;
62 | 0 1 0]
63 | Bd = [zeros(3);
64 | eye(3);
65 | zeros(1,3)]
66 | n = 7 % number of outputs x(t)
67 | m = 3 % number of inputs u(t)
68 | nz = 5 % dimension of z(t)
69 | nd = 3 % output dimension of d(z)
70 | end
71 |
72 | methods(Static)
73 | function x = clip(x,lb,ub)
74 | % standard nonsmooth clip (saturation) function
75 | x = min(max(x,lb),ub);
76 | end
77 | end
78 |
79 | methods
80 | function obj = MotionModelGP_SingleTrack_true(d,sigmaw)
81 | %------------------------------------------------------------------
82 | % object constructor. Create model and report model stability
83 | % analysis
84 | %------------------------------------------------------------------
85 | % call superclass constructor
86 | obj = obj@MotionModelGP(d,sigmaw);
87 | end
88 |
89 | function xdot = f (obj, x, u)
90 | %------------------------------------------------------------------
91 | % Continuous time dynamics of the single track (including
92 | % disturbance):
93 | %------------------------------------------------------------------
94 |
95 | %--------------------------------------------------------------
96 | % Model parameters
97 | %--------------------------------------------------------------
98 | g = obj.g;
99 | M = obj.M;
100 | I_z = obj.I_z;
101 | l_f = obj.l_f;
102 | l_r = obj.l_r;
103 | deltamax = obj.deltamax;
104 | maxbrakeWForce = obj.maxbrakeWForce;
105 | maxmotorWForce = obj.maxmotorWForce;
106 | B_f = obj.B_f;
107 | C_f = obj.C_f;
108 | D_f = obj.D_f;
109 | E_f = obj.E_f;
110 | B_r = obj.B_r;
111 | C_r = obj.C_r;
112 | D_r = obj.D_r;
113 | E_r = obj.E_r;
114 |
115 | %--------------------------------------------------------------
116 | % State Vector
117 | %--------------------------------------------------------------
118 | I_x = x(1);
119 | I_y = x(2);
120 | psi = x(3);
121 | V_vx = x(4);
122 | V_vy = x(5);
123 | psi_dot = x(6);
124 | track_dist = x(7);
125 | beta = atan2(V_vy,V_vx);
126 |
127 | %--------------------------------------------------------------
128 | % Inputs
129 | %--------------------------------------------------------------
130 | delta = u(1); % steering angle
131 | T = u(2); % wheel torque gain, -1=max.braking, 1=max acc.
132 | track_vel = u(3); % track centerline velocity
133 |
134 | %--------------------------------------------------------------
135 | % Saturate inputs and
136 | %--------------------------------------------------------------
137 | % saturate steering angle
138 | delta = obj.clip(delta, -deltamax, deltamax); % (NOT DIFFERENTIABLE)
139 |
140 | % saturate pedal input
141 | T = obj.clip( T, -1, 1); % % (NOT DIFFERENTIABLE)
142 |
143 | %--------------------------------------------------------------
144 | % Wheel slip angles (slip ration not being used for now)
145 | %--------------------------------------------------------------
146 | a_r = atan2(V_vy-l_r*psi_dot,V_vx);
147 | a_f = atan2(V_vy+l_f*psi_dot,V_vx) - delta;
148 |
149 | %--------------------------------------------------------------
150 | % Tyre forces
151 | %--------------------------------------------------------------
152 | % desired total wheel torque to be applied
153 | totalWForce = T * ( (T>0)*maxmotorWForce+(T<0)*maxbrakeWForce*sign(V_vx) ); % % (NOT DIFFERENTIABLE)
154 | % longitudinal wheel torque distribution
155 | zeta = 0.5;
156 |
157 | % Wheel forces in wheel coordinates (z-axis points down, x-axis front)
158 | % This means positive W_Fy_f turns vehicle to the right
159 | W_Fx_r = zeta * totalWForce;
160 | W_Fx_f = (1-zeta) * totalWForce;
161 | % Pacejka tyre lateral dynamics
162 | W_Fy_r = D_r*sin(C_r*atan(B_r*a_r-E_r*(B_r*a_r -atan(B_r*a_r)))); % rear lateral force
163 | W_Fy_f = D_f*sin(C_f*atan(B_f*a_f-E_f*(B_f*a_f -atan(B_f*a_f)))); % front lateral force
164 |
165 | % Wheel forces in vehicle coordinates (z-axis points up, x-axis front)
166 | V_Fx_r = W_Fx_r;
167 | V_Fx_f = W_Fx_f;
168 | V_Fy_r = - W_Fy_r;
169 | V_Fy_f = - W_Fy_f;
170 |
171 | %--------------------------------------------------------------
172 | % Calculate state space time derivatives
173 | %--------------------------------------------------------------
174 | % vector field (right-hand side of differential equation)
175 | I_x_dot = V_vx*cos(psi) - V_vy*sin(psi); % longitudinal velocity
176 | I_y_dot = V_vx*sin(psi) + V_vy*cos(psi); % lateral velocity
177 | V_vx_dot = 1/M * (V_Fx_r + V_Fx_f*cos(delta) - V_Fy_f*sin(delta) + V_vy*psi_dot);
178 | V_vy_dot = 1/M * (V_Fy_r + V_Fx_f*sin(delta) + V_Fy_f*cos(delta) - V_vy*psi_dot);
179 | psi_dot_dot = 1/I_z * (V_Fy_f*l_f*cos(delta) + V_Fx_f*l_f*sin(delta) - V_Fy_r*l_r);
180 | track_dist_dot = track_vel; % Traveled distance in the track centerline
181 |
182 | %--------------------------------------------------------------
183 | % write outputs
184 | %--------------------------------------------------------------
185 | xdot = [I_x_dot; I_y_dot; psi_dot; V_vx_dot; V_vy_dot; psi_dot_dot; track_dist_dot];
186 |
187 | %--------------------------------------------------------------
188 | % check model integrity
189 | %--------------------------------------------------------------
190 | if any(isnan(xdot)) || any(isinf(xdot)) || any(imag(xdot)~=0)
191 | error('Single Track Model evaluated to Inf of NaN... CHECK MODEL!!!')
192 | end
193 | end
194 |
195 | function gradx = gradx_f(obj, ~, ~)
196 | %------------------------------------------------------------------
197 | % Continuous time dynamics.
198 | % out:
199 | % gradx: gradient of xdot w.r.t. x
200 | %------------------------------------------------------------------
201 | gradx = zeros(obj.n);
202 | end
203 |
204 | function gradu = gradu_f(obj, ~, ~)
205 | %------------------------------------------------------------------
206 | % Continuous time dynamics.
207 | % out:
208 | % gradu: gradient of xdot w.r.t. u
209 | %------------------------------------------------------------------
210 | gradu = zeros(obj.m,obj.n);
211 | end
212 |
213 | end
214 | end
215 |
--------------------------------------------------------------------------------
/classes/NMPC.m:
--------------------------------------------------------------------------------
1 | %------------------------------------------------------------------
2 | % Programed by:
3 | % - Lucas Rath (lucasrm25@gmail.com)
4 | % -
5 | % -
6 | %------------------------------------------------------------------
7 |
8 | classdef NMPC < handle
9 | %------------------------------------------------------------------
10 | % Nonlinear MPC class
11 | %
12 | % solve:
13 | %
14 | % MIN { SUM_{k=0:N-1} fo(tk,xk,uk,r(t)) } + fend(tN,xN,r(tN))
15 | %
16 | % s.t. xk+1 = E[f(xk,uk)]
17 | % h(xk,uk) == 0
18 | % g(xk,uk) <= 0
19 | %
20 | % where the motion model evaluates [E[xk+1],Var[xk+1]] = f(xk,uk)
21 | %
22 | % for [u0;...;uN-1; e1;...;eN]
23 | %
24 | % where xk: state variables
25 | % zk: selected state variables zk=Bd'*xk
26 | % r(tk): trajectory
27 | % tk: current time
28 | %------------------------------------------------------------------
29 |
30 | properties
31 | % Optimizer settings
32 | maxiter = 200 % max solver iterations
33 | tol = 1e-6 % optimizer tolerance
34 | N = 30 % prediction horizon
35 |
36 | % Define optimization problem
37 | f % @fun nonlinear dynamics: [E[xk+1],Var[xk+1]] = f(xk,uk)
38 | h % nonlinear equality constraint function
39 | g % nonlinear inequality constraint function
40 |
41 | fo % @fun nonlinear cost function
42 | fend % @fend nonlinear cost function for the final state
43 |
44 | % Optimization dimension
45 | n % dim(x) state space dimension
46 | m % dim(u) input dimension
47 | ne % dim(e) additional optimization variables dimension
48 | dt % time step size
49 | nh % number of additional eq. constraints for every time step
50 | ng % number of additional ineq. constraints for every time step
51 |
52 | % save last optimal results computed, in order to use as initial guess
53 | uguess % initial guess for inputs
54 | eguess % initial guess for extra variables
55 | end
56 |
57 | properties(Access=private)
58 | lb % lower bound constraints lb <= vars
59 | ub % upper bound constraints vars <= ub
60 | end
61 |
62 | methods
63 |
64 | function obj = NMPC (f, h, g, u_lb, u_ub, n, m, ne, fo, fend, N, dt)
65 | %------------------------------------------------------------------
66 | % MPC constructor
67 | %
68 | % args:
69 | % f: motion model that evaluates [E[xk+1],Var[xk+1]] = f(xk)
70 | %
71 | % varargin:
72 | % provideDynamicsGradient: when set to true, then the
73 | % parameter f must return []
74 | %------------------------------------------------------------------
75 | % constraints
76 | obj.f = f;
77 | obj.h = h;
78 | obj.g = g;
79 | % variable dimensions
80 | obj.n = n;
81 | obj.m = m;
82 | obj.ne = ne;
83 | % get size of additional constraints
84 | obj.nh = length(h(zeros(n,1),zeros(m,1),zeros(ne,1)));
85 | obj.ng = length(g(zeros(n,1),zeros(m,1),zeros(ne,1)));
86 | % cost functions
87 | obj.fo = fo;
88 | obj.fend = fend;
89 | % optimizer parameters
90 | obj.N = N;
91 | obj.dt = dt;
92 |
93 | % set vector of initial guess for optimization
94 | obj.uguess = zeros(m,N);
95 | obj.eguess = zeros(ne,N);
96 |
97 | % define lower and upper bound constraints
98 | if ~isempty(u_lb)
99 | obj.lb = [repmat(u_lb,obj.N,1); % repeat lower bound for all u0,...,uN-1
100 | -Inf(obj.ne*obj.N,1)];
101 | else
102 | obj.lb = [];
103 | end
104 | if ~isempty(u_ub)
105 | obj.ub = [repmat(u_ub,obj.N,1); % repeat upper bound for all u0,...,uN-1
106 | Inf(obj.ne*obj.N,1)];
107 | else
108 | obj.ub = [];
109 | end
110 | end
111 |
112 |
113 | function numvars = optSize(obj)
114 | %------------------------------------------------------------------
115 | % How many variables we need to optimize?
116 | %
117 | % vars_opt = [x0; u0;...;uN-1; e1;...;eN]
118 | %------------------------------------------------------------------
119 | numvars = obj.N*obj.m + obj.N*obj.ne;
120 | end
121 |
122 |
123 | function [u_opt, e_opt] = optimize(obj, x0, t0, r, UseParallel)
124 | %------------------------------------------------------------------
125 | % Calculate first uptimal control input
126 | %------------------------------------------------------------------
127 |
128 | %-------- Set initial guess for optimization variables -------
129 | varsguess = [obj.uguess(:); obj.eguess(:)];
130 |
131 |
132 | %------------------ Optimize ---------------------------------
133 | assert(all(size(x0)==[obj.n,1]), 'x0 has wrong dimension!!')
134 | assert(numel(varsguess) == obj.optSize(), ...
135 | 'There is something wrong with the code. Number of optimization variables does not match!' );
136 |
137 | % define cost and constr. functions, as a function only of the
138 | % optimazation variables. This is a prerequisite for the function fmincon
139 | costfun = @(vars) obj.costfun(vars, t0, r, x0);
140 | nonlcon = @(vars) obj.nonlcon(vars, t0, x0);
141 |
142 | % define optimizer settings
143 | options = optimoptions('fmincon',...
144 | 'Display','iter',...
145 | 'Algorithm', 'interior-point',... % 'interior-point',... % 'sqp','interior-point'
146 | 'SpecifyConstraintGradient',false,...
147 | 'UseParallel',UseParallel,... %'ConstraintTolerance',obj.tol,...
148 | 'MaxIterations',obj.maxiter);
149 |
150 | % solve optimization problem
151 | [vars_opt,~] = fmincon(costfun,varsguess,[],[],[],[],obj.lb,obj.ub,nonlcon,options);
152 |
153 |
154 | %------------------ Output results ---------------------------
155 | % split variables since vars_opt = [x_opt; u_opt; e_opt]
156 | [u_opt, e_opt] = splitvariables(obj, vars_opt);
157 |
158 | % store current optimization results to use as initial guess for future optimizations
159 | obj.uguess = u_opt(:,[2:end,end]);
160 | obj.eguess = e_opt(:,[2:end,end]);
161 | end
162 |
163 |
164 | function [uvec, evec] = splitvariables(obj, vars)
165 | %------------------------------------------------------------------
166 | % args:
167 | % vars: optimization variables
168 | % out:
169 | % uvec:
170 | % evec:
171 | %------------------------------------------------------------------
172 | % split variables
173 | uvec = vars( (1:obj.N*obj.m) );
174 | evec = vars( (1:obj.N*obj.ne) + length(uvec) );
175 | % reshape the column vector to
176 | uvec = reshape(uvec, obj.m, obj.N);
177 | % reshape the column vector to
178 | evec = reshape(evec, obj.ne, obj.N);
179 | end
180 |
181 |
182 | function [mu_xk,var_xk] = predictStateSequence(obj, mu_x0, var_x0, uk)
183 | %------------------------------------------------------------------
184 | % Propagate mean and covariance of state sequence, given control
185 | % input sequence.
186 | % out:
187 | % mu_xk:
188 | % var_xk:
189 | %------------------------------------------------------------------
190 | mu_xk = zeros(obj.n,obj.N+1);
191 | var_xk = zeros(obj.n,obj.n,obj.N+1);
192 |
193 | mu_xk(:,1) = mu_x0;
194 | var_xk(:,:,1) = var_x0;
195 |
196 | for iN=1:obj.N % [x1,...,xN]
197 | [mu_xk(:,iN+1),var_xk(:,:,iN+1)] = obj.f(mu_xk(:,iN),var_xk(:,:,iN),uk(:,iN));
198 |
199 | % % % % if sum(isnan(mu_xk),'all') || sum(isinf(mu_xk),'all')
200 | % % % % error('%s','System dynamics evaluated to NaN or Inf')
201 | % % % % end
202 |
203 | end
204 | end
205 |
206 |
207 | function cost = costfun(obj, vars, t0, r, mu_x0)
208 | %------------------------------------------------------------------
209 | % Evaluate cost function for the whole horizon, given variables
210 | %------------------------------------------------------------------
211 | % split variables
212 | [uvec, evec] = obj.splitvariables(vars);
213 | var_x0 = zeros(obj.n);
214 |
215 | % calculate state sequence for given control input sequence and x0
216 | [mu_xvec,var_xvec] = obj.predictStateSequence(mu_x0, var_x0, uvec);
217 |
218 | cost = 0;
219 | t = t0;
220 | for iN=1:obj.N % i=0:N-1
221 | % add cost: fo=@(t,mu_x,var_x,u,e,r)
222 | cost = cost + obj.fo(t, mu_xvec(:,iN), var_xvec(:,:,iN), uvec(:,iN), evec(:,iN), r);
223 |
224 | % % % % if sum(isnan(cost),'all') || sum(isinf(cost),'all')
225 | % % % % error('Cost function evaluated to NaN or Inf')
226 | % % % % end
227 |
228 | % update current time
229 | t = t + iN * obj.dt;
230 | end
231 | % final cost: fend=@(t,mu_x,var_x,e,r)
232 | cost = cost + obj.fend(t, mu_xvec(:,end), var_xvec(:,:,end), evec(:,iN), r);
233 |
234 | % normalize cost by horizon size
235 | cost = cost / (obj.N+1);
236 | end
237 |
238 |
239 | function [cineq,ceq] = nonlcon(obj, vars, t0, mu_x0)
240 | % function [cineq,ceq,gradvars_cineq,gradvars_ceq] = nonlcon(obj, vars, t0, x0)
241 | %------------------------------------------------------------------
242 | % Evaluate nonlinear equality and inequality constraints
243 | % out:
244 | % cineq = g(x,u) <= 0 : inequality constraint function
245 | % ceq = h(x,u) == 0 : equality constraint function
246 | % gradx_cineq(x,u): gradient of g(x,u) w.r.t. x
247 | % gradx_ceq(x,u): gradient of h(x,u) w.r.t. x
248 | %------------------------------------------------------------------
249 | % init outputs
250 | ceq = [];
251 | cineq = [];
252 |
253 | % if there are no constraints, then there is nothing to do here
254 | if obj.nh==0 && obj.ng==0
255 | return
256 | end
257 |
258 | % init vectors to speedup calculations
259 | ceq_h = zeros(obj.nh, obj.N);
260 | cineq_g = zeros(obj.ng, obj.N);
261 |
262 | % vars_size = obj.optSize();
263 | % gradvars_cineq = zeros(vars_size,obj.n);
264 |
265 | % split variables
266 | [uvec, evec] = obj.splitvariables(vars);
267 | var_x0 = zeros(obj.n);
268 |
269 | % calculate state sequence for given control input sequence and x0
270 | [mu_xvec,var_xvec] = obj.predictStateSequence(mu_x0, var_x0, uvec);
271 |
272 |
273 | t = t0;
274 | for iN=1:obj.N
275 | % append provided equality constraints(h==0)
276 | ceq_h(:,iN) = obj.h(mu_xvec(:,iN),uvec(:,iN), evec(:,iN));
277 | % provided inequality constraints (g<=0)
278 | cineq_g(:,iN) = obj.g(mu_xvec(:,iN),uvec(:,iN),evec(:,iN));
279 | t = t + iN * obj.dt;
280 | end
281 |
282 | ceq = ceq_h(:);
283 | cineq = cineq_g(:);
284 | end
285 |
286 | end
287 | end
288 |
289 |
290 |
--------------------------------------------------------------------------------
/classes/ODE.m:
--------------------------------------------------------------------------------
1 | classdef ODE < handle
2 |
3 | properties(Constant)
4 | RK4 = [ 0 0 0 0 0;
5 | .5 .5 0 0 0;
6 | .5 0 .5 0 0;
7 | 1 0 0 1 0;
8 | 0 1/6 1/3 1/3 1/6]
9 | RK2 = [0 0 0;
10 | .5 .5 0;
11 | 0 0 1]
12 | RK1 = [0 0;
13 | 0 1]
14 | end
15 |
16 | methods
17 | function obj = ODE()
18 | end
19 | end
20 |
21 | methods(Static)
22 |
23 | function [t,x] = ode1(f, x0, t_end, dt)
24 | [t,x] = ODE.RK(ODE.RK1, f, x0, t_end, dt);
25 | end
26 |
27 | function [t,x] = ode2(f, x0, t_end, dt)
28 | [t,x] = ODE.RK(ODE.RK2, f, x0, t_end, dt);
29 | end
30 |
31 | function [t,x] = ode4(f, x0, t_end, dt)
32 | [t,x] = ODE.RK(ODE.RK4, f, x0, t_end, dt);
33 | end
34 |
35 | function [t,x] = RK(ButcherT, f, x0, t_end, dt)
36 | %------------------------------------------------------------------
37 | % ButcherT: Butcher tableau of order n-1
38 | % f: <@(t,x)> ode function, e.g. f = @(t,x) lambda*x;
39 | %------------------------------------------------------------------
40 | dim = size(x0,1);
41 | ord = size(ButcherT,1)-1;
42 | N = ceil(t_end/dt);
43 | t = zeros(1,N);
44 | x = zeros(dim,N);
45 | x(:,1) = x0;
46 | for it = 2:N+1
47 | t(it) = t(it-1) + dt;
48 | K = zeros(ord,dim);
49 | for ki=1:ord
50 | K(ki,:) = f( t(it-1)+ButcherT(ki,1)*dt , x(:,it-1)+ dt*(ButcherT(ki,2:end)*K)' );
51 | end
52 | x(:,it) = x(:,it-1) + dt * (ButcherT(end,2:end)*K)';
53 | end
54 | t = t(2:end);
55 | x = x(:,2:end);
56 | end
57 | end
58 | end
59 |
60 |
--------------------------------------------------------------------------------
/classes/RaceTrack.m:
--------------------------------------------------------------------------------
1 | %------------------------------------------------------------------
2 | % Programed by:
3 | % - Lucas Rath (lucasrm25@gmail.com)
4 | % -
5 | % -
6 | %------------------------------------------------------------------
7 |
8 | classdef RaceTrack < handle
9 | %------------------------------------------------------------------
10 | % The Race Track constitutes of a left lane, right lane and center line
11 | % coordinates, which are parametrized by the traveled distance (center
12 | % line length).
13 | %
14 | % This also means that the center line coordinate, pos_c = (x_c,y_c), the
15 | % track radius and the track orientation can be obtained as a function of
16 | % the traveled distance:
17 | %
18 | % x_c(dist), y_c(dist), psi_c(dist), R_c(dist) -> see method getTrackInfo
19 | %
20 | %
21 | % % Example how to create and use this class:
22 | %
23 | % % load a predefined track called track01:
24 | % [trackdata, x0, th0, w] = RaceTrack.loadTrack_02();
25 | %
26 | % % init object:
27 | % track = RaceTrack(trackdata, x0, th0, w);
28 | %
29 | % % plot track:
30 | % track.plotTrack()
31 | %
32 | % % what is the coordinate of the track centerline, track radius and
33 | % % tangent angle for a centerline distance of 1000 meters ?
34 | % [pos_c, psi_c, R_c] = track.getTrackInfo(1000)
35 | %
36 | % % lets say the vehicle is at position [10 1]' (in inertial coordinates)
37 | % vehicle_pos = [10 5]';
38 | %
39 | % % How far has the vehicle traveled along the centerline if the vehicle
40 | % % position is [10 1]' ? (the reference used is the closest point in the track to the vehicle)
41 | % vehicle_dist = track.getTrackDistance(vehicle_pos)
42 | %
43 | % % what is the lag and contour error for 2 meters ahead (along the track centerline)?
44 | % targetTrackDist = vehicle_dist + 2;
45 | % [lag_error, countour_error, offroad_error] = track.getVehicleDeviation(vehicle_pos, targetTrackDist)
46 | %
47 | % % lets verify visually if the calculation is correct!!
48 | % track.plotTrack();
49 | % hold on;
50 | % sc1 = scatter(vehicle_pos(1), vehicle_pos(2), 50, 'filled', 'DisplayName','vehicle pos')
51 | % [pos_c, psi_c, R_c] = track.getTrackInfo(vehicle_dist);
52 | % sc2 = scatter(pos_c(1),pos_c(2), 50, 'filled', 'DisplayName','closest track point')
53 | % [pos_cT, psi_cT, R_cT] = track.getTrackInfo(targetTrackDist);
54 | % sc3 = scatter(pos_cT(1),pos_cT(2), 50, 'filled', 'DisplayName','target track point')
55 | % legend([sc1,sc2,sc3])
56 | %
57 | %------------------------------------------------------------------
58 |
59 | properties
60 | end
61 |
62 | properties(SetAccess=public)
63 | w
64 | track_l % <2,L>: left track coordinates
65 | track_r % <2,L>: right track coordinates
66 |
67 | track_c
68 | psi_c
69 | R_c
70 |
71 | dist % traveled distance from the starting point
72 | end
73 |
74 | methods
75 | function obj = RaceTrack(trackdata, x0, th0, w)
76 | %------------------------------------------------------------------
77 | % object constructor
78 | % args:
79 | % x0: <2,1> initial vehicle position
80 | % tho: <1> initial vehicle orientation
81 | % w: <1> track width
82 | % trackdata: sequence of track instructions
83 | %------------------------------------------------------------------
84 | [obj.track_l, obj.track_r, obj.psi_c] = obj.generateTrackPoints(trackdata, x0, th0, w);
85 | obj.track_c = (obj.track_r + obj.track_l)/2;
86 |
87 | % parametrize track by traveled distance
88 | trackdisplacements = vecnorm( conv2(obj.track_c,[1,-1],'same') );
89 | obj.dist = cumsum( trackdisplacements );
90 |
91 | % remove repeated track points
92 | idx_repeated = (trackdisplacements < eps);
93 |
94 | obj.dist = obj.dist(:,~idx_repeated);
95 | obj.track_l = obj.track_l(:,~idx_repeated);
96 | obj.track_r = obj.track_r(:,~idx_repeated);
97 | obj.track_c = obj.track_c(:,~idx_repeated);
98 | obj.psi_c = obj.psi_c(:,~idx_repeated);
99 |
100 | % figure
101 | % plot(obj.dist)
102 |
103 | % distFromCenter = sqrt(obj.track_c(1,:).^2 + obj.track_c(2,:).^2);
104 | % obj.dist = cumsum( abs(conv(distFromCenter,[1+1e-10,-1],'same')) );
105 |
106 | % track width
107 | obj.w = w;
108 | end
109 |
110 |
111 | function [pos_c, psi_c, R_c] = getTrackInfo(obj,dist)
112 | %------------------------------------------------------------------
113 | % Given a distance 'dist' returns the track centerline carthesian
114 | % position pos_c=(x_c,y_c), the track orientation psi_c, and the
115 | % track radius R_c
116 | %------------------------------------------------------------------
117 | dist = mod(dist,max(obj.dist));
118 | pos_c = interp1(obj.dist',obj.track_c',dist,'linear','extrap')';
119 | psi_c = interp1(obj.dist',obj.psi_c',dist,'linear','extrap');
120 | R_c = obj.w/2;
121 | end
122 |
123 | function dist = getTrackDistance(obj, pos_vehicle, varargin)
124 | %------------------------------------------------------------------
125 | % Given the vehicle position, calculates the traveled distance
126 | % of the vehicle 'dist' along the centerline of the track
127 | %------------------------------------------------------------------
128 | if length(varargin)==1
129 | olddist = varargin{1};
130 | idxsearchspace = find( obj.dist > olddist-10 & obj.dist < olddist+20 );
131 | else
132 | idxsearchspace = 1:length(obj.dist);
133 | end
134 | [~,I] = pdist2(obj.track_c(:,idxsearchspace)',pos_vehicle','euclidean','Smallest',1);
135 | I = mod(I+idxsearchspace(1)-1, length(obj.dist))+1;
136 | dist = obj.dist(I);
137 | end
138 |
139 | function [lag_error, countour_error, offroad_error, orientation_error] = ...
140 | getVehicleDeviation(obj, pos_vehicle, psi_vehicle, track_dist)
141 | %------------------------------------------------------------------
142 | % outs:
143 | % - lag_error, countour_error: lag and countour errors from a
144 | % track position that is given by the traveled distance along
145 | % the centerline. Normalized by track radius at that point
146 | % - offroad_error: \in [0 Inf] how far the vehicle is from the
147 | % track borders. Normalized by track radius at that point
148 | % - orientation_error \in [0 1], where 0 means that vehicle and
149 | % track have the same orientation and 1 mean they are orthogonal
150 | %
151 | % NOTE:
152 | % - positive lag_error means that the vehicle is lagging behind
153 | % - positive countour_error means the vehicle is to the right size
154 | % of the track
155 | %------------------------------------------------------------------
156 |
157 | % get information (x,y,track radius and track orientation) of the point
158 | % in the track that corresponds to a traveled distance of 'dist' meters.
159 | [pos_c, psi_c, R_c] = obj.getTrackInfo(track_dist);
160 |
161 | % ---------------------------------------------------------------------
162 | % contour and lag error
163 | % ---------------------------------------------------------------------
164 | % vehicle position in inertial coordinates
165 | I_x = pos_vehicle(1);
166 | I_y = pos_vehicle(2);
167 | % rotation to a frame with x-axis tangencial to the track (T frame)
168 | A_TI = [ cos(psi_c) -sin(psi_c);
169 | sin(psi_c) cos(psi_c)];
170 | % error in the inertial coordinates
171 | I_error = pos_c - [I_x;I_y];
172 | % error in the T frame [lag_error; contouring_error]
173 | T_error = A_TI' * I_error;
174 |
175 | % get lag and countour error (normalized by the road radius).
176 | % contour_error=+-1, when we are at the border
177 | % contour_error=0, when we are at the middle at the track
178 | % lag_error>0, when we are lagging behind
179 | lag_error = T_error(1) / R_c;
180 | countour_error = T_error(2) / R_c;
181 |
182 | % calculate normalized offroad_error (desired is to be < 0)
183 | offroad_error = norm(T_error)/R_c - 1;
184 |
185 | % calculate orientation error (\in [0 1]) - cosinus distance
186 | orientation_error = 1 - abs([cos(psi_c); sin(psi_c)]' * [cos(psi_vehicle); sin(psi_vehicle)]);
187 | end
188 |
189 |
190 | function h_fig = plotTrack(obj)
191 | % -------------------------------------------------------------
192 | % plot track asphalt and boarders. Returns the figure handle
193 | % -------------------------------------------------------------
194 | h_fig = figure('Color','w','Position',[468 128 872 633]);
195 | title('Racing Track')
196 | hold on;
197 | grid on;
198 | axis equal;
199 |
200 | % -------------------------------------------------------------
201 | % plot track asphalt
202 | % -------------------------------------------------------------
203 | n = length(obj.track_l);
204 | v = [obj.track_l(:,1:n)'; obj.track_r(:,1:n)']; %
205 | f = [1:n-1 ; 2:n; n+2:2*n; n+1:2*n-1]';
206 | patch('Faces',f,'Vertices',v,'FaceColor',[0.95 0.95 0.95],'LineStyle', 'none')
207 |
208 | % -------------------------------------------------------------
209 | % plot track borders
210 | % -------------------------------------------------------------
211 | h_ltrack = plot(obj.track_l(1,:),obj.track_l(2,:),'k');
212 | h_rtrack = plot(obj.track_r(1,:),obj.track_r(2,:),'k');
213 | end
214 | end
215 |
216 |
217 | methods(Static)
218 |
219 | function [laptimes, idxnewlaps] = getLapTimes( trackDist, dt)
220 | %------------------------------------------------------------------
221 | % Calculate laptimes.
222 | % args:
223 | % trackdist: a vector of centerline track distances. The vector
224 | % must have been reset to zero whenever one lap is
225 | % completed, i.e. 0 < trackdist(i)< trackLength, forall i
226 | % dt: simulation time step
227 | %------------------------------------------------------------------
228 | % calc lap times
229 | idxnewlaps = find( conv(trackDist, [1 -1]) < -10 );
230 | laptimes = conv(idxnewlaps, [1,-1], 'valid') * dt;
231 | end
232 |
233 | function dispLapTimes(laptimes)
234 | %------------------------------------------------------------------
235 | % Display laptimes.
236 | % args:
237 | % laptimes: vector of lap times
238 | %------------------------------------------------------------------
239 | % calc best lap time
240 | [bestlaptime,idxbestlap] = min(laptimes);
241 |
242 | fprintf('\n--------------- LAP RECORD -------------------\n');
243 | fprintf('------ (Best Lap: %.2d laptime: %4.2f) ------\n\n',idxbestlap,bestlaptime);
244 | for i=1:numel(laptimes)
245 | if i==idxbestlap
246 | fprintf(2,' (best lap)-> ')
247 | else
248 | fprintf('\t\t');
249 | end
250 | fprintf('Lap %.2d laptime: %4.2fs',i,laptimes(i));
251 | fprintf(2,' (+%.3fs)\n',laptimes(i)-bestlaptime)
252 |
253 | end
254 | fprintf('--------------- LAP RECORD -------------------\n');
255 |
256 | % figure('Color','w','Position',[441 389 736 221]); hold on; grid on;
257 | % plot(laptimes,'-o')
258 | % xlabel('Lap')
259 | % ylabel('Lap time [s]')
260 | end
261 |
262 |
263 | function [trackdata, x0, th0, w] = loadTrack_01()
264 | %------------------------------------------------------------------
265 | % Racetrack examples, to be used with class constructor. Ex:
266 | % [trackdata, x0, th0, w] = RaceTrack.loadTrack_01()
267 | % trackobject = RaceTrack(trackdata, x0, th0, w)
268 | %------------------------------------------------------------------
269 | x0 = [0;0];
270 | th0 = 0;
271 | w = 6;
272 | trackdata = {
273 | 's',14;
274 | 'c',{15,-90};
275 | 's',5;
276 | 'c',{4,90};
277 | 'c',{4,-90};
278 | 's',5;
279 | 'c',{3.5,-90};
280 | 's',16;
281 | 'c',{3.5,-120};
282 | 's',10;
283 | 'c',{10,120};
284 | 's',10;
285 | 'c',{5,90};
286 | 's',5;
287 | 'c',{5,150};
288 | 's',5;
289 | 'c',{3.2,-180};
290 | 's',12;
291 | 'c',{10,-150};
292 | 's',12.3;
293 | 'c',{12,-90};
294 | };
295 | end
296 |
297 | function [trackdata, x0, th0, w] = loadTrack_02()
298 | %------------------------------------------------------------------
299 | % Racetrack examples, to be used with class constructor. Ex:
300 | % [trackdata, x0, th0, w] = RaceTrack.loadTrack_01()
301 | % trackobject = RaceTrack(trackdata, x0, th0, w)
302 | %------------------------------------------------------------------
303 | x0 = [0;0];
304 | th0 = 0;
305 | w = 6;
306 | trackdata = {
307 | 's',14;
308 | 'c',{15,-90};
309 | 's',5;
310 | 'c',{4,90};
311 | 'c',{4,-90};
312 | 's',5;
313 | 'c',{3.5,-90};
314 | 's',16;
315 | 'c',{3.5,-120};
316 | 's',10;
317 | 'c',{10,120};
318 | 's',10;
319 | 'c',{5,90};
320 | 's',5;
321 | 'c',{5,90};
322 | 'c',{7,-180};
323 | 's',2.3;
324 | 'c',{10,-90};
325 | 's',14.6;
326 | 'c',{12,-90};
327 | };
328 | end
329 |
330 | function [track_l,track_r,psi_c] = generateTrackPoints(trackdata, x0, th0, w)
331 | %------------------------------------------------------------------
332 | % Help function to generate Track datapoints, given track
333 | % instructions (turn left 30deg with radius=10m, go straight for 20m,
334 | % turn right 45deg with radius 10m,...)
335 | %------------------------------------------------------------------
336 | track_l = [];
337 | track_r = [];
338 | psi_c = [];
339 |
340 | ds = 0.5;
341 | dth = deg2rad(5);
342 |
343 | A_z = @(th) [cos(th) -sin(th);
344 | sin(th) cos(th)];
345 |
346 | A_IV = A_z(th0);
347 | r_IV = x0;
348 |
349 | for idx = 1:size(trackdata,1)
350 | if strcmp( trackdata{idx,1},'s')
351 | % distance
352 | dist = trackdata{idx,2};
353 | % calculate new track
354 | newtrack_l = r_IV + w/2*A_IV(:,2) + (ds:ds:dist).*A_IV(:,1);
355 | newtrack_r = r_IV - w/2*A_IV(:,2) + (ds:ds:dist).*A_IV(:,1);
356 | newpsi_c = th0 * ones(1,size(newtrack_l,2));
357 | % update current position
358 | r_IV = r_IV + dist*A_IV(:,1);
359 |
360 | elseif strcmp( trackdata{idx,1},'c')
361 | % center curve radius
362 | rad = trackdata{idx,2}{1};
363 | % curvature
364 | ang = deg2rad( trackdata{idx,2}{2} );
365 |
366 | % th = 0:dth:abs(ang);
367 | % arc = [cos(th); sin(th)];
368 | if ang > 0
369 | th = dth:dth:ang;
370 | arc = [cos(th); sin(th)];
371 | newtrack_l = arc*(rad-w/2);
372 | newtrack_r = arc*(rad+w/2);
373 | A = [ 0 1
374 | -1 0];
375 | newtrack_l = A*newtrack_l + [0;1]*rad;
376 | newtrack_r = A*newtrack_r + [0;1]*rad;
377 | else
378 | th = -dth:-dth:ang;
379 | arc = [cos(th); sin(th)];
380 | newtrack_l = arc*(rad+w/2);
381 | newtrack_r = arc*(rad-w/2);
382 | A = [0 -1
383 | 1 0];
384 | newtrack_l = A*newtrack_l + [0;-1]*rad;
385 | newtrack_r = A*newtrack_r + [0;-1]*rad;
386 | end
387 |
388 | newtrack_l = A_IV * newtrack_l + r_IV;
389 | newtrack_r = A_IV * newtrack_r + r_IV;
390 | newpsi_c = th0 + th;
391 | th0 = newpsi_c(end);
392 |
393 | A_IV = A_z(ang) * A_IV;
394 | r_IV = 0.5* (newtrack_l(:,end) + newtrack_r(:,end));
395 | else
396 | error('Not implemented');
397 | end
398 |
399 | track_l = [track_l newtrack_l];
400 | track_r = [track_r newtrack_r];
401 | psi_c = [psi_c newpsi_c];
402 | end
403 | end
404 | end
405 | end
406 |
407 |
--------------------------------------------------------------------------------
/classes/SingleTrackAnimation.m:
--------------------------------------------------------------------------------
1 | %------------------------------------------------------------------
2 | % Programed by:
3 | % - Lucas Rath (lucasrm25@gmail.com)
4 | % -
5 | % -
6 |
7 | % Generate Animation for the main_singletrack.m script
8 | %------------------------------------------------------------------
9 |
10 |
11 | classdef SingleTrackAnimation < handle
12 |
13 | properties
14 | % object that contains track coordinates
15 | racetrack @ RaceTrack
16 |
17 | % variables that contain history of vehicle states and inputs to be ploted
18 | mu_x_pred_opt
19 | var_x_pred_opt
20 | u_pred_opt
21 | x_ref
22 |
23 | % Track animation handles
24 | h_fig
25 | h_ltrack
26 | h_rtrack
27 | h_mu_x_pred_opt
28 | h_var_x_pred_opt
29 | h_x_ref
30 | h_x_trace
31 | h_car
32 |
33 | % Scope handles
34 | h_scopex
35 | h_scopeu
36 |
37 | % covariance ellipse properties
38 | ell_npoints = 30 % number of points to make an ellipse
39 | ell_level = 2 % plot ell_level*sigma ellipse curves
40 |
41 | k % current time step
42 | N % horizon length
43 | end
44 |
45 | methods
46 | function obj = SingleTrackAnimation(racetrack, mu_x_pred_opt, var_x_pred_opt, u_pred_opt, x_ref)
47 | obj.racetrack = racetrack;
48 | obj.mu_x_pred_opt = mu_x_pred_opt;
49 | obj.var_x_pred_opt = var_x_pred_opt;
50 | obj.u_pred_opt = u_pred_opt;
51 | obj.x_ref = x_ref;
52 |
53 | % get horizon length from inputs
54 | obj.N = size(obj.mu_x_pred_opt,2);
55 | end
56 |
57 | function initTrackAnimation(obj)
58 | % -----------------------------------------------------------------
59 | % Init Animation of the vehicle driving the track. Please call
60 | % updateTrackAnimation(k) to move forward with the animation.
61 | % -----------------------------------------------------------------
62 | obj.h_fig = figure('Color','w','Position',[468 128 872 633]);
63 | title('Adaptive Gaussian-Process MPC')
64 | hold on;
65 | grid on;
66 | axis equal;
67 |
68 | % -------------------------------------------------------------
69 | % plot track asphalt
70 | % -------------------------------------------------------------
71 | n = length(obj.racetrack.track_l);
72 | v = [obj.racetrack.track_l(:,1:n)'; obj.racetrack.track_r(:,1:n)']; %
73 | f = [1:n-1 ; 2:n; n+2:2*n; n+1:2*n-1]';
74 | patch('Faces',f,'Vertices',v,'FaceColor',[0.95 0.95 0.95],'LineStyle', 'none')
75 |
76 | % -------------------------------------------------------------
77 | % plot track borders
78 | % -------------------------------------------------------------
79 | obj.h_ltrack = plot(obj.racetrack.track_l(1,:),obj.racetrack.track_l(2,:),'k');
80 | obj.h_rtrack = plot(obj.racetrack.track_r(1,:),obj.racetrack.track_r(2,:),'k');
81 |
82 | % -------------------------------------------------------------
83 | % setup state predictions
84 | % -------------------------------------------------------------
85 | k = 1;
86 |
87 | % trace vehicle path
88 | obj.h_x_trace = patch(obj.mu_x_pred_opt(1,1,k),obj.mu_x_pred_opt(2,1,k),obj.mu_x_pred_opt(3,1,k),...
89 | 'EdgeColor','interp',...
90 | 'Marker','none',...
91 | 'LineWidth',2,...
92 | 'EdgeAlpha',0.5);
93 |
94 | % plot car
95 | obj.h_car = patch('Faces',1:4,'Vertices',NaN(4,2),...
96 | 'EdgeColor','black',...
97 | 'FaceColor','none',...
98 | 'LineWidth',1);
99 |
100 | % reference trajectory
101 | obj.h_x_ref = plot(NaN,NaN,...
102 | '-','LineWidth',1.0, 'Marker','o',...
103 | 'MarkerFaceColor',[0.5 0.5 0.5],'MarkerEdgeColor','k', 'Color','k',...
104 | 'MarkerSize',5,...
105 | 'DisplayName','Projected optimal trajectory',...
106 | 'XDataSource', 'obj.x_ref(1,:,obj.k)',...
107 | 'YDataSource', 'obj.x_ref(2,:,obj.k)');
108 |
109 | % optimal prediction means
110 | obj.h_mu_x_pred_opt = patch(obj.mu_x_pred_opt(1,:,k),obj.mu_x_pred_opt(2,:,k),obj.mu_x_pred_opt(3,:,k),...
111 | 'EdgeColor','interp','Marker','o',...
112 | 'MarkerSize',5,...
113 | 'MarkerFaceColor','flat',...
114 | 'DisplayName','Optimal trajectory' );
115 |
116 | % plot prediction covariance ellipses
117 | ell = NaN(obj.ell_level, obj.ell_npoints);
118 | for i=1:obj.N
119 | obj.h_var_x_pred_opt{i} = patch('Faces',1:obj.ell_npoints,'Vertices',ell','FaceColor',[1 0 0],'FaceAlpha',0.3,'LineStyle', 'none');
120 | end
121 |
122 |
123 | % display legend, colorbar, etc.
124 | leg = legend([obj.h_mu_x_pred_opt,obj.h_x_ref],'Location','northeast');
125 | c = colorbar;
126 | c.Label.String = 'Vehicle predicted velocity [m/s]';
127 | caxis([5 25])
128 |
129 | % lock up axis limits
130 | xlim('manual')
131 | ylim('manual')
132 | drawnow
133 | end
134 |
135 | function initScope(obj)
136 | % -----------------------------------------------------------------
137 | % Init Scope that shows vehicle prediction signals
138 | % -----------------------------------------------------------------
139 | obj.h_scopex = figure('Position',[-1006 86 957 808]);
140 | names = {'I-x','I-y','psi','V-vx','V-vy','psidot','track_dist'};
141 | angles = [0 0 1 0 0 1 0];
142 | for i=1:numel(names)
143 | subplot(4,2,i);
144 | p = plot(NaN);
145 | if angles(i)
146 | p.YDataSource = sprintf('rad2deg(obj.mu_x_pred_opt(%d,:,obj.k))',i);
147 | else
148 | p.YDataSource = sprintf('obj.mu_x_pred_opt(%d,:,obj.k)',i);
149 | end
150 | xlabel('Prediction horizon')
151 | grid on;
152 | title(names{i});
153 | end
154 |
155 | obj.h_scopeu = figure('Position',[-1879 93 867 795]);
156 | names = {'delta','T','Track vel'};
157 | angles = [1 0 0];
158 | for i=1:numel(names)
159 | subplot(2,2,i);
160 | p = plot(NaN);
161 | if angles(i)
162 | p.YDataSource = sprintf('rad2deg(obj.u_pred_opt(%d,:,obj.k))',i);
163 | else
164 | p.YDataSource = sprintf('obj.u_pred_opt(%d,:,obj.k)',i);
165 | end
166 | xlabel('Prediction horizon')
167 | grid on;
168 | title(names{i});
169 | end
170 | end
171 |
172 | function status = updateTrackAnimation(obj,k)
173 | % -----------------------------------------------------------------
174 | % Update track animation with the current time step k. Beware
175 | % that the vectors obj.mu_x_pred_opt and obj.x_ref must have the
176 | % correct values at position (:,:,k)
177 | % -----------------------------------------------------------------
178 | status = 0;
179 | if k < 1 || k > size(obj.mu_x_pred_opt,3) || any(isnan(obj.mu_x_pred_opt(:,:,k)),'all')
180 | return;
181 | end
182 |
183 | vel = vecnorm(obj.mu_x_pred_opt(4:5,:,k));
184 | % update predicted trajectory
185 | obj.h_mu_x_pred_opt.XData = [obj.mu_x_pred_opt(1,:,k) 0];
186 | obj.h_mu_x_pred_opt.YData = [obj.mu_x_pred_opt(2,:,k) NaN];
187 | obj.h_mu_x_pred_opt.CData = [vel min(vel)];
188 |
189 | % update state covariances
190 | for i=1:obj.N
191 | mean = obj.mu_x_pred_opt(1:2,i,k);
192 | var = obj.var_x_pred_opt(1:2,1:2,i,k);
193 | if all(svd(var)>1e-6)
194 | ell = sigmaEllipse2D(mean, var, obj.ell_level, obj.ell_npoints);
195 | else
196 | ell = repmat(mean,1,obj.ell_npoints);
197 | end
198 | obj.h_var_x_pred_opt{i}.Vertices = ell';
199 | end
200 |
201 | % update projected reference
202 | obj.h_x_ref.XData = obj.x_ref(1,:,k);
203 | obj.h_x_ref.YData = obj.x_ref(2,:,k);
204 |
205 | % update trace
206 | veltrace = vecnorm(squeeze(obj.mu_x_pred_opt(4:5,1,1:k)));
207 | obj.h_x_trace.XData = [squeeze(obj.mu_x_pred_opt(1,1,1:k))' NaN];
208 | obj.h_x_trace.YData = [squeeze(obj.mu_x_pred_opt(2,1,1:k))' NaN];
209 | obj.h_x_trace.CData = [veltrace NaN];
210 |
211 | % update car
212 | carpos = obj.mu_x_pred_opt(1:2,1,k); %[0;0]
213 | psi = obj.mu_x_pred_opt(3,1,k); %deg2rad(30);
214 | car_w = 1;
215 | car_l = 2;
216 | V_carpoints = [[car_l/2;car_w/2],[car_l/2;-car_w/2],[-car_l/2;-car_w/2],[-car_l/2;car_w/2]];
217 | I_carpoints = [cos(psi) -sin(psi);
218 | sin(psi) cos(psi)] * V_carpoints + carpos;
219 | obj.h_car.Vertices = I_carpoints';
220 |
221 | % no error when updating graphics
222 | status = 1;
223 | end
224 |
225 | function updateScope(obj,k)
226 | % -----------------------------------------------------------------
227 | % Update scope with signals from the current time step k. Beware
228 | % that the vectors obj.mu_x_pred_opt and obj.u_pred_opt must have
229 | % the correct values at position (:,:,k)
230 | % -----------------------------------------------------------------
231 | obj.k = k;
232 | refreshdata(obj.h_scopex,'caller');
233 | refreshdata(obj.h_scopeu,'caller');
234 | end
235 |
236 | function recordvideo(obj, videoName, format, FrameRate)
237 | % -----------------------------------------------------------------
238 | % Record video of the track animation
239 | % -----------------------------------------------------------------
240 | % video rec
241 | videoframes = struct('cdata',[],'colormap',[]);
242 | obj.initTrackAnimation();
243 | xlim manual
244 | ylim manual
245 | for k=1:size(obj.mu_x_pred_opt,3)
246 | status = obj.updateTrackAnimation(k);
247 | if status == 0
248 | break;
249 | end
250 | videoframes(k) = getframe(obj.h_fig);
251 | end
252 | % -----------------------------------------------------------------
253 | % Save video
254 | % -----------------------------------------------------------------
255 | writerObj = VideoWriter(videoName,format);
256 | writerObj.FrameRate = FrameRate;
257 | open(writerObj);
258 | % Write out all the frames.
259 | numberOfFrames = length(videoframes);
260 | for k=1:numberOfFrames
261 | writeVideo(writerObj, videoframes(k));
262 | end
263 | close(writerObj);
264 | disp('Video saved successfully')
265 | end
266 | end
267 | end
--------------------------------------------------------------------------------
/classes/fp.m:
--------------------------------------------------------------------------------
1 | %------------------------------------------------------------------
2 | % Programed by:
3 | % - Lucas Rath (lucasrm25@gmail.com, https://github.com/lucasrm25)
4 | % -
5 | % -
6 |
7 | % Generate fancy plots (fp)
8 | %------------------------------------------------------------------
9 |
10 | classdef fp
11 | properties
12 | end
13 |
14 | methods(Static)
15 |
16 | function savefig(fname,varargin)
17 |
18 | p = inputParser;
19 |
20 | defaultFormat = 'epsc';
21 | expectedFormat = {'epsc','svg','png','jpg'};
22 | addParameter(p,'format',defaultFormat, @(x) any(validatestring(x,expectedFormat)));
23 | addParameter(p,'fighandle',gcf, @(x) isa(x,'matlab.ui.Figure'));
24 | addParameter(p,'folder',fullfile(pwd,'images'), @(x) isa(x,'char'));
25 | parse(p,varargin{:});
26 |
27 | if ~ exist(p.Results.folder,'dir'), mkdir(p.Results.folder); end
28 | % set(p.Results.fighandle.CurrentAxes,'LooseInset',p.Results.fighandle.CurrentAxes.TightInset)
29 | saveas(p.Results.fighandle, fullfile(p.Results.folder,fname), p.Results.format)
30 | end
31 |
32 | function fig = f()
33 | fig = figure('Color','white');
34 | hold on, grid on;
35 | end
36 |
37 | function latexcode = m2latex(matrix)
38 | if ~isa(matrix,'sym')
39 | matrix = sym(matrix);
40 | end
41 | latexcode = latex(vpa(simplify(matrix)));
42 | if numel(latexcode)>=6 && strcmp(latexcode(6),'(') && strcmp(latexcode(end),')')
43 | latexcode(6) = '[';
44 | latexcode(end) = ']';
45 | end
46 | clipboard('copy',latexcode);
47 | end
48 |
49 | function str = figpos()
50 | a = gcf;
51 | pos = a.Position;
52 | str = ['figure(''Color'',''white'',''Position'',[' num2str(pos) ']);'];
53 | clipboard('copy',str);
54 | end
55 |
56 | % varargin{1}: line opacity
57 | function cl = getColor(n, varargin)
58 | colrs = lines(max(n));
59 | if nargin >= 2
60 | opacity = varargin{1};
61 | else
62 | opacity = [];
63 | end
64 | cl = [colrs(n,:), repmat(opacity,numel(n),1)];
65 | end
66 | end
67 | end
68 |
--------------------------------------------------------------------------------
/functions/drawpendulum.m:
--------------------------------------------------------------------------------
1 | function drawpendulum(t,x, m1, m2,g,l)
2 | %% drawpendulum
3 | % adapted from Patrick Suhms code
4 | % (source: https://github.com/PatrickSuhm/LqrControlTutorial)
5 | % ------------------------------------------------------------------------
6 | % input:
7 | % t ... time (,1)
8 | % x ... state ([s, ds, phi, dphi])
9 | % m1 ... mass of the cart
10 | % m2 ... mass of pole
11 | % ------------------------------------------------------------------------
12 |
13 | s = x(1,:); % position
14 | phi = x(3,:); % angle
15 |
16 |
17 | % dimensions of cart and mass
18 | W = 1*sqrt(m1/5); % cart width
19 | H = .5*sqrt(m1/5); % cart height
20 | mr = .3*sqrt(m2); % mass radius
21 |
22 | % position mass
23 | px = s - l*sin(phi);
24 | py = H/2 + l*cos(phi);
25 |
26 | % create new figure and
27 | figure
28 | plot([-25 25],[0 0],'w','LineWidth',2)
29 | hold on
30 |
31 | % plot the cart
32 | h1=rectangle('Position',[s(1)-W/2,0,W,H],'Curvature',.1,'FaceColor',[1 0.1 0.1],'EdgeColor',[1 1 1]);
33 |
34 | % plot the pole
35 | h2=plot([s(1) px(1)],[0 py(1)],'w','LineWidth',2);
36 | h3=rectangle('Position',[px(1)-mr/2,py(1)-mr/2,mr,mr],'Curvature',1,'FaceColor',[.3 0.3 1],'EdgeColor',[1 1 1]);
37 | h4=text(0.95, 1.5, ['phi: ', num2str(1)]);
38 | set(h4,'color','w', 'fontsize', 14);
39 | xlim([-15 5]);
40 | ylim([-5 5]);
41 | set(gca,'Color','k','XColor','w','YColor','w')
42 | set(gcf,'Color','k')
43 |
44 | pause(0.1)
45 | tic
46 |
47 | % animation in a for loop
48 | for k=1:length(t)
49 |
50 | % update pole and cart position
51 | set(h1, 'position',[s(k)-W/2,0,W,H]);
52 | set(h2, 'XData',[s(k) px(k)], 'YData', [H/2 py(k)]);
53 | set(h3, 'position', [px(k)-mr/2,py(k)-mr/2,mr,mr]);
54 | set(h4, 'string',['time: ', num2str(t(k))]);
55 |
56 | drawnow();
57 | pause(0.1)
58 |
59 | % strop the animation when q is pressed
60 | % if k==length(t)
61 | % stop
62 | % end
63 |
64 | % meassure the time and create a fixed time loop
65 | % t2=toc;
66 | % while t2 < t(k)
67 | % t2 = toc;
68 | % end
69 | % t3(k) = t2;
70 |
71 | end
72 |
--------------------------------------------------------------------------------
/functions/logdet.m:
--------------------------------------------------------------------------------
1 | function v = logdet(A, op)
2 | %LOGDET Computation of logarithm of determinant of a matrix
3 | %
4 | % v = logdet(A);
5 | % computes the logarithm of determinant of A.
6 | %
7 | % Here, A should be a square matrix of double or single class.
8 | % If A is singular, it will returns -inf.
9 | %
10 | % Theoretically, this function should be functionally
11 | % equivalent to log(det(A)). However, it avoids the
12 | % overflow/underflow problems that are likely to
13 | % happen when applying det to large matrices.
14 | %
15 | % The key idea is based on the mathematical fact that
16 | % the determinant of a triangular matrix equals the
17 | % product of its diagonal elements. Hence, the matrix's
18 | % log-determinant is equal to the sum of their logarithm
19 | % values. By keeping all computations in log-scale, the
20 | % problem of underflow/overflow caused by product of
21 | % many numbers can be effectively circumvented.
22 | %
23 | % The implementation is based on LU factorization.
24 | %
25 | % v = logdet(A, 'chol');
26 | % If A is positive definite, you can tell the function
27 | % to use Cholesky factorization to accomplish the task
28 | % using this syntax, which is substantially more efficient
29 | % for positive definite matrix.
30 | %
31 | % Remarks
32 | % -------
33 | % logarithm of determinant of a matrix widely occurs in the
34 | % context of multivariate statistics. The log-pdf, entropy,
35 | % and divergence of Gaussian distribution typically comprises
36 | % a term in form of log-determinant. This function might be
37 | % useful there, especially in a high-dimensional space.
38 | %
39 | % Theoretially, LU, QR can both do the job. However, LU
40 | % factorization is substantially faster. So, for generic
41 | % matrix, LU factorization is adopted.
42 | %
43 | % For positive definite matrices, such as covariance matrices,
44 | % Cholesky factorization is typically more efficient. And it
45 | % is STRONGLY RECOMMENDED that you use the chol (2nd syntax above)
46 | % when you are sure that you are dealing with a positive definite
47 | % matrix.
48 | %
49 | % Examples
50 | % --------
51 | % % compute the log-determinant of a generic matrix
52 | % A = rand(1000);
53 | % v = logdet(A);
54 | %
55 | % % compute the log-determinant of a positive-definite matrix
56 | % A = rand(1000);
57 | % C = A * A'; % this makes C positive definite
58 | % v = logdet(C, 'chol');
59 | %
60 | % Copyright 2008, Dahua Lin, MIT
61 | % Email: dhlin@mit.edu
62 | %
63 | % This file can be freely modified or distributed for any kind of
64 | % purposes.
65 | %
66 | %% argument checking
67 | assert(isfloat(A) && ndims(A) == 2 && size(A,1) == size(A,2), ...
68 | 'logdet:invalidarg', ...
69 | 'A should be a square matrix of double or single class.');
70 | if nargin < 2
71 | use_chol = 0;
72 | else
73 | assert(strcmpi(op, 'chol'), ...
74 | 'logdet:invalidarg', ...
75 | 'The second argument can only be a string ''chol'' if it is specified.');
76 | use_chol = 1;
77 | end
78 | %% computation
79 | if use_chol
80 | v = 2 * sum(log(diag(chol(A))));
81 | else
82 | [L, U, P] = lu(A);
83 | du = diag(U);
84 | c = det(P) * prod(sign(du));
85 | v = log(c) + sum(log(abs(du)));
86 | end
--------------------------------------------------------------------------------
/functions/sigmaEllipse2D.m:
--------------------------------------------------------------------------------
1 | function [ xy ] = sigmaEllipse2D( mu, Sigma, level, npoints )
2 | %SIGMAELLIPSE2D generates x,y-points which lie on the ellipse describing
3 | % a sigma level in the Gaussian density defined by mean and covariance.
4 | %
5 | %Input:
6 | % MU [2 x 1] Mean of the Gaussian density
7 | % SIGMA [2 x 2] Covariance matrix of the Gaussian density
8 | % LEVEL Which sigma level curve to plot. Can take any positive value,
9 | % but common choices are 1, 2 or 3. Default = 3.
10 | % NPOINTS Number of points on the ellipse to generate. Default = 32.
11 | %
12 | %Output:
13 | % XY [2 x npoints] matrix. First row holds x-coordinates, second
14 | % row holds the y-coordinates. First and last columns should
15 | % be the same point, to create a closed curve.
16 |
17 |
18 | %Setting default values, in case only mu and Sigma are specified.
19 | if nargin < 3
20 | level = 3;
21 | end
22 | if nargin < 4
23 | npoints = 32;
24 | end
25 |
26 | % Procedure:
27 | % - A 3 sigma level curve is given by {x} such that (x-mux)'*Q^-1*(x-mux) = 3^2
28 | % or in scalar form: (x-mux) = sqrt(Q)*3
29 | % - replacing z= sqrtm(Q^-1)*(x-mux), such that we have now z'*z = 3^2
30 | % which is now a circle with radius equal 3.
31 | % - Sampling in z, we have z = 3*[cos(theta); sin(theta)]', for theta=1:2*pi
32 | % - Back to x we get: x = mux + 3* sqrtm(Q)*[cos(theta); sin(theta)]'
33 |
34 | ang = linspace(0,2*pi,npoints);
35 | xy = mu + level * sqrtm(Sigma) * [cos(ang); sin(ang)];
36 |
37 | % % Alternative approach
38 | % [V,D] = eig(Sigma);
39 | % ang = linspace(0,2*pi,npoints);
40 | % circle = [cos(ang); sin(ang)];
41 | % xy = V*level*sqrt(D)*circle + mu;
42 | end
--------------------------------------------------------------------------------
/main_invertedPendulum.m:
--------------------------------------------------------------------------------
1 | %------------------------------------------------------------------
2 | % Programed by:
3 | % - Lucas Rath (lucasrm25@gmail.com)
4 | % -
5 | % -
6 |
7 | % 1D Toy example:
8 | %
9 | % - Simulate the GP learning of the nonlinear part of the plant
10 | % dynamics
11 | % - System is being currently controlled with a state feedback control
12 | %------------------------------------------------------------------
13 |
14 | clear all; close all; clc;
15 |
16 |
17 |
18 | %--------------------------------------------------------------------------
19 | % Quick Access Simulation and controller parameters
20 | %------------------------------------------------------------------
21 | dt = 0.1; % simulation timestep size
22 | tf = 7; % simulation time
23 | maxiter = 15; % max NMPC iterations per time step
24 | N = 10; % NMPC prediction horizon
25 |
26 |
27 | useParallel = false;
28 |
29 |
30 | lookahead = dt*N;
31 | fprintf('\nPrediction lookahead: %.1f [s]\n',lookahead);
32 |
33 |
34 | % inverted pendulum parameters
35 | Mc = 5;
36 | Mp = 2;
37 | b = 0.1;
38 | I = 0.6;
39 | l = 3;
40 | g = 9.81;
41 |
42 |
43 | %% True Dynamics Model
44 | %--------------------------------------------------------------------------
45 | % xk+1 = fd_true(xk,uk) + Bd * ( w ),
46 | %
47 | % where: w ~ N(0,var_w)
48 | %------------------------------------------------------------------
49 |
50 | % define noise for true disturbance
51 | var_w = 1e-8;
52 |
53 | % create true dynamics model
54 | trueModel = MotionModelGP_InvPendulum_deffect(Mc, Mp, b, I, l, [], var_w);
55 |
56 | %% Create Estimation Model and Nominal Model
57 |
58 | % -------------------------------------------------------------------------
59 | % Create nominal model (no disturbance):
60 | % xk+1 = fd_nom(xk,uk)
61 | % -------------------------------------------------------------------------
62 |
63 | % create nominal dynamics model (no disturbance)
64 | nomModel = MotionModelGP_InvPendulum_nominal(Mc, Mp, b, I, l, [], []);
65 |
66 |
67 | % -------------------------------------------------------------------------
68 | % Create adaptive dynamics model
69 | % (unmodeled dynamics will be estimated by Gaussian Process GP)
70 | % xk+1 = fd_nom(xk,uk) + Bd * ( d_GP(zk) + w )
71 | % -------------------------------------------------------------------------
72 |
73 | % GP input dimension
74 | gp_n = MotionModelGP_InvPendulum_nominal.nz;
75 | % GP output dimension
76 | gp_p = MotionModelGP_InvPendulum_nominal.nd;
77 |
78 | % GP hyperparameters
79 | var_f = 0.01; % output variance
80 | M = diag([1e-1,1e-1].^2); % length scale
81 | var_n = var_w; % measurement noise variance
82 | maxsize = 100; % maximum number of points in the dictionary
83 |
84 | % create GP object
85 | d_GP = GP(gp_n, gp_p, var_f, var_n, M, maxsize);
86 |
87 | % create estimation dynamics model (disturbance is the Gaussian Process GP)
88 | estModel = MotionModelGP_InvPendulum_nominal(Mc, Mp, b, I, l, @d_GP.eval, var_w);
89 |
90 |
91 |
92 | %% Controller
93 |
94 | n = estModel.n;
95 | m = estModel.m;
96 | ne = 0;
97 |
98 | % -------------------------------------------------------------------------
99 | % LQR CONTROLLER
100 | [A,B] = estModel.linearize();
101 | Ak = eye(n)+dt*A;
102 | Bk = B*dt;
103 | Ck=[0 1 0 0; 0 0 1 0; 0 0 0 1];
104 | Q = 1e3*eye(4);
105 | R = 1;
106 | [~,~,K] = dare(Ak,Bk,Q,R);
107 | % Prefilter
108 | Kr = pinv(Ck/(eye(n)-Ak+Bk*K)*Bk);
109 | % check eigenvalues
110 | eig(Ak-Bk*K);
111 | % -------------------------------------------------------------------------
112 |
113 | % -------------------------------------------------------------------------
114 | % NONLINEAR MPC CONTROLLER
115 | % define cost function
116 | Q = diag([1e-1 1e5 1e0]);
117 | Qf= diag([1e-1 1e5 1e0]);
118 | R = 10;
119 | Ck = [0 1 0 0; 0 0 1 0; 0 0 0 1];
120 | fo = @(t,mu_x,var_x,u,e,r) (Ck*mu_x-r(t))'*Q *(Ck*mu_x-r(t)) + R*u^2; % cost function
121 | fend = @(t,mu_x,var_x,e,r) (Ck*mu_x-r(t))'*Qf*(Ck*mu_x-r(t)); % end cost function
122 | f = @(mu_xk,var_xk,u) estModel.xkp1(mu_xk, var_xk, u, dt);
123 | h = @(x,u,e) []; % @(x,u) 0; % h(x)==0
124 | g = @(x,u,e) []; % @(x,u) 0; % g(x)<=0
125 |
126 | u_lb = [];
127 | u_ub = [];
128 |
129 | mpc = NMPC (f, h, g, u_lb, u_ub, n, m, ne, fo, fend, N, dt);
130 | mpc.tol = 1e-3;
131 | mpc.maxiter = maxiter;
132 | % -------------------------------------------------------------------------
133 |
134 |
135 | %% Simulate
136 |
137 | % define input
138 | r = @(t) [0 0 0]';
139 | % r = @(t) 1*sin(10*t);
140 | % r = @(t) 2*sin(5*t) + 2*sin(15*t) + 6*exp(-t) - 4 ;
141 | % r = @(t) 4*sin(5*t) + 4*sin(15*t);
142 | nr = size(r(0),1); % dimension of r(t)
143 |
144 | % initial state
145 | x0 = [0,0,deg2rad(5),0]';
146 |
147 | % initialize variables to store simulation results
148 | out.t = 0:dt:tf;
149 | out.x = [x0 nan(n,length(out.t)-1)];
150 | out.xhat = [x0 nan(n,length(out.t)-1)];
151 | out.xnom = [x0 nan(n,length(out.t)-1)];
152 | out.u = nan(m,length(out.t)-1);
153 | out.r = nan(nr,length(out.t)-1);
154 |
155 |
156 | d_GP.isActive = false;
157 |
158 |
159 | ki = 1;
160 | % ki = 40;
161 | % mpc.uguess = out.u(:,ki);
162 |
163 | for k = ki:numel(out.t)-1
164 | disp(out.t(k))
165 |
166 | % ---------------------------------------------------------------------
167 | % Read new reference
168 | % ---------------------------------------------------------------------
169 | out.r(:,k) = r(out.t(k));
170 |
171 | % ---------------------------------------------------------------------
172 | % LQR controller
173 | % ---------------------------------------------------------------------
174 | % % out.u(:,i) = Kr*out.r(:,i) - K*out.xhat(:,i);
175 |
176 | % ---------------------------------------------------------------------
177 | % NPMC controller
178 | % ---------------------------------------------------------------------
179 | [u_opt, e_opt] = mpc.optimize(out.xhat(:,k), out.t(k), r, useParallel);
180 | out.u(:,k) = u_opt(:,1);
181 |
182 |
183 | % ---------------------------------------------------------------------
184 | % simulate real model
185 | % ---------------------------------------------------------------------
186 | [mu_xkp1,var_xkp1] = trueModel.xkp1(out.x(:,k),zeros(trueModel.n),out.u(:,k),dt);
187 | out.x(:,k+1) = mvnrnd(mu_xkp1, var_xkp1, 1)';
188 |
189 | % ---------------------------------------------------------------------
190 | % measure data
191 | % ---------------------------------------------------------------------
192 | out.xhat(:,k+1) = out.x(:,k+1); % perfect observer
193 |
194 | % ---------------------------------------------------------------------
195 | % Safety
196 | % ---------------------------------------------------------------------
197 | if abs(out.xhat(3,k+1)) > deg2rad(60)
198 | error('Pole is completely unstable. theta = %.f[deg]... aborting',rad2deg(out.xhat(3,k+1)));
199 | end
200 |
201 | % ---------------------------------------------------------------------
202 | % calculate nominal model
203 | % ---------------------------------------------------------------------
204 | out.xnom(:,k+1) = nomModel.xkp1(out.xhat(:,k),zeros(nomModel.n),out.u(:,k),dt);
205 |
206 |
207 | % ---------------------------------------------------------------------
208 | % add data to GP model
209 | % ---------------------------------------------------------------------
210 | if mod(k-1,1)==0
211 | % calculate disturbance (error between measured and nominal)
212 | d_est = estModel.Bd \ (out.xhat(:,k+1) - out.xnom(:,k+1));
213 | % select subset of coordinates that will be used in GP prediction
214 | zhat = [ estModel.Bz_x * out.xhat(:,k); estModel.Bz_u * out.u(:,k) ];
215 | % add data point to the GP dictionary
216 | d_GP.add(zhat,d_est);
217 | end
218 |
219 | if d_GP.N > 20 && out.t(k) > 3
220 | d_GP.updateModel();
221 | d_GP.isActive = true;
222 | end
223 |
224 | % check if these values are the same:
225 | % d_est == mu_d(zhat) == [mud,~]=trueModel.d(zhat)
226 |
227 | end
228 |
229 |
230 |
231 |
232 | return
233 |
234 |
235 |
236 |
237 | %% Optimize GP hyperparameters ??? (Offline procedure, after simulation)
238 |
239 | d_GP.setHyperParameters( M, var_f, var_n )
240 | % d_GP.optimizeHyperParams('ga');
241 | d_GP.optimizeHyperParams('fmincon');
242 |
243 | d_GP.M
244 | d_GP.var_f
245 | d_GP.var_n
246 |
247 |
248 | %% Evaluate results
249 | close all;
250 |
251 | % plot reference and state signal
252 | figure('Color','w','Position',[-1836 535 560 420]);
253 | subplot(2,1,1); hold on; grid on;
254 | % plot(out.t(1:end-1), out.r, 'DisplayName', 'r(t)')
255 | plot(out.t, out.x(3,:), 'DisplayName', 'x(t) [rad]')
256 | ylabel('Pole angle \theta [rad]');
257 | xlabel('time [s]')
258 |
259 | subplot(2,1,2); hold on; grid on;
260 | plot(out.t(1:end-1), out.u, 'DisplayName', 'u(t)')
261 | ylabel('Force on the carriage F [N]');
262 | xlabel('time [s]')
263 | % legend;
264 |
265 | % true GP function that is meant to be learned
266 | Bz_x = trueModel.Bz_x;
267 | Bz_u = trueModel.Bz_u;
268 | Bd = trueModel.Bd;
269 | n = trueModel.n;
270 |
271 | % define the true expected disturbance model
272 | % z = [0;0.1];
273 | gptrue = @(z) Bd'*( trueModel.xkp1(Bz_x'*z, zeros(n), 0, dt)...
274 | - nomModel.xkp1(Bz_x'*z, zeros(n), 0, dt) );
275 |
276 | % plot prediction bias and variance
277 | d_GP.plot2d( gptrue )
278 |
279 | %% animation of inverse pendulum
280 |
281 | % animation of inverse pendulum
282 | drawpendulum(out.t,out.x,Mc,Mp,g,l)
283 |
284 |
285 | %% Analyse learning
286 | % ---------------------------------------------------------------------
287 | % Check how the GP reduces the prediction error
288 | % ---------------------------------------------------------------------
289 |
290 | % d_GP.optimizeHyperParams('fmincon')
291 | % d_GP.optimizeHyperParams('ga')
292 |
293 |
294 | k = find(~isnan(out.xhat(1,:)), 1, 'last' ) - 1;
295 |
296 | % prediction error without GP
297 | % predErrorNOgp = estModel.Bd\(out.xhat - out.xnom);
298 | predErrorNOgp = estModel.Bd\(out.xhat(:,1:k-1) - out.xnom(:,1:k-1));
299 |
300 |
301 | % prediction error with trained GP
302 | zhat = estModel.z( out.xhat(:,1:k-1), out.u(:,1:k-1) )
303 | dgp = d_GP.eval(zhat,true);
304 | predErrorWITHgp = estModel.Bd\( out.xhat(:,2:k) - (out.xnom(:,2:k) + estModel.Bd*dgp) );
305 |
306 |
307 | disp('Prediction mean squared error without GP:')
308 | disp( mean(predErrorNOgp(:,all(~isnan(predErrorNOgp))).^2 ,2) )
309 | disp('Prediction mean squared error with trained GP:')
310 | disp( mean(predErrorWITHgp(:,all(~isnan(predErrorWITHgp))).^2 ,2) )
311 |
312 |
313 |
314 | % Visualize error
315 | figure('Color','w'); hold on; grid on;
316 | subplot(1,2,1)
317 | plot( predErrorNOgp' )
318 | subplot(1,2,2)
319 | hist(predErrorNOgp')
320 | sgtitle('Prediction error - without GP')
321 |
322 |
323 | figure('Color','w'); hold on; grid on;
324 | subplot(1,2,1)
325 | plot( predErrorWITHgp' )
326 | subplot(1,2,2)
327 | hist(predErrorWITHgp')
328 | sgtitle('Prediction error - with GP')
329 |
330 |
331 |
332 |
--------------------------------------------------------------------------------
/main_singletrack.m:
--------------------------------------------------------------------------------
1 | %------------------------------------------------------------------
2 | % Programed by:
3 | % - Lucas Rath (lucasrm25@gmail.com)
4 | % -
5 | % -
6 |
7 | % Control of a Race Car in a Race Track using Gaussian Process Model Predictive Control:
8 | %------------------------------------------------------------------
9 |
10 | clear all; close all; clc;
11 |
12 |
13 | %--------------------------------------------------------------------------
14 | % Quick Access Simulation and controller parameters
15 | %------------------------------------------------------------------
16 | dt = 0.15; % simulation timestep size
17 | tf = 15*12; % simulation time
18 | maxiter = 30; % max NMPC iterations per time step
19 | N = 10; % NMPC prediction horizon
20 |
21 | loadPreTrainedGP = true;
22 | GPfile = fullfile(pwd,'/simresults/20-01-15-out-GP-without-GP.mat');
23 | % GPfile = fullfile(pwd,'/simresults/20-01-15-out-GP-with-GP-optimized.mat');
24 | useGP = false;
25 | trainGPonline = true;
26 | useParallel = true;
27 |
28 |
29 | % display info
30 | lookahead = dt*N;
31 | fprintf('\nPrediction lookahead: %.1f [s]\n',lookahead);
32 |
33 |
34 |
35 | %% Create True Dynamics Simulation Model
36 | %--------------------------------------------------------------------------
37 | % xk+1 = fd_true(xk,uk) + Bd * ( w ),
38 | %
39 | % where: w ~ N(0,var_w)
40 | %------------------------------------------------------------------
41 |
42 | % define noise for true disturbance
43 | var_w = diag([(1/3)^2 (1/3)^2 (deg2rad(3)/3)^2]);
44 | % var_w = zeros(3);
45 |
46 | % create true dynamics model
47 | trueModel = MotionModelGP_SingleTrack_true( [], var_w);
48 | % trueModel = MotionModelGP_SingleTrack_nominal(d,var_w);
49 |
50 |
51 | %% Create Estimation Model and Nominal Model
52 |
53 | % -------------------------------------------------------------------------
54 | % Create nominal model (no disturbance):
55 | % xk+1 = fd_nom(xk,uk)
56 | % -------------------------------------------------------------------------
57 |
58 | nomModel = MotionModelGP_SingleTrack_nominal( [], [] );
59 | % nomModel = MotionModelGP_SingleTrack_true( [], [] );
60 |
61 | nomModel.analyseSingleTrack();
62 |
63 |
64 | % -------------------------------------------------------------------------
65 | % Create adaptive dynamics model
66 | % (unmodeled dynamics will be estimated by Gaussian Process GP)
67 | % xk+1 = fd_nom(xk,uk) + Bd * ( d_GP(zk) + w )
68 | % -------------------------------------------------------------------------
69 |
70 | if ~loadPreTrainedGP
71 | % GP input dimension
72 | gp_n = MotionModelGP_SingleTrack_nominal.nz;
73 | % GP output dimension
74 | gp_p = MotionModelGP_SingleTrack_nominal.nd;
75 |
76 | % GP hyperparameters
77 | var_f = repmat(0.01,[gp_p,1]); % output variance
78 | var_n = diag(var_w/3); % measurement noise variance
79 | M = repmat(diag([1e0,1e0,1e0,1e0,1e0].^2),[1,1,gp_p]); % length scale
80 | maxsize = 300; % maximum number of points in the dictionary
81 |
82 | % create GP object
83 | d_GP = GP(gp_n, gp_p, var_f, var_n, M, maxsize);
84 | else
85 | load(GPfile); %,'d_GP'
86 | fprintf('\nGP model loaded succesfuly\n\n')
87 | end
88 |
89 | % create nominal model with GP model as d(zk)
90 | estModel = MotionModelGP_SingleTrack_nominal(@d_GP.eval, var_w);
91 | % estModel = MotionModelGP_SingleTrack_true(@d_GP.eval, var_w);
92 |
93 |
94 | %% Initialize Controller
95 |
96 | % -------------------------------------------------------------------------
97 | % Create perception model (in this case is the saved track points)
98 | % this is needed to for the MPC cost function
99 | % -------------------------------------------------------------------------
100 | [trackdata, x0, th0, w] = RaceTrack.loadTrack_02();
101 | track = RaceTrack(trackdata, x0, th0, w);
102 | % TEST: [Xt, Yt, PSIt, Rt] = track.getTrackInfo(1000)
103 | % trackAnim = SingleTrackAnimation(track,mpc.N);
104 | % trackAnim.initGraphics()
105 |
106 |
107 | % -------------------------------------------------------------------------
108 | % Nonlinear Model Predictive Controller
109 | % -------------------------------------------------------------------------
110 |
111 | % define cost function
112 | n = estModel.n;
113 | m = estModel.m;
114 | ne = 0;
115 |
116 | % define cost functions
117 | fo = @(t,mu_x,var_x,u,e,r) costFunction(mu_x, var_x, u, track); % e = track distance
118 | fend = @(t,mu_x,var_x,e,r) 2 * costFunction(mu_x, var_x, zeros(m,1), track); % end cost function
119 |
120 | % define dynamics
121 | f = @(mu_x,var_x,u) estModel.xkp1(mu_x, var_x, u, dt);
122 | %f = @(mu_x,var_x,u) trueModel.xkp1(mu_x, var_x, u, dt);
123 | % define additional constraints
124 | h = @(x,u,e) [];
125 | g = @(x,u,e) [];
126 | u_lb = [-deg2rad(20); % >= steering angle
127 | -1; % >= gas pedal
128 | 5]; % >= centerline track velocity
129 | u_ub = [deg2rad(20); % <= steering angle
130 | 1; % <= gas pedal
131 | 30]; % <= centerline track velocity
132 |
133 | % Initialize NMPC object;
134 | mpc = NMPC(f, h, g, u_lb, u_ub, n, m, ne, fo, fend, N, dt);
135 | mpc.tol = 1e-2;
136 | mpc.maxiter = maxiter;
137 |
138 |
139 |
140 | %% Prepare simulation
141 | % ---------------------------------------------------------------------
142 | % Prepare simulation (initialize vectors, initial conditions and setup
143 | % animation
144 | % ---------------------------------------------------------------------
145 |
146 | % define variable sizes
147 | true_n = trueModel.n;
148 | true_m = trueModel.m;
149 | est_n = estModel.n;
150 | est_m = estModel.m;
151 |
152 | % initial state
153 | x0 = [5;0;0; 10;0;0; 0]; % true initial state
154 | x0(end) = track.getTrackDistance(x0(1:2)); % get initial track traveled distance
155 |
156 | % change initial guess for mpc solver. Set initial track velocity as
157 | % initial vehicle velocity (this improves convergence speed a lot)
158 | mpc.uguess(end,:) = x0(4)*2;
159 |
160 | % define simulation time
161 | out.t = 0:dt:tf; % time vector
162 | kmax = length(out.t)-1; % steps to simulate
163 |
164 | % initialize variables to store simulation results
165 | out.x = [x0 NaN(true_n,kmax)]; % true states
166 | out.xhat = [x0 NaN(est_n, kmax)]; % state estimation
167 | out.xnom = [x0 NaN(est_n, kmax)]; % predicted nominal state
168 | out.u = NaN(est_m, kmax); % applied input
169 | out.x_ref = NaN(2, mpc.N+1, kmax); % optimized reference trajectory
170 | out.mu_x_pred_opt = NaN(mpc.n, mpc.N+1, kmax); % mean of optimal state prediction sequence
171 | out.var_x_pred_opt = NaN(mpc.n, mpc.n, mpc.N+1, kmax); % variance of optimal state prediction sequence
172 | out.u_pred_opt = NaN(mpc.m, mpc.N, kmax); % open-loop optimal input prediction
173 |
174 |
175 | % start animation
176 | trackAnim = SingleTrackAnimation(track, out.mu_x_pred_opt, out.var_x_pred_opt, out.u_pred_opt, out.x_ref);
177 | trackAnim.initTrackAnimation();
178 | trackAnim.initScope();
179 | drawnow;
180 |
181 | % deactivate GP evaluation in the prediction
182 | d_GP.isActive = useGP;
183 | fprintf('\nGP active? %s\n\n',string(useGP))
184 |
185 |
186 |
187 |
188 | %% Start simulation
189 |
190 | ki = 1;
191 | % ki = 310;
192 | % mpc.uguess = out.u_pred_opt(:,:,ki);
193 |
194 |
195 | for k = ki:kmax
196 | disp('------------------------------------------------------')
197 | fprintf('time: %.3f [s]\n',out.t(k))
198 |
199 | % ---------------------------------------------------------------------
200 | % LQR controller
201 | % ---------------------------------------------------------------------
202 | % % out.u(:,i) = Kr*out.r(:,i) - K*out.xhat(:,i);
203 |
204 | % ---------------------------------------------------------------------
205 | % NPMC controller
206 | % ---------------------------------------------------------------------
207 | % calculate optimal input
208 | [u_opt, e_opt] = mpc.optimize(out.xhat(:,k), out.t(k), 0, useParallel);
209 | out.u(:,k) = u_opt(:,1);
210 | sprintf('\nSteering angle: %d\nTorque gain: %.1f\nTrack vel: %.1f\n',rad2deg(out.u(1,k)),out.u(2,k),out.u(3,k))
211 |
212 | % ---------------------------------------------------------------------
213 | % Calculate predicted trajectory from optimal open-loop input sequence
214 | % and calculate optimized reference trajectory for each prediction
215 | % ---------------------------------------------------------------------
216 | % get optimal state predictions from optimal input and current state
217 | out.u_pred_opt(:,:,k) = u_opt;
218 | [out.mu_x_pred_opt(:,:,k),out.var_x_pred_opt(:,:,:,k)] = mpc.predictStateSequence(out.xhat(:,k), zeros(estModel.n), u_opt);
219 | % get target track distances from predictions (last state)
220 | out.x_ref(:,:,k) = track.getTrackInfo(out.mu_x_pred_opt(end,:,k));
221 |
222 | % ---------------------------------------------------------------------
223 | % update race animation and scopes
224 | % ---------------------------------------------------------------------
225 | trackAnim.mu_x_pred_opt = out.mu_x_pred_opt;
226 | trackAnim.var_x_pred_opt = out.var_x_pred_opt;
227 | trackAnim.u_pred_opt = out.u_pred_opt;
228 | trackAnim.x_ref = out.x_ref;
229 | trackAnim.updateTrackAnimation(k);
230 | trackAnim.updateScope(k);
231 | drawnow;
232 |
233 | % ---------------------------------------------------------------------
234 | % Simulate real model
235 | % ---------------------------------------------------------------------
236 | [mu_xkp1,var_xkp1] = trueModel.xkp1(out.x(:,k),zeros(trueModel.n),out.u(:,k),dt);
237 | % out.x(:,k+1) = mvnrnd(mu_xkp1, var_xkp1, 1)';
238 | out.x(:,k+1) = mu_xkp1;
239 |
240 | % ---------------------------------------------------------------------
241 | % Measure data
242 | % ---------------------------------------------------------------------
243 | out.xhat(:,k+1) = out.x(:,k+1); % perfect observer
244 | % get traveled distance, given vehicle coordinates
245 | out.xhat(end,k+1) = track.getTrackDistance( out.xhat([1,2],k+1) , out.xhat(end,k) );
246 |
247 |
248 | % ---------------------------------------------------------------------
249 | % Lap timer
250 | % ---------------------------------------------------------------------
251 | [laptimes, idxnewlaps] = RaceTrack.getLapTimes(out.xhat(end,:),dt);
252 | if any(k==idxnewlaps)
253 | RaceTrack.dispLapTimes(laptimes);
254 | end
255 |
256 |
257 | % ---------------------------------------------------------------------
258 | % Safety - Stop simulation in case vehicle is completely unstable
259 | % ---------------------------------------------------------------------
260 | V_vx = out.xhat(4,k+1);
261 | V_vy = out.xhat(5,k+1);
262 | beta = atan2(V_vy,V_vx);
263 | if V_vx < 0
264 | error('Vehicle is driving backwards... aborting');
265 | end
266 | if abs(rad2deg(beta)) > 80
267 | error('Vehicle has a huge sideslip angle... aborting')
268 | end
269 |
270 | % ---------------------------------------------------------------------
271 | % calculate nominal model
272 | % ---------------------------------------------------------------------
273 | out.xnom(:,k+1) = nomModel.xkp1(out.xhat(:,k),zeros(nomModel.n),out.u(:,k),dt);
274 |
275 |
276 | % ---------------------------------------------------------------------
277 | % Add data to GP model
278 | % ---------------------------------------------------------------------
279 | if mod(k-1,1)==0
280 | % calculate disturbance (error between measured and nominal)
281 | d_est = estModel.Bd \ (out.xhat(:,k+1) - out.xnom(:,k+1));
282 | % d_est = estModel.Bd \ (mu_xkp1 - out.xnom(:,k+1));
283 | % select subset of coordinates that will be used in GP prediction
284 | zhat = [ estModel.Bz_x * out.xhat(:,k); estModel.Bz_u * out.u(:,k) ];
285 | % add data point to the GP dictionary
286 | if trainGPonline
287 | d_GP.add(zhat,d_est');
288 | d_GP.updateModel();
289 | end
290 |
291 | fprintf('Prediction Error norm WITHOUT GP: %f\n',norm(d_est));
292 | disp(d_est)
293 | fprintf('Prediction Error norm WITH GP: %f\n',norm(d_est-d_GP.eval(zhat,true)));
294 | % fprintf('Prediction Error norm WITH GP: %f\n',norm(d_est-estModel.d(zhat,true)));
295 | disp(d_est-d_GP.eval(zhat,true))
296 | end
297 |
298 | % if length(laptimes) >= 6
299 | % d_GP.isActive = true;
300 | % mpc.maxiter = 30;
301 | % end
302 |
303 | end
304 |
305 |
306 | % Display Lap times
307 |
308 | [laptimes, idxnewlaps] = RaceTrack.getLapTimes(out.xhat(end,:),dt);
309 | RaceTrack.dispLapTimes(laptimes)
310 |
311 |
312 |
313 |
314 | return
315 | % STOP here. Next sections are intended to be executed separately
316 |
317 |
318 |
319 | %% Readd simulation data to GP, uddate model and optimize parameters
320 |
321 | k = find(~isnan(out.xhat(1,:)), 1, 'last' ) - 20;
322 |
323 | % create new instance of GP class
324 | d_GP = GP(gp_n, gp_p, var_f, var_n, M, maxsize);
325 |
326 | % readd points
327 | d_est = estModel.Bd \ (out.xhat(:,2:k) - out.xnom(:,2:k));
328 | zhat = estModel.z( out.xhat(:,1:k-1), out.u(:,1:k-1) );
329 | d_GP.add(zhat,d_est');
330 |
331 | % update and optimize model
332 | d_GP.updateModel();
333 | d_GP.optimizeHyperParams('fmincon')
334 |
335 | d_GP.M
336 | d_GP.var_f
337 | d_GP.var_n
338 |
339 |
340 | %% Analyse learning
341 | % ---------------------------------------------------------------------
342 | % Check how the GP reduces the prediction error
343 | % ---------------------------------------------------------------------
344 |
345 | % d_GP.optimizeHyperParams('fmincon')
346 | % d_GP.optimizeHyperParams('ga')
347 |
348 |
349 | k = find(~isnan(out.xhat(1,:)), 1, 'last' ) - 20;
350 |
351 | % prediction error without GP
352 | % predErrorNOgp = estModel.Bd\(out.xhat - out.xnom);
353 | predErrorNOgp = estModel.Bd\(out.xhat(:,1:k-1) - out.xnom(:,1:k-1));
354 |
355 |
356 | % prediction error with trained GP
357 | zhat = estModel.z( out.xhat(:,1:k-1), out.u(:,1:k-1) );
358 | dgp = d_GP.eval(zhat,true);
359 | predErrorWITHgp = estModel.Bd\( out.xhat(:,2:k) - (out.xnom(:,2:k) + estModel.Bd*dgp) );
360 |
361 |
362 | disp('Prediction mean squared error without GP:')
363 | disp( mean(predErrorNOgp(:,all(~isnan(predErrorNOgp))).^2 ,2) )
364 | disp('Prediction mean squared error with trained GP:')
365 | disp( mean(predErrorWITHgp(:,all(~isnan(predErrorWITHgp))).^2 ,2) )
366 |
367 |
368 |
369 | % Visualize error
370 | figure('Color','w'); hold on; grid on;
371 | subplot(1,2,1)
372 | plot( predErrorNOgp' )
373 | subplot(1,2,2)
374 | hist(predErrorNOgp')
375 | sgtitle('Prediction error - without GP')
376 |
377 |
378 | figure('Color','w'); hold on; grid on;
379 | subplot(1,2,1)
380 | plot( predErrorWITHgp' )
381 | subplot(1,2,2)
382 | hist(predErrorWITHgp')
383 | sgtitle('Prediction error - with GP')
384 |
385 |
386 | % ---------------------------------------------------------------------
387 | % Check in which region of the tyre dynamics we are working
388 | % ---------------------------------------------------------------------
389 |
390 | % % % % simulation
391 | % % %
392 | % trueModel.testTyres
393 | %
394 | % l_f = 0.9;
395 | % l_r = 1.5;
396 | % V_vx = out.xhat(4,:);
397 | % V_vy = out.xhat(5,:);
398 | % psi_dot = out.xhat(6,:);
399 | % delta = out.u(1,:);
400 | % a_r = atan2(V_vy-l_r.*psi_dot,V_vx);
401 | % a_f = atan2(V_vy+l_f.*psi_dot,V_vx) - [delta 0];
402 | %
403 | % figure('Color','w'); hold on; grid on;
404 | % plot(rad2deg(a_r))
405 | % plot(rad2deg(a_f))
406 | % ylabel('slip angle')
407 | % xlabel('time step')
408 |
409 |
410 |
411 | %% Show animation
412 | close all;
413 |
414 | % start animation
415 | trackAnim = SingleTrackAnimation(track, out.mu_x_pred_opt, out.var_x_pred_opt, out.u_pred_opt, out.x_ref);
416 | trackAnim.initTrackAnimation();
417 | % trackAnim.initScope();
418 | for k=1:kmax
419 | if ~ trackAnim.updateTrackAnimation(k)
420 | break;
421 | end
422 | % trackAnim.updateScope(k);
423 | % pause(0.15);
424 | drawnow;
425 | end
426 |
427 |
428 | %% Record video
429 |
430 | FrameRate = 7;
431 | videoName = fullfile('simresults',sprintf('trackAnimVideo-%s',date));
432 | videoFormat = 'Motion JPEG AVI';
433 | trackAnim.recordvideo(videoName, videoFormat, FrameRate);
434 |
435 |
436 | %% Cost function for the MPC controller
437 |
438 | function cost = costFunction(mu_x, var_x, u, track)
439 |
440 | % Track oriented penalization
441 | q_l = 50; % penalization of lag error
442 | q_c = 20; % penalization of contouring error
443 | q_o = 5; % penalization for orientation error
444 | q_d = -3; % reward high track centerline velocites
445 | q_r = 100; % penalization when vehicle is outside track
446 |
447 | % state and input penalization
448 | q_v = -0; % reward high absolute velocities
449 | q_st = 0; % penalization of steering
450 | q_br = 0; % penalization of breaking
451 | q_psidot = 8; % penalize high yaw rates
452 | q_acc = -0; % reward for accelerating
453 |
454 | % label inputs and outputs
455 | I_x = mu_x(1); % x position in global coordinates
456 | I_y = mu_x(2); % y position in global coordinates
457 | psi = mu_x(3); % yaw
458 | V_vx = mu_x(4); % x velocity in vehicle coordinates
459 | V_vy = mu_x(5); % x velocity in vehicle coordinates
460 | psidot = mu_x(6);
461 | track_dist = mu_x(7); % track centerline distance
462 | delta = u(1); % steering angle rad2deg(delta)
463 | T = u(2); % torque gain (1=max.acc, -1=max.braking)
464 | track_vel = u(3); % track centerline velocity
465 |
466 |
467 | % ---------------------------------------------------------------------
468 | % cost of contour, lag and orientation error
469 | % ---------------------------------------------------------------------
470 |
471 | % get lag, contour, offroad and orientation error of the vehicle w.r.t.
472 | % a point in the trajectory that is 'track_dist' far away from the
473 | % origin along the track centerline (traveled distance)
474 | [lag_error, countour_error, offroad_error, orientation_error] = ...
475 | track.getVehicleDeviation([I_x;I_y], psi, track_dist);
476 |
477 | cost_contour = q_c * countour_error^2;
478 | cost_lag = q_l * lag_error^2;
479 | cost_orientation = q_o * orientation_error^2;
480 |
481 | % ---------------------------------------------------------------------
482 | % cost for being outside track
483 | % ---------------------------------------------------------------------
484 | % % apply smooth barrier function (we want: offroad_error < 0).
485 | % alpha = 40; % smoothing factor... the smaller the smoother
486 | % offroad_error = (1+exp(-alpha*(offroad_error+0.05))).^-1;
487 | gamma = 1000;
488 | lambda = -0.1;
489 | offroad_error = 5*(sqrt((4+gamma*(lambda-offroad_error).^2)/gamma) - (lambda-offroad_error));
490 | cost_outside = q_r * offroad_error^2;
491 |
492 | % ---------------------------------------------------------------------
493 | % reward high velocities
494 | % ---------------------------------------------------------------------
495 | cost_vel = q_v * norm([V_vx; V_vy]);
496 |
497 | % ---------------------------------------------------------------------
498 | % penalize high yaw rates
499 | % ---------------------------------------------------------------------
500 | cost_psidot = q_psidot * psidot^2;
501 |
502 | % ---------------------------------------------------------------------
503 | % reward high track velocities
504 | % ---------------------------------------------------------------------
505 | cost_dist = q_d * track_vel;
506 |
507 | % ---------------------------------------------------------------------
508 | % penalize acceleration, braking and steering
509 | % ---------------------------------------------------------------------
510 | cost_inputs = (T>0)*q_acc*T^2 + (T<0)*q_br*T^2 + q_st*(delta)^2 ;
511 |
512 | % ---------------------------------------------------------------------
513 | % Calculate final cost
514 | % ---------------------------------------------------------------------
515 | cost = cost_contour + ...
516 | cost_lag + ...
517 | cost_orientation + ...
518 | cost_dist + ...
519 | cost_outside + ...
520 | cost_inputs + ...
521 | cost_vel + ...
522 | cost_psidot;
523 | end
--------------------------------------------------------------------------------
/simresults/20-01-15-out-GP-with-GP-optimized.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/lucasrm25/Gaussian-Process-based-Model-Predictive-Control/ef00c0df1ff25fb75f6f9c3d9099d47c9cfe1078/simresults/20-01-15-out-GP-with-GP-optimized.mat
--------------------------------------------------------------------------------
/simresults/20-01-15-out-GP-without-GP.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/lucasrm25/Gaussian-Process-based-Model-Predictive-Control/ef00c0df1ff25fb75f6f9c3d9099d47c9cfe1078/simresults/20-01-15-out-GP-without-GP.mat
--------------------------------------------------------------------------------
/simresults/trackAnimVideo-16-Jan-2020-with-GP-optimized.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/lucasrm25/Gaussian-Process-based-Model-Predictive-Control/ef00c0df1ff25fb75f6f9c3d9099d47c9cfe1078/simresults/trackAnimVideo-16-Jan-2020-with-GP-optimized.gif
--------------------------------------------------------------------------------
/simresults/trackAnimVideo-16-Jan-2020-without-GP.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/lucasrm25/Gaussian-Process-based-Model-Predictive-Control/ef00c0df1ff25fb75f6f9c3d9099d47c9cfe1078/simresults/trackAnimVideo-16-Jan-2020-without-GP.gif
--------------------------------------------------------------------------------
/test-files/genFigs.m:
--------------------------------------------------------------------------------
1 | close all; clear all
2 |
3 | imgname = 'trace-without-GP';
4 | outfile = fullfile(pwd,'/simresults/20-01-15-out-GP-without-GP.mat');
5 | kplot = 558;
6 |
7 | imgname = 'trace-with-GP';
8 | outfile = fullfile(pwd,'/simresults/20-01-15-out-GP-with-GP-optimized.mat');
9 | kplot = 412
10 |
11 | load(outfile)
12 |
13 |
14 | [trackdata, x0, th0, w] = RaceTrack.loadTrack_02();
15 | track = RaceTrack(trackdata, x0, th0, w);
16 |
17 | trackAnim = SingleTrackAnimation(track, out.mu_x_pred_opt, out.var_x_pred_opt, out.u_pred_opt, out.x_ref);
18 | trackAnim.initTrackAnimation();
19 | drawnow;
20 |
21 | % k = find(~isnan(out.xhat(1,:)), 1, 'last' ) - 1;
22 |
23 | trackAnim.mu_x_pred_opt = out.mu_x_pred_opt;
24 | trackAnim.var_x_pred_opt = out.var_x_pred_opt;
25 | trackAnim.u_pred_opt = out.u_pred_opt;
26 | trackAnim.x_ref = out.x_ref;
27 | trackAnim.updateTrackAnimation(kplot); % 558
28 |
29 |
30 | delete(trackAnim.h_car)
31 | delete(trackAnim.h_x_ref)
32 | delete(trackAnim.h_mu_x_pred_opt)
33 | cellfun( @(h) delete(h), trackAnim.h_var_x_pred_opt )
34 | legend('off')
35 | title('')
36 | fp.savefig(imgname,'format','epsc')
37 |
38 |
39 |
40 |
41 |
42 | %% CHECK RELAXED BARRIER FUNCTION PLOT
43 |
44 |
45 | x = -0.5:0.001:0.5;
46 | % Relaxied barrier function for (x<=lambda)
47 | gamma = 10000;
48 | lambda = -0.2;
49 | y = 0.5*(sqrt((4+gamma*(lambda-x).^2)/gamma) - (lambda-x));
50 | figure('Color','w'); hold on; %grid on;
51 | plot(x,y,'LineWidth',2)
52 | ylim([0,0.1])
53 | xlim([-0.4,0])
54 | set(gca,'YTickLabel',[]);
55 | set(gca,'XTickLabel',[]);
56 | %%%%
57 | lambda = -0.2;
58 | x = -0.6:0.001:(lambda-eps);
59 | y = 0.1*-log(lambda-x);
60 | figure('Color','w'); hold on; %grid on;
61 | plot(x,y,'LineWidth',2)
62 | xlim([-0.6,0])
63 |
64 |
--------------------------------------------------------------------------------
/test-files/test_GP.m:
--------------------------------------------------------------------------------
1 | clear all; close all; clc;
2 |
3 | var_w = 1e-8;
4 | var_f = 1e4; % output variance
5 | M = diag(1e0); % length scale
6 | var_n = var_w; % measurement noise variance
7 | maxsize = 100; % maximum number of points in the dictionary
8 |
9 | % create GP object
10 | gp = GP(1, 1, var_f, var_n, M, maxsize);
11 |
12 | %% true function
13 | Xt = [-4 -3 -2 -1 0 1 2 3 4];
14 | Yt = [-1 0.5 1.5 1.5 1 0.8 1 1.5 2]';
15 | pp = spline(Xt,Yt');
16 | truefun = @(x) ppval(pp, x)';
17 |
18 | %% measure data
19 | X = Xt([1 2 3 5 7 8]);
20 | Y = truefun(X) + sqrt(var_n)*randn(length(X),1);
21 |
22 | gp.add(X,Y);
23 |
24 |
25 | %% plot
26 | close all;
27 |
28 | % test 1
29 | gp.setHyperParameters(1e-2, 2, var_w)
30 | gp.plot1d(truefun)
31 | xlim([-6 5]); ylim([-4 4]);
32 |
33 | % test 2
34 | gp.setHyperParameters(50, 100, var_w)
35 | gp.plot1d(truefun)
36 | xlim([-6 5]); ylim([-4 4]);
37 |
38 | % test 3
39 | gp.setHyperParameters(1e-3, 1e-2, var_w)
40 | gp.plot1d(truefun)
41 | xlim([-6 5]); ylim([-4 4]);
42 |
43 | % test 4
44 | gp.setHyperParameters(50, 1e7, var_w)
45 | gp.plot1d(truefun)
46 | xlim([-6 5]); ylim([-4 4]);
47 |
48 | % test 4
49 | gp.setHyperParameters(100^2, 100^2, 1^2)
50 | gp.plot1d(truefun)
51 | xlim([-6 5]); ylim([-4 4]);
52 |
53 | %% optimize and plot
54 |
55 | gp.setHyperParameters(0.1^2, 0.1^2, 1e-8)
56 | % gp.optimizeHyperParams('ga');
57 | gp.optimizeHyperParams('fmincon');
58 |
59 | close all
60 | gp.plot1d(truefun)
61 | 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]);
68 |
--------------------------------------------------------------------------------
/test-files/tyres_Pacejka_iterativedesign.mlx:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/lucasrm25/Gaussian-Process-based-Model-Predictive-Control/ef00c0df1ff25fb75f6f9c3d9099d47c9cfe1078/test-files/tyres_Pacejka_iterativedesign.mlx
--------------------------------------------------------------------------------
/test-files/tyres_diff.m:
--------------------------------------------------------------------------------
1 |
2 | clear all; close all; clc;
3 |
4 | % Linear wheel dynamics from nominal model
5 | c_f = MotionModelGP_SingleTrack_nominal.c_f;
6 | c_r = MotionModelGP_SingleTrack_nominal.c_r;
7 |
8 |
9 | % Pacejka lateral dynamics parameters
10 | B_f = MotionModelGP_SingleTrack_true.B_f; % stiffnes factor (Pacejka) (front wheel)
11 | C_f = MotionModelGP_SingleTrack_true.C_f; % shape factor (Pacejka) (front wheel)
12 | D_f = MotionModelGP_SingleTrack_true.D_f; % peak value (Pacejka) (front wheel)
13 | E_f = MotionModelGP_SingleTrack_true.E_f; % curvature factor (Pacejka) (front wheel)
14 |
15 | B_r = MotionModelGP_SingleTrack_true.B_r; % stiffnes factor (Pacejka) (rear wheel)
16 | C_r = MotionModelGP_SingleTrack_true.C_r; % shape factor (Pacejka) (rear wheel)
17 | D_r = MotionModelGP_SingleTrack_true.D_r; % peak value (Pacejka) (rear wheel)
18 | E_r = MotionModelGP_SingleTrack_true.E_r; % curvature factor (Pacejka) (rear wheel)
19 |
20 | a_r = deg2rad(-30:0.1:30);
21 | a_f = deg2rad(-30:0.1:30);
22 | W_Fy_r = D_r*sin(C_r*atan(B_r*a_r-E_r*(B_r*a_r -atan(B_r*a_r)))); % rear lateral force
23 | W_Fy_f = D_f*sin(C_f*atan(B_f*a_f-E_f*(B_f*a_f -atan(B_f*a_f)))); % front lateral force
24 |
25 | figure('Color','w'); hold on; grid on;
26 | plot(rad2deg(a_r),W_Fy_r/1000,'DisplayName','Pacejka tyre model')
27 | plot(rad2deg(a_r),a_r*c_r/1000,'DisplayName','Constant cornering stiffness model')
28 | % title('Rear tyre')
29 | xlabel('Slip angle [deg]');
30 | ylabel('Tyre lateral force [kN]')
31 | legend('Location','northwest')
32 | xlim([-30,30])
33 | ylim([-6,6])
34 | fp.savefig('rear_tyre')
35 |
36 | figure('Color','w'); hold on; grid on;
37 | plot(rad2deg(a_f),W_Fy_f/1000,'DisplayName','Pacejka tyre model')
38 | plot(rad2deg(a_f),a_r*c_f/1000,'DisplayName','Constant cornering stiffness model')
39 | % title('Front tyre')
40 | xlabel('Slip angle [deg]');
41 | ylabel('Tyre lateral force [kN]')
42 | legend('Location','northwest')
43 | xlim([-30,30])
44 | ylim([-6,6])
45 | fp.savefig('front_tyre')
--------------------------------------------------------------------------------