├── README.md ├── Report_Project.pdf ├── check_lifted.m ├── getWeights.m ├── get_Fcontr.m~ ├── get_Fd_contr.m ├── get_jacobians.m ├── get_lifted_repr.m ├── get_optimal_trajectory.m ├── init.m ├── main.m ├── nonlinearconstraints.m ├── pendulum_ur_CT0.m ├── pendulum_ur_CT0_real.m ├── pendulum_ur_DT0.m ├── pendulum_ur_DT0_real.m ├── plot_trajectories.m ├── plot_trajectory.m ├── saveJson.m ├── simulation_linearized.m ├── simulation_nonlinearized.m ├── simulation_nonlinearized_idealwnoise.m ├── simulation_nonlinearized_real.m └── video_ILC.mp4 /README.md: -------------------------------------------------------------------------------- 1 |

Optimization-Based Iterative Learning Control for Trajectory Tracking

2 | 3 | Developed by: S. Saif, A. Santopaolo (2020). 4 | 5 | Supervisor: prof. G. Oriolo, L. Lanari. 6 | 7 | Achievement: Underactuated robot exam. 8 | 9 |

Project description:

10 | 11 | In this project an iterative learning controller is realized and tested on a Cart-Pole system. Starting from a feasible swing-up trajectory, at each trial, the uncertainties over the dynamics parameters of the model are estimated and incorporated in the controller formulation during the next iteration. Simulation implemented in Matlab. 12 |

File details:

13 | 14 | 15 | - [MAIN.M](main.m) : is the main file. 16 | 17 | 1. Section A: there is an INIT.M call to initialize the parameters. 18 | 1. Section B: Using get_optimal_trajectory function a NPLMC algotithm is run to find the ideal trajectory. There is the plotting of Ideal trajectory. 19 | 1. Section C: Number of simulation for ILC to converge. 20 | 1. Section D: Setted up the real nominal condition that varies at each iteration. First simulation run with optimal input. Computed Lifted representation. Initilization of Kalman filter matrices and cycle with simulation. Run control step using fmicon. Computed each simulation trajectory and plotted all of them. 21 | 22 | * INIT.M: Parameters initialization. 23 | 24 | * PENDULUM_ur_CT0: Function with continuos time cart-pendulum equations of motion with nominal parameter. 25 | 26 | * PENDULUM_ur_CT0_real: Function with continuos time cart-pendulum equations of motion with real parameters. 27 | 28 | * PENDULUM_ur_DT0: Function with discrete time cart-pendulum equations of motions using Euler method. (Nominal parameter setup) 29 | 30 | * PENDULUM_ur_DT0_real: Function with discrete time cart-pendulum equations of motions using Euler method. (Real parameters setup). 31 | 32 | * Plot_trajectory: Script to plot in a same images plots of cart position, cart velocity, pendulum angle, pendulum velocity and input with respect to time. 33 | 34 | * Plot_trajectories: Script to plot cart position, cart velocity, pendulumangle, pendulum velocity and input with respect to time for each single trial in the same image. 35 | 36 | * Get_Fd_contr: Script to get F and D matrices related to angle position dynamics. 37 | 38 | * get_jacobians: Script to get A and B matrices for state space model computing at each time istant the jacobians. 39 | 40 | * get_lifted_repr: Algorithm to compute F,G and D_0 given column vectors of all state and input. F and G represent the model dyamics. Algorithm present in the report. 41 | 42 | * get_optimal_trajectory: Script to run the NLMPC and get optimal trajectory. Implementation details in the report. 43 | 44 | * nonlinearconstraints: Constraints of cart position and cart velocity due to rail length and cart actuator. Used to run fmincon algorithm. 45 | 46 | > For further detail open the [ Project_Report](Report_Project.pdf) 47 | 48 | >Watch the [video](https://youtu.be/KscG354lbqI) with the results 49 | 50 |

References

51 | Schöllig, A., & D'Andrea, R. (2009, August). Optimization-based iterative learning control for trajectory tracking. In 2009 European Control Conference (ECC) (pp. 1505-1510). IEEE. 52 | 53 | -------------------------------------------------------------------------------- /Report_Project.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AlexSantopaolo/Optimization-Based-Iterative-Learning-Control-for-Trajectory-Tracking/7292f44225fdb6fd8bbbd640fe284b2396a50047/Report_Project.pdf -------------------------------------------------------------------------------- /check_lifted.m: -------------------------------------------------------------------------------- 1 | if ~mpcchecktoolboxinstalled('optim') 2 | disp('Optimization Toolbox is required to run this example.') 3 | return 4 | end 5 | clear all; 6 | close all; 7 | clc; 8 | 9 | %% main script 10 | 11 | %initialize parameters 12 | init; 13 | 14 | %% GET IDEAL TRAJECTORY 15 | 16 | %get ideal trajectory 17 | [x_optimal_History, u_optimal_History]= get_optimal_trajectory(); 18 | plot_trajectory(x_optimal_History, u_optimal_History,"IDEAL TRAJECTORY"); 19 | 20 | %% GET REAL TRAJECTORY 21 | 22 | u_real_History=u_optimal_History+0.0001; % just added a bias on u 23 | x_real_History=simulation_nonlinearized(u_real_History,[0.0001;0;-pi+0.0001;0]); % little changes in initial conditions 24 | plot_trajectory(x_real_History, u_real_History,"REAL TRAJECTORY"); 25 | 26 | % COMPUTE TRAJECTORY DEVIATIONS 27 | 28 | u_dev_History=u_real_History-u_optimal_History; 29 | x_dev_History=x_real_History-x_optimal_History; 30 | 31 | plot_trajectory(x_dev_History, u_dev_History, "DEVIATION"); 32 | 33 | %% COMPUTE TRAJECTORY DEVIATIONS WITH LINEARIZED MODEL 34 | 35 | u_dev_History=u_real_History-u_optimal_History; 36 | x0_dev=x_dev_History(:,1); 37 | x_dev_History_lin=simulation_linearized(x0_dev,u_dev_History,x_optimal_History,u_optimal_History); 38 | plot_trajectory(x_dev_History_lin, u_dev_History, "DEVIATION (LINEARIZED)"); 39 | 40 | %% GET LIFTED REPRESENTATION 41 | 42 | %get Matrices for lifted representation x=F*u+d0 43 | x0_dev=x_dev_History(:,1); 44 | [F,d0,G]=get_lifted_repr(x_optimal_History,u_optimal_History,x0_dev); 45 | 46 | %get x_dev_LIFT_History 47 | x_dev_LIFT_History=reshape(F*transpose(u_dev_History)+d0,size(x_optimal_History)); 48 | plot_trajectory(x_dev_LIFT_History, u_dev_History, "DEVIATION (LIFTED REPRESENTATION)"); 49 | 50 | %% GET TRAJECTORY FROM LIFTED REPRESENTATION 51 | 52 | plot_trajectory(x_dev_LIFT_History+x_optimal_History, u_dev_History+u_optimal_History, "REAL TRAJECTORY (LIFTED REPRESENTATION)"); 53 | 54 | -------------------------------------------------------------------------------- /getWeights.m: -------------------------------------------------------------------------------- 1 | function N = getWeights() 2 | 3 | N=zeros(101); 4 | for i=0:20 5 | N(101-i,101-i)=100; 6 | end 7 | 8 | N=eye(101); 9 | end 10 | 11 | -------------------------------------------------------------------------------- /get_Fcontr.m~: -------------------------------------------------------------------------------- 1 | function [Fcontr,d_estimated_contr] = get_Fd_contr(F,d_estimated) 2 | N=size(F,2); 3 | Fcontr=F; 4 | d_estimated_contr=d_estimated_contr; 5 | c_list=[]; 6 | for i=1:N 7 | c_list=[c_list i*4-3]; 8 | c_list=[c_list i*4-3+1]; 9 | c_list=[c_list i*4-3+3]; 10 | end 11 | 12 | J=length(c_list); 13 | for j=J:-1:1 14 | Fcontr(c_list(j),:)=[]; 15 | end 16 | 17 | for j=J:-1:1 18 | d_estimated_contr(c_list(j))=[]; 19 | end 20 | 21 | 22 | end 23 | 24 | -------------------------------------------------------------------------------- /get_Fd_contr.m: -------------------------------------------------------------------------------- 1 | function [Fcontr,d_estimated_contr] = get_Fd_contr(F,d_estimated) 2 | N=size(F,2); 3 | Fcontr=F; 4 | d_estimated_contr=d_estimated; 5 | c_list=[]; 6 | for i=1:N 7 | %c_list=[c_list i*4-3]; 8 | c_list=[c_list i*4-3+1]; 9 | c_list=[c_list i*4-3+3]; 10 | end 11 | 12 | J=length(c_list); 13 | for j=J:-1:1 14 | Fcontr(c_list(j),:)=[]; 15 | end 16 | 17 | for j=J:-1:1 18 | d_estimated_contr(c_list(j))=[]; 19 | end 20 | 21 | 22 | end 23 | 24 | -------------------------------------------------------------------------------- /get_jacobians.m: -------------------------------------------------------------------------------- 1 | function [A,B] = get_jacobians(x,u) 2 | 3 | z_dot=x(2); 4 | theta=x(3); 5 | theta_dot=x(4); 6 | 7 | global alpha_1 alpha_2 m_p m_c l_p g; 8 | 9 | a22=alpha_2/(m_p*(sin(theta)^2 + m_c/m_p)); 10 | a23=(g*sin(theta)^2 - g*cos(theta)^2 + l_p*theta_dot^2*cos(theta))/(sin(theta)^2 + m_c/m_p) - (2*cos(theta)*sin(theta)*(l_p*sin(theta)*theta_dot^2 + (alpha_1*u + alpha_2*z_dot)/m_p - g*cos(theta)*sin(theta)))/(sin(theta)^2 + m_c/m_p)^2; 11 | a24=(2*l_p*theta_dot*sin(theta))/(sin(theta)^2 + m_c/m_p); 12 | a42=-(alpha_2*cos(theta))/(l_p*m_p*(sin(theta)^2 + m_c/m_p)); 13 | a43=((sin(theta)*(alpha_1*u + alpha_2*z_dot))/m_p - cos(theta)*(l_p*cos(theta)*theta_dot^2 - (g*(m_c + m_p))/m_p) + l_p*theta_dot^2*sin(theta)^2)/(l_p*(sin(theta)^2 + m_c/m_p)) + (2*cos(theta)*sin(theta)*(sin(theta)*(l_p*cos(theta)*theta_dot^2 - (g*(m_c + m_p))/m_p) + (cos(theta)*(alpha_1*u + alpha_2*z_dot))/m_p))/(l_p*(sin(theta)^2 + m_c/m_p)^2); 14 | a44=-(2*theta_dot*cos(theta)*sin(theta))/(sin(theta)^2 + m_c/m_p); 15 | 16 | A= [ 0 , 1 , 0 , 0 ; 17 | 0 , a22 , a23 , a24 ; 18 | 0 , 0 , 0 , 1 ; 19 | 0 , a42 , a43 , a44 ]; 20 | 21 | b2= alpha_1/(m_p*(sin(theta)^2 + m_c/m_p)); 22 | b4= -(alpha_1*cos(theta))/(l_p*m_p*(sin(theta)^2 + m_c/m_p)); 23 | 24 | B = [0; b2; 0; b4]; 25 | 26 | end 27 | 28 | -------------------------------------------------------------------------------- /get_lifted_repr.m: -------------------------------------------------------------------------------- 1 | function [F,d0,G] = get_lifted_repr(xHistory,uHistory,x0_dev) 2 | 3 | N=size(xHistory,2); %number of timesteps 4 | 5 | %get jacobians 6 | A=zeros(4,4,N); 7 | B=zeros(4,N); 8 | for ct=1:N 9 | [nA,nB]=get_jacobians(xHistory(:,ct),uHistory(:,ct)); 10 | A(:,:,ct)=nA; 11 | B(:,ct)=nB; 12 | end 13 | 14 | global Ts; 15 | 16 | %compute F 17 | F=zeros(N*4,N); 18 | for m=1:N 19 | for l=1:N 20 | if m 80 | uHistory = [uHistory mv]; 81 | 82 | waitbar(ct*Ts/Duration,hbar); 83 | end 84 | uHistory=[uHistory(2:length(uHistory)) 0]; 85 | 86 | close(hbar); 87 | -------------------------------------------------------------------------------- /init.m: -------------------------------------------------------------------------------- 1 | %initialization of model parameters 2 | 3 | global Ts Duration m_p m_c l_p alpha_1 alpha_2 m_p_real m_c_real l_p_real alpha_1_real alpha_2_real g x_bound x_1_bound u_bound u_bound_opt x0; 4 | 5 | %Time parameters 6 | Ts = 0.01; %Sampling time (seconds) 7 | Duration = 1; %Simulation time (seconds) 8 | 9 | g = 9.81; %gravitational constant (meters/second^2) 10 | 11 | %structural parameters (NOMINAL) 12 | m_p = 0.175; %mass of pendulum (kilograms) 13 | m_c = 1.5; %mass of the cart (kilograms) 14 | l_p = 0.28; %distance from pivot to pendulum's center of mass (meters) 15 | alpha_1 = 159; %motor constant 1 (voltage-to-force) (Newtons) 16 | alpha_2 = -22.5; %%motor constant 2 (electrical resistance-to-force) (Newtons*second/meters) 17 | 18 | %structural parameters (REAL) 19 | m_p_real = m_p*0.93; %mass of pendulum (kilograms) 20 | m_c_real = m_c*0.97; %mass of the cart (kilograms) 21 | l_p_real = l_p*1.10; %distance from pivot to pendulum's center of mass (meters) 22 | alpha_1_real = alpha_1*0.81; %motor constant 1 (voltage-to-force) (Newtons) 23 | alpha_2_real = alpha_2*0.86; %%motor constant 2 (electrical resistance-to-force) (Newtons*second/meters) 24 | 25 | % Bounds 26 | x_bound = 0.5; %bound due to finite rail length (meters) 27 | x_1_bound = 5; %bound due to limited velocity of the cart (meters/seconds) 28 | u_bound = 0.45; %bound due to limited velocity of motor (meters/seconds) 29 | u_bound_opt = 0.2; %bound due to limited velocity of motor (meters/seconds) 30 | 31 | % Initial conditions (NOMINAL) 32 | x0= zeros(4,1); 33 | x0(1) = 0; %initial position x of cart (meters) 34 | x0(2) = 0; %initial linear velocity x_1 of cart (meters/seconds) 35 | x0(3) = -pi; %initial angle phi of pendulum (radians) 36 | x0(4) = 0; %initial angularvelocity phi_1 of pendulum (radians/seconds) -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | if ~mpcchecktoolboxinstalled('optim') 2 | disp('Optimization Toolbox is required to run this example.') 3 | return 4 | end 5 | clear all; 6 | close all; 7 | clc; 8 | 9 | %% main script 10 | 11 | %initialize parameters 12 | init; 13 | 14 | %% GET IDEAL TRAJECTORY 15 | 16 | %get ideal trajectory 17 | global x_optimal_History u_optimal_History N; 18 | [x_optimal_History, u_optimal_History]= get_optimal_trajectory(); 19 | N=size(x_optimal_History,2); 20 | plot_trajectory(x_optimal_History, u_optimal_History,"IDEAL TRAJECTORY"); 21 | 22 | %% ITERATIVE LEARNING CONTROL 23 | 24 | close all; 25 | clc; 26 | 27 | n_iterations=4; 28 | 29 | %SIMULATE 30 | 31 | %compute initial conditions 32 | global x0_real; 33 | x0_real=[0;0;-pi;0]; 34 | sigma_x0=0.001; 35 | x0_real=x0_real+normrnd(0,sigma_x0,[4,1]); 36 | 37 | %simulate applying nominal u 38 | [x,y]=simulation_nonlinearized_real(transpose(u_optimal_History),x0_real); 39 | y_iterations=[y]; 40 | x_iterations=[x]; 41 | u_iterations=[ u_optimal_History ]; 42 | 43 | %get matrices for lifted representation 44 | global F d_estimated; 45 | [F,~,G]=get_lifted_repr(x_optimal_History,u_optimal_History,[0;0;0;0]); 46 | 47 | %parameters initialization for estimation 48 | coeff_trial=2; 49 | P=eye(N*4)*coeff_trial; 50 | M_coeff=0.01; 51 | M=eye(4*N)*M_coeff; 52 | d_estimated=zeros([N*4,1]); 53 | Kgain=zeros([4*N,N]); 54 | 55 | u=zeros([N,1]); 56 | 57 | for j=1:n_iterations 58 | 59 | %ESTIMATION STEP 60 | eps=0.01; 61 | OMEGA=eye(N*4)*eps; 62 | P_cond=P+OMEGA; 63 | THETA=G*P_cond*transpose(G)+M; 64 | Kgain=P_cond*transpose(G)*inv(THETA); 65 | P=(eye(4*N)-Kgain*G)*P_cond; 66 | 67 | y_dev=reshape(y-x_optimal_History,[4*N,1]); 68 | d_estimated=d_estimated+Kgain*(y_dev-G*d_estimated-(G*F)*u); 69 | 70 | %CONTROL STEP 71 | global u_bounds 72 | lb=-u_bound-transpose(u_optimal_History); 73 | ub=u_bound-transpose(u_optimal_History); 74 | u0=zeros([N,1]); 75 | 76 | % solve optimization problem with fmincon 77 | [Fcontr,d_estimated_contr]=get_Fd_contr(F,d_estimated); 78 | 79 | W=getWeights(); 80 | fun = @(u_opt)transpose((Fcontr*u_opt+d_estimated_contr))*((Fcontr*u_opt+d_estimated_contr)); 81 | options = optimoptions(@fmincon,'MaxFunctionEvaluations',100000000,'MaxIterations',10000); 82 | %problem = createOptimProblem('fmincon','objective',fun,'x0',u0,'lb',lb,'ub',ub,'options',options); 83 | %gs = GlobalSearch; 84 | 85 | %u = run(gs,problem); 86 | u = fmincon(fun,u0,[],[],[],[],lb,ub,@nonlinearconstraints,options); 87 | 88 | j 89 | transpose((Fcontr*u+d_estimated_contr))*((Fcontr*u+d_estimated_contr)) 90 | 91 | u_iterations=[u_iterations; transpose(u)+u_optimal_History]; 92 | 93 | %SIMULATE 94 | [x,y]=simulation_nonlinearized_real(u+transpose(u_optimal_History),x0_real); 95 | x_iterations=[x_iterations; x]; 96 | y_iterations=[y_iterations; y]; 97 | 98 | %compute new initial conditions for next iteration (random walk) 99 | x0_real=x0_real+normrnd(0,sigma_x0,[4,1]); 100 | end 101 | 102 | num_trials=0; 103 | for j=1:num_trials 104 | if (j==1) 105 | x_iterations=[]; 106 | y_iterations=[]; 107 | u_iterations=[]; 108 | end 109 | [x,y]=simulation_nonlinearized_idealwnoise(transpose(u_optimal_History),x0_real); 110 | x0_real=[0;0;-pi;0]+normrnd(0,sigma_x0,[4,1]); 111 | u_iterations=[u_iterations; u_optimal_History]; 112 | x_iterations=[x_iterations; x]; 113 | y_iterations=[y_iterations; y]; 114 | end 115 | 116 | plot_trajectories(x_iterations,u_iterations,x_optimal_History,u_optimal_History,"Real output trajectory"); 117 | saveJson(x_iterations); 118 | 119 | plot_trajectories(y_iterations,u_iterations,x_optimal_History,u_optimal_History,"Observed output trajectory"); 120 | 121 | -------------------------------------------------------------------------------- /nonlinearconstraints.m: -------------------------------------------------------------------------------- 1 | function [c,ceq] = nonlinearconstraints(u) 2 | global x0_real u_optimal_History N x_optimal_History F d_estimated; 3 | 4 | x=x_optimal_History+reshape(F*u+d_estimated,[4,N]); 5 | 6 | c(1) = max(abs(x(1,:)))-0.5; 7 | c(2) = max(abs(x(2,:)))-5; 8 | %c(3) = max(movvar(u,5))-0.001; 9 | %c=[]; 10 | ceq = []; 11 | 12 | end 13 | -------------------------------------------------------------------------------- /pendulum_ur_CT0.m: -------------------------------------------------------------------------------- 1 | function dxdt = pendulum_ur_CT0(x, u) 2 | %% Continuous-time nonlinear dynamic model of a pendulum on a cart 3 | % 4 | % 4 states (x): 5 | % cart position (z) 6 | % cart velocity (z_dot): when positive, cart moves to right 7 | % angle (theta): when 0, pendulum is at upright position 8 | % angular velocity (theta_dot): when positive, pendulum moves anti-clockwisely 9 | % 10 | % 1 inputs: (u) 11 | % force (F): when positive, force pushes cart to right 12 | % 13 | % Copyright 2018 The MathWorks, Inc. 14 | 15 | %#codegen 16 | 17 | %% parameters 18 | 19 | global g; 20 | global m_c m_p l_p alpha_1 alpha_2; 21 | 22 | mp=m_p; 23 | mc=m_c; 24 | lp=l_p; 25 | alpha1=alpha_1; 26 | alpha2=alpha_2; 27 | 28 | %% Obtain x, u and y 29 | % x 30 | z_dot = x(2); 31 | theta = x(3); 32 | theta_dot = x(4); 33 | % u 34 | F = alpha1*u + alpha2*z_dot; 35 | 36 | %% Compute dxdt 37 | dxdt = x; 38 | % z_dot 39 | dxdt(1) = z_dot; 40 | % z_dot_dot 41 | dxdt(2) = (F/mp - g*sin(theta)*cos(theta) + lp*(theta_dot)^2*sin(theta))/(mc/mp + sin(theta)^2); 42 | % theta_dot 43 | dxdt(3) = theta_dot; 44 | % theta_dot_dot 45 | dxdt(4) = (-cos(theta)*F/mp + (((mc + mp)/mp)*g - lp*theta_dot^2*cos(theta))*sin(theta))/(lp*(mc/mp + sin(theta)^2)); -------------------------------------------------------------------------------- /pendulum_ur_CT0_real.m: -------------------------------------------------------------------------------- 1 | function dxdt = pendulum_ur_CT0_real(x, u) 2 | %% Continuous-time nonlinear dynamic model of a pendulum on a cart 3 | % 4 | % 4 states (x): 5 | % cart position (z) 6 | % cart velocity (z_dot): when positive, cart moves to right 7 | % angle (theta): when 0, pendulum is at upright position 8 | % angular velocity (theta_dot): when positive, pendulum moves anti-clockwisely 9 | % 10 | % 1 inputs: (u) 11 | % force (F): when positive, force pushes cart to right 12 | % 13 | % Copyright 2018 The MathWorks, Inc. 14 | 15 | %#codegen 16 | 17 | %% parameters 18 | 19 | global g; 20 | global m_c_real m_p_real l_p_real alpha_1_real alpha_2_real; 21 | 22 | mp=m_p_real; 23 | mc=m_c_real; 24 | lp=l_p_real; 25 | alpha1=alpha_1_real; 26 | alpha2=alpha_2_real; 27 | 28 | %% Obtain x, u and y 29 | % x 30 | z_dot = x(2); 31 | theta = x(3); 32 | theta_dot = x(4); 33 | % u 34 | F = alpha1*u + alpha2*z_dot; 35 | 36 | %% Compute dxdt 37 | dxdt = x; 38 | % z_dot 39 | dxdt(1) = z_dot; 40 | % z_dot_dot 41 | dxdt(2) = (F/mp - g*sin(theta)*cos(theta) + lp*(theta_dot)^2*sin(theta))/(mc/mp + sin(theta)^2); 42 | % theta_dot 43 | dxdt(3) = theta_dot; 44 | % theta_dot_dot 45 | dxdt(4) = (-cos(theta)*F/mp + (((mc + mp)/mp)*g - lp*theta_dot^2*cos(theta))*sin(theta))/(lp*(mc/mp + sin(theta)^2)); -------------------------------------------------------------------------------- /pendulum_ur_DT0.m: -------------------------------------------------------------------------------- 1 | function xk1 = pendulum_ur_DT0(xk, uk, Ts) 2 | %% Discrete-time nonlinear dynamic model of a pendulum on a cart at time k 3 | % 4 | % 4 states (xk): 5 | % cart position (z) 6 | % cart velocity (z_dot): when positive, cart moves to right 7 | % angle (theta): when 0, pendulum is at upright position 8 | % angular velocity (theta_dot): when positive, pendulum moves anti-clockwisely 9 | % 10 | % 1 inputs: (uk) 11 | % force (F): when positive, force pushes cart to right 12 | % 13 | % xk1 is the states at time k+1. 14 | % 15 | % Copyright 2018 The MathWorks, Inc. 16 | 17 | %#codegen 18 | 19 | % Repeat application of Euler method sampled at Ts/M. 20 | 21 | delta = Ts; 22 | 23 | xk1 = xk + delta*pendulum_ur_CT0(xk,uk); 24 | 25 | % Note that we choose the Euler method (first oder Runge-Kutta method) 26 | % because it is more efficient for plant with non-stiff ODEs. You can 27 | % choose other ODE solvers such as ode23, ode45 for better accuracy or 28 | % ode15s and ode23s for stiff ODEs. Those solvers are available from 29 | % MATLAB. 30 | -------------------------------------------------------------------------------- /pendulum_ur_DT0_real.m: -------------------------------------------------------------------------------- 1 | function xk1 = pendulum_ur_DT0_real(xk, uk, Ts) 2 | %% Discrete-time nonlinear dynamic model of a pendulum on a cart at time k 3 | % 4 | % 4 states (xk): 5 | % cart position (z) 6 | % cart velocity (z_dot): when positive, cart moves to right 7 | % angle (theta): when 0, pendulum is at upright position 8 | % angular velocity (theta_dot): when positive, pendulum moves anti-clockwisely 9 | % 10 | % 1 inputs: (uk) 11 | % force (F): when positive, force pushes cart to right 12 | % 13 | % xk1 is the states at time k+1. 14 | % 15 | % Copyright 2018 The MathWorks, Inc. 16 | 17 | %#codegen 18 | 19 | % Repeat application of Euler method sampled at Ts/M. 20 | 21 | delta = Ts; 22 | 23 | xk1 = xk + delta*pendulum_ur_CT0_real(xk,uk); 24 | 25 | % Note that we choose the Euler method (first oder Runge-Kutta method) 26 | % because it is more efficient for plant with non-stiff ODEs. You can 27 | % choose other ODE solvers such as ode23, ode45 for better accuracy or 28 | % ode15s and ode23s for stiff ODEs. Those solvers are available from 29 | % MATLAB. 30 | -------------------------------------------------------------------------------- /plot_trajectories.m: -------------------------------------------------------------------------------- 1 | function plot_trajectories(xHistory ,uHistory, xOptimal, uOptimal, name) 2 | %% 3 | 4 | global Ts Duration; 5 | I=size(uHistory,1); 6 | 7 | figure('Name',name,'NumberTitle','off') 8 | for i=1:I 9 | % Plot the closed-loop response. 10 | subplot(2,3,1) 11 | plot(0:Ts:Duration,xHistory(1+(i-1)*4,:),'DisplayName',"iteration "+i) 12 | hold on 13 | 14 | subplot(2,3,2) 15 | plot(0:Ts:Duration,xHistory(2+(i-1)*4,:),'DisplayName',"iteration "+i) 16 | hold on 17 | 18 | subplot(2,3,3) 19 | plot(0:Ts:Duration,xHistory(3+(i-1)*4,:),'DisplayName',"iteration "+i) 20 | hold on 21 | 22 | subplot(2,3,4) 23 | plot(0:Ts:Duration,xHistory(4+(i-1)*4,:),'DisplayName',"iteration "+i) 24 | hold on 25 | 26 | subplot(2,3,5) 27 | plot(0:Ts:Duration,uHistory(i,:),'DisplayName',"iteration "+i) 28 | hold on 29 | end 30 | 31 | % Plot the closed-loop response. 32 | subplot(2,3,1) 33 | plot(0:Ts:Duration,xOptimal(1,:),'DisplayName',"nominal trajectory") 34 | xlabel('time (seconds)') 35 | ylabel('x (meters)') 36 | title({"",'cart position',""}) 37 | lgd = legend; 38 | hold on 39 | 40 | subplot(2,3,2) 41 | plot(0:Ts:Duration,xOptimal(2,:),'DisplayName',"nominal trajectory") 42 | xlabel('time (seconds)') 43 | ylabel("x' (meters/seconds)") 44 | title({"",'cart velocity',""}) 45 | lgd = legend; 46 | hold on 47 | 48 | subplot(2,3,3) 49 | plot(0:Ts:Duration,xOptimal(3,:),'DisplayName',"nominal trajectory") 50 | xlabel('time (seconds)') 51 | ylabel('\phi (radians)') 52 | title({"",'pendulum angle',""}) 53 | lgd = legend; 54 | hold on 55 | 56 | subplot(2,3,4) 57 | plot(0:Ts:Duration,xOptimal(4,:),'DisplayName',"nominal trajectory") 58 | xlabel('time (seconds)') 59 | ylabel("\phi' (radians/seconds)") 60 | title({"",'pendulum velocity',""}) 61 | lgd = legend; 62 | hold on 63 | 64 | subplot(2,3,5) 65 | plot(0:Ts:Duration,uOptimal,'DisplayName',"nominal trajectory") 66 | xlabel('time (seconds)') 67 | ylabel("u") 68 | title({"",'input u',""}) 69 | lgd = legend; 70 | hold on -------------------------------------------------------------------------------- /plot_trajectory.m: -------------------------------------------------------------------------------- 1 | function plot_trajectory(xHistory ,uHistory, name) 2 | %% 3 | 4 | global Ts Duration; 5 | 6 | % Plot the closed-loop response. 7 | figure('Name',name,'NumberTitle','off') 8 | subplot(2,3,1) 9 | plot(0:Ts:Duration,xHistory(1,:)) 10 | xlabel('time (seconds)') 11 | ylabel('x (meters)') 12 | title({"",'cart position',""}) 13 | subplot(2,3,2) 14 | plot(0:Ts:Duration,xHistory(2,:)) 15 | xlabel('time (seconds)') 16 | ylabel("x' (meters/seconds)") 17 | title({"",'cart velocity',""}) 18 | subplot(2,3,3) 19 | plot(0:Ts:Duration,xHistory(3,:)) 20 | xlabel('time (seconds)') 21 | ylabel('\phi (radians)') 22 | title({"",'pendulum angle',""}) 23 | subplot(2,3,4) 24 | plot(0:Ts:Duration,xHistory(4,:)) 25 | xlabel('time (seconds)') 26 | ylabel("\phi' (radians/seconds)") 27 | title({"",'pendulum velocity',""}) 28 | subplot(2,3,5) 29 | plot(0:Ts:Duration,uHistory) 30 | xlabel('time (seconds)') 31 | ylabel("u") 32 | title({"",'input u',""}) -------------------------------------------------------------------------------- /saveJson.m: -------------------------------------------------------------------------------- 1 | function saveJson(data) 2 | 3 | jsonStr = jsonencode(data); 4 | fid = fopen(pwd+"/animation/data_hystory.json", 'w'); 5 | if fid == -1, error('Cannot create JSON file'); end 6 | fwrite(fid, jsonStr, 'char'); 7 | fclose(fid); 8 | 9 | end 10 | 11 | -------------------------------------------------------------------------------- /simulation_linearized.m: -------------------------------------------------------------------------------- 1 | function [xHistory_dev] = simulation_linearized(x0_dev,uHistory_dev,xHistory_optimal,uHistory_optimal) 2 | 3 | global Ts; 4 | N=size(xHistory_optimal,2); %number of timesteps 5 | xHistory_dev=zeros([4,N]); 6 | xHistory_dev(:,1)=x0_dev; 7 | 8 | %get jacobians 9 | A=zeros(4,4,N); 10 | B=zeros(4,N); 11 | for ct=1:N 12 | [nA,nB]=get_jacobians(xHistory_optimal(:,ct),uHistory_optimal(ct)); 13 | A(:,:,ct)=nA; 14 | B(:,ct)=nB; 15 | end 16 | 17 | for i=2:N 18 | Ad=eye([4,4])+Ts*A(:,:,i-1); 19 | Bd=Ts*B(:,i-1); 20 | xHistory_dev(:,i)= Ad*xHistory_dev(:,i-1)+Bd*uHistory_dev(i-1); 21 | end 22 | end 23 | 24 | -------------------------------------------------------------------------------- /simulation_nonlinearized.m: -------------------------------------------------------------------------------- 1 | function xHistory = simulation_nonlinearized(uHistory,x0) 2 | 3 | global Ts; 4 | N=length(uHistory); 5 | xHistory=zeros([4,N]); 6 | xHistory(:,1)=x0; 7 | for i=2:N 8 | xHistory(:,i)= pendulum_ur_DT0(xHistory(:,i-1), uHistory(i-1), Ts); 9 | end 10 | end 11 | 12 | -------------------------------------------------------------------------------- /simulation_nonlinearized_idealwnoise.m: -------------------------------------------------------------------------------- 1 | function [xHistory ,yHistory] = simulation_nonlinearized_idealwnoise(uHistory,x0) 2 | 3 | global Ts; 4 | N=length(uHistory); 5 | xHistory=zeros([4,N]); 6 | yHistory=zeros([4,N]); 7 | xHistory(:,1)=x0; 8 | yHistory(:,1)=x0; 9 | 10 | %noises 11 | sigma_xi=0.000; 12 | sigma_ni_x=0.001; 13 | sigma_ni_theta=0.01; 14 | 15 | 16 | for i=2:N 17 | 18 | %compute next state 19 | disturbance=normrnd(0,sigma_xi,[4,1]); 20 | disturbance(1)=0; 21 | disturbance(3)=0; 22 | xHistory(:,i)= pendulum_ur_DT0(xHistory(:,i-1), uHistory(i-1), Ts)+disturbance; 23 | 24 | %observe output 25 | y_x=xHistory(1,i)+normrnd(0,sigma_ni_x); 26 | y_x_1=(y_x-yHistory(1,i-1))/Ts; 27 | y_theta=xHistory(3,i)+normrnd(0,sigma_ni_theta); 28 | y_theta_1=(y_theta-yHistory(3,i-1))/Ts; 29 | y=[y_x,y_x_1,y_theta,y_theta_1]; 30 | yHistory(:,i)=y; 31 | 32 | end 33 | 34 | end 35 | 36 | -------------------------------------------------------------------------------- /simulation_nonlinearized_real.m: -------------------------------------------------------------------------------- 1 | function [xHistory,yHistory] = simulation_nonlinearized_real(uHistory,x0) 2 | 3 | global Ts; 4 | N=length(uHistory); 5 | xHistory=zeros([4,N]); 6 | yHistory=zeros([4,N]); 7 | xHistory(:,1)=x0; 8 | yHistory(:,1)=x0; 9 | 10 | %noises 11 | sigma_xi=0.005; 12 | sigma_ni_x=0.001; 13 | sigma_ni_theta=0.01; 14 | 15 | 16 | for i=2:N 17 | 18 | %compute next state 19 | disturbance=normrnd(0,sigma_xi,[4,1]); 20 | disturbance(1)=0; 21 | disturbance(3)=0; 22 | xHistory(:,i)= pendulum_ur_DT0_real(xHistory(:,i-1), uHistory(i-1), Ts)+disturbance; 23 | 24 | %observe output 25 | y_x=xHistory(1,i)+normrnd(0,sigma_ni_x); 26 | y_x_1=(y_x-yHistory(1,i-1))/Ts; 27 | y_theta=xHistory(3,i)+normrnd(0,sigma_ni_theta); 28 | y_theta_1=(y_theta-yHistory(3,i-1))/Ts; 29 | y=[y_x,y_x_1,y_theta,y_theta_1]; 30 | yHistory(:,i)=y; 31 | end 32 | 33 | end 34 | 35 | -------------------------------------------------------------------------------- /video_ILC.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AlexSantopaolo/Optimization-Based-Iterative-Learning-Control-for-Trajectory-Tracking/7292f44225fdb6fd8bbbd640fe284b2396a50047/video_ILC.mp4 --------------------------------------------------------------------------------