├── .gitignore ├── CODEGEN ├── invertedPendulum_f.m ├── invertedPendulum_gradu_f.m ├── invertedPendulum_gradx_f.m ├── singletrack_gradu_f.m └── singletrack_gradx_f.m ├── Presentation.pdf ├── README.md ├── Report.pdf ├── classes ├── GP.m ├── MotionModelGP.m ├── MotionModelGP_InvPendulum_deffect.m ├── MotionModelGP_InvPendulum_nominal.m ├── MotionModelGP_SingleTrack_nominal.m ├── MotionModelGP_SingleTrack_true.m ├── NMPC.m ├── ODE.m ├── RaceTrack.m ├── SingleTrackAnimation.m └── fp.m ├── functions ├── drawpendulum.m ├── logdet.m └── sigmaEllipse2D.m ├── images ├── trace-with-GP.eps └── trace-without-GP.eps ├── main_invertedPendulum.m ├── main_singletrack.m ├── simresults ├── 20-01-15-out-GP-with-GP-optimized.mat ├── 20-01-15-out-GP-without-GP.mat ├── trackAnimVideo-16-Jan-2020-with-GP-optimized.gif └── trackAnimVideo-16-Jan-2020-without-GP.gif └── test-files ├── genFigs.m ├── test_GP.m ├── tyres_Pacejka_iterativedesign.mlx └── tyres_diff.m /.gitignore: -------------------------------------------------------------------------------- 1 | *.m~ 2 | *.asv 3 | *.slxc 4 | *slprj/* 5 | *.mat 6 | *.avi 7 | *.mp4 8 | *.png 9 | *.jpg 10 | *.eps 11 | *Dateien_Projektwettbewerb/* 12 | *GPRegression/* 13 | 14 | simresults/ 15 | -------------------------------------------------------------------------------- /CODEGEN/invertedPendulum_f.m: -------------------------------------------------------------------------------- 1 | function xdot = invertedPendulum_f(in1,F,in3) 2 | %INVERTEDPENDULUM_F 3 | % XDOT = INVERTEDPENDULUM_F(IN1,F,IN3) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.3. 6 | % 09-Jan-2020 11:59:40 7 | 8 | I = in3(3,:); 9 | Mc = in3(1,:); 10 | Mp = in3(2,:); 11 | b = in3(6,:); 12 | ds = in1(2,:); 13 | dth = in1(4,:); 14 | g = in3(4,:); 15 | l = in3(5,:); 16 | th = in1(3,:); 17 | t2 = cos(th); 18 | t3 = sin(th); 19 | t4 = Mp.^2; 20 | t5 = dth.^2; 21 | t6 = l.^2; 22 | t7 = th.*2.0; 23 | t8 = I.*Mc.*4.0; 24 | t9 = I.*Mp.*4.0; 25 | t10 = Mc.*Mp.*t6; 26 | xdot = [ds;(F.*I.*8.0+F.*Mp.*t6.*2.0-I.*b.*ds.*8.0+l.*t3.*t5.*t9+l.^3.*t3.*t4.*t5+g.*t4.*t6.*sin(t7)-Mp.*b.*ds.*t6.*2.0)./(t8.*2.0+t9.*2.0+t10.*2.0-t4.*t6.*cos(t7).*2.0);dth;(Mp.*l.*(F.*t2.*2.0+Mc.*g.*t3+Mp.*g.*t3-b.*ds.*t2.*2.0+Mp.*l.*t2.*t3.*t5).*-2.0)./(t8+t9+t10-t4.*t6.*(t2.^2.*2.0-1.0))]; 27 | -------------------------------------------------------------------------------- /CODEGEN/invertedPendulum_gradu_f.m: -------------------------------------------------------------------------------- 1 | function gradu_f = invertedPendulum_gradu_f(in1,F,in3) 2 | %INVERTEDPENDULUM_GRADU_F 3 | % GRADU_F = INVERTEDPENDULUM_GRADU_F(IN1,F,IN3) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.3. 6 | % 09-Jan-2020 11:59:41 7 | 8 | I = in3(3,:); 9 | Mc = in3(1,:); 10 | Mp = in3(2,:); 11 | l = in3(5,:); 12 | th = in1(3,:); 13 | t2 = cos(th); 14 | t3 = Mp.^2; 15 | t4 = l.^2; 16 | t5 = I.*Mc.*4.0; 17 | t6 = I.*Mp.*4.0; 18 | t7 = Mc.*Mp.*t4; 19 | gradu_f = [0.0,(I.*4.0+Mp.*t4)./(t5+t6+t7-t3.*t4.*cos(th.*2.0)),0.0,(Mp.*l.*t2.*-4.0)./(t5+t6+t7-t3.*t4.*(t2.^2.*2.0-1.0))]; 20 | -------------------------------------------------------------------------------- /CODEGEN/invertedPendulum_gradx_f.m: -------------------------------------------------------------------------------- 1 | function gradx_f = invertedPendulum_gradx_f(in1,F,in3) 2 | %INVERTEDPENDULUM_GRADX_F 3 | % GRADX_F = INVERTEDPENDULUM_GRADX_F(IN1,F,IN3) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.3. 6 | % 09-Jan-2020 11:59:41 7 | 8 | I = in3(3,:); 9 | Mc = in3(1,:); 10 | Mp = in3(2,:); 11 | b = in3(6,:); 12 | ds = in1(2,:); 13 | dth = in1(4,:); 14 | g = in3(4,:); 15 | l = in3(5,:); 16 | th = in1(3,:); 17 | t2 = cos(th); 18 | t3 = sin(th); 19 | t4 = I.*4.0; 20 | t5 = Mp.^2; 21 | t6 = dth.^2; 22 | t7 = l.^2; 23 | t8 = l.^3; 24 | t9 = th.*2.0; 25 | t10 = cos(t9); 26 | t11 = sin(t9); 27 | t12 = Mc.*t4; 28 | t13 = Mp.*t4; 29 | t14 = Mp.*t7; 30 | t15 = Mc.*t14; 31 | t16 = t4+t14; 32 | t17 = t5.*t7.*t10; 33 | t18 = -t17; 34 | t19 = t12+t13+t15+t18; 35 | t20 = 1.0./t19; 36 | t21 = t20.^2; 37 | gradx_f = reshape([0.0,1.0,0.0,0.0,0.0,-b.*t16.*t20,(t20.*(g.*t17.*2.0+l.*t2.*t6.*t13+t2.*t5.*t6.*t8))./2.0-t5.*t7.*t11.*t21.*(F.*t14.*2.0+F.*I.*8.0-I.*b.*ds.*8.0-b.*ds.*t14.*2.0+g.*t5.*t7.*t11+l.*t3.*t6.*t13+t3.*t5.*t6.*t8),Mp.*dth.*l.*t3.*t16.*t20,0.0,0.0,0.0,1.0,0.0,(Mp.*b.*l.*t2.*4.0)./(t12+t13+t15-t5.*t7.*(t2.^2.*2.0-1.0)),Mp.*l.*t20.*(F.*t3.*-2.0+Mc.*g.*t2+Mp.*g.*t2+b.*ds.*t3.*2.0+Mp.*l.*t6.*t10).*-2.0+Mp.^3.*t8.*t11.*t21.*(F.*t2.*2.0+Mc.*g.*t3+Mp.*g.*t3-b.*ds.*t2.*2.0+(Mp.*l.*t6.*t11)./2.0).*4.0,dth.*t5.*t7.*t11.*t20.*-2.0],[4,4]); 38 | -------------------------------------------------------------------------------- /CODEGEN/singletrack_gradu_f.m: -------------------------------------------------------------------------------- 1 | function gradu = singletrack_gradu_f(in1,in2,in3) 2 | %SINGLETRACK_GRADU_F 3 | % GRADU = SINGLETRACK_GRADU_F(IN1,IN2,IN3) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.3. 6 | % 09-Jan-2020 11:52:34 7 | 8 | I_z = in3(2,:); 9 | M = in3(1,:); 10 | T = in2(2,:); 11 | V_vx = in1(4,:); 12 | V_vy = in1(5,:); 13 | c_f = in3(8,:); 14 | delta = in2(1,:); 15 | deltamax = in3(5,:); 16 | l_f = in3(3,:); 17 | maxbrakeWForce = in3(6,:); 18 | maxmotorWForce = in3(7,:); 19 | psi_dot = in1(6,:); 20 | t2 = 1.0./I_z; 21 | t3 = 1.0./M; 22 | t4 = T.*5.0e+1; 23 | t5 = delta.*5.0e+1; 24 | t6 = deltamax.*5.0e+1; 25 | t8 = V_vy.*1i; 26 | t9 = V_vx.*1.0e+2; 27 | t13 = l_f.*psi_dot.*1i; 28 | t7 = -t4; 29 | t10 = -t5; 30 | t11 = -t6; 31 | t12 = tanh(t9); 32 | t24 = V_vx+t8+t13; 33 | t14 = t7+5.0e+1; 34 | t15 = t7-5.0e+1; 35 | t18 = t6+t10; 36 | t21 = t10+t11; 37 | t26 = angle(t24); 38 | t16 = exp(t14); 39 | t17 = exp(t15); 40 | t19 = exp(t18); 41 | t22 = exp(t21); 42 | t20 = t16+1.0; 43 | t23 = t17+1.0; 44 | t25 = t19+1.0; 45 | t29 = t22+1.0; 46 | t27 = 1.0./t20; 47 | t30 = 1.0./t23; 48 | t32 = 1.0./t25; 49 | t34 = 1.0./t29; 50 | t28 = t27.^2; 51 | t31 = t30.^2; 52 | t33 = t32.^2; 53 | t35 = t34.^2; 54 | t36 = t27.*5.0e+1; 55 | t37 = t27-1.0; 56 | t38 = t30.*5.0e+1; 57 | t40 = deltamax.*t32; 58 | t41 = t32-1.0; 59 | t43 = t34-1.0; 60 | t39 = -t36; 61 | t42 = -t38; 62 | t44 = -t40; 63 | t45 = deltamax.*t43; 64 | t47 = t16.*t28.*5.0e+1; 65 | t49 = t17.*t31.*5.0e+1; 66 | t50 = t16.*t28.*2.5e+3; 67 | t53 = t17.*t31.*2.5e+3; 68 | t55 = t6.*t19.*t33; 69 | t56 = deltamax.*t19.*t33.*-5.0e+1; 70 | t57 = t6.*t22.*t35; 71 | t58 = t30.*t37; 72 | t59 = deltamax.*t22.*t35.*-5.0e+1; 73 | t61 = t37.*t38; 74 | t64 = t34.*t41; 75 | t67 = t4.*t16.*t28.*t30; 76 | t69 = t4.*t17.*t31.*t37; 77 | t70 = t5.*t19.*t33.*t34; 78 | t72 = t5.*t22.*t35.*t41; 79 | t46 = -t45; 80 | t48 = -t47; 81 | t51 = -t49; 82 | t52 = -t50; 83 | t54 = -t53; 84 | t60 = T.*t58; 85 | t63 = t4.*t58; 86 | t65 = delta.*t64; 87 | t68 = T.*t30.*t50; 88 | t71 = T.*t37.*t53; 89 | t90 = t56+t59+t64+t70+t72; 90 | t62 = -t60; 91 | t66 = -t65; 92 | t74 = t39+t42+t63+5.0e+1; 93 | t86 = t26+t44+t46+t65; 94 | t87 = t48+t51+t58+t67+t69; 95 | t89 = t52+t54+t61+t68+t71; 96 | t73 = t30+t37+t62; 97 | t75 = exp(t74); 98 | t77 = t40+t45+t66; 99 | t76 = t75+1.0; 100 | t78 = sin(t77); 101 | t81 = cos(t77); 102 | t79 = 1.0./t76; 103 | t80 = t79.^2; 104 | t82 = maxmotorWForce.*t79; 105 | t83 = t79-1.0; 106 | t84 = maxbrakeWForce.*t12.*t83; 107 | t91 = maxmotorWForce.*t75.*t80.*t89; 108 | t92 = maxbrakeWForce.*t12.*t75.*t80.*t89; 109 | t85 = -t84; 110 | t93 = -t92; 111 | t88 = t82+t85; 112 | t94 = t91+t93; 113 | gradu = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t3.*(c_f.*t78.*t90-c_f.*t81.*t86.*t90+(t73.*t78.*t88.*t90)./2.0),-t3.*((t73.*t94)./2.0+(t87.*t88)./2.0+(t73.*t81.*t94)./2.0+(t81.*t87.*t88)./2.0),0.0,-t3.*(c_f.*t81.*t90+c_f.*t78.*t86.*t90+(t73.*t81.*t88.*t90)./2.0),-t3.*((t73.*t78.*t94)./2.0+(t78.*t87.*t88)./2.0),0.0,-t2.*(c_f.*l_f.*t81.*t90+c_f.*l_f.*t78.*t86.*t90+(l_f.*t73.*t81.*t88.*t90)./2.0),-t2.*((l_f.*t73.*t78.*t94)./2.0+(l_f.*t78.*t87.*t88)./2.0),0.0,0.0,0.0,1.0],[3,7]); 114 | -------------------------------------------------------------------------------- /CODEGEN/singletrack_gradx_f.m: -------------------------------------------------------------------------------- 1 | function gradx = singletrack_gradx_f(in1,in2,in3) 2 | %SINGLETRACK_GRADX_F 3 | % GRADX = SINGLETRACK_GRADX_F(IN1,IN2,IN3) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.3. 6 | % 09-Jan-2020 11:52:33 7 | 8 | I_z = in3(2,:); 9 | M = in3(1,:); 10 | T = in2(2,:); 11 | V_vx = in1(4,:); 12 | V_vy = in1(5,:); 13 | c_f = in3(8,:); 14 | c_r = in3(9,:); 15 | delta = in2(1,:); 16 | deltamax = in3(5,:); 17 | l_f = in3(3,:); 18 | l_r = in3(4,:); 19 | maxbrakeWForce = in3(6,:); 20 | psi_dot = in1(6,:); 21 | vpsi = in1(3,:); 22 | t2 = cos(vpsi); 23 | t3 = sin(vpsi); 24 | t4 = l_f.*psi_dot; 25 | t5 = l_r.*psi_dot; 26 | t6 = V_vx.^2; 27 | t7 = 1.0./I_z; 28 | t8 = 1.0./M; 29 | t9 = T.*5.0e+1; 30 | t10 = delta.*5.0e+1; 31 | t11 = deltamax.*5.0e+1; 32 | t14 = V_vx.*1.0e+2; 33 | t12 = -t5; 34 | t13 = -t9; 35 | t15 = -t10; 36 | t16 = -t11; 37 | t17 = V_vy+t4; 38 | t18 = tanh(t14); 39 | t19 = t17.^2; 40 | t20 = V_vy+t12; 41 | t21 = t13+5.0e+1; 42 | t22 = t18.^2; 43 | t23 = t13-5.0e+1; 44 | t27 = t11+t15; 45 | t31 = t15+t16; 46 | t24 = t20.^2; 47 | t25 = exp(t21); 48 | t26 = exp(t23); 49 | t28 = exp(t27); 50 | t29 = t6+t19; 51 | t32 = exp(t31); 52 | t34 = t22.*1.0e+2; 53 | t30 = t25+1.0; 54 | t33 = t26+1.0; 55 | t35 = t6+t24; 56 | t36 = t28+1.0; 57 | t38 = t32+1.0; 58 | t39 = 1.0./t29; 59 | t44 = t34-1.0e+2; 60 | t37 = 1.0./t30; 61 | t40 = 1.0./t33; 62 | t41 = 1.0./t36; 63 | t42 = 1.0./t35; 64 | t43 = 1.0./t38; 65 | t45 = t37.*5.0e+1; 66 | t46 = t37-1.0; 67 | t47 = t40.*5.0e+1; 68 | t49 = deltamax.*t41; 69 | t50 = t41-1.0; 70 | t52 = V_vx.*c_r.*l_r.*t42; 71 | t53 = t43-1.0; 72 | t48 = -t45; 73 | t51 = -t47; 74 | t54 = deltamax.*t53; 75 | t55 = T.*t40.*t46; 76 | t57 = t9.*t40.*t46; 77 | t58 = delta.*t43.*t50; 78 | t56 = -t55; 79 | t59 = -t58; 80 | t61 = t48+t51+t57+5.0e+1; 81 | t60 = t40+t46+t56; 82 | t62 = exp(t61); 83 | t64 = t49+t54+t59; 84 | t63 = t62+1.0; 85 | t65 = sin(t64); 86 | t67 = cos(t64); 87 | t66 = 1.0./t63; 88 | t69 = V_vx.*c_f.*l_f.*t39.*t67; 89 | t68 = t66-1.0; 90 | gradx = reshape([0.0,0.0,-V_vx.*t3-V_vy.*t2,t2,-t3,0.0,0.0,0.0,0.0,V_vx.*t2-V_vy.*t3,t3,t2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,t8.*(-c_f.*t17.*t39.*t65+(maxbrakeWForce.*t44.*t60.*t68)./2.0+(maxbrakeWForce.*t44.*t60.*t67.*t68)./2.0),t8.*(psi_dot+V_vx.*c_f.*t39.*t65),t8.*(V_vy+V_vx.*c_f.*l_f.*t39.*t65),0.0,0.0,0.0,0.0,t8.*(c_r.*t20.*t42+c_f.*t17.*t39.*t67+(maxbrakeWForce.*t44.*t60.*t65.*t68)./2.0),-t8.*(psi_dot+V_vx.*c_r.*t42+V_vx.*c_f.*t39.*t67),-t8.*(V_vy-t52+t69),0.0,0.0,0.0,0.0,t7.*(-c_r.*l_r.*t20.*t42+c_f.*l_f.*t17.*t39.*t67+(l_f.*maxbrakeWForce.*t44.*t60.*t65.*t68)./2.0),t7.*(t52-t69),-t7.*(l_f.*t69+l_r.*t52),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[7,7]); 91 | -------------------------------------------------------------------------------- /Presentation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Gaussian-Process-based-Model-Predictive-Control/ef00c0df1ff25fb75f6f9c3d9099d47c9cfe1078/Presentation.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Gaussian-Process based Model Predictive Control [IN PROGRESS] 2 | Project for the course "Statistical Learning and Stochastic Control" at University of Stuttgart 3 | 4 | 5 | For detailed information about the project, please refer to the [Presentation](./Presentation.pdf) and [Report](./Report.pdf). 6 | 7 | Supported Matlab Version **>= R2019a** 8 | 9 | 10 | ## Control of a Race Vehicle with unkown complex dynamics 11 | 12 | To run the Race Car example execute: 13 | > main_singletrack.m 14 | 15 |
16 | 17 | A Gaussian process is used to learn unmodeled dynamics 18 | ```math 19 | x_{k+1} = f_d(x_k,u_k) + B_d * ( GP(z_k) + w ) 20 | 21 | , where z_k = [Bz_x*xk ; Bz_u*uk] is the vector of selected features 22 | f_d is the dicrete nominal model 23 | w ~ N(0,\sigma_n) is the process WG noise 24 | GP is the Gaussian Process model reponsible for learning the unmodeled dynamics 25 | 26 | ``` 27 | The Gaussian Process model GP is then fed with data (X,Y+w) collected online, such that: 28 | ```math 29 | X = [x_k,u_k] 30 | Y + w = pinv(B_d) * ( x_{k+1} - f_d(x_k,u_k) ) 31 | ``` 32 | and it is trained (hyperparameter optimization) by maximizing the log Likelihood p(Y|X,theta), where theta is the vector of hyperparameters. 33 | 34 | 35 | 36 | ### Results 37 | 38 | 39 | | NMPC controller with unmodelled dynamics | Learning-Based NMPC controller (with trained Gaussian Process) | 40 | | ------------- |-------------| 41 | | drawing | drawing | 42 | 43 |
44 | 45 | ## Control of an Inverted Pendulum with deffect motor 46 | 47 | To run the Inverted Pendulum please execute 48 | > main_invertedPendulum.m 49 | -------------------------------------------------------------------------------- /Report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasrm25/Gaussian-Process-based-Model-Predictive-Control/ef00c0df1ff25fb75f6f9c3d9099d47c9cfe1078/Report.pdf -------------------------------------------------------------------------------- /classes/GP.m: -------------------------------------------------------------------------------- 1 | %------------------------------------------------------------------ 2 | % Programed by: 3 | % - Lucas Rath (lucasrm25@gmail.com) 4 | % - 5 | % - 6 | %------------------------------------------------------------------ 7 | 8 | classdef GP < handle 9 | %------------------------------------------------------------------ 10 | % Gaussian Process model fitting with SEQ kernel function 11 | % 12 | % This is a MIMO fitting function x \in R^n -> y \in R^p. 13 | % each output dimension is treated as a different GP 14 | % 15 | % The Kernel parameters, when optimized, are optimized for each output 16 | % dimension separately. 17 | % 18 | %------------------------------------------------------------------ 19 | 20 | properties 21 | isActive = true 22 | end 23 | 24 | properties(SetAccess=private) 25 | % kernel parameters (one for each output dimension) 26 | M % 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') --------------------------------------------------------------------------------