├── _config.yml ├── Harish_ME762_project ├── SMC_simulation_code │ ├── startup.m │ ├── fncs │ │ ├── generate_seed.m │ │ ├── numerical_derivative.m │ │ ├── saturate.m │ │ ├── sgn.m │ │ ├── numerical_derivative2.m │ │ ├── fwrap.m │ │ ├── skew.m │ │ ├── splineTrajectory.m │ │ ├── randinterval.m │ │ ├── ddt_atan.m │ │ ├── generate_powers.m │ │ ├── d2dt2_atan.m │ │ ├── create_poly.m │ │ ├── create_poly_derivative.m │ │ ├── fval.m │ │ ├── measr.m │ │ ├── invT_bicycle.m │ │ ├── create_poly_coefficients.m │ │ ├── getLocalError.m │ │ ├── parseInput.m │ │ └── find_and_replace.m │ ├── KINSM.mj2 │ ├── global_test1_6.mat │ ├── html │ │ └── KinSliding.doc │ ├── Models │ │ ├── generateNoise.m │ │ ├── description_mapping.pdf │ │ ├── mappings │ │ │ ├── map0.m │ │ │ ├── ForceController_pacejka.m │ │ │ └── AXController_pacejka.m │ │ ├── vmodel_A │ │ │ ├── load_parameters_orend.m │ │ │ └── vmodel_A.m │ │ ├── MAPPING INPUTS.txt │ │ └── CARparameters.m │ ├── preload │ │ ├── Kopt_lqr_mu_1.mat │ │ └── Kopt_lqr_mu_0.6.mat │ ├── Scenarios │ │ ├── 01_single_lane_change.mat │ │ └── 02_double_lane_change.mat │ ├── Visualization │ │ ├── myquiver.m │ │ ├── vplot_ref.m │ │ ├── vplot_aggregate.m │ │ ├── vplot.m │ │ └── visualize.m │ ├── default_options.m │ ├── getVehicleX0.m │ ├── @Trajectory │ │ ├── solveID.m │ │ ├── Trajectory.m │ │ ├── make_static.m │ │ ├── internalDynamics_vehicle_A.m │ │ ├── load.m │ │ ├── transform.m │ │ └── arcLength.m │ ├── myode4_fixed_state.m │ ├── KinSliding.m │ ├── TEST.m │ ├── simulate.m │ └── latex │ │ └── latexTable.m ├── Harish_N_Sathishchandra_project_report.pdf ├── references │ ├── Sliding Mode control a tutorial.pdf │ ├── Trajectory planning and sliding-mode control.pdf │ ├── Automatic Steering Methods for Autonomous Automobile Path Tracking.pdf │ └── Sliding Mode Control for trajectory tracking of nonholonomic wheeled mobile robots.pdf └── Trajectory Planning for Autonomous Vehicles using Sliding Mode Control.pptx └── README.md /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-cayman -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/startup.m: -------------------------------------------------------------------------------- 1 | addpath( genpath( cd ) ); -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/generate_seed.m: -------------------------------------------------------------------------------- 1 | function [ seed ] = generate_seed(n) 2 | %GENERATE_SEED 3 | seed=2*pi*rand(n,1); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/numerical_derivative.m: -------------------------------------------------------------------------------- 1 | function dy = numerical_derivative(f,x) 2 | eps = 1e-8; 3 | dy = (f(x+eps)-f(x-eps))./eps./2; 4 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/KINSM.mj2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/SMC_simulation_code/KINSM.mj2 -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/saturate.m: -------------------------------------------------------------------------------- 1 | function [ saturated_x ] = saturate( x,lb,ub) 2 | %SATURATE 3 | saturated_x = min(ub, max(lb, x)); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/sgn.m: -------------------------------------------------------------------------------- 1 | function [ sgn ] = sgn( x ) 2 | %SGN %analytical sign approximation function 3 | sgn=tanh(10*x); 4 | 5 | end 6 | 7 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/global_test1_6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/SMC_simulation_code/global_test1_6.mat -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/numerical_derivative2.m: -------------------------------------------------------------------------------- 1 | function dy = numerical_derivative2(f,x) 2 | eps = 1e-6; 3 | dy = ( f(x+2*eps) - 2.*f(x) + f(x-2*eps) ) ./ (4*eps^2); 4 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/html/KinSliding.doc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/SMC_simulation_code/html/KinSliding.doc -------------------------------------------------------------------------------- /Harish_ME762_project/Harish_N_Sathishchandra_project_report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/Harish_N_Sathishchandra_project_report.pdf -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Models/generateNoise.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/SMC_simulation_code/Models/generateNoise.m -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/fwrap.m: -------------------------------------------------------------------------------- 1 | function y = fwrap(f, x) 2 | if size(x,2) == 1 3 | y = f(x); 4 | else 5 | y = arrayfun(f, x, 'uni', 0); 6 | y = cat(2, y{:}); 7 | end -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/skew.m: -------------------------------------------------------------------------------- 1 | % Author: Davide Calzolari 2 | 3 | function [ X ] = skew( x ) 4 | 5 | X=[0 -x(3) x(2) ; x(3) 0 -x(1) ; -x(2) x(1) 0 ]; 6 | 7 | end 8 | 9 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/splineTrajectory.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/SMC_simulation_code/fncs/splineTrajectory.m -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/randinterval.m: -------------------------------------------------------------------------------- 1 | function [ out ] = randinterval( x,dim) 2 | %RANDINTERVAL random numbers in interval +/- x 3 | out=-x + (x+x)*rand(dim,1); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/preload/Kopt_lqr_mu_1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/SMC_simulation_code/preload/Kopt_lqr_mu_1.mat -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/preload/Kopt_lqr_mu_0.6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/SMC_simulation_code/preload/Kopt_lqr_mu_0.6.mat -------------------------------------------------------------------------------- /Harish_ME762_project/references/Sliding Mode control a tutorial.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/references/Sliding Mode control a tutorial.pdf -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Models/description_mapping.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/SMC_simulation_code/Models/description_mapping.pdf -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Scenarios/01_single_lane_change.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/SMC_simulation_code/Scenarios/01_single_lane_change.mat -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Scenarios/02_double_lane_change.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/SMC_simulation_code/Scenarios/02_double_lane_change.mat -------------------------------------------------------------------------------- /Harish_ME762_project/references/Trajectory planning and sliding-mode control.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/references/Trajectory planning and sliding-mode control.pdf -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/ddt_atan.m: -------------------------------------------------------------------------------- 1 | function [ derivative ] = ddt_atan( y,x,dy,dx ) 2 | %ATAN_DERIVATIVE 3 | if y^2+x^2==0 4 | derivative=0; 5 | return 6 | end 7 | derivative=(y*dx-x*dy)/(y^2+x^2); 8 | end 9 | 10 | -------------------------------------------------------------------------------- /Harish_ME762_project/Trajectory Planning for Autonomous Vehicles using Sliding Mode Control.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/Trajectory Planning for Autonomous Vehicles using Sliding Mode Control.pptx -------------------------------------------------------------------------------- /Harish_ME762_project/references/Automatic Steering Methods for Autonomous Automobile Path Tracking.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/references/Automatic Steering Methods for Autonomous Automobile Path Tracking.pdf -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/generate_powers.m: -------------------------------------------------------------------------------- 1 | % Author: Davide Calzolari 2 | 3 | function [ powers ] = generate_powers( n, x) 4 | powers=zeros(1,n+1); 5 | powers(1) = 1.0; 6 | for i=2:1:(n+1); 7 | powers(i) = powers(i-1)*x; 8 | end 9 | end 10 | 11 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/d2dt2_atan.m: -------------------------------------------------------------------------------- 1 | function [ derivative ] = d2dt2_atan( y,x,dy,dx,ddy,ddx ) 2 | %d2dt2_atan 3 | derivative=-(y*ddx)/(x^2+y^2)+(y*dx*(2*x*dx+2*y*dy))/(x^2+y^2)^2 ... 4 | -(x*dy*(2*x*dx+2*y*dy))/(x^2+y^2)^2+(x*ddy)/(x^2+y^2); 5 | end 6 | 7 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Visualization/myquiver.m: -------------------------------------------------------------------------------- 1 | function myquiver(t,x,y,psi,n,c) 2 | tn = linspace(t(1),t(end),n); 3 | quiver(interp1(t,x,tn),... 4 | interp1(t,y,tn),... 5 | cos(interp1(t,psi,tn)),... 6 | sin(interp1(t,psi,tn)),0.2,'Color',c); 7 | 8 | -------------------------------------------------------------------------------- /Harish_ME762_project/references/Sliding Mode Control for trajectory tracking of nonholonomic wheeled mobile robots.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/harishsatishchandra/SMC-controller/HEAD/Harish_ME762_project/references/Sliding Mode Control for trajectory tracking of nonholonomic wheeled mobile robots.pdf -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/create_poly.m: -------------------------------------------------------------------------------- 1 | function f = create_poly(p) 2 | %creates polynomial function for coefficients in p 3 | %p is ordered by increasing exponent 4 | f = @(x)poly(x,p); 5 | 6 | function y = poly(x,p) 7 | y = ones(size(x)).*p(end); 8 | for i=1:length(p)-1 9 | y = y .* x; 10 | y = y + p(end-i); 11 | end -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/create_poly_derivative.m: -------------------------------------------------------------------------------- 1 | function pd = create_poly_derivative(p,n) 2 | pd = p; 3 | if n>0 4 | if length(pd)>1 5 | pd = pd(2:end); 6 | for i=1:length(pd) 7 | pd(i) = pd(i) * i; 8 | end 9 | pd = create_poly_derivative(pd,n-1); 10 | else 11 | pd(1) = 0; 12 | end 13 | end -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Visualization/vplot_ref.m: -------------------------------------------------------------------------------- 1 | function TT=vplot_ref(bench) 2 | %% trajectory 3 | TT=bench.data.T; 4 | XT=bench.data.tau.X(TT)'; 5 | YT=bench.data.tau.Y(TT)'; 6 | 7 | figure(1); 8 | hold all;box on; 9 | h1 = plot(XT,YT,'-.','Color','red','LineWidth',1.5); 10 | %uistack(h1,'top') 11 | xlabel('X (m)'); 12 | ylabel('Y (m)'); 13 | drawnow 14 | 15 | end -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/default_options.m: -------------------------------------------------------------------------------- 1 | function [ options ] = default_options() 2 | %DEFAULT_OPTIONS 3 | 4 | options.DISPLAY_OUTPUT=1; 5 | options.V=0; %noise 6 | CARparameters; 7 | 8 | options.p=CARparameters; 9 | options.pc=CARparameters; 10 | %time span 11 | options.stepSize=0.02; 12 | %model types 13 | options.types={'vmodel_A','vmodel_K'}; 14 | end 15 | 16 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/fval.m: -------------------------------------------------------------------------------- 1 | function Y=fval(pp,x_int) 2 | 3 | br = pp.breaks.'; 4 | cf = pp.coefs; 5 | 6 | [~, inds] = histc(x_int, [-inf; br(2:end-1); +inf]); 7 | 8 | x_shf = x_int - br(inds); 9 | zero = ones(size(x_shf)); 10 | one = x_shf; 11 | two = one .* x_shf; 12 | three = two .* x_shf; 13 | 14 | Y = sum( [three two one zero] .* cf(inds,:), 2); -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Visualization/vplot_aggregate.m: -------------------------------------------------------------------------------- 1 | function vplot_aggregate(bench,c) 2 | %% trajectory 3 | TT=bench.data.T; 4 | XCGT=bench.data.X(:,1); 5 | YCGT=bench.data.X(:,2); 6 | PsiT=bench.data.X(:,3); 7 | 8 | figure(1); 9 | hold all;box on; 10 | h2 = plot(XCGT,YCGT,'-','Color',c,'LineWidth',1.2); 11 | 12 | %myquiver(TT,XCGT,YCGT,PsiT,20,h2.Color); 13 | 14 | drawnow 15 | 16 | end -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Models/mappings/map0.m: -------------------------------------------------------------------------------- 1 | function [u] = map0(ax_ref,y,steer,p) 2 | %MAP0 Summary of this function goes here 3 | % 4 | % Syntax: MAP0 5 | % 6 | % Inputs: 7 | % 8 | % Outputs: 9 | % 10 | % See also: --- 11 | 12 | % Author: Davide Calzolari 13 | % Date: 2016/09/20 14 | % Revision: 0.1 15 | % Technische Universitaet Muenchen 2016 16 | 17 | u=[steer,ax_ref]; 18 | 19 | end 20 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Models/vmodel_A/load_parameters_orend.m: -------------------------------------------------------------------------------- 1 | %% vehicle 2 | m = 1750; 3 | J = 2500; 4 | l_F = 1.43; 5 | l_R = 1.27; 6 | L = l_F + l_R; 7 | h = 0.5; 8 | mu0 = 1; 9 | B_F = 10.4; 10 | C_F = 1.3; 11 | Kfz_F = 0.1; 12 | B_R = 21.4; 13 | C_R = 1.1; 14 | Kfz_R = 0.1; 15 | R = 0.32; 16 | Jwheel = 1.2; 17 | kA0 = 5; 18 | kA1 = 3.3541; 19 | kB0 = 22.3607; 20 | kB1 = 17.254; 21 | kB2 = 5.8744; 22 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/measr.m: -------------------------------------------------------------------------------- 1 | function [ measure ] = measr(t,y,Q,seed) 2 | %MEASR 3 | % Q: diagonal error variance matrix 4 | % seed: phase vector 5 | % 6 | % add measurments noise 7 | % needs to be function of time so that ode45 can solve it 8 | % w=sqrt(2.*Q)*sin(repmat(t'*1e2,length(y),1)+repmat(seed,1,length(t))); 9 | % measure=repmat(y,1,length(t))+w; 10 | w=sqrt(2.*Q)*sin(t*1e2+seed); 11 | measure=y+w; 12 | 13 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Models/MAPPING INPUTS.txt: -------------------------------------------------------------------------------- 1 | The simulation model inputs are front wheel speed and steering angle. 2 | 3 | - DynFlatness generates desired forces on the front tire for tracking, 4 | ForceController_pacejka realises these forces by means of wheel speed and steering angle. 5 | 6 | - All the other controllers implements AXController_pacejka which translates 7 | the required longitudinal acceleration into a traction force and then again 8 | into front wheel speed. -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/getVehicleX0.m: -------------------------------------------------------------------------------- 1 | function [x0] = getVehicleX0(model,tau,options) 2 | 3 | n=int32(length(model(0,zeros(10,1),zeros(5,1),options.p))); 4 | 5 | x0 = zeros(n,1); 6 | x0(1)=tau.X(0); 7 | x0(2)=tau.Y(0); 8 | x0(3)=tau.theta(0); 9 | x0(4)=tau.v(0); 10 | x0(5)=0; 11 | x0(6)=0; 12 | 13 | switch n 14 | case 9 %5DOF model 15 | w0=tau.v(0)/options.p.R; %wheel speed 16 | x0(7)=w0; 17 | x0(8)=w0; 18 | case 7 19 | %x0(1)=tau.X(0)-(options.p.l_R); 20 | end 21 | 22 | 23 | end 24 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/@Trajectory/solveID.m: -------------------------------------------------------------------------------- 1 | function solveID(obj,p) 2 | 3 | f = @(t,x)obj.internalDynamics_vehicle_A(x,p,obj.dX(t),obj.ddX(t),obj.dY(t),obj.ddY(t)); 4 | [TXIode,XIode] = ode113(f,0:0.01:obj.T,[0;0],odeset('AbsTol',1e-10,'RelTol',1e-10)); 5 | for i=1:length(TXIode) 6 | dxi = f(TXIode(i),XIode(i,1:2)); 7 | XIode(i,3) = dxi(2); 8 | end 9 | obj.psi0 = @(t)interp1(TXIode,XIode(:,1),t,'spline'); 10 | obj.dpsi0 = @(t)interp1(TXIode,XIode(:,2),t,'spline'); 11 | obj.ddpsi0 = @(t)interp1(TXIode,XIode(:,3),t,'spline'); 12 | end 13 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Models/CARparameters.m: -------------------------------------------------------------------------------- 1 | function [ p ] = CARparameters( ) 2 | %CARPARAMETERS 3 | p.m = 1750; 4 | p.J = 2500; 5 | p.l_F = 1.43; 6 | p.l_R = 1.27; 7 | p.L = p.l_F + p.l_R; 8 | p.h = 0.5; 9 | p.mu0 = 1; 10 | p.B_F = 10.4; 11 | p.C_F = 1.3; 12 | p.Kfz_F = 0.1; 13 | p.B_R = 21.4; 14 | p.C_R = 1.1; 15 | p.Kfz_R = 0.1; 16 | p.R = 0.32; 17 | p.Jwheel = 1.2; 18 | 19 | %linear approximation values for control 20 | p.cf=1.0299e+05; 21 | p.cr=1.8917e+05; 22 | 23 | %maximum values 24 | p.MAX_delta=pi/4; 25 | 26 | %controller A gains 27 | p.kA0 = 5; 28 | p.kA1 = 3.3541; 29 | 30 | end 31 | 32 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/invT_bicycle.m: -------------------------------------------------------------------------------- 1 | function [ invT, C] = invT_bicycle( b,steer,y,p) 2 | %INVT_BICYCLE 3 | theta=y(3); 4 | v=y(4); 5 | 6 | invT=[cos(theta)/(cos(theta)^2 + sin(theta)^2), sin(theta)/(cos(theta)^2 + sin(theta)^2) 7 | -(p.L*sin(theta) + b*cos(theta)*tan(steer))/(b*cos(theta)^2*v + b*sin(theta)^2*v + b*cos(theta)^2*tan(steer)^2*v + b*sin(theta)^2*tan(steer)^2*v), (p.L*cos(theta) - b*sin(theta)*tan(steer))/(b*cos(theta)^2*v + b*sin(theta)^2*v + b*cos(theta)^2*tan(steer)^2*v + b*sin(theta)^2*tan(steer)^2*v)]; 8 | 9 | C=[-(tan(steer)*v^2*(p.L*sin(theta) + b*cos(theta)*tan(steer)))/p.L^2 10 | (tan(steer)*v^2*(p.L*cos(theta) - b*sin(theta)*tan(steer)))/p.L^2]; 11 | 12 | end 13 | 14 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/myode4_fixed_state.m: -------------------------------------------------------------------------------- 1 | function [T,X,intState] = myode4_fixed_state(f,Tint,x0,intState0) 2 | 3 | X = [x0';zeros(length(Tint)-1,length(x0))]; 4 | T = Tint; 5 | 6 | intState=intState0;%;zeros(length(Tint)-1,length(intState0))]; 7 | 8 | for i = 1:length(T)-1 9 | j = i+1; 10 | ti = T(i); 11 | tj = T(j); 12 | h = tj-ti; 13 | 14 | 15 | xi = X(i,:)'; 16 | 17 | 18 | [k1,intState{j}] = f( ti, xi,1,intState{j-1}); 19 | k2 = f( ti + h/2, xi + h/2 * k1,0,intState{j}); 20 | k3 = f( ti + h/2, xi + h/2 * k2,0,intState{j}); 21 | k4 = f( ti + h, xi + h * k3,0,intState{j}); 22 | 23 | %R-K-4 24 | xj_4 = xi + h * (k1 + 2*k2 + 2*k3 + k4)/6; 25 | X(j,:) = xj_4'; 26 | end 27 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Visualization/vplot.m: -------------------------------------------------------------------------------- 1 | function vplot( bench,filename) 2 | %% trajectory 3 | TT=bench.data.T; 4 | XT=bench.data.tau.X(TT)'; 5 | YT=bench.data.tau.Y(TT)'; 6 | XCGT=bench.data.X(:,1); 7 | YCGT=bench.data.X(:,2); 8 | PsiT=bench.data.X(:,3); 9 | 10 | figure(1);clf;hold all;box on; 11 | set(gcf(),'Name',[filename '- trajectory']); 12 | h1 = plot(XT,YT,'Color','red'); 13 | h2 = plot(XCGT,YCGT,'Color','blue'); 14 | myquiver(TT,XCGT,YCGT,PsiT,20,'blue'); 15 | xlabel('X (m)'); 16 | ylabel('Y (m)'); 17 | legend([h1,h2],'[X,Y]','[X^{CG},Y^{CG}]','Location','NorthWest'); 18 | myprint([filename 'trajectory']); 19 | drawnow 20 | 21 | end 22 | 23 | function myprint(filename) 24 | set(gcf,'PaperPositionMode','auto'); 25 | saveas(gcf(),[filename '.fig']); 26 | %print([filename '.png'],'-dpng','-r600'); 27 | %print([filename '.eps'],'-deps2','-r300'); 28 | end 29 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/create_poly_coefficients.m: -------------------------------------------------------------------------------- 1 | function p = create_poly_coefficients(g) 2 | n = size(g,1); 3 | assert(size(g,2)==3); 4 | 5 | % degree n-1 6 | % coefficients p 7 | % list with n constraints g 8 | 9 | % create factors of derivatives 10 | C = {ones(1,n)}; 11 | for i=1:n-1 12 | c = C{i}; 13 | c = c(2:end); 14 | m = length(c); 15 | for j = 1:m 16 | c(j) = c(j).* j; 17 | end 18 | C{end+1} = c; %#ok 19 | end 20 | 21 | % create matrix with contraints 22 | b = g(:,3); %constraint value 23 | A = zeros(n,n); 24 | for i=1:n; 25 | m = g(i,1);%derivative order of constraint 26 | a = C{m+1};%corresponding derivative factors 27 | t = g(i,2);%time of constraint 28 | for j = 1:length(a) 29 | a(j) = a(j) .* (t.^(j-1)); 30 | end 31 | A(i,(n-length(a)+1):n) = a; 32 | end 33 | % constraints: A*p = b 34 | p = A\b; -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/getLocalError.m: -------------------------------------------------------------------------------- 1 | function [ e,de ] = getLocalError( state, pd,vd,thetad,wd, DL, varargin) 2 | %GETLOCALERROR 3 | x=state(1); 4 | y=state(2); 5 | psi=state(3); 6 | vx=state(4); 7 | vy=state(5); 8 | omega=state(6); 9 | 10 | ref=1; %default: local vehicle frame 11 | if nargin > 6 12 | ref=varargin{1}; %1:local vehicle frame, 2:local desired vehicle frame 13 | end 14 | if ref==1 15 | rot_z=[cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1]; 16 | drot_z=skew([0,0,omega]')*rot_z; 17 | else if ref==2 18 | rot_z=[cos(thetad) -sin(thetad) 0; sin(thetad) cos(thetad) 0; 0 0 1]; 19 | drot_z=skew([0,0,wd]')*rot_z; 20 | else 21 | error('Not a valid frame'); 22 | end 23 | end 24 | 25 | e_cart=[[x+DL*cos(psi);y+DL*sin(psi)]-pd; psi-thetad]; 26 | e=rot_z.'*e_cart; 27 | Rot=[cos(psi) -sin(psi); sin(psi) cos(psi)]; 28 | v_xy_cart=Rot*[vx;vy] + [-sin(psi);cos(psi)]*DL*omega; 29 | de=rot_z.'*[v_xy_cart-vd; omega-wd] + drot_z.'*e_cart; 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Models/mappings/ForceController_pacejka.m: -------------------------------------------------------------------------------- 1 | function [u] = ForceController_pacejka(ud,x,Ff_abs,Fmaxf,p) 2 | %FORCECONTROLLERPACEJKA 3 | % 4 | % Syntax: FORCECONTROLLERPACEJKA 5 | % 6 | % Inputs: 7 | % 8 | % Outputs: 9 | % 10 | % See also: --- 11 | 12 | % Authors: Daniel Heß (Original Author), Davide Calzolari 13 | % Date: 2016/10/15 14 | % Revision: 0.1 15 | % Technische Universitaet Muenchen 2016 16 | 17 | Fxf=ud(1); 18 | Fyf=ud(2); 19 | 20 | mu0=p.mu0; 21 | R=p.R; 22 | vx=x(4); 23 | vy=x(5); 24 | omega=x(6); 25 | B_F=p.B_F; 26 | C_F=p.C_F; 27 | l_F=p.l_F; 28 | 29 | %non commanded 30 | omega_R = vx/R; 31 | 32 | %% calculate required speed 33 | sf_abs = mu0/B_F * tan( 1/C_F * asin( Ff_abs/Fmaxf ) ); 34 | 35 | % slip vector 36 | if Ff_abs==0 37 | sf = zeros(2,1); 38 | else 39 | sf = [ -Fxf/Ff_abs * sf_abs; -Fyf/Ff_abs * sf_abs ]; 40 | end 41 | % velocity at front axle 42 | vf = [ vx; vy + l_F * omega ]; 43 | vf_abs = norm(vf); 44 | %Latsch velocity 45 | vLf = sf * vf_abs - vf; 46 | % required front wheel speed 47 | omega_F = norm(vLf)/R; 48 | % steering angle 49 | delta = atan2( -vLf(2), -vLf(1) ); 50 | 51 | u=[delta,omega_F,omega_R]; 52 | 53 | end 54 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Sliding mode controller for trajectory tracking 2 | 3 | The following two animations show the sliding mode controller in action for a single lane change and double lane change manoeuver. The red curve is the output that we get from a trajectory planner, which here is treated as the ground truth trajectory to follow. The green curve is the output of the trajectory tracker (sliding mode controller). The dashed lines are the overshoot that we get from the rear wheels. 4 | 5 | ![smc1_cropped](https://user-images.githubusercontent.com/19624843/69922347-9ad51980-1469-11ea-8484-017e435a9664.gif)

**Single lane change**

6 | 7 | ![smc2_cropped](https://user-images.githubusercontent.com/19624843/69922358-bb04d880-1469-11ea-8b88-028c6fa6d524.gif)

**Double lane change**

8 | 9 | The implementation details are as follows - 10 | 11 | ![github_intro1_Page_1_cropped](https://user-images.githubusercontent.com/19624843/63885285-0361b580-c9a6-11e9-8cb3-d3dfb8da265a.png) 12 | 13 | ![github_intro1_Page_2_cropped](https://user-images.githubusercontent.com/19624843/63885292-05c40f80-c9a6-11e9-9096-14586a80bd33.png) 14 | 15 | ![github_intro1_Page_3_cropped](https://user-images.githubusercontent.com/19624843/63885296-08266980-c9a6-11e9-9f92-d7e1f5fc2b38.png) 16 | 17 | ![github_intro1_Page_4_cropped](https://user-images.githubusercontent.com/19624843/63885299-09f02d00-c9a6-11e9-96a3-7c7918dcb6e4.png) 18 | 19 | 20 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/@Trajectory/Trajectory.m: -------------------------------------------------------------------------------- 1 | classdef Trajectory0 44 | % omega_R0 = v(1)/R+0.001; 45 | % else 46 | % omega_R0 = v(1)/R-0.001; 47 | % end 48 | % if Fxr==0 49 | omega_R = v(1)/R; 50 | % else 51 | % try 52 | % omega_R = fminsearch(@(omega)(fx(omega)-Fxr).^2,omega_R0); 53 | % catch 54 | % asdf = 0; 55 | % end 56 | % end 57 | 58 | % vr_abs = norm(vr); 59 | % sr = [0;vr(2)]/vr_abs; 60 | % fyr = -sin(C_R*atan(B_R*sr(2)/mu0)); 61 | % Fyr = fyr * Fz0_R * mu0; 62 | if norm(vr-[R*omega_R;0])==0 63 | Fyr = 0; 64 | else 65 | Fyr = [0,1]*fxy(omega_R); 66 | end 67 | 68 | %rotational acceleration 69 | domega = l_F*m/J * a(2) - L/J * Fyr; 70 | dxi(1) = omega; 71 | dxi(2) = domega; -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/@Trajectory/load.m: -------------------------------------------------------------------------------- 1 | function load(obj,tau_file) 2 | 3 | load(tau_file); 4 | 5 | cx = create_poly_coefficients(gx); 6 | cy = create_poly_coefficients(gy); 7 | 8 | Xr = create_poly(cx); 9 | dXr = create_poly(create_poly_derivative(cx,1)); 10 | ddXr = create_poly(create_poly_derivative(cx,2)); 11 | 12 | Yr = create_poly(cy); 13 | dYr = create_poly(create_poly_derivative(cy,1)); 14 | ddYr = create_poly(create_poly_derivative(cy,2)); 15 | 16 | [S,W] = obj.arcLength(Xr,Yr); 17 | %set the endpoint constraint to arc-length 18 | row = find(gs(:,1)==0 & gs(:,2)>0,1); 19 | gs(row,3) = S(1); 20 | obj.T = gs(row,2); %end-time 21 | 22 | cs = create_poly_coefficients(gs); 23 | s = create_poly(cs); 24 | v = create_poly(create_poly_derivative(cs,1)); 25 | a = create_poly(create_poly_derivative(cs,2)); 26 | j = create_poly(create_poly_derivative(cs,3)); 27 | 28 | %% time transform 29 | theta_r = @(r)atan2(dYr(r),dXr(r)); 30 | v_r = @(r)sqrt(dXr(r).^2+dYr(r).^2); 31 | dtheta_r = @(r)(dXr(r).*ddYr(r)-dYr(r).*ddXr(r))... 32 | ./(dXr(r).^2+dYr(r).^2); 33 | 34 | %time parametrized path 35 | theta = @(t)theta_r(W(s(t))); 36 | kappa = @(t)dtheta_r(W(s(t)))./v_r(W(s(t))); 37 | dtheta = @(t)kappa(t).*v(t); 38 | 39 | obj.id = tau_file; 40 | 41 | obj.s = s; 42 | obj.v = v; 43 | obj.a = a; 44 | obj.j = j; 45 | obj.theta = theta; 46 | obj.kappa = kappa; 47 | obj.dtheta = dtheta; 48 | obj.X = @(t)Xr(W(s(t))); 49 | obj.Y = @(t)Yr(W(s(t))); 50 | obj.dX = @(t)v(t).*cos(theta(t)); 51 | obj.dY = @(t)v(t).*sin(theta(t)); 52 | obj.ddX = @(t)a(t).*cos(theta(t)) - v(t).^2.*kappa(t).*sin(theta(t)); 53 | obj.ddY = @(t)a(t).*sin(theta(t)) + v(t).^2.*kappa(t).*cos(theta(t)); 54 | 55 | obj.ddtheta = @(t)numerical_derivative(dtheta,t); 56 | obj.dddtheta = @(t)numerical_derivative2(dtheta,t); 57 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/TEST.m: -------------------------------------------------------------------------------- 1 | % addpath( genpath( cd ) ); 2 | function [ benchmark ] = TEST() 3 | %TEST 4 | 5 | %specify scenario: 01_single_lane_change or 02_double_lane_change 6 | scenario = '01_single_lane_change'; 7 | controller = KinSliding; 8 | model = @vmodel_A; 9 | options = default_options; 10 | options.p.mu0=1; 11 | options.pc.mu0=1; 12 | 13 | tau = Trajectory(); 14 | tau.load(['Scenarios/' scenario]); 15 | 16 | if strcmp(controller.control_point,'CO') 17 | tau.solveID(options.pc); 18 | tauD = tau.transform(options.p.J/options.p.l_R/options.p.m).make_static(100); 19 | else if strcmp(controller.control_point,'REAR') 20 | tau.solveID(options.pc); 21 | tauD = tau.transform(-options.p.l_R).make_static(100); 22 | else 23 | tauD=tau; 24 | end 25 | end 26 | 27 | %scale cf,cr 28 | if (options.pc.mu0<1) 29 | options.p.cf=options.p.cf*options.p.mu0; 30 | options.p.cr=options.p.cr*options.p.mu0; 31 | options.pc.cf=options.pc.cf*options.pc.mu0; 32 | options.pc.cr=options.pc.cr*options.pc.mu0; 33 | end 34 | 35 | % options.p.m=1.3*options.p.m; 36 | % options.p.J=1.3*options.p.J; 37 | % options.p.l_R=1.3*options.p.l_R; 38 | % options.p.L=options.p.l_R+options.p.l_F; 39 | 40 | %tau=load_path(scenario,options.duration); 41 | %initial state 42 | options.n=int32(length(model(0,zeros(10,1),zeros(5,1),options.p))); 43 | x0=getVehicleX0(model,tau,options)+0*[0 -0.2 degtorad(-3) 0 0 0 0]'; 44 | 45 | %initialize controller; 46 | controller=controller.init(model,options); 47 | 48 | %run 49 | %profile on 50 | benchmark=simulate(tau,tauD,controller,model,x0,options); 51 | %profile off 52 | %profile viewer 53 | 54 | %visualize 55 | if options.DISPLAY_OUTPUT 56 | disp(benchmark) 57 | % draw real time plot 58 | visualize(benchmark) 59 | end 60 | end 61 | 62 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/@Trajectory/transform.m: -------------------------------------------------------------------------------- 1 | function objD = transform(obj,lambda) 2 | %% transforms the trajectory for the trajectory of a reference position 3 | 4 | XR = @(t)obj.X(t) + cos(obj.psi0(t)).*lambda; 5 | YR = @(t)obj.Y(t) + sin(obj.psi0(t)).*lambda; 6 | dXR = @(t)obj.dX(t) - obj.dpsi0(t).*sin(obj.psi0(t)).*lambda; 7 | dYR = @(t)obj.dY(t) + obj.dpsi0(t).*cos(obj.psi0(t)).*lambda; 8 | ddXR = @(t)obj.ddX(t) - obj.ddpsi0(t).*sin(obj.psi0(t)).*lambda - obj.dpsi0(t).^2.*cos(obj.psi0(t)).*lambda; 9 | ddYR = @(t)obj.ddY(t) + obj.ddpsi0(t).*cos(obj.psi0(t)).*lambda - obj.dpsi0(t).^2.*sin(obj.psi0(t)).*lambda; 10 | 11 | vR = @(t)sqrt(dXR(t).^2+dYR(t).^2); 12 | dvR = @(t)(dXR(t).*ddXR(t)+dYR(t).*ddYR(t))./vR(t); 13 | thetaR = @(t)atan2(dYR(t),dXR(t)); 14 | dthetaR = @(t)(dXR(t).*ddYR(t)-dYR(t).*ddXR(t))./vR(t).^2; 15 | 16 | 17 | TS = 0:0.001:obj.T; 18 | TS2 = TS; 19 | TS2(1) = TS(1) + 1e-5; 20 | TS2(end) = TS(end) - 1e-5; 21 | 22 | %numerisch 23 | ddthetaS = numerical_derivative(dthetaR,TS2); 24 | dddthetaS = numerical_derivative2(dthetaR,TS2); 25 | ddvS = numerical_derivative(dvR,TS2); 26 | 27 | %interpolation 28 | ddthetaR = @(t)interp1(TS,ddthetaS,t); 29 | dddthetaR = @(t)interp1(TS,dddthetaS,t); 30 | ddvR = @(t)interp1(TS,ddvS,t); 31 | 32 | 33 | objD = Trajectory(); 34 | objD.T = obj.T; 35 | objD.lambda = lambda; 36 | objD.X = XR; 37 | objD.dX = dXR; 38 | objD.ddX = ddXR; 39 | objD.Y = YR; 40 | objD.dY = dYR; 41 | objD.ddY = ddYR; 42 | objD.s = @(t)0; %not defined (not used) 43 | objD.v = vR; 44 | objD.a = dvR; 45 | objD.j = ddvR; 46 | objD.kappa = @(t)0; %not defined (not used) 47 | objD.theta = thetaR; 48 | objD.dtheta = dthetaR; 49 | objD.ddtheta = ddthetaR; 50 | objD.dddtheta = dddthetaR; 51 | % objD.psi0 = @(t)obj.psi0(t); 52 | % objD.dpsi0 = @(t)obj.dpsi0(t); 53 | % objD.ddpsi0 = @(t)obj.ddpsi0(t); 54 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/parseInput.m: -------------------------------------------------------------------------------- 1 | function [x,tau,t,p,newTick,intState] = parseInput(input) 2 | %EXTRACTINPUT Summary of this function goes here 3 | % 4 | % Syntax: EXTRACTINPUT 5 | % 6 | % Inputs: 7 | % 8 | % Outputs: 9 | % 10 | % See also: --- 11 | 12 | % Author: Davide Calzolari 13 | % Date: 2016/09/16 13:07:02 14 | % Revision: 0.1 15 | % Technische Universitaet Muenchen 2016 16 | 17 | newTick=1; 18 | 19 | switch length(input) 20 | case 3 21 | % dicrete version for CORA 22 | x=input{1}; 23 | tauREF=input{2}; 24 | p=input{3}; 25 | %reference trajectory 26 | pd=tauREF(1:2); 27 | vd=tauREF(3:4); 28 | ad=tauREF(5:6); 29 | jd=tauREF(7:8); 30 | 31 | tau.X=pd(1); 32 | tau.Y=pd(2); 33 | tau.dX=vd(1); 34 | tau.dY=vd(2); 35 | tau.ddX=ad(1); 36 | tau.ddY=ad(2); 37 | %path info 38 | tau.dtheta=(ad(2)*vd(1)-ad(1)*vd(2))/(vd(1)^2+vd(2)^2); 39 | tau.theta=atan2(vd(2),vd(1)); 40 | % %desired longitudinal speed 41 | tau.v=sqrt(vd(1)^2+vd(2)^2); 42 | tau.a=(vd(1)*ad(1)+vd(2)*ad(2))/sqrt(vd(1)^2+vd(2)^2); 43 | t=1; %first element 44 | %if length(ref)>8 45 | %measurement noise 46 | v=tauREF(9:end); 47 | x=x+v; 48 | %end 49 | case 4 50 | x=input{1}; 51 | tau=input{2}; 52 | t=input{3}; 53 | p=input{4}; 54 | % %reference trajectory 55 | % [pd,vd,ad,jd]=get_trajectory(tau,t); 56 | % 57 | % th=atan2(vd(2),vd(1)); 58 | % dth=(ad(2)*vd(1)-ad(1)*vd(2))/(vd(1)^2+vd(2)^2); 59 | % ddth=d2dt2_atan(vd(2),vd(1),ad(2),ad(1),jd(2),jd(1)); 60 | % otherwise 61 | % error('wrong numer of arguments'); 62 | case 6 %for discrete systems 63 | x=input{1}; 64 | tau=input{2}; 65 | t=input{3}; 66 | p=input{4}; 67 | newTick=input{5}; 68 | intState=input{6}; 69 | end 70 | 71 | end 72 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/@Trajectory/arcLength.m: -------------------------------------------------------------------------------- 1 | function [S,W] = arcLength(X,Y) 2 | % X: tau->X, tau in [0,1] 3 | % Y: tau->Y, tau in [0,1] 4 | % S: tau->int(sqrt(dX^2+dY^2)) 5 | % W: S^-1 6 | dTauR = 0.05; 7 | nTauF = 5; 8 | taunum_rough = 0:dTauR:1; 9 | taunum_fine = linspace(0,dTauR,nTauF); 10 | 11 | xj = X(taunum_rough(1)); 12 | yj = Y(taunum_rough(1)); 13 | snum = zeros(size(taunum_rough)); 14 | snum(1) = 0; 15 | Si_num = zeros(size(taunum_fine)); 16 | gi = zeros(length(taunum_fine),3); 17 | % P = zeros(size(gi,1),length(taunum_fine)-1); 18 | Q = cell(length(taunum_rough)-1); 19 | 20 | for i = 1:length(taunum_rough)-1 21 | Si_num(1) = snum(i); 22 | gi(1,3) = taunum_rough(i); 23 | gi(1,2) = snum(i); 24 | for k = 2:length(taunum_fine); 25 | xk = X(taunum_rough(i)+taunum_fine(k)); 26 | yk = Y(taunum_rough(i)+taunum_fine(k)); 27 | dist = sqrt((xk-xj).^2+(yk-yj).^2); 28 | Si_num(k) = Si_num(k-1) + dist; 29 | gi(k,3) = taunum_rough(i) + taunum_fine(k); 30 | gi(k,2) = Si_num(k); 31 | xj = xk; 32 | yj = yk; 33 | end 34 | Q{i} = create_poly(create_poly_coefficients(gi)); 35 | snum(i+1) = Si_num(end); 36 | end 37 | S = @(tau)interp1(taunum_rough,snum,tau); 38 | W = @(s)interpWn(snum,Q,s); 39 | % clf; hold all; 40 | % s = S(0):0.01:S(1); 41 | % plot(s,W(s)); 42 | % plot(s(1:end-1),diff(W(s))); 43 | % asdf = 0; 44 | 45 | 46 | 47 | function tau = interpWn(snum,Q,s) 48 | tau = zeros(size(s)); 49 | k = 1; 50 | for i = 1:length(s) 51 | %search for the index / Definitionsbereich des Polynoms 52 | while(1) 53 | if snum(k)>s(i) 54 | k = k-1; 55 | if k<1 56 | k = 1; 57 | break; 58 | end 59 | else if snum(k+1)length(snum) 62 | k = length(snum)-1; 63 | break; 64 | end 65 | else 66 | break; 67 | end 68 | end 69 | end 70 | tau(i) = Q{k}(s(i)); 71 | end 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Models/vmodel_A/vmodel_A.m: -------------------------------------------------------------------------------- 1 | function [dx,y] = vmodel_A(t,x,u,p) 2 | %Bicycle Model with 3 | % - normal force equilibrium for pitching-moments 4 | % - nonlinear tyre model 5 | % state x=[X,Y,psi,vx,vy,omega] 6 | % input u=[delta,omega_f,omega_r] 7 | 8 | %get state and parameters from x, p vectors 9 | m=p.m; 10 | J=p.J; 11 | l_F=p.l_F; 12 | l_R=p.l_R; 13 | L=p.L; 14 | h=p.h; 15 | mu0=p.mu0; 16 | B_F=p.B_F; 17 | C_F=p.C_F; 18 | B_R=p.B_R; 19 | C_R=p.C_R; 20 | R=p.R; 21 | g=9.81; 22 | 23 | %state 24 | %position 25 | X = x(1); 26 | Y = x(2); 27 | psi = x(3); 28 | 29 | %velocity 30 | vx = x(4); 31 | vy = x(5); 32 | omega = x(6); 33 | 34 | %input (with input limitations) 35 | delta = max(-pi/4,min(u(1),pi/4)); 36 | vf = [vx;vy+l_F*omega]; %velocity at wheel center front 37 | vfW = [cos(delta),sin(delta);-sin(delta),cos(delta)] * vf; % in wheel coordinates 38 | omega_F = max(min(vfW(1)/R,0),min(u(2),max(vfW(1)/R,0))); 39 | vr = [vx;vy-l_R*omega]; %velocity at wheel center rear 40 | omega_R = max(min(vr(1)/R,0),min(u(3),max(vr(1)/R,0))); 41 | 42 | %FRONT 43 | vf_abs = norm(vf); 44 | vfL = [-cos(delta);-sin(delta)]*omega_F*R; %velocity of Latsch 45 | vfL_abs = norm(vfL); 46 | sf = (vf+vfL)/max(vf_abs,vfL_abs); %slip vector 47 | sf_abs = norm(sf); 48 | ff_abs = sin(C_F*atan(B_F*sf_abs/mu0)); 49 | if sf_abs==0 50 | ff = zeros(2,1); 51 | else 52 | ff =ff_abs * -sf/sf_abs; 53 | end 54 | 55 | %REAR 56 | vr_abs = norm(vr); 57 | vrL = [-1;0]*omega_R*R; %velocity of Latsch 58 | vrL_abs = norm(vrL); 59 | sr = (vr+vrL)/max(vr_abs,vrL_abs); %slip vector 60 | sr_abs = norm(sr); 61 | fr_abs = sin(C_R*atan(B_R*sr_abs/mu0)); 62 | if sr_abs==0 63 | fr = zeros(2,1); 64 | else 65 | fr =fr_abs * -sr/sr_abs; 66 | end 67 | 68 | %calculate normal forces 69 | Fzf = l_R*m*g /(L+ff(1)*mu0*h); 70 | Fzr = m*g - Fzf; 71 | 72 | %FRONT FORCES 73 | Ff = ff * mu0 * Fzf; 74 | 75 | %REAR FORCES 76 | Fr = fr * mu0 * Fzr; 77 | 78 | %ACCELERATIONS 79 | dv = []; 80 | dv(1,1) = (Ff(1)+Fr(1))/m + vy * omega; 81 | dv(2,1) = (Ff(2)+Fr(2))/m - vx * omega; 82 | domega = (l_F * Ff(2) - l_R * Fr(2))/J; 83 | 84 | %dx = zeros(size(x)); 85 | %position 86 | cp = cos(psi);sp = sin(psi); 87 | dx(1,1) = cp * vx - sp * vy; 88 | dx(2,1) = sp * vx + cp * vy; 89 | dx(3,1) = omega; 90 | %velocity 91 | dx(4,1) = dv(1); 92 | dx(5,1) = dv(2); 93 | dx(6,1) = domega; 94 | dx(7,1) = u(4); 95 | 96 | %disp(['AX: ' num2str(dv(1))]); 97 | 98 | y = [ff;fr;delta;Ff;Fr]; -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/fncs/find_and_replace.m: -------------------------------------------------------------------------------- 1 | function find_and_replace(file, expression, replacement) 2 | 3 | % find_and_replace(file, to_find, to_replace) 4 | % 5 | % Replaces a string (expression) in a given file (file) with a new string 6 | % (replacement). 7 | % 8 | % See regexprep for info on valid 'expression' and 'replace' inputs. 9 | % 10 | % Examples: 11 | % 12 | % Replace all instances of the string 'abs', including 'cabs', with 'mag': 13 | % 14 | % find_and_replace('my_file.m', 'abs', 'mag'); 15 | % 16 | % Replace all the exact variable names 'my_var' with 'my_other_var'. This 17 | % *won't* match 'my_var_2' or 'this_is_my_var'. 18 | % 19 | % find_and_replace('my_file.m', '\', 'my_other_var'); 20 | % 21 | % Replace all calls to sqrt(...) with my_sqrt(...), keeping the function's 22 | % argument (this is using regular expressions). 23 | % 24 | % find_and_replace(tc.file_name, 'sqrt\((.*?)\)', 'my_sqrt\($1\)'); 25 | % 26 | % Note that the above does *not* work for nested parentheses (regular 27 | % expressions can't match nested parentheses to an arbitrary depth). 28 | % 29 | % This function can also handle multiple files, where the first input is 30 | % either a cell array of file names (e.g., {'file1.txt', 'file2.m'}) or a 31 | % struct with a 'name' field that contains the file name, such as is output 32 | % from files = dir('*.m'); 33 | % 34 | % For example, to replace in all .m files in the current directory: 35 | % 36 | % find_and_replace(dir('*.m'), 'string_1', 'string_2'); 37 | % 38 | % See doc regexprep for more examples. 39 | % 40 | % Tucker McClure 41 | % Copyright 2013, The MathWorks, Inc. 42 | 43 | % If user passed in a single file name, wrap it as a cell. 44 | if ischar(file) 45 | file = {file}; 46 | elseif isstruct(file) && any(strcmp(fieldnames(file), 'name')) 47 | file = {file(:).name}; 48 | elseif ~iscell(file); 49 | error('find_and_replace:invalid_inputs', ... 50 | 'Unknown input type for ''file''.'); 51 | end 52 | 53 | % For all files in the cell array... 54 | for k = 1:length(file) 55 | 56 | % Make sure the file exists. 57 | if ~exist(file{k}, 'file') 58 | error('find_and_replace:no_file', ... 59 | ['File doesn''t exist. To replace strings in text, ' ... 60 | 'use regexprep.']); 61 | end 62 | 63 | % Read in the file as binary and convert to chars. 64 | fid = fopen(file{k}); 65 | text = fread(fid, inf, '*char')'; 66 | fclose(fid); 67 | 68 | % Find and replace. 69 | text = regexprep(text, expression, replacement); 70 | 71 | % Write out the new file. 72 | fid = fopen(file{k}, 'w'); 73 | fwrite(fid, text); 74 | fclose(fid); 75 | 76 | end 77 | 78 | end 79 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Visualization/visualize.m: -------------------------------------------------------------------------------- 1 | function [ ] = visualize( benchmark ) 2 | %VISUALIZE 3 | close all 4 | 5 | steps=size(benchmark.data.T,1); 6 | tspan=benchmark.data.T; 7 | curve(1,:)=benchmark.data.tau.X(tspan)'; 8 | curve(2,:)=benchmark.data.tau.Y(tspan)'; 9 | curveD(1,:)=benchmark.data.tauD.X(tspan)'; 10 | curveD(2,:)=benchmark.data.tauD.Y(tspan)'; 11 | Y=benchmark.data.X; 12 | d=2; 13 | DISPLAY_AXIS=[min(curve(1,:))-d max(curve(1,:))+d min(curve(2,:))-d max(curve(2,:))+d];%benchmark.data.tau.DISPLAY_AXIS; 14 | ts=mean(diff(benchmark.data.T)); 15 | dyn=CARparameters; 16 | 17 | % %setup video 18 | % vidObj = VideoWriter([benchmark.controller],'Archival'); 19 | % %vidObj.Quality = 100; 20 | % vidObj.FrameRate = 30; 21 | % open(vidObj); 22 | % x0=10; 23 | % y0=10; 24 | % width=800; 25 | % height=600; 26 | % set(gcf,'units','points','position',[x0,y0,width,height]) 27 | figure('units','normalized','position',[.2 .4 .6 .4]) 28 | 29 | for k=1:steps 30 | tim=tic; 31 | 32 | %Y 1,2 is center of mass 33 | x=Y(k,1); 34 | y=Y(k,2); 35 | theta=Y(k,3); 36 | delta=benchmark.data.U(k,1); 37 | 38 | front=[x+dyn.l_F*cos(theta),y+dyn.l_F*sin(theta)]; 39 | rear=[x-dyn.l_R*cos(theta),y-dyn.l_R*sin(theta)]; 40 | wheel=[0.5*cos(theta+delta);0.5*sin(theta+delta)]; 41 | heading=front-rear; 42 | %ref=get_trajectory(benchmark.data.path,t); 43 | 44 | plot(curve(1,:),curve(2,:),'r-','LineWidth',1); 45 | hold on 46 | plot(curveD(1,:),curveD(2,:),'b-.','LineWidth',1); 47 | axis(DISPLAY_AXIS); 48 | hold on 49 | %plot XY trajectory 50 | plot(Y(:,1),Y(:,2),'k-','LineWidth',1); 51 | %plot traj of a look-ahead wrt vehicle 52 | look_ahead=0; 53 | if strcmp(benchmark.data.control_point,'CO') 54 | look_ahead=dyn.J/dyn.m/dyn.l_R + dyn.l_R*0; 55 | else 56 | if strcmp(benchmark.data.control_point,'REAR') 57 | look_ahead=-dyn.l_R; 58 | end 59 | end 60 | plot(Y(:,1)+look_ahead.*cos(Y(:,3)), Y(:,2)+look_ahead.*sin(Y(:,3)),'g-.','LineWidth',0.5); 61 | %plot(Y(:,1)+cos(Y(:,3)).*0.5,Y(:,2)+sin(Y(:,3)).*0.5); 62 | plot([front(1),front(1)+wheel(1)],[front(2),front(2)+wheel(2)],'LineWidth',2); 63 | plot([front(1),front(1)-wheel(1)],[front(2),front(2)-wheel(2)],'LineWidth',2); 64 | quiver(rear(1),rear(2),heading(1),heading(2),'LineWidth',3,'MaxHeadSize', 2/norm(heading)); 65 | plot(x,y,'.k', 'MarkerSize',25); 66 | plot(x,y,'.w', 'MarkerSize',20); 67 | 68 | %plot(ref(1),ref(2),'xr'); 69 | %axis equal 70 | drawnow limitrate 71 | % writeVideo(vidObj, getframe(gca)); 72 | 73 | hold off 74 | %semirt 75 | dtoc=toc(tim); 76 | while dtoc<=ts 77 | dtoc=toc(tim); 78 | end 79 | end 80 | % 81 | % close(gcf) 82 | % close(vidObj) 83 | 84 | % plot(curve(1,:),curve(2,:),'LineWidth',1); 85 | % axis(DISPLAY_AXIS); 86 | % hold on 87 | % plot(Y(:,1),Y(:,2),'--','LineWidth',1.5); 88 | % legend \tau_d HCO 89 | end 90 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/Models/mappings/AXController_pacejka.m: -------------------------------------------------------------------------------- 1 | function [u] = AXController_pacejka(AX_REF,x,delta,p) 2 | %AXController_pacejka Summary of this function goes here 3 | % 4 | % Syntax: AXController_pacejka 5 | % 6 | % Inputs: 7 | % 8 | % Outputs: 9 | % 10 | % See also: --- 11 | 12 | % Author: Davide Calzolari 13 | % Date: 2016/10/14 14:19:23 14 | % Revision: 0.1 15 | % Technische Universitaet Muenchen 2016 16 | 17 | mu0=p.mu0; 18 | R=p.R; 19 | m=p.m; 20 | vx=x(4); 21 | vy=x(5); 22 | omega=x(6); 23 | l_F=p.l_F; 24 | l_R=p.l_R; 25 | C_R=p.C_R; 26 | B_R=p.B_R; 27 | L=p.L; 28 | h=p.h; 29 | J=p.J; 30 | g=9.81; 31 | Fb=( AX_REF - vy * omega ) * m; 32 | Fxf=Fb; 33 | Fxr=0; 34 | 35 | % normal forces 36 | fzf_max = (m*g*l_R)/(-mu0*h+L); %the maximum normal force if all normal force is utilized for breaking 37 | Fzf = max(0,min(fzf_max,l_R/L * m*g - h/L * Fb)); 38 | Fzr = m*g - Fzf; 39 | 40 | % maximal absolute force 41 | Fmaxf = mu0 * Fzf; 42 | Fmaxr = mu0 * Fzr; 43 | 44 | % velocity at rear axle 45 | vr = [vx; vy - l_R * omega]; 46 | % velocity at front axle 47 | vf = [ vx; vy + l_F * omega ]; 48 | vf_abs = norm(vf); 49 | 50 | %rear lateral tire force 51 | fr = @(omega_R)-mu0*Fmaxr*sin(C_R*atan(B_R*norm((vr-[R*omega_R;0])./norm(vr))/mu0))... 52 | .*(vr-[R*omega_R;0])./norm(vr)/norm((vr-[R*omega_R;0])./norm(vr)); 53 | % if Fxr>0 54 | % omega_R0 = vx/R+0.001; 55 | % else 56 | % omega_R0 = vx/R-0.001; 57 | % end 58 | %if Fxr==0 59 | omega_R = vx/R; 60 | % else 61 | % omega_R = fminsearch(@(omega)([1,0]*fr(omega)-Fxr).^2,omega_R0); 62 | % end 63 | if norm((vr-[R*omega_R;0])./norm(vr))==0 64 | Fyr = 0; 65 | else 66 | Fyr = [0,1] * fr(omega_R); 67 | end 68 | 69 | %front lateral tire force 70 | Fyf = 0; %(m*(0+vx*omega) - 0*Fyr); 71 | 72 | % absolute front tire force 73 | Ff_abs = sqrt(Fxf*Fxf+Fyf*Fyf); 74 | 75 | % maximal tire force exceeded? 76 | if Ff_abs>Fmaxf 77 | %disp(['mu=' num2str(Ff_abs/Fmaxf)]); 78 | Fxf = Fxf * Fmaxf / Ff_abs; 79 | Fyf = Fyf * Fmaxf / Ff_abs; 80 | Ff_abs = Fmaxf; 81 | end 82 | 83 | ud = [Fxf;Fyf]; 84 | 85 | %u=ForceController_pacejka(ud,x,Ff_abs,Fmaxf,p); 86 | B_F=p.B_F; 87 | C_F=p.C_F; 88 | 89 | %non commanded 90 | omega_R = vx/R; 91 | 92 | %% calculate required speed 93 | sf_abs = mu0/B_F * tan( 1/C_F * asin( Ff_abs/Fmaxf ) ); 94 | 95 | % slip vector 96 | if Ff_abs==0 97 | sf = zeros(2,1); 98 | else 99 | sf = [ -Fxf/Ff_abs * sf_abs; -Fyf/Ff_abs * sf_abs ]; 100 | end 101 | % velocity at front axle 102 | vf = [ vx; vy + l_F * omega ]; 103 | vf_abs = norm(vf); 104 | %Latsch velocity 105 | Rot_delta=[cos(delta),-sin(delta);sin(delta),cos(delta)]; 106 | vLf = (sf * vf_abs - vf); 107 | %RvLF = Rot_delta*vLf; 108 | % required front wheel speed for slip along x 109 | omega_F = -vLf(1)/R/Rot_delta(1,1); 110 | %omega_F = norm(vLf)/R; 111 | 112 | %disp(['AX_REF: ' num2str(AX_REF)]); 113 | 114 | u=[delta,omega_F,omega_R]; 115 | end 116 | 117 | 118 | 119 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/simulate.m: -------------------------------------------------------------------------------- 1 | function [benchmark] = simulate(tau,tauD,controller,model,x0,options) 2 | %% INITIALIZATION PROCEDURE 3 | % time step 4 | h=options.stepSize; 5 | %real parameters 6 | p=options.p; 7 | %parameter known to the controller 8 | pc=options.pc; 9 | %noise variables 10 | xi_m=generateNoise(options.V,tau.T,options.stepSize); 11 | 12 | 13 | tspan=0:h:tau.T; 14 | isDiscrete=(isfield(controller,'type') && strcmp(controller.type,'discrete')); 15 | 16 | tic 17 | %profile on 18 | if isDiscrete 19 | disp('discrete') 20 | odef=@(t,x,newTick,intState)fullsystemDISC(t,x,@(t)0,xi_m,tauD,model,controller,p,pc,newTick,intState); 21 | intState0=controller.intState0; 22 | [T,X,intState]=myode4_fixed_state(odef,tspan,x0,intState0); 23 | T=T'; 24 | else 25 | disp('continuous') 26 | odef=@(t,x)model(t,x,controller.compute_input(x+xi_m(t),tauD,t,pc),p); 27 | [T,X]=ode45(odef,tspan,x0); 28 | T=reshape(T,[length(T) 1]); 29 | end 30 | %profile off 31 | toc 32 | 33 | 34 | if isDiscrete 35 | U=zeros(size(T,1),length(controller.compute_input(X(1,:)',tauD,T(1),pc,0,intState{1}))); 36 | else 37 | U=zeros(size(T,1),length(controller.compute_input(X(1,:)',tauD,T(1),pc))); 38 | end 39 | 40 | ERROR=zeros(size(T,1),2); 41 | MUXY=zeros(size(T,1),2); 42 | for k=1:size(T,1) 43 | %input 44 | if isDiscrete 45 | [U(k,:)]=controller.compute_input(X(k,:)',tauD,T(k),pc,0,intState{k}); 46 | else 47 | [U(k,:)]=controller.compute_input(X(k,:)',tauD,T(k),pc); 48 | end 49 | %error 50 | Xd = tau.X(T(k)); Yd = tau.Y(T(k)); theta=tau.theta(T(k)); 51 | ct = cos(theta); st = sin(theta); 52 | Rti = [ct,st;-st,ct]; 53 | ERROR(k,:)=Rti*(X(k,1:2)'-[Xd,Yd]'); 54 | %tire saturation 55 | [~,y]=model(0,X(k,:)',U(k,:),p); 56 | sf = y(1:2); 57 | sr = y(3:4); 58 | MUXY(k,:)=[norm(sf), norm(sr)]; 59 | end 60 | ET=(ERROR(:,1)); 61 | EN=(ERROR(:,2)); 62 | MUF=(MUXY(:,1)); 63 | MUR=(MUXY(:,2)); 64 | %calculate averages 65 | IntET = trapz(T,abs(ET))/tau.T; 66 | IntEN = trapz(T,abs(EN))/tau.T; 67 | IntMUF = trapz(T,MUF)/tau.T; 68 | IntMUR = trapz(T,MUR)/tau.T; 69 | 70 | 71 | benchmark.controller=controller.name; 72 | benchmark.model=model; 73 | benchmark.scenario=tau.id; 74 | benchmark.max_error=[max(abs(ET)) max(abs(EN))]; 75 | benchmark.avg_error=[IntET IntEN];%mean(abs(ERROR)); 76 | benchmark.end_error=[ET(end) EN(end)]; 77 | benchmark.avg_musat=[IntMUF IntMUR];%mean(MUXY); 78 | benchmark.total_time=T(end); 79 | benchmark.data.error=[ET EN]; 80 | benchmark.data.U=U; 81 | if (isDiscrete) benchmark.data.intState=intState; end 82 | benchmark.data.control_point=controller.control_point; 83 | benchmark.data.X=X; 84 | benchmark.data.T=T; 85 | benchmark.data.tau=tau; 86 | benchmark.data.tauD=tauD; 87 | benchmark.data.options=options; 88 | end 89 | 90 | 91 | function [dx,intState] = fullsystemDISC(t,x,xi_dx,xi_m,tauD,model,controller,p,pc,newTick,intState) 92 | % t - time 93 | % x - state 94 | % xi_dx - process disturbance 95 | % xi_m - measurement error 96 | % xi_p - parameter mismatch 97 | [u,intState] = controller.compute_input(x+xi_m(t),tauD,t,pc,newTick,intState); 98 | [dx,y] = model(t,x,u,p); 99 | dx = dx + xi_dx(t); 100 | end 101 | -------------------------------------------------------------------------------- /Harish_ME762_project/SMC_simulation_code/latex/latexTable.m: -------------------------------------------------------------------------------- 1 | function latex = latexTable(input) 2 | % An easy to use function that generates a LaTeX table from a given MATLAB 3 | % input struct containing numeric values. The LaTeX code is printed in the 4 | % command window for quick copy&paste and given back as a cell array. 5 | % 6 | % Author: Eli Duenisch 7 | % Contributor: Pascal E. Fortin 8 | % Date: April 20, 2016 9 | % License: This code is licensed using BSD 2 to maximize your freedom of using it :) 10 | % ---------------------------------------------------------------------------------- 11 | % Copyright (c) 2016, Eli Duenisch 12 | % All rights reserved. 13 | % 14 | % Redistribution and use in source and binary forms, with or without 15 | % modification, are permitted provided that the following conditions are met: 16 | % 17 | % * Redistributions of source code must retain the above copyright notice, this 18 | % list of conditions and the following disclaimer. 19 | % 20 | % * Redistributions in binary form must reproduce the above copyright notice, 21 | % this list of conditions and the following disclaimer in the documentation 22 | % and/or other materials provided with the distribution. 23 | % 24 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | % AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | % IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | % FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | % DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | % SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | % CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | % OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | % OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | % ---------------------------------------------------------------------------------- 35 | % 36 | % Input: 37 | % input struct containing your data and optional fields (details described below) 38 | % 39 | % Output: 40 | % latex cell array containing LaTex code 41 | % 42 | % Example and explanation of the input struct fields: 43 | % 44 | % % numeric values you want to tabulate: 45 | % % this field has to be a matrix or MATLAB table datatype 46 | % % missing values have to be NaN 47 | % % in this example we use an array 48 | % input.data = [1.12345 2.12345 3.12345; ... 49 | % 4.12345 5.12345 6.12345; ... 50 | % 7.12345 NaN 9.12345; ... 51 | % 10.12345 11.12345 12.12345]; 52 | % 53 | % % Optional fields (if not set default values will be used): 54 | % 55 | % % Set the position of the table in the LaTex document using h, t, p, b, H or ! 56 | % input.tablePositioning = 'h'; 57 | % 58 | % % Set column labels (use empty string for no label): 59 | % input.tableColLabels = {'col1','col2','col3'}; 60 | % % Set row labels (use empty string for no label): 61 | % input.tableRowLabels = {'row1','row2','','row4'}; 62 | % 63 | % % Switch transposing/pivoting your table: 64 | % input.transposeTable = 0; 65 | % 66 | % % Determine whether input.dataFormat is applied column or row based: 67 | % input.dataFormatMode = 'column'; % use 'column' or 'row'. if not set 'column' is used 68 | % 69 | % % Formatting-string to set the precision of the table values: 70 | % % For using different formats in different rows use a cell array like 71 | % % {myFormatString1,numberOfValues1,myFormatString2,numberOfValues2, ... } 72 | % % where myFormatString_ are formatting-strings and numberOfValues_ are the 73 | % % number of table columns or rows that the preceding formatting-string applies. 74 | % % Please make sure the sum of numberOfValues_ matches the number of columns or 75 | % % rows in input.tableData! 76 | % % 77 | % % input.dataFormat = {'%.3f'}; % uses three digit precision floating point for all data values 78 | % input.dataFormat = {'%.3f',2,'%.1f',1}; % three digits precision for first two columns, one digit for the last 79 | % 80 | % % Define how NaN values in input.tableData should be printed in the LaTex table: 81 | % input.dataNanString = '-'; 82 | % 83 | % % Column alignment in Latex table ('l'=left-justified, 'c'=centered,'r'=right-justified): 84 | % input.tableColumnAlignment = 'c'; 85 | % 86 | % % Switch table borders on/off: 87 | % input.tableBorders = 1; 88 | % 89 | % % Switch table booktabs on/off: 90 | % input.booktabs = 1; 91 | % 92 | % % LaTex table caption: 93 | % input.tableCaption = 'MyTableCaption'; 94 | % 95 | % % LaTex table label: 96 | % input.tableLabel = 'MyTableLabel'; 97 | % 98 | % % Switch to generate a complete LaTex document or just a table: 99 | % input.makeCompleteLatexDocument = 1; 100 | % 101 | % % % Now call the function to generate LaTex code: 102 | % latex = latexTable(input); 103 | 104 | %%%%%%%%%%%%%%%%%%%%%%%%%% Default settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 105 | % These settings are used if the corresponding optional inputs are not given. 106 | % 107 | % Placement of the table in LaTex document 108 | if isfield(input,'tablePlacement') && (length(input.tablePlacement)>0) 109 | input.tablePlacement = ['[',input.tablePlacement,']']; 110 | else 111 | input.tablePlacement = ''; 112 | end 113 | % Pivoting of the input data switched off per default: 114 | if ~isfield(input,'transposeTable'),input.transposeTable = 0;end 115 | % Default mode for applying input.tableDataFormat: 116 | if ~isfield(input,'dataFormatMode'),input.dataFormatMode = 'column';end 117 | % Sets the default display format of numeric values in the LaTeX table to '%.4f' 118 | % (4 digits floating point precision). 119 | if ~isfield(input,'dataFormat'),input.dataFormat = {'%.4f'};end 120 | % Define what should happen with NaN values in input.tableData: 121 | if ~isfield(input,'dataNanString'),input.dataNanString = '-';end 122 | % Specify the alignment of the columns: 123 | % 'l' for left-justified, 'c' for centered, 'r' for right-justified 124 | if ~isfield(input,'tableColumnAlignment'),input.tableColumnAlignment = 'c';end 125 | % Specify whether the table has borders: 126 | % 0 for no borders, 1 for borders 127 | if ~isfield(input,'tableBorders'),input.tableBorders = 1;end 128 | % Specify whether to use booktabs formatting or regular table formatting: 129 | if ~isfield(input,'booktabs') 130 | input.booktabs = 0; 131 | else 132 | if input.booktabs 133 | input.tableBorders = 0; 134 | end 135 | end 136 | % Other optional fields: 137 | if ~isfield(input,'tableCaption'),input.tableCaption = 'MyTableCaption';end 138 | if ~isfield(input,'tableLabel'),input.tableLabel = 'MyTableLabel';end 139 | if ~isfield(input,'makeCompleteLatexDocument'),input.makeCompleteLatexDocument = 0;end 140 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 141 | 142 | % process table datatype 143 | if isa(input.data,'table') 144 | if(~isempty(input.data.Properties.RowNames)) 145 | input.tableRowLabels = input.data.Properties.RowNames'; 146 | end 147 | if(~isempty(input.data.Properties.VariableNames)) 148 | input.tableColLabels = input.data.Properties.VariableNames'; 149 | end 150 | input.data = table2array(input.data); 151 | end 152 | 153 | % get size of data 154 | numberDataRows = size(input.data,1); 155 | numberDataCols = size(input.data,2); 156 | 157 | % obtain cell array for the table data and labels 158 | colLabelsExist = isfield(input,'tableColLabels'); 159 | rowLabelsExist = isfield(input,'tableRowLabels'); 160 | cellSize = [numberDataRows+colLabelsExist,numberDataCols+rowLabelsExist]; 161 | C = cell(cellSize); 162 | C(1+colLabelsExist:end,1+rowLabelsExist:end) = num2cell(input.data); 163 | if rowLabelsExist 164 | C(1+colLabelsExist:end,1)=input.tableRowLabels'; 165 | end 166 | if colLabelsExist 167 | C(1,1+rowLabelsExist:end)=input.tableColLabels; 168 | end 169 | 170 | % obtain cell array for the format 171 | lengthDataFormat = length(input.dataFormat); 172 | if lengthDataFormat==1 173 | tmp = repmat(input.dataFormat(1),numberDataRows,numberDataCols); 174 | else 175 | dataFormatList={}; 176 | for i=1:2:lengthDataFormat 177 | dataFormatList(end+1:end+input.dataFormat{i+1},1) = repmat(input.dataFormat(i),input.dataFormat{i+1},1); 178 | end 179 | if strcmp(input.dataFormatMode,'column') 180 | tmp = repmat(dataFormatList',numberDataRows,1); 181 | end 182 | if strcmp(input.dataFormatMode,'row') 183 | tmp = repmat(dataFormatList,1,numberDataCols); 184 | end 185 | end 186 | if ~isequal(size(tmp),size(input.data)) 187 | error(['Please check your values in input.dataFormat:'... 188 | 'The sum of the numbers of fields must match the number of columns OR rows '... 189 | '(depending on input.dataFormatMode)!']); 190 | end 191 | dataFormatArray = cell(cellSize); 192 | dataFormatArray(1+colLabelsExist:end,1+rowLabelsExist:end) = tmp; 193 | 194 | % transpose table (if this switched on) 195 | if input.transposeTable 196 | C = C'; 197 | dataFormatArray = dataFormatArray'; 198 | end 199 | 200 | % make table header lines: 201 | hLine = '\hline'; 202 | if input.tableBorders 203 | header = ['\begin{tabular}','{|',repmat([input.tableColumnAlignment,'|'],1,size(C,2)),'}']; 204 | else 205 | header = ['\begin{tabular}','{',repmat(input.tableColumnAlignment,1,size(C,2)),'}']; 206 | end 207 | latex = {['\begin{table}',input.tablePlacement];'\centering';header}; 208 | 209 | % generate table 210 | if input.booktabs 211 | latex(end+1) = {'\toprule'}; 212 | end 213 | 214 | for i=1:size(C,1) 215 | if i==2 && input.booktabs 216 | latex(end+1) = {'\midrule'}; 217 | end 218 | if input.tableBorders 219 | latex(end+1) = {hLine}; 220 | end 221 | rowStr = ''; 222 | for j=1:size(C,2) 223 | dataValue = C{i,j}; 224 | if iscell(dataValue) 225 | dataValue = dataValue{:}; 226 | elseif isnan(dataValue) 227 | dataValue = input.dataNanString; 228 | elseif isnumeric(dataValue) 229 | dataValue = num2str(dataValue,dataFormatArray{i,j}); 230 | end 231 | if j==1 232 | rowStr = dataValue; 233 | else 234 | rowStr = [rowStr,' & ',dataValue]; 235 | end 236 | end 237 | latex(end+1) = {[rowStr,' \\']}; 238 | end 239 | 240 | if input.booktabs 241 | latex(end+1) = {'\bottomrule'}; 242 | end 243 | 244 | 245 | % make footer lines for table: 246 | tableFooter = {'\end{tabular}';['\caption{',input.tableCaption,'}']; ... 247 | ['\label{table:',input.tableLabel,'}'];'\end{table}'}; 248 | if input.tableBorders 249 | latex = [latex;{hLine};tableFooter]; 250 | else 251 | latex = [latex;tableFooter]; 252 | end 253 | 254 | % add code if a complete latex document should be created: 255 | if input.makeCompleteLatexDocument 256 | % document header 257 | latexHeader = {'\documentclass[a4paper,10pt]{article}'}; 258 | if input.booktabs 259 | latexHeader(end+1) = {'\usepackage{booktabs}'}; 260 | end 261 | latexHeader(end+1) = {'\begin{document}'}; 262 | % document footer 263 | latexFooter = {'\end{document}'}; 264 | latex = [latexHeader';latex;latexFooter]; 265 | end 266 | 267 | % print latex code to console: 268 | disp(char(latex)); 269 | 270 | end 271 | --------------------------------------------------------------------------------