├── .gitignore ├── SOS-sols ├── acrobot_sol.mat ├── cartpole_sol.mat ├── pendulum_sol.mat ├── cartpole_tv_sol.mat └── quadrotor2d_sol.mat ├── double_integrator_mpc ├── double_integrator_dynamics.m ├── blkIndices.m ├── double_integrator_ocp.m ├── double_integrator_mpc.m └── double_integrator_stability.m ├── Animations ├── acrobot_animation.gif ├── cartpole_animation.gif └── pendulum_animation.gif ├── pendulum_mpc ├── pendulum_ode.m ├── pendulum_open_loop.m ├── pendulum_mpc.m └── PendulumTrajOpt.m ├── grid_simple.m ├── Neural-FVI ├── CostToGoFunction.py ├── LinearDynamics.py ├── utils.py └── Training_batch.py ├── pendulum_control_lyapunov.m ├── pendulum_luenberger.m ├── polygon_inside_circle.m ├── mpt_examples ├── double_integrator_feasible_sets.m ├── maximal_control_invariant_set.m └── controllable_reachable_sets.m ├── finite_HJ.m ├── pendulum_stabilization_lqr.m ├── RK4_example.m ├── fire_canon_ball_fmincon.m ├── double_integrator_collocation_sdp.m ├── pendulum_sos_observer.m ├── pendulum_stabilization_lqg.m ├── robot_localization_particle_filter.m ├── LQR_FVI_Example.m ├── quadrotor2d_sos_observer.m ├── cartpole_highgain_observer_simulation.m ├── cartpole_sos_observer_refine.m ├── double_integrator_collocation_sparse_sdp.m ├── LQR_FQI_Example.m ├── pendulum_highgain_observer_simulation.m ├── double_integrator_collocation.m ├── cartpole_sos_observer_tv.m ├── acrobot_sos_observer.m ├── grid_world_value_iteration.m ├── cartpole_sos_observer.m ├── quadrotor2d_luenberger.m ├── double_integrator_multiple_shooting.m ├── pendulum_saturated_control.m ├── pendulum_local_lyapunov_certificate.m ├── pendulum_saturated_control_sol.m ├── quadrotor2d_sos_observer_simulation.m ├── pendulum_integrator_collocation.m ├── pendulum_value_iteration_barycentric.m ├── pendulum_sos_observer_simulation.m ├── cartpole_sos_observer_simulation.m ├── acrobot_highgain_observer.m └── quadrotor_highgain_observer_simulation.m /.gitignore: -------------------------------------------------------------------------------- 1 | *.asv 2 | *.m~ 3 | *.DS_Store 4 | *.mex* 5 | data 6 | mytest -------------------------------------------------------------------------------- /SOS-sols/acrobot_sol.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ComputationalRobotics/OptimalControlEstimation-Examples/HEAD/SOS-sols/acrobot_sol.mat -------------------------------------------------------------------------------- /SOS-sols/cartpole_sol.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ComputationalRobotics/OptimalControlEstimation-Examples/HEAD/SOS-sols/cartpole_sol.mat -------------------------------------------------------------------------------- /SOS-sols/pendulum_sol.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ComputationalRobotics/OptimalControlEstimation-Examples/HEAD/SOS-sols/pendulum_sol.mat -------------------------------------------------------------------------------- /double_integrator_mpc/double_integrator_dynamics.m: -------------------------------------------------------------------------------- 1 | function xnew = double_integrator_dynamics(x,u) 2 | xnew = [1, 1; 0, 1]*x + [0; 1]*u; 3 | end -------------------------------------------------------------------------------- /SOS-sols/cartpole_tv_sol.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ComputationalRobotics/OptimalControlEstimation-Examples/HEAD/SOS-sols/cartpole_tv_sol.mat -------------------------------------------------------------------------------- /SOS-sols/quadrotor2d_sol.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ComputationalRobotics/OptimalControlEstimation-Examples/HEAD/SOS-sols/quadrotor2d_sol.mat -------------------------------------------------------------------------------- /Animations/acrobot_animation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ComputationalRobotics/OptimalControlEstimation-Examples/HEAD/Animations/acrobot_animation.gif -------------------------------------------------------------------------------- /Animations/cartpole_animation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ComputationalRobotics/OptimalControlEstimation-Examples/HEAD/Animations/cartpole_animation.gif -------------------------------------------------------------------------------- /Animations/pendulum_animation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ComputationalRobotics/OptimalControlEstimation-Examples/HEAD/Animations/pendulum_animation.gif -------------------------------------------------------------------------------- /pendulum_mpc/pendulum_ode.m: -------------------------------------------------------------------------------- 1 | function dx = pendulum_ode(t,states,u_grid,t_grid,m,l,g,b,noise) 2 | u_t = interp1(t_grid,u_grid,t); % piece-wise linear 3 | x = states; 4 | dx = [x(2); 5 | -1/(m*l^2) * (-u_t + b*x(2)+m*g*l*sin(x(1))) ]; 6 | % dx = dx + noise*randn(2,1); 7 | dx = dx + [0; noise]; 8 | end -------------------------------------------------------------------------------- /double_integrator_mpc/blkIndices.m: -------------------------------------------------------------------------------- 1 | function matrixIndices = blkIndices(blockIndex, blockSize) 2 | numBlocks = length(blockIndex); 3 | matrixIndices = zeros(numBlocks*blockSize,1); 4 | for i=1:numBlocks 5 | matrixIndices(blockSize*i-(blockSize-1) : blockSize*i) = ... 6 | [blockSize*blockIndex(i)-(blockSize-1) : blockSize*blockIndex(i)]; 7 | end 8 | end -------------------------------------------------------------------------------- /grid_simple.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | close all 4 | 5 | g = [1;0;1;1;1;0;1;1]; 6 | P = [1,0,0,0; 7 | 0,1,0,0; 8 | 1,0,0,0; 9 | 0,1,0,0; 10 | 0,1,0,0; 11 | 0,1,0,0; 12 | 0,0,0,1; 13 | 0,0,0,1]; 14 | P = sparse(P); 15 | 16 | J = rand(4,1); 17 | 18 | MAX_ITERS = 1e3; 19 | iter = 1; 20 | while iter < MAX_ITERS 21 | tmp = P * J + g; 22 | tmp = reshape(tmp,4,2); 23 | Jnew = min(tmp,[],2); 24 | if norm(Jnew - J) < 1e-6 25 | break; 26 | end 27 | J = Jnew; 28 | iter = iter + 1; 29 | end 30 | -------------------------------------------------------------------------------- /Neural-FVI/CostToGoFunction.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | import torch.nn.init as init 4 | 5 | class CostToGo(nn.Module): 6 | def __init__(self, input_size=2, hidden_size=3072, output_size=1024): 7 | super(CostToGo, self).__init__() 8 | self.fc1 = nn.Linear(input_size, hidden_size, bias=True) 9 | self.fc2 = nn.Linear(hidden_size, hidden_size, bias=True) 10 | self.fc3 = nn.Linear(hidden_size, output_size, bias=True) 11 | self.activation = nn.ReLU() 12 | 13 | def forward(self, x): 14 | x = self.activation(self.fc1(x)) 15 | x = self.activation(self.fc2(x)) 16 | x = self.fc3(x) 17 | return x -------------------------------------------------------------------------------- /Neural-FVI/LinearDynamics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch as th 3 | 4 | class LinearSystem: 5 | def __init__(self, dt = 0.01): 6 | self.dt = dt 7 | 8 | def dynamics(self, x, u): 9 | # Double Integrator 10 | self.fx = np.array([x[1], 0]) 11 | self.gx = np.array([0, 1]) 12 | xdot = self.fx + self.gx * u 13 | return self.fx, self.gx, xdot 14 | 15 | def dynamics_torch(self, x_batch, device): 16 | x0, x1 = x_batch[:, 0], x_batch[:, 1] 17 | zero_tensor = th.zeros_like(x1) 18 | fx = th.stack((x1, zero_tensor), dim=1) 19 | gx = th.tensor([0, 1], dtype=th.float32, device=device, requires_grad=True).repeat(x_batch.size(0), 1) 20 | return fx, gx 21 | 22 | -------------------------------------------------------------------------------- /Neural-FVI/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.linalg 3 | from scipy.integrate import odeint 4 | from control import ctrb 5 | import matplotlib.pyplot as plt 6 | import torch as th 7 | from torch.func import jacrev, hessian, vmap 8 | 9 | def sample_state(x0max, x1max): 10 | x0 = (-1 + 2 * np.random.rand()) * x0max 11 | x1 = (-1 + 2 * np.random.rand()) * x1max 12 | 13 | return(np.array([x0, x1])) 14 | 15 | def prepare_data(sample_number, x0max, x1max, vector_length=2): 16 | x_train = np.empty((sample_number,vector_length)) 17 | for i in range(sample_number): 18 | new_element = sample_state(x0max=x0max, x1max=x1max) 19 | x_train[i] = new_element 20 | 21 | return x_train 22 | 23 | 24 | if __name__ == "__main__": 25 | pass -------------------------------------------------------------------------------- /pendulum_control_lyapunov.m: -------------------------------------------------------------------------------- 1 | % Code for Validating a Control Lyaponov Function 2 | % for a Pendulum with an Observer 3 | 4 | clear; clc; close all; format compact; 5 | syms b m l g real % damping, mass, length, gravity 6 | syms theta theta_dot real % theta = output 7 | syms theta_hat theta_dot_hat real 8 | syms u real % u = control input 9 | syms k1 k2 real % k1, k2 = observer gains 10 | A = [0,1; 0, -b/(m*l^2)]; 11 | B = [0; (u - m*g*l*sin(theta))/(m*l^2)]; 12 | C = [1,0]; 13 | D = [C,0]; 14 | K = [k1; k2]; 15 | 16 | F = [A,zeros(2,2);zeros(2,2),(A - K*C)]; 17 | G = [B;0;0]; 18 | z0 = [pi;0;0;0]; 19 | x = [theta; theta_dot]; 20 | x_hat = [theta_hat; theta_dot_hat]; 21 | e = x_hat - x; 22 | z = [x;e]; 23 | %syms z1 z2 z3 z4 real 24 | %z = [z1 z2 z3 z4]'; 25 | % 26 | V = z'*z; 27 | gradient(V); 28 | Lf = (combine(dot(gradient(V),F*z))) 29 | Lg = (combine(dot(gradient(V),G))) 30 | -------------------------------------------------------------------------------- /pendulum_luenberger.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | % 3 | cvxpath = "../cvx"; 4 | addpath(genpath(cvxpath)) 5 | 6 | m = 1; g = 9.8; l = 1; b = 0.1; 7 | A = [0, 1; 0, -b/(m*l^2)]; 8 | C = [1, 0]; 9 | 10 | %% Estimate convergence rate gamma for varying k 11 | k_choices = [0.1,1,10,100,1000,10000]; 12 | gams = zeros(length(k_choices),1); 13 | for i = 1:length(k_choices) 14 | k = k_choices(i); 15 | K = [k; 0]; 16 | AKC = A - K*C; 17 | cvx_begin 18 | variable P(2,2) symmetric 19 | minimize(0) 20 | subject to 21 | P == semidefinite(2) 22 | AKC'*P + P*AKC == - eye(2) 23 | cvx_end 24 | max_eig_P = max(eig(P)); 25 | gams(i) = 0.5 / max_eig_P; 26 | end 27 | 28 | %% Find the optimal K that maximizes convergence rate 29 | cvx_begin 30 | variable H(2) 31 | variable P(2,2) symmetric 32 | minimize(lambda_max(P)) 33 | subject to 34 | P == semidefinite(2) 35 | A'*P - C'*H' + P*A - H*C == - eye(2); 36 | cvx_end 37 | max_gam = 0.5 / max(eig(P)); 38 | K = P \ H; -------------------------------------------------------------------------------- /double_integrator_mpc/double_integrator_ocp.m: -------------------------------------------------------------------------------- 1 | function uopt = double_integrator_ocp(xt,N,P,Q,R,terminal_constraint) 2 | if nargin < 6 3 | terminal_constraint = false; 4 | end 5 | 6 | cvx_begin 7 | variable u(N,1) 8 | variable x(2,N) 9 | f = cost(x,u,xt,P,Q,R,N); 10 | minimize f 11 | subject to 12 | for k = 0:N-1 13 | u(k+1) >= -0.5 14 | u(k+1) <= 0.5 15 | % if k < N-1 16 | x(:,k+1) >= [-5;-5] 17 | x(:,k+1) <= [5;5] 18 | if k == 0 19 | x(:,k+1) == double_integrator_dynamics(xt,u(k+1)); 20 | else 21 | x(:,k+1) == double_integrator_dynamics(x(:,k),u(k+1)) 22 | end 23 | % end 24 | end 25 | cvx_end 26 | 27 | if cvx_status == "Infeasible" 28 | uopt = []; 29 | else 30 | uopt = u; 31 | end 32 | 33 | end 34 | 35 | 36 | function f = cost(x,u,xt,P,Q,R,N) 37 | f = 0; 38 | for k = 0:N-1 39 | uk = u(k+1); 40 | if k==0 41 | xk = xt; 42 | else 43 | xk = x(:,k+1); 44 | end 45 | f = f + xk'*Q*xk + uk'*R*uk; 46 | end 47 | 48 | f = f + x(:,end)'*P*x(:,end); 49 | end -------------------------------------------------------------------------------- /polygon_inside_circle.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | % number of points to be placed 4 | N = 3; 5 | 6 | % define the objective function 7 | % fmincon assumes minimization 8 | % We minimize the negative perimeter so as to maximize the perimeter 9 | objective = @(u) -1*perimeter(u); 10 | 11 | % choose which algorithm to use for solving 12 | options = optimoptions('fmincon', 'Algorithm', 'interior-point'); 13 | 14 | % supply an initial guess 15 | % since this is a convex problem, we can use any initial guess 16 | u0 = rand(N,1); 17 | 18 | % solve 19 | uopt = fmincon(objective,u0,... % objective and initial guess 20 | -eye(N),zeros(N,1),... % linear inequality constraints 21 | ones(1,N),2*pi,... % linear equality constraints 22 | [],[],[],... % we do not have lower/upper bounds and nonlinear constraints 23 | options); 24 | 25 | % plot the solution 26 | x = zeros(N,1); 27 | for k = 2:N 28 | x(k) = x(k-1) + uopt(k-1); 29 | end 30 | figure; 31 | % plot a circle 32 | viscircles([0,0],1); 33 | hold on 34 | % scatter the placed points 35 | scatter(cos(x),sin(x),'blue','filled'); 36 | axis equal; 37 | 38 | %% helper functions 39 | % The objective function 40 | function f = perimeter(u) 41 | f = sum(2*sin(u/2)); 42 | end -------------------------------------------------------------------------------- /mpt_examples/double_integrator_feasible_sets.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | % requires the Multi-Parametric Toolbox (MPT) version 3 3 | % https://www.mpt3.org/ 4 | 5 | % create double integrator system 6 | A = [1, 1; 0, 1]; B = [0; 1]; 7 | sys = LTISystem('A',A,'B',B); 8 | 9 | % define state constraint 10 | calX = Polyhedron('A',... 11 | [1,0;0,1;-1,0;0,-1], ... 12 | 'b',[5;5;5;5]); 13 | % define control constraint 14 | calU = Polyhedron('A',[1;-1],'b',[0.5;0.5]); 15 | N = 3; 16 | 17 | %% compute feasible sets 18 | % Xf = calX; 19 | Xf = Polyhedron('A',... 20 | [1,0;0,1;-1,0;0,-1], ... 21 | 'b',[5;5;5;5]); 22 | F = [Xf]; 23 | for i = 1:N 24 | last_X = F(end); 25 | new_X = intersect(calX,... 26 | sys.reachableSet('X',last_X,'U',calU,'N',1,'direction','backwards')); 27 | F = [F;new_X]; 28 | end 29 | colors = [0,0,0;1,0,0;0,1,0;0,0,1]; 30 | figure; F.plot('Alpha',0.4,'ColorMap',colors); 31 | legend('$\mathcal{X}_3$',... 32 | '$\mathcal{X}_2$',... 33 | '$\mathcal{X}_1$',... 34 | '$\mathcal{X}_0$','FontSize',20,'Interpreter','latex'); 35 | ax = gca; ax.FontSize = 16; 36 | xlabel('$x_1$','FontSize',20,'Interpreter','latex'); 37 | ylabel('$x_2$','FontSize',20,'Interpreter','latex'); 38 | 39 | 40 | -------------------------------------------------------------------------------- /mpt_examples/maximal_control_invariant_set.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | % requires the Multi-Parametric Toolbox (MPT) version 3 3 | % https://www.mpt3.org/ 4 | % Installation: https://www.mpt3.org/Main/Installation 5 | % Example 10.3 and 10.4 in BBM book 6 | 7 | % define LTI system 8 | A = [1.5, 0; 1, -1.5]; B = [1; 0]; 9 | sys = LTISystem('A',A,'B',B); 10 | 11 | % define state constraint 12 | calX = Polyhedron('A',... 13 | [1,0;0,1;-1,0;0,-1], ... 14 | 'b',[10;10;10;10]); 15 | % define control constraint 16 | calU = Polyhedron('A',[1;-1],'b',[5;5]); 17 | 18 | Omega = [calX]; 19 | while true 20 | last_Omega = Omega(end); 21 | pre_omega = sys.reachableSet('X',last_Omega,'U',calU,'N',1,... 22 | 'direction','backwards'); 23 | new_Omega = intersect(pre_omega,last_Omega); 24 | Omega = [Omega;new_Omega]; 25 | if new_Omega == last_Omega 26 | fprintf("Converged to maximal control invariant set.\n"); 27 | break; 28 | end 29 | end 30 | figure; Omega.plot('Color','gray','Alpha',0.2); 31 | ax = gca; ax.FontSize = 16; 32 | xlabel('$x_1$','FontSize',20,'Interpreter','latex'); 33 | ylabel('$x_2$','FontSize',20,'Interpreter','latex'); 34 | 35 | %% use MPT function 36 | C = sys.invariantSet('X', calX, 'U', calU); 37 | figure; C.plot(); 38 | 39 | -------------------------------------------------------------------------------- /finite_HJ.m: -------------------------------------------------------------------------------- 1 | %% parameters 2 | dim = 1; 3 | P = eye(dim); 4 | Q = eye(dim); 5 | R = 1; 6 | T = 0.5; 7 | x_max = 8; 8 | N_mesh_x = 1000; 9 | N_mesh_t = 1000; 10 | A = 1; 11 | B = 1; 12 | %% solve it 13 | % generate a mesh 14 | mesh_x = linspace(-1,1,N_mesh_x)*x_max; 15 | mesh_T = linspace(1,0,N_mesh_t)*T; 16 | V = {}; 17 | % figure; 18 | % terminal cost 19 | V{end+1} = mesh_x.^2*P; 20 | for i = 1:N_mesh_t-1 21 | V_cur = V{end}; 22 | % gradient on x 23 | dV = zeros(1,N_mesh_x); 24 | dV(1) = (V_cur(2)-V_cur(1))/(mesh_x(2)-mesh_x(1)); 25 | dV(end) = (V_cur(end)-V_cur(end-1))/(mesh_x(end)-mesh_x(end-1)); 26 | for j = 2:N_mesh_x-1 27 | dV(j) = (V_cur(j+1)-V_cur(j-1))/(mesh_x(j+1)-mesh_x(j-1)); 28 | end 29 | mesh_u = -B/2/R*dV; 30 | f = A*mesh_x + B*mesh_u; 31 | %gradient on t 32 | dV_t = -(mesh_x.^2*Q+mesh_u.^2*R+dV.*f); 33 | %simply Euler forward 34 | V_next = V_cur + dV_t*(mesh_T(i+1)-mesh_T(i)); 35 | V{end+1} = V_next; 36 | % plot(mesh_x,V_next); 37 | % hold on 38 | end 39 | [X,Y] = meshgrid(mesh_x,mesh_T); 40 | Z = zeros(N_mesh_t,N_mesh_x); 41 | 42 | for i = 1:N_mesh_t 43 | Z(i,:) = V{i}; 44 | end 45 | figure; 46 | ax = gca; ax.FontSize = 20; 47 | surf(X,Y,Z); 48 | shading interp 49 | xlabel('$x$','FontSize',24,'Interpreter','latex'); 50 | ylabel('$t$','FontSize',24,'Interpreter','latex'); 51 | zlabel('$V(t,x)$','FontSize',24,'Interpreter','latex'); 52 | ax = gca; ax.FontSize = 20; 53 | %V_t = -(x^TQx + u^TRu + V_x^Tf(x,u)) 54 | %f(x,u) = Ax+Bu; 55 | %% useful function 56 | -------------------------------------------------------------------------------- /double_integrator_mpc/double_integrator_mpc.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | cvxpath = "../../cvx"; 4 | addpath(genpath(cvxpath)); 5 | 6 | 7 | P = eye(2); Q = eye(2); R = 10; N = 3; 8 | x1 = [-4.5;2]; 9 | x2 = [-4.5;3]; 10 | 11 | [u1_traj, x1_traj] = double_integrator_rhc(x1,P,Q,R,N); 12 | [u2_traj, x2_traj] = double_integrator_rhc(x2,P,Q,R,N); 13 | 14 | %% plot the results 15 | figure; 16 | scatter(x1_traj(1,:),x1_traj(2,:),100,'blue','filled'); 17 | hold on 18 | plot(x1_traj(1,:),x1_traj(2,:),'blue','LineWidth',2); 19 | 20 | scatter(x2_traj(1,:),x2_traj(2,:),100,'red','filled'); 21 | hold on 22 | plot(x2_traj(1,:),x2_traj(2,:),'red','LineWidth',2); 23 | grid on 24 | xlabel('$x_1$','FontSize',24,'Interpreter','latex'); 25 | ylabel('$x_2$','FontSize',24,'Interpreter','latex'); 26 | ax = gca; ax.FontSize = 20; 27 | 28 | 29 | %% Receding horizon control 30 | function [u_traj,x_traj] = double_integrator_rhc(x,P,Q,R,N) 31 | x_traj = x; 32 | u_traj = []; 33 | while true 34 | % solve open-loop convex optimization 35 | uopt = double_integrator_ocp(x,N,P,Q,R); 36 | % check feasibility 37 | if isempty(uopt) 38 | fprintf("Subproblem infeasible.\n") 39 | break; 40 | end 41 | u = uopt(1); 42 | u_traj = [u_traj;u]; 43 | 44 | % go to next step 45 | x = double_integrator_dynamics(x,u); 46 | x_traj = [x_traj,x]; 47 | 48 | % check convergence 49 | dist_to_origin = norm(x); 50 | if dist_to_origin < 1e-6 51 | fprintf("Current state distance to origin is %3.2e < %3.2e.\n",dist_to_origin); 52 | break 53 | end 54 | end 55 | 56 | end 57 | 58 | -------------------------------------------------------------------------------- /pendulum_stabilization_lqr.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | m = 1; g = 9.8; l = 1; b = 0.1; 4 | 5 | Ac = [ 6 | 0, 1; 7 | g/l, -b / (m*l^2) 8 | ]; 9 | 10 | Bc = [0; 1/(m*l^2)]; 11 | 12 | dt = 0.01; 13 | 14 | A = dt*Ac + eye(2); 15 | B = dt*Bc; 16 | 17 | Q = eye(2); 18 | R = 1; 19 | 20 | [K,~,~] = dlqr(A,B,Q,R,zeros(2,1)); 21 | 22 | z0 = [0.1;0.1]; 23 | num_steps = 1000; 24 | 25 | z_traj = zeros(2,num_steps+1); 26 | z_traj(:,1) = z0; 27 | u_traj = zeros(1,num_steps); 28 | 29 | z = z0; 30 | for i = 1:num_steps 31 | ui = -K*z; 32 | % ui = clip(-K*z,-5,5); 33 | zdoti = pendulum_f_z(z,ui,m,g,l,b); 34 | z = zdoti * dt + z; 35 | 36 | u_traj(:,i) = ui; 37 | z_traj(:,i+1) = z; 38 | end 39 | 40 | %% plot comparison 41 | labelsize = 20; 42 | 43 | t_traj = (1:(num_steps)) * dt; 44 | figure; 45 | tiledlayout(3,1) 46 | nexttile 47 | plot(t_traj,z_traj(1,1:end-1),'LineWidth',2) 48 | ylabel('$z_1$','Interpreter','latex','FontSize',labelsize) 49 | xlabel('time','FontSize',labelsize) 50 | grid on 51 | ax = gca; 52 | ax.FontSize = 16; 53 | 54 | nexttile 55 | plot(t_traj,z_traj(2,1:end-1),'LineWidth',2) 56 | ylabel('$z_2$','Interpreter','latex','FontSize',labelsize) 57 | xlabel('time','FontSize',labelsize) 58 | grid on 59 | ax = gca; 60 | ax.FontSize = 16; 61 | 62 | nexttile 63 | plot(t_traj,u_traj,'LineWidth',2) 64 | ylabel('$u$','Interpreter','latex','FontSize',labelsize) 65 | xlabel('time','FontSize',labelsize) 66 | grid on 67 | ax = gca; 68 | ax.FontSize = 16; 69 | 70 | 71 | 72 | function zdot = pendulum_f_z(z,u,m,g,l,b) 73 | z1 = z(1); 74 | z2 = z(2); 75 | 76 | zdot = [ 77 | z2; 78 | 1/(m*l^2)*(u - b*z2 + m*g*l*sin(z1)) 79 | ]; 80 | end 81 | 82 | function u_clip = clip(u,umin,umax) 83 | u_clip = min(umax,max(u,umin)); 84 | end -------------------------------------------------------------------------------- /pendulum_mpc/pendulum_open_loop.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | % pendulum parameters 4 | m = 1; l = 1; g = 9.8; b = 0.1; umax = g/2; 5 | 6 | initial_state = [0;0]; 7 | N = 101; 8 | h = 0.1; 9 | 10 | % trajectory optimization 11 | [uopt,xopt] = PendulumTrajOpt(N,h,initial_state,m,l,g,b,umax,[],[]); 12 | 13 | %% plot optimized plans 14 | T = (N-1)*h; 15 | t_grid = linspace(0,1,N)*T; 16 | 17 | figure; 18 | tiledlayout(2,1) 19 | nexttile 20 | scatter(t_grid,uopt,100,'filled'); hold on; 21 | plot(t_grid,uopt,'LineWidth',2); 22 | xlabel('$t$','FontSize',24,'Interpreter','latex'); 23 | ylabel('$u(t)$','FontSize',24,'Interpreter','latex'); 24 | ax = gca; ax.FontSize = 20; 25 | grid on; 26 | 27 | nexttile 28 | scatter(t_grid,xopt,100,[0,0,1;1,0,0],'filled'); hold on; 29 | plot(t_grid,xopt,'LineWidth',2); 30 | xlabel('$t$','FontSize',24,'Interpreter','latex'); 31 | ylabel('$x(t)$','FontSize',24,'Interpreter','latex'); 32 | grid on; 33 | legend('$\theta(t)$','$\dot{\theta}(t)$','FontSize',20,'Interpreter','latex'); 34 | ax = gca; ax.FontSize = 20; 35 | 36 | %% deploy the controls 37 | x = initial_state; 38 | state_trajectory = []; 39 | time_trajectory = []; 40 | noise = 0.5; 41 | for k = 1:N-1 42 | uk = uopt(k); 43 | [t,sol] = ode89(@(t,y) pendulum_ode(t,y,[uk;uk],[0;h],m,l,g,b,noise),[0,h],x); 44 | sol = sol'; 45 | x = sol(:,end); 46 | % save trajectory 47 | state_trajectory = [state_trajectory,sol]; 48 | time_trajectory = [time_trajectory,(k-1)*h+t']; 49 | end 50 | 51 | figure; 52 | tiledlayout(1,1) 53 | nexttile 54 | plot(time_trajectory,state_trajectory,'LineWidth',2); 55 | xlabel('$t$','FontSize',24,'Interpreter','latex'); 56 | ylabel('$x(t)$','FontSize',24,'Interpreter','latex'); 57 | grid on; 58 | legend('$\theta(t)$','$\dot{\theta}(t)$','FontSize',20,'Interpreter','latex'); 59 | ax = gca; ax.FontSize = 20; 60 | 61 | 62 | -------------------------------------------------------------------------------- /RK4_example.m: -------------------------------------------------------------------------------- 1 | % time step and num of step 2 | clc; clear; close all 3 | h = 0.01; 4 | N = 10000; 5 | % initial x_0 6 | x_ini = [1,0,0]; 7 | 8 | %% for one turn 9 | % u_0...u_{k-1} 10 | u0 = 0; 11 | u_traj_fix = randn(1,N-1); 12 | u_traj = [u0,u_traj_fix]; 13 | x_traj = RK4(@(x,u)f_example(x,u),u_traj,h,N,x_ini); 14 | figure; 15 | plot(0:h:N*h,x_traj(:,1)','g',0:h:N*h,x_traj(:,2)','b'); 16 | legend('x_1','x_2'); 17 | %% plot x_N as a function of u_0 18 | M = 100; 19 | x_N_traj = zeros(M,3); 20 | epi = 1; 21 | u_traj_fix = randn(1,N-1); 22 | for i = 1:M 23 | u0 = i*epi; 24 | u_traj = u0*ones(N,1); 25 | %x_traj = RK4(@(x,u)f_example(x,u),u_traj,h,N,x_ini); 26 | x_traj = RK4(@(x,u)f_example_2(x,u),u_traj,h,N,x_ini); 27 | x_N_traj(i,:) = x_traj(N+1,:); 28 | end 29 | figure; 30 | plot((epi:epi:epi*M),x_N_traj(:,1)','g',... 31 | (epi:epi:epi*M),x_N_traj(:,2)','b',... 32 | (epi:epi:epi*M),x_N_traj(:,3)','r',... 33 | 'LineWidth',2); 34 | xlabel('$u$','FontSize',20,'Interpreter','latex') 35 | ylabel('$x$','FontSize',20,'Interpreter','latex') 36 | legend('$x_1$','$x_2$','$x_3$','FontSize',20,'Interpreter','latex'); 37 | ax = gca; 38 | ax.FontSize = 16; 39 | grid on 40 | %% help function 41 | function y = RK4(f,u,h,N,xini) 42 | x = xini'; 43 | y = zeros(N+1,size(x,1)); 44 | y(1,:) = x'; 45 | for i = 1:N 46 | k1 = f(x,u(i)); 47 | k2 = f(x+h*k1/2,u(i)); 48 | k3 = f(x+h*k2/2,u(i)); 49 | k4 = f(x+h*k3,u(i)); 50 | x = x + (k1+2*k2+2*k3+k4)/6*h; 51 | y(i+1,:) = x'; 52 | end 53 | end 54 | 55 | function y = f_example(x,u) 56 | y = zeros(2,1); 57 | y(1) = (1-x(2)^2)*x(1)-x(2)+u; 58 | y(2) = x(1); 59 | end 60 | 61 | function y = f_example_2(x,u) 62 | y = zeros(3,1); 63 | y(1) = 10*(x(2)-x(1)); 64 | y(2) = x(1)*(u-x(3))-x(2); 65 | y(3) = x(1)*x(2)-3*x(3); 66 | end -------------------------------------------------------------------------------- /pendulum_mpc/pendulum_mpc.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | % pendulum parameters 4 | m = 1; l = 1; g = 9.8; b = 0.1; umax = g/2; 5 | 6 | % system noise 7 | noise = 1.0; 8 | 9 | initial_state = [0;0]; 10 | Ts = 0.1; % solve trajectory optimization every Ts time 11 | N = 51; % fixed horizon for the trajectory optimization problem 12 | h = 0.1; % time interval for trajectory optimization 13 | num_steps = 100; 14 | 15 | xi = initial_state; 16 | 17 | state_trajectory = []; 18 | time_trajectory = []; 19 | control_trajory = []; 20 | uopt = randn(N,1); 21 | xopt = randn(2,N); 22 | for i = 1:num_steps 23 | [uopt,xopt] = PendulumTrajOpt(N,h,xi,m,l,g,b,umax,uopt,xopt); 24 | 25 | % only choose the first control 26 | ui = uopt(1); 27 | 28 | % apply the first control to the plant for Ts duration 29 | [t,sol] = ode89(@(t,y) pendulum_ode(t,y,[ui;ui],[0;Ts],m,l,g,b,noise),[0,Ts],xi); 30 | sol = sol'; 31 | 32 | % start from the next state 33 | xi = sol(:,end); 34 | 35 | % save trajectory 36 | state_trajectory = [state_trajectory,sol]; 37 | time_trajectory = [time_trajectory,(i-1)*Ts+t']; 38 | control_trajory = [control_trajory,ui]; 39 | end 40 | 41 | %% plot trajectory 42 | figure; 43 | tiledlayout(2,1) 44 | nexttile 45 | t_grid = 0:Ts:((num_steps-1)*Ts); 46 | u_grid = control_trajory; 47 | control_trajory_tt = interp1(t_grid,u_grid,time_trajectory,'previous'); 48 | plot(time_trajectory,control_trajory_tt,'LineWidth',2); 49 | xlabel('$t$','FontSize',24,'Interpreter','latex'); 50 | ylabel('$u(t)$','FontSize',24,'Interpreter','latex'); 51 | ax = gca; ax.FontSize = 20; 52 | grid on; 53 | 54 | nexttile 55 | plot(time_trajectory,state_trajectory,'LineWidth',2); 56 | xlabel('$t$','FontSize',24,'Interpreter','latex'); 57 | ylabel('$x(t)$','FontSize',24,'Interpreter','latex'); 58 | legend('$\theta(t)$','$\dot{\theta}(t)$','FontSize',20,'Interpreter','latex') 59 | ax = gca; ax.FontSize = 20; 60 | grid on; 61 | -------------------------------------------------------------------------------- /fire_canon_ball_fmincon.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | g = 9.8; 4 | x0 = [0;0;10]; % initial guess of the solution 5 | 6 | % define objective function 7 | obj = @(x) objective(x); 8 | 9 | % define nonlinear constraints 10 | nonlincon = @(x) nonlinear_con(x,g); 11 | 12 | % define options for fmincon 13 | options = optimoptions('fmincon','Algorithm','interior-point',... 14 | 'SpecifyObjectiveGradient',true,'SpecifyConstraintGradient',true,... 15 | 'checkGradients',false); 16 | 17 | % call fmincon 18 | [xopt,fopt,~,out] = fmincon(obj,x0,... % objective and initial guess 19 | -eye(3),zeros(3,1),... % linear inequality constraints 20 | [],[],... % no linear equality constraints 21 | [],[],... % no upper and lower bounds 22 | nonlincon,... % nonlinear constraints 23 | options); 24 | 25 | fprintf("Maximum constraint violation: %3.2f.\n",out.constrviolation); 26 | fprintf("Objective: %3.2f.\n",fopt); 27 | 28 | % plot the solution 29 | T = xopt(3); 30 | t = 0:0.01:T; 31 | xt = xopt(1)*t; 32 | yt = xopt(2)*t - 0.5*g*(t.^2); 33 | figure; 34 | plot(xt,yt,'LineWidth',2); 35 | hold on 36 | scatter(10,10,100,"red",'filled','diamond'); 37 | axis equal; grid on; 38 | xlabel('$x_1(t)$','FontSize',24,'Interpreter','latex'); 39 | ylabel('$x_2(t)$','FontSize',24,'Interpreter','latex'); 40 | ax = gca; ax.FontSize = 20; 41 | 42 | 43 | %% helper functions 44 | function [f,df] = objective(x) 45 | % x is our decision variable: [v1, v2, T] 46 | 47 | % objective function 48 | f = 0.5 * (x(1)^2 + x(2)^2); 49 | 50 | % gradient of the objective function (optional) 51 | df = [x(1); x(2); 0]; % column vector 52 | end 53 | 54 | function [c,ceq,dc,dceq] = nonlinear_con(x,g) 55 | % no inequality constraints 56 | c = []; dc = []; 57 | 58 | % two equality constraints 59 | ceq = [x(1)*x(3)-10; 60 | x(2)*x(3)-0.5*g*x(3)^2-10]; 61 | 62 | % explicit gradient of ceq 63 | dceq = [x(3), 0; 64 | 0, x(3); 65 | x(1), x(2)-g*x(3)]; 66 | 67 | end 68 | -------------------------------------------------------------------------------- /double_integrator_collocation_sdp.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | spotpath = '../spotless'; 4 | sospath = '../SOSprograms'; 5 | mosekpath= '/Users/hengyang/mosek'; 6 | 7 | addpath(genpath(spotpath)); 8 | addpath(genpath(sospath)); 9 | addpath(genpath(mosekpath)); 10 | 11 | initial_state = [-10;0]; 12 | final_state = [0;0]; 13 | 14 | N = 6; 15 | d = 2*(N-2) + N + 1; 16 | v = msspoly('v',d); 17 | h = v(1); % time interval 18 | u = v(2:1+N); % unknown controls 19 | x = reshape(v(N+2:end),2,N-2); % unknown states 20 | 21 | x_full = [initial_state,x,final_state]; 22 | 23 | problem.vars = v; 24 | problem.objective = (N-1)*h; 25 | eqs = []; 26 | for k=1:N-1 27 | uk = u(k); 28 | ukp1 = u(k+1); 29 | xk = x_full(:,k); 30 | xkp1 = x_full(:,k+1); 31 | fk = double_integrator(xk,uk); 32 | fkp1 = double_integrator(xkp1,ukp1); 33 | 34 | % collocation points 35 | xkc = 0.5*(xk+xkp1) + h/8 * (fk - fkp1); 36 | ukc = 0.5*(uk + ukp1); 37 | % dxkc = -3/(2*h) * (xk-xkp1) - 0.25*(fk + fkp1); 38 | dxkc_h = -3/2 * (xk-xkp1) - 0.25*h*(fk + fkp1); 39 | 40 | % collocation constraint 41 | eqs = [eqs; 42 | dxkc_h - h*double_integrator(xkc,ukc)]; 43 | end 44 | problem.equality = eqs; 45 | 46 | ineqs = [h;100*(N+1)+N - v'*v]; 47 | for k=1:N 48 | ineqs = [ineqs; 49 | 1 - u(k)^2]; 50 | end 51 | 52 | problem.inequality = ineqs; 53 | kappa = 2; % relaxation order 54 | [SDP,info] = dense_sdp_relax(problem,kappa); 55 | 56 | prob = convert_sedumi2mosek(SDP.sedumi.At,... 57 | SDP.sedumi.b,... 58 | SDP.sedumi.c,... 59 | SDP.sedumi.K); 60 | [~,res] = mosekopt('minimize info',prob); 61 | [Xopt,yopt,Sopt,obj] = recover_mosek_sol_blk(res,SDP.blk); 62 | 63 | figure; bar(eig(Xopt{1})); 64 | 65 | 66 | %% helper functions 67 | function dx = double_integrator(x,u) 68 | A = [0 1; 0 0]; 69 | B = [0; 1]; 70 | dx = A * x + B * u; 71 | end -------------------------------------------------------------------------------- /pendulum_sos_observer.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | sostoolspath = "../SOSTOOLS"; 4 | mosekpath = "../../../mosek"; 5 | addpath(genpath(sostoolspath)) 6 | addpath(genpath(mosekpath)) 7 | 8 | m = 1; g = 9.8; l = 1; b = 0.1; 9 | 10 | gamma = 0.2; % desired exponential rate 11 | deg_V = 2; % user-chosen degree of V 12 | deg_K = 2; % user-chosen degree of K 13 | kappa_plus = 0; % if >0 then go above the minimum relaxation order 14 | 15 | % compute other degrees 16 | deg_Q = deg_V - 2; 17 | deg_M = deg_Q + deg_K; 18 | 19 | e = mpvar('e',[3,1]); 20 | x = mpvar('x',[3,1]); 21 | C = [1, 0, 0; 0, 1, 0]; 22 | Ce = C*e; 23 | Cx = C*x; 24 | h = x(1)^2 + x(2)^2 - 1; 25 | deg_h = h.maxdeg; 26 | eps = 0.1; 27 | 28 | delta_f = pendulum_f(x+e,m,b,l) - pendulum_f(x,m,b,l); 29 | deg_delta_f = delta_f.maxdeg; 30 | 31 | max_deg = max([deg_V - 1 + deg_delta_f,2+deg_M,deg_h]); 32 | % minimum relaxation order 33 | kappa = ceil(max_deg / 2) + kappa_plus; 34 | 35 | fprintf("deg_V: %d, deg_K: %d, kappa: %d.\n",deg_V,deg_K,kappa); 36 | 37 | prog = sosprogram([e;x]); 38 | 39 | [prog,V] = sossosvar(prog,monomials(e,0:deg_V)); 40 | [prog,Q] = sospolymatrixvar(prog,monomials(Ce,0:deg_Q),[3,3],'symmetric'); 41 | [prog,M] = sospolymatrixvar(prog,monomials([Ce;Cx],0:deg_M),[3,2]); 42 | 43 | [prog,sig0] = sossosvar(prog,monomials([e;x],0:kappa)); 44 | [prog,lam1] = sospolyvar(prog,monomials([e;x],0:(2*kappa-deg_h))); 45 | 46 | eq = e'*(Q+eps*eye(3))*delta_f + e'*M*(Ce) + ... 47 | sig0 + lam1 * h + gamma * V; 48 | 49 | prog = soseq(prog,eq); 50 | prog = sosmatrixineq(prog,Q); 51 | prog = soseq(prog,diff(V,e)-e'*(Q+eps*eye(3))); 52 | prog = soseq(prog,subs(V,e,zeros(3,1))); 53 | 54 | options.solver = 'mosek'; 55 | prog = sossolve(prog,options); 56 | 57 | threshold = 1e-6; 58 | sol.V = cleanpoly(sosgetsol(prog,V),threshold); 59 | sol.Q = cleanpoly(sosgetsol(prog,Q),threshold); 60 | sol.M = cleanpoly(sosgetsol(prog,M),threshold); 61 | sol.eps = eps; 62 | 63 | save('SOS-sols/pendulum_sol.mat', 'sol') 64 | 65 | %% helper function 66 | function f = pendulum_f(x,m,b,l) 67 | const = b / (m * l^2); 68 | f = [x(2)*x(3); 69 | -x(1)*x(3); 70 | -const*x(3) 71 | ]; 72 | end 73 | 74 | -------------------------------------------------------------------------------- /pendulum_stabilization_lqg.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | m = 1; g = 9.8; l = 1; b = 0.1; 4 | 5 | Ac = [ 6 | 0, 1; 7 | g/l, -b / (m*l^2) 8 | ]; 9 | 10 | Bc = [0; 1/(m*l^2)]; 11 | 12 | dt = 0.01; 13 | 14 | A = dt*Ac + eye(2); 15 | B = dt*Bc; 16 | 17 | Q = diag([1,1]); 18 | R = 1; 19 | C = [1, 0]; 20 | sigma = 0.1; 21 | M = diag([0,sigma^2]); 22 | N = sigma^2; 23 | 24 | [K,~,~] = dlqr(A,B,Q,R,zeros(2,1)); 25 | [L,SIG,~] = dlqr(A,C',M,N,zeros(2,1)); 26 | L = L'; 27 | % L = SIG*C'*inv(C*SIG*C'+N); 28 | 29 | num_steps = 1000; 30 | x = [2;-2]; 31 | xhat = [0;0]; 32 | 33 | xhat_traj = zeros(2,num_steps+1); 34 | x_traj = zeros(2,num_steps+1); 35 | x_traj(:,1) = x; 36 | xhat_traj(:,1) = xhat; 37 | u_traj = zeros(1,num_steps); 38 | 39 | for k = 1:num_steps 40 | % design control 41 | uk = -K*xhat; 42 | u_traj(k) = uk; 43 | 44 | % update true state 45 | % x = A*x + B*uk + [0; sigma*randn]; 46 | xdot = pendulum_f_z(x,uk,m,g,l,b); 47 | x = xdot * dt + x + [0; sigma*randn]; 48 | x_traj(:,k+1) = x; 49 | 50 | % generate measurement 51 | y = C*x + sigma*randn; 52 | 53 | % update xhat 54 | xhat = A*xhat + B*uk + L*(y - C*(A*xhat + B*uk)); 55 | xhat_traj(:,k+1) = xhat; 56 | end 57 | 58 | %% plot comparison 59 | labelsize = 20; 60 | 61 | t_traj = (1:(num_steps)) * dt; 62 | figure; 63 | tiledlayout(2,1) 64 | nexttile 65 | plot(t_traj,x_traj(1,1:end-1),'LineWidth',2) 66 | hold on 67 | plot(t_traj,xhat_traj(1,1:end-1),'red','LineWidth',2) 68 | ylabel('$x_1,\hat{x}_1$','Interpreter','latex','FontSize',labelsize) 69 | xlabel('time','FontSize',labelsize) 70 | legend('$x_1$','$\hat{x}_1$','Interpreter','latex','FontSize',labelsize) 71 | grid on 72 | ax = gca; 73 | ax.FontSize = 16; 74 | 75 | nexttile 76 | plot(t_traj,x_traj(2,1:end-1),'LineWidth',2) 77 | hold on 78 | plot(t_traj,xhat_traj(2,1:end-1),'red','LineWidth',2) 79 | ylabel('$x_2,\hat{x}_2$','Interpreter','latex','FontSize',labelsize) 80 | xlabel('time','FontSize',labelsize) 81 | legend('$x_2$','$\hat{x}_2$','Interpreter','latex','FontSize',labelsize) 82 | grid on 83 | ax = gca; 84 | ax.FontSize = 16; 85 | 86 | 87 | 88 | function zdot = pendulum_f_z(z,u,m,g,l,b) 89 | z1 = z(1); 90 | z2 = z(2); 91 | 92 | zdot = [ 93 | z2; 94 | 1/(m*l^2)*(u - b*z2 + m*g*l*sin(z1)) 95 | ]; 96 | end -------------------------------------------------------------------------------- /robot_localization_particle_filter.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | N = 20; % number of landmarks 4 | landmarks = randn(2,N); % random landmarks 5 | sig_w = 0.01; 6 | sig_v = 0.01; 7 | M = 1000; % number of samples 8 | dt = 0.1; % sampling time 9 | 10 | 11 | s0 = [0,0,0]; % initial state of the vehicle 12 | calX0 = mvnrnd(ones(3,1),0.1*eye(3),M)'; % initial set of particles 13 | 14 | % number of steps 15 | num_steps = 200; 16 | 17 | s = s0; 18 | calX = calX0; 19 | s_traj = zeros(3,num_steps); 20 | s_traj(:,1) = s0; 21 | calX_traj = zeros(3,M,num_steps); 22 | calX_traj(:,:,1) = calX0; 23 | alpha = zeros(M,1); 24 | 25 | figure; 26 | 27 | for i = 1:num_steps 28 | % generate true next state 29 | a = randn(2,1); 30 | s = vehicle(s,a,dt) + sig_w * randn(3,1); 31 | % generate measurement 32 | o = measure(s,landmarks); 33 | 34 | %% particle filter 35 | % propagate particles 36 | calX_p = zeros(3,M); 37 | for j = 1:M 38 | calX_p(:,j) = vehicle(calX(:,j),a,dt) + sig_w * randn(3,1); 39 | end 40 | % compute measurement errors 41 | errs = zeros(N,M); 42 | for j = 1:M 43 | errs(:,j) = o - measure(calX_p(:,j),landmarks); 44 | end 45 | % compute weights 46 | alpha = exp(-0.5*sum(errs.^2,1)); 47 | alpha = alpha / (sum(alpha)); alpha = alpha(:); 48 | % importance sampling 49 | ids = randsample(M,M,true,alpha); 50 | calX = calX_p(:,ids); 51 | 52 | clf; 53 | scatter(landmarks(1,:),landmarks(2,:),150,"black",'filled','square'); 54 | hold on 55 | scatter(calX(1,:),calX(2,:),30,'blue','filled','o'); 56 | scatter(s(1),s(2),150,"red",'filled','diamond'); 57 | xlim([-2,2]) 58 | ylim([-2,2]) 59 | axis equal 60 | mytitle = sprintf("Particle Filter: Step %d",i); 61 | title(mytitle,'FontSize',22) 62 | pause(0.1) 63 | 64 | end 65 | 66 | 67 | 68 | %% helper functions 69 | function snew = vehicle(s,a,dt) 70 | x = s(1); y = s(2); theta = s(3); 71 | l = a(1); u = a(2); 72 | xnew = x + l*cos(theta)*dt; 73 | ynew = y + l*sin(theta)*dt; 74 | thetanew = theta + u*dt; 75 | snew = [xnew; ynew; thetanew]; 76 | end 77 | 78 | function o = measure(s,landmarks) 79 | N = size(landmarks,2); 80 | o = zeros(N,1); 81 | for i = 1:N 82 | o(i) = norm(landmarks(:,i) - s(1:2)); 83 | end 84 | end -------------------------------------------------------------------------------- /LQR_FVI_Example.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | %% system parameters 4 | h = 0.01; 5 | A = [1, h; 0, 1]; 6 | B = [0; h]; 7 | Q = 0.1*eye(2); 8 | R = 1; 9 | 10 | %% groundtruth S matrix from LQR 11 | [~,S_lqr,~] = dlqr(A,B,Q,R,zeros(2,1)); 12 | 13 | %% Fitted value iteration 14 | num_samples = 2; 15 | num_iterations = 1e4; 16 | th = 1e-8; 17 | S_vec = randn(3,1); 18 | S_traj = S_vec; 19 | S_mat = vec2mat(S_vec); 20 | iter = 1; 21 | 22 | while iter < num_iterations 23 | X = []; % Feature matrix 24 | Y = []; % Target cost vector 25 | 26 | for i = 1:num_samples 27 | x = randn(2,1); 28 | 29 | % solve u 30 | u = - ( (1 + B'*S_mat*B) \ B'*S_mat*A*x ); 31 | 32 | % compute beta 33 | beta = running_cost(x,u,Q,R) + (A*x+B*u)'*S_mat*(A*x+B*u); 34 | 35 | % Store data 36 | X = [X; [x(1)^2, 2*x(1)*x(2), x(2)^2]]; 37 | Y = [Y; beta]; 38 | end 39 | 40 | % Update weights 41 | S_vec_new = X \ Y; 42 | 43 | % Convergence check 44 | if norm(S_vec_new - S_vec) < th 45 | fprintf("FVI converged in %d iterations.\n",iter); 46 | break; 47 | end 48 | 49 | S_vec = S_vec_new; 50 | S_mat = vec2mat(S_vec); 51 | S_traj = [S_traj,S_vec]; 52 | disp(S_vec); 53 | 54 | iter = iter + 1; 55 | end 56 | 57 | S_fvi = S_mat; 58 | 59 | %% plot 60 | labelsize = 20; 61 | figure; 62 | tiledlayout(3,1) 63 | nexttile 64 | plot(S_traj(1,:),'LineWidth',2) 65 | ylabel('$S_1$','Interpreter','latex','FontSize',labelsize) 66 | xlabel('Iteration','FontSize',labelsize) 67 | grid on 68 | ax = gca; 69 | ax.FontSize = 16; 70 | 71 | nexttile 72 | plot(S_traj(2,:),'LineWidth',2) 73 | ylabel('$S_2$','Interpreter','latex','FontSize',labelsize) 74 | xlabel('Iteration','FontSize',labelsize) 75 | grid on 76 | ax = gca; 77 | ax.FontSize = 16; 78 | 79 | nexttile 80 | plot(S_traj(3,:),'LineWidth',2) 81 | ylabel('$S_3$','Interpreter','latex','FontSize',labelsize) 82 | xlabel('Iteration','FontSize',labelsize) 83 | grid on 84 | ax = gca; 85 | ax.FontSize = 16; 86 | 87 | 88 | %% helper functions 89 | function g = running_cost(x,u,Q,R) 90 | g = x'*Q*x + u'*R*u; 91 | end 92 | 93 | function S_mat = vec2mat(S_vec) 94 | S_mat = [S_vec(1), S_vec(2); 95 | S_vec(2), S_vec(3)]; 96 | end 97 | -------------------------------------------------------------------------------- /pendulum_mpc/PendulumTrajOpt.m: -------------------------------------------------------------------------------- 1 | function [uopt,xopt] = PendulumTrajOpt(N,h,initial_state,m,l,g,b,umax,u_guess,x_guess) 2 | if isempty(x_guess) 3 | x0 = randn(2,N); 4 | else 5 | x0 = x_guess; 6 | end 7 | 8 | if isempty(u_guess) 9 | u0 = randn(N,1); 10 | else 11 | u0 = u_guess; 12 | end 13 | 14 | v0 = [x0(:); u0(:)]; 15 | lb = [-Inf*ones(2*N,1); 16 | -umax*ones(N,1)]; 17 | ub = [Inf*ones(2*N,1); 18 | umax*ones(N,1)]; 19 | 20 | obj = @(v) objective(v,N,h); 21 | nonlincon = @(v) collocation(v,N,h,initial_state,m,l,g,b); 22 | 23 | options = optimoptions('fmincon','Algorithm','interior-point',... 24 | 'display','iter','MaxFunctionEvaluations',1e6,'MaxIterations',1e4); 25 | 26 | [vopt,fopt,~,out] = fmincon(obj,v0,... 27 | [],[],[],[],... % no linear (in)equality constraints 28 | lb,ub,... 29 | nonlincon,options); 30 | 31 | fprintf("Maximum constraint violation: %3.2f.\n",out.constrviolation); 32 | fprintf("Objective: %3.2f.\n",fopt); 33 | 34 | uopt = vopt(2*N+1:end); 35 | xopt = vopt(1:2*N); 36 | xopt = reshape(xopt,2,N); 37 | 38 | end 39 | 40 | 41 | %% helper functions 42 | function f = objective(v,N,h) 43 | x = v(1:2*N); x = reshape(x,2,N); 44 | theta = x(1,:); 45 | thetadot = x(2,:); 46 | u = v(2*N+1:end); 47 | 48 | tmp = [cos(theta); 49 | sin(theta); 50 | thetadot]; 51 | 52 | x_cost = sum((tmp - [-1;0;0]).^2,1); 53 | u_cost = u.^2; 54 | f = sum(x_cost(:) + u_cost(:)) * h; 55 | end 56 | 57 | 58 | function dx = pendulum(x,u,m,l,g,b) 59 | dx = [x(2); 60 | -1/(m*l^2) * (-u + b*x(2)+m*g*l*sin(x(1))) ]; 61 | end 62 | 63 | function [c,ceq] = collocation(v,N,h,initial_state,m,l,g,b) 64 | x = reshape(v(1:2*N),2,N); 65 | u = v(2*N+1:end); 66 | 67 | c = []; 68 | ceq = []; 69 | 70 | for k=1:N-1 71 | uk = u(k); 72 | ukp1 = u(k+1); 73 | xk = x(:,k); 74 | xkp1 = x(:,k+1); 75 | fk = pendulum(xk,uk,m,l,g,b); 76 | fkp1 = pendulum(xkp1,ukp1,m,l,g,b); 77 | 78 | % collocation points 79 | xkc = 0.5*(xk+xkp1) + h/8 * (fk - fkp1); 80 | ukc = 0.5*(uk + ukp1); 81 | dxkc = -3/(2*h) * (xk-xkp1) - 0.25*(fk + fkp1); 82 | 83 | % collocation constraint 84 | ceq = [ceq; 85 | dxkc - pendulum(xkc,ukc,m,l,g,b)]; 86 | end 87 | 88 | ceq = [ceq; 89 | x(:,1) - initial_state; 90 | x(:,end) - [pi;0]]; 91 | end 92 | 93 | 94 | -------------------------------------------------------------------------------- /quadrotor2d_sos_observer.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | sostoolspath = "../SOSTOOLS"; 4 | mosekpath = "../../../mosek"; 5 | addpath(genpath(sostoolspath)) 6 | addpath(genpath(mosekpath)) 7 | 8 | % Model parameters 9 | m = 1; l = 1; r = l/2; I = m*l^2/12; 10 | ns = 7; ny = 4; nu = 2; 11 | 12 | gamma = 0.0000001; % desired exponential rate 13 | deg_V = 2; % user-chosen degree of V 14 | deg_K = 2; % user-chosen degree of K 15 | kappa_plus = 0; % if >0 then go above the minimum relaxation order 16 | 17 | % compute other degrees 18 | deg_Q = deg_V - 2; 19 | deg_M = deg_Q + deg_K; 20 | 21 | e = mpvar('e',[ns,1]); 22 | x = mpvar('x',[ns,1]); 23 | C = [ 24 | 1, 0, 0, 0, 0, 0, 0; 25 | 0, 0, 1, 0, 0, 0, 0; 26 | 0, 0, 0, 0, 1, 0, 0; 27 | 0, 0, 0, 0, 0, 1, 0; 28 | ]; 29 | Ce = C*e; 30 | Cx = C*x; 31 | h = x(5)^2 + x(6)^2 - 1; 32 | deg_h = h.maxdeg; 33 | eps = 0.1; 34 | 35 | delta_f = quadrotor2d_f(x+e) - quadrotor2d_f(x); 36 | deg_delta_f = delta_f.maxdeg; 37 | 38 | max_deg = max([deg_V - 1 + deg_delta_f,2+deg_M,deg_h]); 39 | % minimum relaxation order 40 | kappa = ceil(max_deg / 2) + kappa_plus; 41 | 42 | fprintf("deg_V: %d, deg_K: %d, kappa: %d.\n",deg_V,deg_K,kappa); 43 | 44 | prog = sosprogram([e;x]); 45 | 46 | [prog,V] = sossosvar(prog,monomials(e,0:deg_V)); 47 | [prog,Q] = sospolymatrixvar(prog,monomials(Ce,0:deg_Q),[ns,ns],'symmetric'); 48 | [prog,M] = sospolymatrixvar(prog,monomials([Ce;Cx],0:deg_M),[ns,ny]); 49 | 50 | [prog,sig0] = sossosvar(prog,monomials([e;x],0:kappa)); 51 | [prog,lam1] = sospolyvar(prog,monomials([e;x],0:(2*kappa-deg_h))); 52 | 53 | eq = e'*(Q+eps*eye(ns))*delta_f + e'*M*(Ce) + ... 54 | sig0 + lam1 * h + gamma * V; 55 | 56 | prog = soseq(prog,eq); 57 | prog = sosmatrixineq(prog,Q); 58 | prog = soseq(prog,diff(V,e)-e'*(Q+eps*eye(ns))); 59 | prog = soseq(prog,subs(V,e,zeros(ns,1))); 60 | 61 | options.solver = 'mosek'; 62 | prog = sossolve(prog,options); 63 | 64 | threshold = 1e-4; 65 | sol.V = cleanpoly(sosgetsol(prog,V),threshold); 66 | sol.Q = cleanpoly(sosgetsol(prog,Q),threshold); 67 | sol.M = cleanpoly(sosgetsol(prog,M),threshold); 68 | sol.eps = eps; 69 | 70 | save('SOS-sols/quadrotor2d_sol.mat', 'sol') 71 | 72 | %% helper function 73 | % State defined as x = [x, x_dot, y, y_dot, sin(theta), cos(theta), theta_dot] 74 | function f = quadrotor2d_f(x) 75 | f = [x(2); 76 | 0; 77 | x(4); 78 | 0; 79 | x(6)*x(7); 80 | -x(5)*x(7); 81 | 0; 82 | ]; 83 | end 84 | -------------------------------------------------------------------------------- /cartpole_highgain_observer_simulation.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | mp = 1; g = 9.8; l = 1; b = 0.1; mc = 1; 4 | 5 | L = 1; 6 | k = [1;1]; 7 | num_steps = 59999; 8 | dt = 0.001; 9 | 10 | 11 | %% simulate real dynamics 12 | x0 = [0;1;1;0]; 13 | u_traj = zeros(1,num_steps); 14 | x_traj = zeros(4,num_steps+1); 15 | x_traj(:,1) = x0; 16 | x = x0; 17 | for i = 1:num_steps 18 | ui = -1; 19 | if(x(2)<0) 20 | ui = -ui; 21 | end 22 | u_traj(i) = ui; 23 | y = x(1:2); 24 | xdot = [x(3);x(4);(ui+mp*sin(x(2))*(l*x(4)^2+g*cos(x(2))))/(mc+mp*sin(x(2))^2);-(ui*cos(x(2))+mp*l*x(4)^2*cos(x(2))*sin(x(2))+(mp+mc)*g*sin(x(2)))/l/(mc+mp*sin(x(2))^2)]; 25 | x = xdot * dt + x; 26 | x_traj(:,i+1) = x; 27 | end 28 | y_traj = x_traj(1:2,:); 29 | 30 | 31 | %% simulate observer 32 | xhat0 = [0;1;4;5]; 33 | xhat_traj = zeros(4,num_steps+1); 34 | xhat_traj(:,1) = xhat0; 35 | xhat = xhat0; 36 | for i = 1:num_steps 37 | yi = y_traj(:,i); 38 | ui = u_traj(i); 39 | yhati = xhat(1:2); 40 | Ce = yhati - yi; 41 | % not use y as yhat(1) 42 | %xhatdot = [xhat(2)-L*k(1)*Ce;-(b*xhat(2)+m*g*l^2*sin(xhat(1))-ui)/m/l^2-L^2*k(2)*Ce]; 43 | %not do any thing 44 | xhatdot = [xhat(3)-L*k(1)*Ce(1);xhat(4)-L*k(1)*Ce(2);(ui+mp*sin(yi(2))*(l*xhat(4)^2+g*cos(yi(2))))/(mc+mp*sin(yi(2))^2)-L^2*k(2)*Ce(1);-(ui*cos(yi(2))+mp*l*xhat(4)^2*cos(yi(2))*sin(yi(2))+(mp+mc)*g*sin(yi(2)))/l/(mc+mp*sin(yi(2))^2)-L^2*k(2)*Ce(2)]; 45 | %xhatdot = [0;0;0;0]; 46 | xhat = xhat + xhatdot * dt; 47 | 48 | xhat_traj(:,i+1) = xhat; 49 | end 50 | 51 | 52 | %% plot comparison 53 | labelsize = 20; 54 | e_norm_traj = sqrt(sum((xhat_traj - x_traj).^2,1)); 55 | 56 | t_traj = (1:(num_steps+1)) * dt; 57 | figure; 58 | tiledlayout(4,1) 59 | nexttile 60 | s_comp = [x_traj(1,:);xhat_traj(1,:)]; 61 | plot(t_traj,x_traj(3,:)','LineWidth',2) 62 | hold on 63 | plot(t_traj,xhat_traj(3,:)','LineWidth',2) 64 | ylabel('$x$','Interpreter','latex','FontSize',labelsize) 65 | xlabel('time','FontSize',labelsize) 66 | legend('x','xhat') 67 | ax = gca; 68 | ax.FontSize = 16; 69 | 70 | nexttile 71 | c_comp = [x_traj(4,:);xhat_traj(4,:)]; 72 | plot(t_traj,c_comp','LineWidth',2) 73 | ylabel('$\theta$','Interpreter','latex','FontSize',labelsize) 74 | xlabel('time','FontSize',labelsize) 75 | ax = gca; 76 | ax.FontSize = 16; 77 | 78 | 79 | nexttile 80 | plot(t_traj,log(e_norm_traj'),'LineWidth',2) 81 | ylabel('$log (\Vert \hat{x} - x \Vert)$','Interpreter','latex','FontSize',labelsize) 82 | xlabel('time','FontSize',labelsize) 83 | ax = gca; 84 | ax.FontSize = 16; 85 | 86 | -------------------------------------------------------------------------------- /cartpole_sos_observer_refine.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | sostoolspath = "../SOSTOOLS"; 4 | mosekpath = "../../../mosek"; 5 | addpath(genpath(sostoolspath)) 6 | addpath(genpath(mosekpath)) 7 | 8 | % state x = [z,s,c,zdot,thetadot] 9 | ns = 5; 10 | ny = 3; 11 | 12 | gamma = 0.0; % desired exponential rate 13 | deg_V = 2; 14 | deg_K = 2; % degree of observer gain 15 | deg_Q = deg_V - 2; 16 | deg_M = deg_Q + deg_K; 17 | kappa_plus = 0; % choose whether to go above the minimum relaxation order 18 | 19 | e = mpvar('e',[ns,1]); 20 | x = mpvar('x',[ns,1]); 21 | Ce = e(1:3); 22 | Cx = x(1:3); 23 | 24 | % equality constraints on x and e 25 | h = [x(2)^2 + x(3)^2 - 1]; 26 | g = [monomials([e;x],0)]; 27 | 28 | eps = 0.1; 29 | prog = sosprogram([e;x]); 30 | [prog,V] = sossosvar(prog,monomials(e,0:deg_V)); 31 | [prog,Q] = sospolymatrixvar(prog,monomials(Ce,0:deg_Q),[ns,ns],'symmetric'); 32 | Qtld = Q + eps*eye(ns); 33 | [prog,M] = sospolymatrixvar(prog,monomials([Ce;Cx],0:deg_M),[ns,ny]); 34 | 35 | scaled_Vdot = e'*Qtld*cartpole_delta_f(x,e) + e'*M*Ce; 36 | 37 | max_deg = max([deg_V,... 38 | full(max(sum(scaled_Vdot.degmat,2))),... 39 | h.maxdeg,g.maxdeg]); 40 | kappa = ceil(max_deg / 2) + kappa_plus; 41 | 42 | fprintf("deg_K: %d, kappa: %d.\n",deg_K,kappa); 43 | 44 | lams = []; 45 | for i = 1:length(h) 46 | deg_hi = h(i).maxdeg; 47 | [prog,lami] = sospolyvar(prog,monomials([e;x],0:(2*kappa-deg_hi))); 48 | lams = [lams;lami]; 49 | end 50 | sigs = []; 51 | for i = 1:length(g) 52 | deg_gi = g(i).maxdeg; 53 | [prog,sigi] = sossosvar(prog,monomials([e;x],0:floor((2*kappa-deg_gi)/2))); 54 | sigs = [sigs;sigi]; 55 | end 56 | 57 | eq = scaled_Vdot + gamma * V + sigs'*g + lams'*h; 58 | 59 | prog = soseq(prog,eq); 60 | prog = sosmatrixineq(prog,Q); 61 | prog = soseq(prog,diff(V,e)-e'*Qtld); 62 | prog = soseq(prog,subs(V,e,zeros(ns,1))); 63 | options.solver = 'mosek'; 64 | prog = sossolve(prog,options); 65 | 66 | threshold = 1e-6; 67 | sol.Q = cleanpoly(sosgetsol(prog,Q),threshold); 68 | sol.eps = eps; 69 | 70 | save('SOS-sols/cartpole_tv_sol.mat', 'sol') 71 | 72 | %% helper function 73 | % state x = [z,s,c,zdot,thetadot] 74 | function f = cartpole_delta_f(x,e) 75 | z = x(1); 76 | s = x(2); 77 | c = x(3); 78 | zdot = x(4); zdothat = zdot + e(4); 79 | thetadot = x(5); thetadothat = thetadot + e(5); 80 | 81 | f = [ 82 | (1+s^2)*(zdothat - zdot); 83 | (1+s^2)*c*(thetadothat - thetadot); 84 | -(1+s^2)*s*(thetadothat - thetadot); 85 | s*(thetadothat^2 - thetadot^2); 86 | -c*s*(thetadothat^2 - thetadot^2) 87 | ]; 88 | end 89 | -------------------------------------------------------------------------------- /double_integrator_collocation_sparse_sdp.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | spotpath = '../spotless'; 4 | sospath = '../SOSprograms'; 5 | mosekpath= '/Users/hengyang/mosek'; 6 | 7 | addpath(genpath(spotpath)); 8 | addpath(genpath(sospath)); 9 | addpath(genpath(mosekpath)); 10 | 11 | initial_state = [-10;0]; 12 | final_state = [0;0]; 13 | 14 | N = 6; 15 | d = 2*(N-2) + N + 1; 16 | h = msspoly('h',1); 17 | u = msspoly('u',N); 18 | x = msspoly('x',2*(N-2)); 19 | v = [h;u;x]; 20 | x = reshape(x,2,N-2); % unknown states 21 | 22 | x_full = [initial_state,x,final_state]; 23 | 24 | objective = (N-1)*h; 25 | eqs = []; 26 | for k=1:N-1 27 | uk = u(k); 28 | ukp1 = u(k+1); 29 | xk = x_full(:,k); 30 | xkp1 = x_full(:,k+1); 31 | fk = double_integrator(xk,uk); 32 | fkp1 = double_integrator(xkp1,ukp1); 33 | 34 | % collocation points 35 | xkc = 0.5*(xk+xkp1) + h/8 * (fk - fkp1); 36 | ukc = 0.5*(uk + ukp1); 37 | % dxkc = -3/(2*h) * (xk-xkp1) - 0.25*(fk + fkp1); 38 | dxkc_h = -3/2 * (xk-xkp1) - 0.25*h*(fk + fkp1); 39 | 40 | % collocation constraint 41 | eqs = [eqs; 42 | dxkc_h - h*double_integrator(xkc,ukc)]; 43 | end 44 | 45 | % array of cliques 46 | cliques = {[1,2,3,N+1+blkIndices(1,2)]}; 47 | for k=2:N-2 48 | cliques = [cliques; 49 | {[1,k+1,k+2,N+1+blkIndices(k-1,2),N+1+blkIndices(k,2)]}]; 50 | 51 | end 52 | cliques = [cliques; 53 | {[1,N,N+1,N+1+blkIndices(N-2,2)]}]; 54 | 55 | % array of equality constraint indices assigned to each clique 56 | I = {}; 57 | for k = 1:N-1 58 | I = [I; 59 | {blkIndices(k,2)}]; 60 | end 61 | 62 | % array of inequality constraint indices assigned to each clique 63 | ineqs = [h]; 64 | for k=1:N 65 | ineqs = [ineqs; 66 | 1 - u(k)^2]; 67 | end 68 | for i = 1:length(cliques) 69 | ids = cliques{i}; 70 | vars = v(ids); 71 | ineqs = [ineqs; 72 | 200 - vars'*vars]; 73 | end 74 | J = {}; 75 | for i = 1:length(cliques) 76 | J{i} = [1,i+1,i+2,N+1+i]; 77 | end 78 | 79 | orders = 2*ones(1,length(cliques)); 80 | [blk, At, C, b] = sparsesos(-1, [objective;-1], ... 81 | ineqs, eqs, v, ... 82 | cliques, J, I, orders); 83 | 84 | % [At,b,c,k] = SDPT3data_SEDUMIdata(blk,At,C,b); 85 | prob = convert_sedumi2mosek(SDPT3data_SEDUMIdata(blk,At,C,b)); 86 | [~,res] = mosekopt('minimize info',prob); 87 | [Xopt,yopt,Sopt,obj] = recover_mosek_sol_blk(res,SDP.blk); 88 | 89 | figure; bar(eig(Xopt{1})); 90 | 91 | 92 | %% helper functions 93 | function dx = double_integrator(x,u) 94 | A = [0 1; 0 0]; 95 | B = [0; 1]; 96 | dx = A * x + B * u; 97 | end -------------------------------------------------------------------------------- /LQR_FQI_Example.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | %% system parameters 4 | h = 0.01; 5 | A = [1, h; 0, 1]; 6 | B = [0; h]; 7 | Q = 0.1*eye(2); 8 | R = 1; 9 | 10 | %% groundtruth S matrix from LQR 11 | [~,S_lqr,~] = dlqr(A,B,Q,R,zeros(2,1)); 12 | M_lqr = [Q+A'*S_lqr*A, A'*S_lqr*B; 13 | B'*S_lqr*A, B'*S_lqr*B + R]; 14 | 15 | %% Fitted Q-value iteration 16 | num_samples = 6; 17 | num_iterations = 1e4; 18 | th = 1e-8; 19 | M_vec = randn(6,1); 20 | M_traj = M_vec; 21 | M_mat = vec2mat(M_vec); 22 | iter = 1; 23 | 24 | while iter < num_iterations 25 | X = []; % Feature matrix 26 | Y = []; % Target cost vector 27 | 28 | for i = 1:num_samples 29 | x = randn(2,1); 30 | u = randn(1); 31 | 32 | ai = [A*x + B*u;0]; 33 | L = [0;0;1]; 34 | 35 | % solve u' 36 | up = - ( (L'*M_mat*L) \ (L'*M_mat*ai) ); 37 | 38 | % compute beta 39 | beta = running_cost(x,u,Q,R) + (L*up+ai)'*M_mat*(L*up+ai); 40 | 41 | % Store data 42 | X = [X; [x(1)^2, 2*x(1)*x(2), 2*x(1)*u, x(2)^2, 2*x(2)*u, u^2]]; 43 | Y = [Y; beta]; 44 | end 45 | 46 | % Update weights 47 | M_vec_new = X \ Y; 48 | 49 | % Convergence check 50 | if norm(M_vec_new - M_vec) < th 51 | fprintf("FQI converged in %d iterations.\n",iter); 52 | break; 53 | end 54 | 55 | M_vec = M_vec_new; 56 | M_mat = vec2mat(M_vec); 57 | M_traj = [M_traj,M_vec]; 58 | disp(M_vec); 59 | 60 | iter = iter + 1; 61 | end 62 | 63 | M_fqi = M_mat; 64 | 65 | %% plot 66 | labelsize = 20; 67 | figure; 68 | tiledlayout(3,1) 69 | nexttile 70 | plot(M_traj(1,:),'LineWidth',2) 71 | ylabel('$M_1$','Interpreter','latex','FontSize',labelsize) 72 | xlabel('Iteration','FontSize',labelsize) 73 | grid on 74 | ax = gca; 75 | ax.FontSize = 16; 76 | 77 | nexttile 78 | plot(M_traj(2,:),'LineWidth',2) 79 | ylabel('$M_2$','Interpreter','latex','FontSize',labelsize) 80 | xlabel('Iteration','FontSize',labelsize) 81 | grid on 82 | ax = gca; 83 | ax.FontSize = 16; 84 | 85 | nexttile 86 | plot(M_traj(3,:),'LineWidth',2) 87 | ylabel('$M_3$','Interpreter','latex','FontSize',labelsize) 88 | xlabel('Iteration','FontSize',labelsize) 89 | grid on 90 | ax = gca; 91 | ax.FontSize = 16; 92 | 93 | 94 | %% helper functions 95 | function g = running_cost(x,u,Q,R) 96 | g = x'*Q*x + u'*R*u; 97 | end 98 | 99 | function M_mat = vec2mat(M_vec) 100 | M_mat = [M_vec(1), M_vec(2), M_vec(3); 101 | M_vec(2), M_vec(4), M_vec(5); 102 | M_vec(3), M_vec(5), M_vec(6)]; 103 | end 104 | -------------------------------------------------------------------------------- /pendulum_highgain_observer_simulation.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | m = 1; g = 9.8; l = 1; b = 0.1; 4 | cvxpath = "../cvx"; 5 | addpath(genpath(cvxpath)) 6 | 7 | L = 200; 8 | k = [1;1]; 9 | num_steps = 599999; 10 | dt = 0.00001; 11 | A = [-k(1) 1;-k(2) 0]; 12 | %A = [1 0;0 1]; 13 | %% confirm lambda 14 | % lambda = 0.249; 15 | cvx_begin sdp 16 | variable P(2,2) symmetric 17 | variable lam(1,1) 18 | maximize(lam) 19 | subject to 20 | % diag(P) == 1 21 | A'*P+P*A+4*lam*P <=0 22 | P >= 0 23 | cvx_end 24 | lambda_max(P) 25 | lambda_min(P) 26 | lambda_max(A'*P + P*A + 4*lambda*P) 27 | lambda_min(A'*P + P*A + 4*lambda*P) 28 | coff_1 = -(2*lambda*L-b*sqrt(2/(L^2-1))*L*sqrt(lambda_max(P)/lambda_min(P))); 29 | %% simulate real dynamics 30 | x0 = [1;0]; 31 | u_traj = zeros(1,num_steps); 32 | x_traj = zeros(2,num_steps+1); 33 | x_traj(:,1) = x0; 34 | x = x0; 35 | for i = 1:num_steps 36 | ui = 0; 37 | u_traj(i) = ui; 38 | y = x(1); 39 | xdot = [x(2);-(b*x(2)+m*g*l^2*sin(x(1))-ui)/m/l^2]; 40 | x = xdot * dt + x; 41 | x_traj(:,i+1) = x; 42 | end 43 | y_traj = x_traj(1,:); 44 | 45 | 46 | %% simulate observer 47 | xhat0 = [1;3]; 48 | xhat_traj = zeros(2,num_steps+1); 49 | xhat_traj(:,1) = xhat0; 50 | xhat = xhat0; 51 | for i = 1:num_steps 52 | yi = y_traj(:,i); 53 | ui = u_traj(i); 54 | yhati = xhat(1); 55 | Ce = yhati - yi; 56 | % not use y as yhat(1) 57 | xhatdot = [xhat(2)-L*k(1)*Ce;-(b*xhat(2)+m*g*l^2*sin(yi)-ui)/m/l^2-L^2*k(2)*Ce]; 58 | xhat = xhat + xhatdot * dt; 59 | 60 | xhat_traj(:,i+1) = xhat; 61 | end 62 | 63 | 64 | %% plot comparison 65 | 66 | coff_2 = log(lambda_max(P)/lambda_min(P))/2+log((xhat0(1)-x0(1))^2+((xhat0(2)-x0(2))/L)^2)/2; 67 | labelsize = 20; 68 | e_norm_traj = sqrt(sum((xhat_traj(1,:) - x_traj(1,:)).^2+(xhat_traj(2,:) - x_traj(2,:)).^2/L^2,1)); 69 | 70 | t_traj = (1:(num_steps+1)) * dt; 71 | figure; 72 | % tiledlayout(3,1) 73 | % nexttile 74 | % s_comp = [x_traj(1,:);xhat_traj(1,:)]; 75 | % plot(t_traj,s_comp','LineWidth',2) 76 | % ylabel('$\theta$','Interpreter','latex','FontSize',labelsize) 77 | % xlabel('time','FontSize',labelsize) 78 | % ax = gca; 79 | % ax.FontSize = 16; 80 | % 81 | % nexttile 82 | % c_comp = [x_traj(2,:);xhat_traj(2,:)]; 83 | % plot(t_traj,c_comp','LineWidth',2) 84 | % ylabel('$\dot{\theta}$','Interpreter','latex','FontSize',labelsize) 85 | % xlabel('time','FontSize',labelsize) 86 | % ax = gca; 87 | % ax.FontSize = 16; 88 | 89 | 90 | nexttile 91 | bound = coff_2+coff_1*t_traj; 92 | plot(t_traj,log(e_norm_traj'),'LineWidth',2) 93 | hold on; 94 | plot(t_traj,bound','LineWidth',2) 95 | ylabel('$log (\Vert \hat{x} - x \Vert)$','Interpreter','latex','FontSize',labelsize) 96 | xlabel('time','FontSize',labelsize) 97 | ax = gca; 98 | ax.FontSize = 16; 99 | -------------------------------------------------------------------------------- /double_integrator_collocation.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | initial_state = [-10;0]; 4 | N = 101; 5 | 6 | T0 = 1; % initial guess on T 7 | x0 = randn(2,N); % initial guess on (x1,...,x_N) 8 | u0 = randn(N,1); % initial guess on (u1,...,u_N) 9 | v0 = [T0; x0(:); u0(:)]; 10 | 11 | lb = [0.1; 12 | -Inf*ones(2*N,1); 13 | -1*ones(N,1)]; 14 | ub = [10; 15 | Inf*ones(2*N,1); 16 | 1*ones(N,1)]; 17 | 18 | obj = @(v) v(1); % minimize time 19 | nonlincon = @(v) collocation(v,N,initial_state); 20 | 21 | options = optimoptions('fmincon','Algorithm','interior-point',... 22 | 'display','iter','MaxFunctionEvaluations',1e6,'MaxIterations',1e4); 23 | 24 | [vopt,fopt,~,out] = fmincon(obj,v0,... 25 | [],[],[],[],... % no linear (in)equality constraints 26 | lb,ub,... 27 | nonlincon,options); 28 | 29 | fprintf("Maximum constraint violation: %3.2f.\n",out.constrviolation); 30 | fprintf("Objective: %3.2f.\n",fopt); 31 | 32 | %% plot solution 33 | uopt = vopt(2*N+2:end); 34 | Topt = vopt(1); 35 | t_grid = linspace(0,1,N)*Topt; 36 | figure; 37 | scatter(t_grid,uopt,100,'filled'); hold on; 38 | plot(t_grid,uopt,'LineWidth',2); 39 | xlabel('$t$','FontSize',24,'Interpreter','latex'); 40 | ylabel('$u(t)$','FontSize',24,'Interpreter','latex'); 41 | ax = gca; ax.FontSize = 20; 42 | grid on; 43 | 44 | % integrate from the initial state using the found controls 45 | tt = linspace(0,Topt,1000); 46 | [t,sol] = ode45(@(t,y) double_integrator_ode(t,y,uopt,t_grid),tt,initial_state); 47 | figure; 48 | plot(tt,sol,'LineWidth',2); 49 | xlabel('$t$','FontSize',24,'Interpreter','latex'); 50 | ylabel('$x(t)$','FontSize',24,'Interpreter','latex'); 51 | ax = gca; ax.FontSize = 20; 52 | legend('$x_1(t)$','$x_2(t)$','FontSize',24,'Interpreter','latex'); 53 | grid on; 54 | 55 | 56 | %% helper functions 57 | function dx = double_integrator(x,u) 58 | A = [0 1; 0 0]; 59 | B = [0; 1]; 60 | dx = A * x + B * u; 61 | end 62 | 63 | function [c,ceq] = collocation(v,N,initial_state) 64 | T = v(1); 65 | h = T/(N-1); 66 | x = reshape(v(2:2*N+1),2,N); 67 | u = v(2*N+2:end); 68 | 69 | c = []; 70 | ceq = []; 71 | 72 | for k=1:N-1 73 | uk = u(k); 74 | ukp1 = u(k+1); 75 | xk = x(:,k); 76 | xkp1 = x(:,k+1); 77 | fk = double_integrator(xk,uk); 78 | fkp1 = double_integrator(xkp1,ukp1); 79 | 80 | % collocation points 81 | xkc = 0.5*(xk+xkp1) + h/8 * (fk - fkp1); 82 | ukc = 0.5*(uk + ukp1); 83 | dxkc = -3/(2*h) * (xk-xkp1) - 0.25*(fk + fkp1); 84 | 85 | % collocation constraint 86 | ceq = [ceq; 87 | dxkc - double_integrator(xkc,ukc)]; 88 | end 89 | 90 | ceq = [ceq; 91 | x(:,1) - initial_state; % initial condition 92 | x(:,end)]; % land at zero 93 | end 94 | 95 | function dx = double_integrator_ode(t,states,u_grid,t_grid) 96 | u_t = interp1(t_grid,u_grid,t); % piece-wise linear 97 | A = [0 1; 0 0]; 98 | B = [0; 1]; 99 | dx = A * states + B * u_t; 100 | end -------------------------------------------------------------------------------- /cartpole_sos_observer_tv.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | sostoolspath = "../SOSTOOLS"; 4 | mosekpath = "../../../mosek"; 5 | addpath(genpath(sostoolspath)) 6 | addpath(genpath(mosekpath)) 7 | 8 | % state x = [z,s,c,zdot,thetadot] 9 | m_c = 1; m_p = 1; l = 1; g = 9.8; 10 | ns = 5; n1 = 3; n2 = 2; 11 | ny = n1; 12 | 13 | % Choose whether to fix Q 14 | FIX_Q = true; 15 | RHO = 0.00001; % V(e) \leq RHO when FIX_Q is true 16 | CONST = (3 - sqrt(5))/2; 17 | 18 | gamma = 0.0; % desired exponential rate 19 | deg_K = 2; % degree of observer gain 20 | kappa_plus = 0; % choose whether to go above the minimum relaxation order 21 | deg_M = deg_K; 22 | 23 | e = mpvar('e',[ns,1]); 24 | x = mpvar('x',[ns,1]); 25 | x1 = x(1:3); 26 | x2 = x(4:ns); 27 | e1 = e(1:3); 28 | e2 = e(4:ns); 29 | 30 | % equality constraints on x and e 31 | h = [x(2)^2 + x(3)^2 - 1]; 32 | g = [monomials([e;x],0)]; 33 | 34 | eps = 0.0; 35 | prog = sosprogram([e;x]); 36 | 37 | if FIX_Q 38 | Q = CONST * eye(n1); 39 | else 40 | [prog,Q] = sospolymatrixvar(prog,monomials([e;x],0),[n1,n1],'symmetric'); 41 | end 42 | Qtld = Q + eps*eye(n1); 43 | 44 | [prog,M1] = sospolymatrixvar(prog,monomials([e1;x1],0:deg_M),[n1,ny]); 45 | [prog,K2] = sospolymatrixvar(prog,monomials([e1;x1],0:deg_K),[n2,ny]); 46 | 47 | V = e1'*Qtld*e1 + e2'*cartpole_M(x,m_p,m_c,l)*e2; 48 | if FIX_Q 49 | g = [g; RHO - V; RHO/CONST - e'*e]; 50 | end 51 | 52 | v_delta = cartpole_v(x+e) - cartpole_v(x); 53 | c_delta = (cartpole_Cor(x,m_p,l) - cartpole_Cor(x+e,m_p,l))*(x2+e2); 54 | Vdot = 2*e1'*Qtld*v_delta + 2*e1'*M1*e1 + 2*e2'*c_delta + 2*e2'*K2*e1; 55 | 56 | max_deg = max([ ... 57 | full(max(sum(Vdot.degmat,2))), ... 58 | full(max(sum(V.degmat,2))), ... 59 | h.maxdeg,g.maxdeg]); 60 | kappa = ceil(max_deg / 2) + kappa_plus; 61 | 62 | fprintf("deg_K: %d, kappa: %d.\n",deg_K,kappa); 63 | 64 | lams = []; 65 | for i = 1:length(h) 66 | deg_hi = h(i).maxdeg; 67 | [prog,lami] = sospolyvar(prog,monomials([e;x],0:(2*kappa-deg_hi))); 68 | lams = [lams;lami]; 69 | end 70 | sigs = []; 71 | for i = 1:length(g) 72 | deg_gi = g(i).maxdeg; 73 | [prog,sigi] = sossosvar(prog,monomials([e;x],0:floor((2*kappa-deg_gi)/2))); 74 | sigs = [sigs;sigi]; 75 | end 76 | 77 | eq = Vdot + gamma * V + sigs'*g + lams'*h; 78 | 79 | prog = soseq(prog,eq); 80 | if ~FIX_Q 81 | prog = sosmatrixineq(prog,Q); 82 | end 83 | options.solver = 'mosek'; 84 | prog = sossolve(prog,options); 85 | 86 | threshold = 1e-6; 87 | if ~FIX_Q 88 | sol.Q = cleanpoly(sosgetsol(prog,Q),threshold); 89 | end 90 | sol.M1 = cleanpoly(sosgetsol(prog,M1),threshold); 91 | sol.eps = eps; 92 | 93 | save('SOS-sols/cartpole_tv_sol.mat', 'sol') 94 | 95 | %% helper function 96 | % state x = [z,s,c,zdot,thetadot] 97 | function v = cartpole_v(x) 98 | v = [ 99 | x(4); 100 | x(3)*x(5); 101 | -x(2)*x(5)]; 102 | end 103 | 104 | function Cor = cartpole_Cor(x,m_p,l) 105 | Cor = [0, -m_p*l*x(5)*x(2); 106 | 0, 0]; 107 | end 108 | 109 | function M = cartpole_M(x,m_p,m_c,l) 110 | M = [m_c+m_p, m_p*l*x(3); 111 | m_p*l*x(3), m_p*l^2]; 112 | end 113 | 114 | -------------------------------------------------------------------------------- /mpt_examples/controllable_reachable_sets.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | % requires the Multi-Parametric Toolbox (MPT) version 3 3 | % https://www.mpt3.org/ 4 | % Installation: https://www.mpt3.org/Main/Installation 5 | % Example 10.3 and 10.4 in BBM book 6 | 7 | % define LTI system 8 | A = [1.5, 0; 1, -1.5]; B = [1; 0]; 9 | sys = LTISystem('A',A,'B',B); 10 | 11 | % define state constraint 12 | calX = Polyhedron('A',... 13 | [1,0;0,1;-1,0;0,-1], ... 14 | 'b',[10;10;10;10]); 15 | % define control constraint 16 | calU = Polyhedron('A',[1;-1],'b',[5;5]); 17 | 18 | %% compute controllable sets 19 | % target set 20 | S = Polyhedron('A',[1,0;0,1;-1,0;0,-1],'b',[1;1;1;1]); 21 | K = [S]; 22 | N = 4; 23 | for i = 1:N 24 | Si = sys.reachableSet('X',K(i),'U',calU,'N',1,'direction','backwards'); 25 | K = [K; intersect(Si,calX)]; 26 | end 27 | 28 | % plot (unshifted) controllable sets 29 | figure; K.plot('Alpha',0.4); 30 | legend('$\mathcal{K}_0(\mathcal{S})$',... 31 | '$\mathcal{K}_1(\mathcal{S})$',... 32 | '$\mathcal{K}_2(\mathcal{S})$',... 33 | '$\mathcal{K}_3(\mathcal{S})$',... 34 | '$\mathcal{K}_4(\mathcal{S})$','FontSize',20,'Interpreter','latex'); 35 | ax = gca; ax.FontSize = 16; 36 | xlabel('$x_1$','FontSize',20,'Interpreter','latex'); 37 | ylabel('$x_2$','FontSize',20,'Interpreter','latex'); 38 | % plot shifted controllable sets 39 | Kshift = []; 40 | for i = 1:length(K) 41 | Kshift = [Kshift; K(i) + (i-1)*[-8;0]]; 42 | end 43 | figure; Kshift.plot(); 44 | legend('$\mathcal{K}_0(\mathcal{S})$',... 45 | '$\mathcal{K}_1(\mathcal{S})$',... 46 | '$\mathcal{K}_2(\mathcal{S})$',... 47 | '$\mathcal{K}_3(\mathcal{S})$',... 48 | '$\mathcal{K}_4(\mathcal{S})$','FontSize',20,'Interpreter','latex'); 49 | ax = gca; ax.FontSize = 16; 50 | xlabel('$x_1$','FontSize',20,'Interpreter','latex'); 51 | ylabel('$x_2$','FontSize',20,'Interpreter','latex'); 52 | 53 | %% Compute reachable sets 54 | % initial set 55 | X0 = Polyhedron('A',[1,0;0,1;-1,0;0,-1],'b',[1;1;1;1]); 56 | R = [X0]; 57 | N = 4; 58 | for i = 1:N 59 | Ri = sys.reachableSet('X',R(i),'U',calU,'N',1,'direction','forward'); 60 | R = [R; intersect(Ri,calX)]; 61 | end 62 | % plot (unshifted) reachable sets 63 | figure; R.plot('Alpha',0.4); 64 | legend('$\mathcal{R}_0(\mathcal{X}_0)$',... 65 | '$\mathcal{R}_1(\mathcal{X}_0)$',... 66 | '$\mathcal{R}_2(\mathcal{X}_0)$',... 67 | '$\mathcal{R}_3(\mathcal{X}_0)$',... 68 | '$\mathcal{R}_4(\mathcal{X}_0)$','FontSize',20,'Interpreter','latex'); 69 | ax = gca; ax.FontSize = 16; 70 | xlabel('$x_1$','FontSize',20,'Interpreter','latex'); 71 | ylabel('$x_2$','FontSize',20,'Interpreter','latex'); 72 | % plot shifted reachable sets 73 | Rshift = []; 74 | for i = 1:length(R) 75 | Rshift = [Rshift; R(i) + (i-1)*[40;0]]; 76 | end 77 | figure; Rshift.plot(); 78 | legend('$\mathcal{R}_0(\mathcal{X}_0)$',... 79 | '$\mathcal{R}_1(\mathcal{X}_0)$',... 80 | '$\mathcal{R}_2(\mathcal{X}_0)$',... 81 | '$\mathcal{R}_3(\mathcal{X}_0)$',... 82 | '$\mathcal{R}_4(\mathcal{X}_0)$','FontSize',20,'Interpreter','latex'); 83 | ax = gca; ax.FontSize = 16; 84 | xlabel('$x_1$','FontSize',20,'Interpreter','latex'); 85 | ylabel('$x_2$','FontSize',20,'Interpreter','latex'); -------------------------------------------------------------------------------- /acrobot_sos_observer.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | sostoolspath = "../SOSTOOLS"; 4 | mosekpath = "../mosek"; 5 | addpath(genpath(sostoolspath)) 6 | addpath(genpath(mosekpath)) 7 | 8 | m_1 = 1; m_2 = 1; l_1 = 1; l_2 = 1; l_c1 = l_1/2; l_c2 = l_2/2; g = 9.8; 9 | I_1 = 1/3*m_1*l_1^2/3; I_2 = 1/3*m_2*l_2^2; 10 | ns = 6; ny = 4; 11 | 12 | gamma = 0.0; % desired exponential rate 13 | deg_V = 2; % degree of Lyapunov function V 14 | deg_K = 2; % degree of observer gain 15 | kappa_plus = 0; % choose whether to go above the minimum relaxation order 16 | 17 | deg_Q = deg_V - 2; 18 | deg_M = deg_Q + deg_K; 19 | 20 | e = mpvar('e',[ns,1]); 21 | x = mpvar('x',[ns,1]); 22 | w = mpvar('w',[3,1]); % note that W is symmetric hence we only need 3 vars 23 | C = [eye(ny), zeros(ny, ns - ny)]; 24 | Ce = C*e; 25 | Cx = C*x; 26 | delta_f = acrobot_f(x+e,w,m_2,l_1,l_c2) - acrobot_f(x,w,m_2,l_1,l_c2); 27 | deg_delta_f = delta_f.maxdeg; 28 | 29 | M_q = [I_1+I_2+m_2*l_1^2+2*m_2*l_1*l_c2*x(4), I_2+m_2*l_1*l_c2*x(4); 30 | I_2+m_2*l_1*l_c2*x(4), I_2]; 31 | W = [w(1), w(2); w(2), w(3)]; 32 | WM = W * M_q; 33 | 34 | % equality constraints on x, e and H 35 | h = [x(1)^2 + x(2)^2 - 1; 36 | x(3)^2 + x(4)^2 - 1; 37 | WM(:) - 1;]; 38 | 39 | % inequality constraints on x and e 40 | g = [1; 41 | 4 - e(1:4).^2; 42 | 25 - e(5:6).^2;]; 43 | 44 | max_deg = max([deg_V-1+deg_delta_f, ... 45 | 2+deg_M, ... 46 | h.maxdeg, ... 47 | g.maxdeg]); 48 | 49 | % minimum relaxation order 50 | kappa = ceil(max_deg / 2) + kappa_plus; 51 | 52 | fprintf("deg_V: %d, deg_K: %d, kappa: %d.\n",deg_V,deg_K,kappa); 53 | 54 | prog = sosprogram([e;x;w]); 55 | 56 | [prog,V] = sossosvar(prog,monomials(e,0:deg_V)); 57 | [prog,Q] = sospolymatrixvar(prog,monomials(Ce,0:deg_Q),[ns,ns],'symmetric'); 58 | [prog,M] = sospolymatrixvar(prog,monomials([Ce;Cx],0:deg_M),[ns,ny]); 59 | 60 | 61 | lams = []; 62 | for i = 1:length(h) 63 | deg_hi = h(i).maxdeg; 64 | [prog,lami] = sospolyvar(prog,monomials([e;x],0:(2*kappa-deg_hi))); 65 | lams = [lams;lami]; 66 | end 67 | sigs = []; 68 | for i = 1:length(g) 69 | deg_gi = g(i).maxdeg; 70 | [prog,sigi] = sossosvar(prog,monomials([e;x],0:floor((2*kappa-deg_gi)/2))); 71 | sigs = [sigs;sigi]; 72 | end 73 | 74 | eps = 0.001; 75 | 76 | eq = e'*(Q+eps*eye(ns))*delta_f + e'*M*(Ce) + gamma * V + ... 77 | sigs'*g + lams'*h; 78 | 79 | prog = soseq(prog,eq); 80 | prog = sosmatrixineq(prog,Q); 81 | prog = soseq(prog,diff(V,e)-e'*(Q+eps*eye(ns))); 82 | prog = soseq(prog,subs(V,e,zeros(ns,1))); 83 | options.solver = 'mosek'; 84 | prog = sossolve(prog,options); 85 | 86 | threshold = 1e-6; 87 | sol.V = cleanpoly(sosgetsol(prog,V),threshold); 88 | sol.Q = cleanpoly(sosgetsol(prog,Q),threshold); 89 | sol.M = cleanpoly(sosgetsol(prog,M),threshold); 90 | 91 | save('SOS-sols/acrobot_sol.mat', 'sol') 92 | 93 | %% helper function 94 | % State defined as x = [sin(theta_1), cos(theta_1), sin(theta_2), 95 | % cos(theta_2), theta_1_dot, theta_2_dot] 96 | function f = acrobot_f(x,t,m_2,l_1,l_c2) 97 | T = [t(1), t(2); 98 | t(2), t(3)]; 99 | C = m_2*l_1*l_c2 * [-2*x(3)*x(6), -x(3)*x(6); 100 | x(3)*x(5), 0]; 101 | f = [x(2)*x(5); 102 | -x(1)*x(5); 103 | x(4)*x(6); 104 | -x(3)*x(6); 105 | -T*C*[x(5); x(6)]]; 106 | end 107 | -------------------------------------------------------------------------------- /double_integrator_mpc/double_integrator_stability.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | A = [1,1;0,1]; B = [0;1]; 4 | Q = [1,0;0,0]; R = 1; 5 | 6 | [~,S,~] = dlqr(A,B,Q,R,zeros(2,1)); 7 | 8 | % P = 0.1*eye(2); 9 | P = 1*S; 10 | N = 3; 11 | 12 | x = [2;1]; 13 | x_traj = x; 14 | u_traj = []; 15 | rho_traj = []; 16 | for t = 1:100 17 | % [uopt,info] = double_integrator_rhc(A,B,P,Q,R,N,x); 18 | uopt = double_integrator_rhc_cvx(A,B,P,Q,R,N,x); 19 | rho = check_clf(x,A,B,P,Q,R); 20 | 21 | xnew = A*x + B*uopt(1); 22 | 23 | if norm(xnew) < 1e-6 24 | fprintf("Close to origin.\n") 25 | break; 26 | end 27 | 28 | if norm(xnew - x) < 1e-9 29 | fprintf("Slow convergence.\n"); 30 | break; 31 | end 32 | 33 | x = xnew; 34 | x_traj = [x_traj,x]; 35 | u_traj = [u_traj,uopt(1)]; 36 | rho_traj = [rho_traj;rho]; 37 | end 38 | 39 | %% plot 40 | figure; plot(x_traj','LineWidth',2); 41 | figure; plot(rho_traj,'LineWidth',2); 42 | 43 | 44 | 45 | %% helper functions 46 | function rho = check_clf(x,A,B,P,Q,R) 47 | cvx_begin 48 | variable u(1,1) 49 | xnew = A*x+B*u; 50 | minimize ((xnew'*P*xnew) - (x'*P*x) + (x'*Q*x) + (u'*R*u)); 51 | subject to 52 | u >= -1; 53 | u <= 1; 54 | cvx_end 55 | 56 | rho = cvx_optval; 57 | 58 | end 59 | 60 | 61 | 62 | 63 | function uopt = double_integrator_rhc_cvx(A,B,P,Q,R,N,xt) 64 | cvx_begin 65 | variable u(N,1) 66 | variable x(2,N) 67 | f = cost(x,u,xt,P,Q,R,N); 68 | minimize f 69 | subject to 70 | for k = 0:N-1 71 | u(k+1) >= -1 72 | u(k+1) <= 1 73 | if k == 0 74 | x(:,k+1) == A*xt + B*u(k+1); 75 | else 76 | x(:,k+1) == A*x(:,k) + B*u(k+1); 77 | end 78 | end 79 | cvx_end 80 | 81 | if cvx_status == "Infeasible" 82 | uopt = []; 83 | else 84 | uopt = u; 85 | end 86 | 87 | end 88 | 89 | 90 | function f = cost(x,u,xt,P,Q,R,N) 91 | f = 0; 92 | for k = 0:N-1 93 | uk = u(k+1); 94 | if k==0 95 | xk = xt; 96 | else 97 | xk = x(:,k); 98 | end 99 | f = f + xk'*Q*xk + uk'*R*uk; 100 | end 101 | 102 | f = f + x(:,N)'*P*x(:,N); 103 | end 104 | 105 | %% implemented using quadprog 106 | function [uopt,info] = double_integrator_rhc(A,B,P,Q,R,N,xt) 107 | % decision variable 108 | % [u(0),...,u(N-1),x(1),...,x(N)] 109 | lb = [-1*ones(N,1);-inf*ones(2*N,1)]; 110 | ub = [1*ones(N,1);inf*ones(2*N,1)]; 111 | 112 | Aeq = []; 113 | beq = []; 114 | for i = 1:N-1 115 | Ai = zeros(2,3*N); 116 | Ai(:,i+1) = B; 117 | Ai(:,N+blkIndices(i,2)) = A; 118 | Ai(:,N+blkIndices(i+1,2)) = -eye(2); 119 | 120 | bi = zeros(2,1); 121 | 122 | Aeq = [Aeq;Ai]; 123 | beq = [beq;bi]; 124 | end 125 | 126 | A0 = zeros(2,3*N); 127 | A0(:,1) = -B; 128 | A0(:,N+blkIndices(1,2)) = eye(2); 129 | b0 = A*xt; 130 | 131 | Aeq = [A0;Aeq]; beq = [b0;beq]; 132 | blks = {}; 133 | for i = 1:N 134 | blks = [blks;{R}]; 135 | end 136 | for i = 1:N-1 137 | blks = [blks;{Q}]; 138 | end 139 | blks = [blks;{P}]; 140 | 141 | H = blkdiag(blks{:}); 142 | 143 | xopt = quadprog(H,[],[],[],Aeq,beq,lb,ub); 144 | 145 | uopt = xopt(1:N); 146 | info.H = H; 147 | info.Aeq = Aeq; 148 | info.beq = beq; 149 | info.xopt = xopt; 150 | end 151 | 152 | 153 | 154 | -------------------------------------------------------------------------------- /grid_world_value_iteration.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | %% create problem data 4 | % states 5 | [X,Y] = meshgrid(1:10,1:10); 6 | X = X(:); 7 | Y = Y(:); 8 | XY = [X,Y]; 9 | nx = length(X); % number of states 10 | 11 | % controls 12 | U = [1, 0; 13 | -1, 0; 14 | 0, 1; 15 | 0, -1; 16 | 0, 0]; 17 | nu = size(U,1); % number of controls 18 | 19 | % create vector g 20 | target = [1,10]; 21 | obstacles = [2,2; 22 | 2,3; 23 | 3,2; 24 | 3,3; 25 | 3,4; 26 | 4,5; 27 | 4,6; 28 | 5,5; 29 | 5,6; 30 | 6,5; 31 | 6,6; 32 | 7,7; 33 | 7,8; 34 | 8,2; 35 | 8,3; 36 | 8,4; 37 | 8,7; 38 | 8,8; 39 | 9,7; 40 | 9,8; 41 | 10,7; 42 | 10,8]; 43 | 44 | g = zeros(nx,nu); 45 | for i = 1:nx 46 | for j = 1:nu 47 | x = XY(i,:); % state 48 | if ismember(x,target,'rows') 49 | state_cost = 0; 50 | elseif ismember(x,obstacles,'rows') 51 | state_cost = 20; 52 | else 53 | state_cost = 1; 54 | end 55 | 56 | u = U(j,:); % control 57 | control_cost = sum(u.^2); 58 | 59 | g(i,j) = state_cost; 60 | end 61 | end 62 | g_mat = g; 63 | g = g(:); 64 | 65 | % construct transition matrix P 66 | P = zeros(nx*nu,nx); 67 | count = 1; 68 | for j = 1:nu 69 | u = U(j,:); % control 70 | for i = 1:nx 71 | x = XY(i,:); % state 72 | xp = x + u; % next state 73 | 74 | [~,loc] = ismember(xp,XY,'rows'); 75 | 76 | if loc == 0 % if the next state is not in the grid, assume the state stays where it was 77 | p = zeros(1,nx); 78 | p(i) = 1; 79 | P(count,:) = p; 80 | else 81 | p = zeros(1,nx); 82 | p(loc) = 1; 83 | P(count,:) = p; 84 | end 85 | count = count + 1; 86 | end 87 | end 88 | 89 | %% start value iteration 90 | Q = zeros(nx*nu,1); 91 | iter = 1; 92 | MAX_ITERS = 1e3; 93 | while iter < MAX_ITERS 94 | Q_mat = reshape(Q,nx,nu); 95 | J_Q = min(Q_mat,[],2); 96 | 97 | Q_new = g + P*J_Q; 98 | Q_diff = norm(Q_new - Q); 99 | Q = Q_new; 100 | iter = iter + 1; 101 | 102 | % check convergence 103 | if Q_diff < 1e-6 104 | fprintf("Value iteration converged in %d iterations, Q_diff = %3.2e.\n",iter,Q_diff) 105 | break 106 | end 107 | end 108 | % compute optimal cost to go 109 | Q_mat = reshape(Q,nx,nu); 110 | J = min(Q_mat,[],2); 111 | 112 | % plot optimal cost to go 113 | J_mat = zeros(10,10); 114 | for cnt = 1:nx 115 | x = XY(cnt,:); 116 | J_mat(x(1),x(2)) = J(cnt); 117 | end 118 | figure; image(J_mat,'CDataMapping','scaled'); colorbar; 119 | set(gca,'XAxisLocation','top') 120 | hold on 121 | 122 | %% plot the optimal trajectory starting from a point 123 | x = [8,5]; 124 | x_traj = x; 125 | while true 126 | [~,loc] = ismember(x,XY,'rows'); 127 | Q_x = Q_mat(loc,:); 128 | [~,id] = min(Q_x); 129 | u_star = U(id,:); 130 | x = x + u_star; 131 | x_traj = [x_traj;x]; 132 | 133 | if norm(x - target) == 0 134 | break; 135 | end 136 | end 137 | 138 | plot(x_traj(:,2),x_traj(:,1),'r-x','LineWidth',2,'MarkerSize',10) 139 | axis square 140 | 141 | 142 | -------------------------------------------------------------------------------- /cartpole_sos_observer.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | sostoolspath = "../SOSTOOLS"; 4 | mosekpath = "../mosek"; 5 | addpath(genpath(sostoolspath)) 6 | addpath(genpath(mosekpath)) 7 | 8 | m_c = 1; m_p = 1; l = 1; 9 | ns = 6; 10 | ny = 4; 11 | 12 | gamma = 0.0; % desired exponential rate 13 | deg_V = 2; % degree of Lyapunov function V 14 | deg_K = 4; % degree of observer gain 15 | kappa_plus = 0; % choose whether to go above the minimum relaxation order 16 | 17 | deg_Q = deg_V - 2; 18 | deg_M = deg_Q + deg_K; 19 | 20 | e = mpvar('e',[ns,1]); 21 | x = mpvar('x',[ns,1]); 22 | C = [1, 0, 0, 0, 0, 0; 23 | 0, 0, 1, 0, 0, 0; 24 | 0, 0, 0, 1, 0, 0; 25 | 0, 0, 0, 0, 0, 1]; 26 | Ce = C*e; 27 | Cx = C*x; 28 | delta_f = cartpole_f(x+e,l) - cartpole_f(x,l); 29 | deg_delta_f = delta_f.maxdeg; 30 | 31 | % equality constraints on x and e 32 | h = [x(3)^2 + x(4)^2 - 1; 33 | x(6)*m_c/m_p + x(6)*x(3)^2 - 1]; 34 | 35 | % inequality constraints on x and e 36 | a_lb = 1 / (m_c/m_p + 1); % lower bound of a 37 | a_ub = 1 / (m_c/m_p); % upper bound of a 38 | thetadot_lb = -50; % lower bound of theta_dot 39 | thetadot_ub = 50; % upper bound of theta_dot 40 | xdot_lb = -10; % lower bound of x_dot 41 | xdot_ub = 10; % upper bound of x_dot 42 | g = [monomials([e;x],0); 43 | -(x(2)-xdot_lb)*(x(2)-xdot_ub); 44 | 1 - x(3)^2; 45 | 1 - x(4)^2; 46 | -(x(5)-thetadot_lb)*(x(5)-thetadot_ub); 47 | -(x(6)-a_lb)*(x(6)-a_ub); 48 | (xdot_ub-xdot_lb)^2 - e(2)^2; 49 | 4 - e(3)^2; 50 | 4 - e(4)^2; 51 | (thetadot_ub-thetadot_lb)^2 - e(5)^2; 52 | (a_ub - a_lb)^2 - e(6)^2]; 53 | 54 | max_deg = max([deg_V-1+deg_delta_f, ... 55 | 2+deg_M, ... 56 | h.maxdeg, ... 57 | g.maxdeg]); 58 | 59 | % minimum relaxation order 60 | kappa = ceil(max_deg / 2) + kappa_plus; 61 | 62 | fprintf("deg_V: %d, deg_K: %d, kappa: %d.\n",deg_V,deg_K,kappa); 63 | 64 | prog = sosprogram([e;x]); 65 | 66 | [prog,V] = sossosvar(prog,monomials(e,0:deg_V)); 67 | [prog,Q] = sospolymatrixvar(prog,monomials(Ce,0:deg_Q),[ns,ns],'symmetric'); 68 | [prog,M] = sospolymatrixvar(prog,monomials([Ce;Cx],0:deg_M),[ns,ny]); 69 | 70 | 71 | lams = []; 72 | for i = 1:length(h) 73 | deg_hi = h(i).maxdeg; 74 | [prog,lami] = sospolyvar(prog,monomials([e;x],0:(2*kappa-deg_hi))); 75 | lams = [lams;lami]; 76 | end 77 | sigs = []; 78 | for i = 1:length(g) 79 | deg_gi = g(i).maxdeg; 80 | [prog,sigi] = sossosvar(prog,monomials([e;x],0:floor((2*kappa-deg_gi)/2))); 81 | sigs = [sigs;sigi]; 82 | end 83 | 84 | eps = 0.01; 85 | 86 | eq = e'*(Q+eps*eye(ns))*delta_f + e'*M*(Ce) + gamma * V + ... 87 | sigs'*g + lams'*h; 88 | 89 | prog = soseq(prog,eq); 90 | prog = sosmatrixineq(prog,Q); 91 | prog = soseq(prog,diff(V,e)-e'*(Q+eps*eye(ns))); 92 | prog = soseq(prog,subs(V,e,zeros(ns,1))); 93 | options.solver = 'mosek'; 94 | prog = sossolve(prog,options); 95 | 96 | threshold = 1e-6; 97 | sol.V = cleanpoly(sosgetsol(prog,V),threshold); 98 | sol.Q = cleanpoly(sosgetsol(prog,Q),threshold); 99 | sol.M = cleanpoly(sosgetsol(prog,M),threshold); 100 | sol.eps = eps; 101 | 102 | save('SOS-sols/cartpole_sol.mat', 'sol') 103 | 104 | %% helper function 105 | % State defined as x = [x, x_dot, sin(theta), cos(theta), theta_dot, a] 106 | % where a = 1/(m_c/m_p+sin(theta)^2) 107 | function f = cartpole_f(x,l) 108 | f = [x(2); 109 | x(6)*x(3)*l*x(5)^2; 110 | x(4)*x(5); 111 | -x(3)*x(5); 112 | -x(6)*x(5)^2*x(3)*x(4); 113 | -2*x(3)*x(4)*x(5)*x(6)^2; 114 | ]; 115 | end 116 | -------------------------------------------------------------------------------- /quadrotor2d_luenberger.m: -------------------------------------------------------------------------------- 1 | clear; clc; close all; format compact; 2 | 3 | cvxpath = "./cvx"; 4 | addpath(genpath(cvxpath)) 5 | 6 | % Constants 7 | g = 9.8; 8 | 9 | % Model parameters 10 | m = 1; l = 1; r = l/2; I = m*l^2/12; 11 | ns = 6; ny = 3; nu = 2; 12 | 13 | % Define system matrices 14 | A = zeros(ns,ns); A(1,2) = 1; A(3,4) = 1; A(5,6) = 1; 15 | % psi defined at the bottom 16 | C = zeros(ny,ns); C(1,1) = 1; C(2,3) = 1; C(3,5) = 1; 17 | obs_idx = [1,3,5]; 18 | 19 | Ob = obsv(A,C); 20 | if rank(Ob) ~= length(A) 21 | disp("(A,C) not observable!") 22 | else 23 | disp("(A,C) is observable!") 24 | end 25 | 26 | % Find optimal K that maximizes convergence rate 27 | cvx_begin 28 | cvx_solver mosek 29 | variable H(ns,ny) 30 | variable P(ns,ns) symmetric 31 | minimize(0) 32 | subject to 33 | P == semidefinite(ns) 34 | A'*P - C'*H' + P*A - H*C == - eye(ns); 35 | cvx_end 36 | max_gam = 0.5 / max(eig(P)); 37 | K = P \ H; 38 | 39 | 40 | %% simulate real dynamics 41 | dt = 0.01; 42 | T = 50; 43 | num_steps = floor(T/dt); 44 | 45 | x0 = [1;0;1;0;0;0]; 46 | u_traj = zeros(nu,num_steps); 47 | x_traj = zeros(ns,num_steps+1); 48 | x_traj(:,1) = x0; 49 | x = x0; 50 | for i = 1:num_steps 51 | u1 = 1 + rand; 52 | u2 = 1 + rand; 53 | u = [u1;u2]; 54 | u = zeros(2,1); 55 | u_traj(:,i) = u; 56 | y = C*x; 57 | xdot = A*x + quadrotor2d_psi(u,y,m,g,I,r); 58 | 59 | x = xdot * dt + x; 60 | x_traj(:,i+1) = x; 61 | end 62 | y_traj = x_traj(obs_idx,:); 63 | 64 | 65 | %% simulate observer 66 | xhat0 = [0;0;0;0;0;0]; 67 | xhat_traj = zeros(ns,num_steps+1); 68 | xhat_traj(:,1) = xhat0; 69 | xhat = xhat0; 70 | Ces = [zeros(4,1)]; 71 | for i = 1:num_steps 72 | yi = y_traj(:,i); 73 | ui = u_traj(:,i); 74 | % yhati = C*xhat; 75 | % Ce = yhati - yi; 76 | 77 | xhatdot = A*xhat + quadrotor2d_psi(u,yi,m,g,I,r) + K*C*(x-xhat); 78 | xhat = xhat + xhatdot * dt; 79 | 80 | xhat_traj(:,i+1) = xhat; 81 | end 82 | 83 | %% plot comparison 84 | labelsize = 20; 85 | e_norm_traj = sqrt(sum((xhat_traj - x_traj).^2,1)); 86 | 87 | t_traj = (1:(num_steps+1)) * dt; 88 | figure; 89 | tiledlayout(4,1) 90 | nexttile 91 | s_comp = [x_traj(5,:);xhat_traj(1,:)]; 92 | plot(t_traj,s_comp','LineWidth',2) 93 | ylabel('$\sin \theta$','Interpreter','latex','FontSize',labelsize) 94 | xlabel('time','FontSize',labelsize) 95 | ax = gca; 96 | ax.FontSize = 16; 97 | 98 | nexttile 99 | c_comp = [x_traj(6,:);xhat_traj(2,:)]; 100 | plot(t_traj,c_comp','LineWidth',2) 101 | ylabel('$\cos \theta$','Interpreter','latex','FontSize',labelsize) 102 | xlabel('time','FontSize',labelsize) 103 | ax = gca; 104 | ax.FontSize = 16; 105 | 106 | nexttile 107 | thdot_comp = [x_traj(7,:);xhat_traj(3,:)]; 108 | plot(t_traj,thdot_comp','LineWidth',2) 109 | ylabel('$\dot{\theta}$','Interpreter','latex','FontSize',labelsize) 110 | xlabel('time','FontSize',labelsize) 111 | ax = gca; 112 | ax.FontSize = 16; 113 | 114 | nexttile 115 | plot(t_traj,e_norm_traj','LineWidth',2) 116 | ylabel('$\Vert \hat{x} - x \Vert$','Interpreter','latex','FontSize',labelsize) 117 | xlabel('time','FontSize',labelsize) 118 | ax = gca; 119 | ax.FontSize = 16; 120 | 121 | %% Helper functions 122 | % State defined as x = [x; xdot; y; ydot; theta; theta_dot] 123 | function psi = quadrotor2d_psi(u,y,m,g,I,r) 124 | psi = [0; 125 | -(u(1,1)+u(2,1))*sin(y(3))/m; 126 | 0; 127 | (u(1)+u(2))*cos(y(3))/m - g; 128 | 0; 129 | 1/I*r*(u(1)-u(2)); 130 | ]; 131 | end 132 | 133 | -------------------------------------------------------------------------------- /double_integrator_multiple_shooting.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | initial_state = [-10;-0]; 4 | N = 51; % number of knot points 5 | % add initial guess 6 | v0 = [1; % guess on terminal time 7 | rand(2*N,1); % guess on states 8 | randn(N,1)]; % guess on control 9 | 10 | lb = [0.01; 11 | -Inf*ones(2*N,1); 12 | -1*ones(N,1)]; 13 | ub = [10; 14 | Inf*ones(2*N,1); 15 | 1*ones(N,1)]; 16 | 17 | obj = @(v) v(1); % minimize time 18 | nonlincon = @(v) double_integrator_nonlincon(v,initial_state); 19 | 20 | options = optimoptions('fmincon','Algorithm','interior-point',... 21 | 'display','iter','MaxFunctionEvaluations',1e6,'MaxIterations',1e4); 22 | 23 | [vopt,fopt,~,out] = fmincon(obj,v0,... 24 | [],[],[],[],... % no linear (in)equality constraints 25 | lb,ub,... 26 | nonlincon,options); 27 | 28 | fprintf("Maximum constraint violation: %3.2f.\n",out.constrviolation); 29 | fprintf("Objective: %3.2f.\n",fopt); 30 | 31 | %% plot solution 32 | uopt = vopt(2+2*N:3*N+1); 33 | Topt = vopt(1); 34 | t_grid = linspace(0,1,N)*Topt; 35 | figure; 36 | scatter(t_grid,uopt,100,'filled'); hold on; 37 | plot(t_grid,uopt,'LineWidth',2); 38 | xlabel('$t$','FontSize',24,'Interpreter','latex'); 39 | ylabel('$u(t)$','FontSize',24,'Interpreter','latex'); 40 | ax = gca; ax.FontSize = 20; 41 | grid on; 42 | 43 | % integrate from the initial state using the found controls 44 | tt = linspace(0,Topt,1000); 45 | [t,sol] = ode45(@(t,y) double_integrator(t,y,vopt),tt,initial_state); 46 | figure; 47 | plot(tt,sol,'LineWidth',2); 48 | xlabel('$t$','FontSize',24,'Interpreter','latex'); 49 | ylabel('$x(t)$','FontSize',24,'Interpreter','latex'); 50 | ax = gca; ax.FontSize = 20; 51 | legend('$x_1(t)$','$x_2(t)$','FontSize',24,'Interpreter','latex'); 52 | grid on; 53 | 54 | 55 | %% helper functions 56 | function dx = double_integrator(t,states,v) 57 | % return xdot at the selected times t and states, using information from v 58 | % assume the controls in v define piece-wise constant control signal 59 | 60 | T = v(1); % final time 61 | N = (length(v) - 1) / 3; % number of knot points 62 | 63 | u_grid = v(2+2*N:3*N+1); % N controls 64 | t_grid = linspace(0,1,N)*T; 65 | 66 | u_t = interp1(t_grid,u_grid,t,'previous'); % piece-wise constant 67 | 68 | A = [0 1; 0 0]; 69 | B = [0; 1]; 70 | 71 | dx = A * states + B * u_t; 72 | end 73 | 74 | function [c,ceq] = double_integrator_nonlincon(v,initial_state) 75 | % enforce x_k+1 = RK45(x_k, u_k); integration done using ode45 76 | 77 | T = v(1); % final time 78 | N = (length(v) - 1) / 3; % number of knot points 79 | t_grid = linspace(0,1,N)*T; 80 | x1 = v(2:N+1); % position 81 | x2 = v(2+N:2*N+1); % velocity 82 | % u = v(2+2*N:3*N+1); % controls 83 | 84 | % no inequality constraints 85 | c = []; 86 | 87 | % equality constraints 88 | ceq = []; 89 | for i = 1:(N-1) 90 | ti = t_grid(i); 91 | tip1 = t_grid(i+1); 92 | 93 | xi = [x1(i);x2(i)]; 94 | xip1 = [x1(i+1);x2(i+1)]; 95 | 96 | % integrate system dynamics starting from xi in [ti,tip1] 97 | tt = ti:(tip1-ti)/20:tip1; % fine-grained time discretization 98 | [~,sol_int] = ode45(@(t,y) double_integrator(t,y,v),tt,xi); 99 | xip1_int = sol_int(end,:); 100 | 101 | % enforce them to be the same 102 | ceq = [ceq; 103 | xip1_int(1) - xip1(1); 104 | xip1_int(2) - xip1(2)]; 105 | end 106 | 107 | % add initial state constraint 108 | ceq = [ceq; 109 | x1(1) - initial_state(1); 110 | x2(1) - initial_state(2)]; 111 | 112 | % add terminal state constraint: land at origin 113 | ceq = [ceq; 114 | x1(end); 115 | x2(end)]; 116 | end -------------------------------------------------------------------------------- /pendulum_saturated_control.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | restoredefaultpath; 3 | 4 | %% parameters of the pendulum 5 | m = 1; g = 9.8; l = 1; b = 0.1; 6 | umax = 2; 7 | A = [0, 1; g/l, -b/(m*l^2)]; 8 | B = [0; 1/(m*l^2)]; 9 | Q = diag([1,1]); 10 | R = 1; 11 | 12 | %% TODO: Compute K and S from LQR 13 | [K,S] = ; 14 | 15 | %% Region to draw samples 16 | x1_lb = -0.2*pi; 17 | x1_ub = 0.2*pi; 18 | x2_lb = -0.2*pi; 19 | x2_ub = 0.2*pi; 20 | 21 | %% Simulate pendulum with clipped control 22 | pendulum_sat = @(t,x) pendulum_f(x,clip(-K*x,-umax,umax),m,g,l,b); 23 | T = 100; 24 | N = 1000; 25 | stable = []; 26 | unstable = []; 27 | for i = 1:N 28 | x1 = x1_lb + rand*(x1_ub - x1_lb); 29 | x2 = x2_lb + rand*(x2_ub - x2_lb); 30 | x0 = [x1;x2]; 31 | [t,y] = ode89(pendulum_sat,[0,T],x0); 32 | if norm(y(end,:)) < 1e-3 33 | stable = [stable,x0]; 34 | else 35 | unstable = [unstable,x0]; 36 | end 37 | end 38 | % plot points 39 | figure; 40 | scatter(stable(1,:),stable(2,:),80,'filled','Marker','o'); 41 | hold on 42 | scatter(unstable(1,:),unstable(2,:),80,'filled','Marker','square'); 43 | xlabel('$x_1$','Interpreter','latex','FontSize',24); 44 | ylabel('$x_2$','Interpreter','latex','FontSize',24); 45 | ax = gca; ax.FontSize = 20; 46 | hold on 47 | 48 | %% certify ROA using SOS programming 49 | sostoolspath = '../SOSTOOLS'; 50 | mosekpath = '../../../mosek'; 51 | addpath(genpath(sostoolspath)); 52 | addpath(genpath(mosekpath)); 53 | 54 | x = mpvar('x',[2,1]); 55 | J = x'*S*x; 56 | eps = 0.01; 57 | rho = 1; 58 | kappa = 4; % relaxation order 59 | 60 | prog = sosprogram(x); 61 | % case 1 62 | Jdot1 = - jacobian(J,x)*pendulum_poly(x,umax,m,g,l,b) - eps*(x'*x); 63 | set1 = [rho - J; -K*x - umax]; 64 | [prog, rhs1] = SOSonSet(prog,x,kappa,set1); 65 | prog = soseq(prog, Jdot1 - rhs1); 66 | 67 | % TODO: case 2 68 | Jdot2 = ; 69 | set2 = ; 70 | [prog, rhs2] = SOSonSet(prog,x,kappa,set2); 71 | prog = soseq(prog, Jdot2 - rhs2); 72 | 73 | % TODO: case 3 74 | Jdot3 = ; 75 | set3 = ; 76 | [prog, rhs3] = SOSonSet(prog,x,kappa,set3); 77 | prog = soseq(prog, Jdot3 - rhs3); 78 | 79 | options.solver = 'mosek'; 80 | prog = sossolve(prog, options); 81 | 82 | e = ellipse(S/rho,zeros(2,1)); 83 | plot(e(1,:),e(2,:),'green','LineWidth',4); 84 | legend('Stabilized','Non-Stabilized','Certified','FontSize',22); 85 | 86 | 87 | %% helper function 88 | function v = clip(u,a,b) 89 | %% TODO: implement the clip function that clips u between [a,b] 90 | end 91 | 92 | function zdot = pendulum_f(z,u,m,g,l,b) 93 | % pendulum true dynamics 94 | z1 = z(1); 95 | z2 = z(2); 96 | 97 | zdot = [ 98 | z2; 99 | 1/(m*l^2)*(u - b*z2 + m*g*l*sin(z1)) 100 | ]; 101 | end 102 | 103 | function zdot = pendulum_poly(z,u,m,g,l,b) 104 | %% TODO: implement pendulum approximate polynomial dynamics 105 | end 106 | 107 | function [prog,rhs] = SOSonSet(prog,var,kappa,set_ineq) 108 | % a polynomial p is SOS on a set K defined by inequality constraints 109 | % relaxation order kappa 110 | v0 = monomials(var,0:kappa); 111 | [prog, sig0] = sossosvar(prog,v0); 112 | rhs = sig0; 113 | for j = 1:length(set_ineq) 114 | gj = set_ineq(j); 115 | degj = floor((2*kappa - gj.maxdeg)/2); 116 | vj = monomials(var,0:degj); 117 | [prog, sigj] = sossosvar(prog,vj); 118 | rhs = rhs + sigj*gj; 119 | end 120 | end 121 | 122 | function e = ellipse(A,center) 123 | [U,~] = eig(A); 124 | theta = linspace(0,2*pi,1000); 125 | elipoid = zeros(2,1000); 126 | for pt = 1:1000 127 | elipoid(:,pt) = U(:,1)*sin(theta(pt)) + U(:,2)*cos(theta(pt)); 128 | elipoid(:,pt) = elipoid(:,pt)/sqrt(elipoid(:,pt)'*A*elipoid(:,pt)); 129 | end 130 | e = elipoid + center; 131 | end 132 | -------------------------------------------------------------------------------- /pendulum_local_lyapunov_certificate.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | sostoolspath = "../SOSTOOLS"; 4 | mosekpath = "../../../mosek"; 5 | addpath(genpath(sostoolspath)) 6 | addpath(genpath(mosekpath)) 7 | 8 | m = 1; g = 9.8; l = 1; b = 0.1; 9 | 10 | deg_V = 2; % degree of Lyapunov function 11 | 12 | x = mpvar('x',[3,1]); 13 | x0 = [0;1;0]; % equilibrium point 14 | prog = sosprogram(x); 15 | [prog,V] = sospolyvar(prog,monomials(x,0:deg_V)); 16 | eps1 = 0.01; 17 | eps2 = 0.01; 18 | fx = pendulum_f(x,m,b,l,g); 19 | Vdot = diff(V,x)*fx; 20 | deg_Vdot = max(full(sum(Vdot.degmat,2))); 21 | 22 | kappa = ceil( max([deg_V/2,deg_Vdot/2]) ); 23 | 24 | h = [x(1)^2 + x(2)^2 - 1]; % equality constraint 25 | g = [monomials(x,0); 26 | (pi/2)^2 - x(3)^2; % velocity bounded 27 | x(2) - 3/4]; % cos theta \geq lower_bound 28 | 29 | %% enforce V \geq eps1 * (x-x0)'*(x-x0) 30 | lams = []; 31 | for i = 1:length(h) 32 | deg_hi = h(i).maxdeg; 33 | [prog,lami] = sospolyvar(prog,monomials(x,0:(2*kappa-deg_hi))); 34 | lams = [lams;lami]; 35 | end 36 | sigs = []; 37 | for i = 1:length(g) 38 | deg_gi = g(i).maxdeg; 39 | [prog,sigi] = sossosvar(prog,monomials(x,0:floor((2*kappa-deg_gi)/2))); 40 | sigs = [sigs;sigi]; 41 | end 42 | 43 | eq = V - (eps1 * (x-x0)'*(x-x0)) - sigs'*g - lams'*h; 44 | prog = soseq(prog,eq); 45 | 46 | %% enforce Vdot \leq -eps1 * (x-x0)'*(x-x0) 47 | lams_dot = []; 48 | for i = 1:length(h) 49 | deg_hi = h(i).maxdeg; 50 | [prog,lami_dot] = sospolyvar(prog,monomials(x,0:(2*kappa-deg_hi))); 51 | lams_dot = [lams_dot;lami_dot]; 52 | end 53 | sigs_dot = []; 54 | for i = 1:length(g) 55 | deg_gi = g(i).maxdeg; 56 | [prog,sigi_dot] = sossosvar(prog,monomials(x,0:floor((2*kappa-deg_gi)/2))); 57 | sigs_dot = [sigs_dot;sigi_dot]; 58 | end 59 | eq_dot = (- eps2 * (x-x0)'*(x-x0)) - Vdot - sigs_dot'*g - lams_dot'*h; 60 | prog = soseq(prog,eq_dot); 61 | 62 | %% enforce V(x0) = 0, Vdot(x0) = 0 63 | prog = soseq(prog,subs(V,x,x0)); 64 | prog = soseq(prog,subs(Vdot,x,x0)); 65 | 66 | options.solver = 'mosek'; 67 | prog = sossolve(prog,options); 68 | 69 | sol.V = cleanpoly(sosgetsol(prog,V),1e-8); 70 | sol.Vdot = [diff(sol.V,x(1)), diff(sol.V,x(2)), diff(sol.V,x(3))] * fx; 71 | 72 | %% plot V and Vdot 73 | thetadot_range = linspace(-pi/2,pi/2,100); 74 | theta_range = linspace(-acos(3/4),acos(3/4),100); 75 | [theta, thetadot] = meshgrid(theta_range, thetadot_range); 76 | V_val = zeros(100,100); 77 | Vdot_val = zeros(100,100); 78 | 79 | for i = 1:100 80 | for j = 1:100 81 | thetai = theta(i,j); 82 | thetadoti = thetadot(i,j); 83 | xi = pendulum_xi([thetai;thetadoti]); 84 | Vi = subs(sol.V,x,xi); 85 | Vdoti = subs(sol.Vdot,x,xi); 86 | 87 | V_val(i,j) = Vi; 88 | Vdot_val(i,j) = Vdoti; 89 | end 90 | end 91 | 92 | %% generate plots 93 | figure; 94 | surf(theta,thetadot,V_val) 95 | xlabel('$\theta$','FontSize',20,'Interpreter','latex') 96 | ylabel('$\dot{\theta}$','FontSize',20,'Interpreter','latex') 97 | zlabel('$V(x)$','FontSize',20,'Interpreter','latex') 98 | 99 | figure; 100 | surf(theta,thetadot,Vdot_val) 101 | xlabel('$\theta$','FontSize',20,'Interpreter','latex') 102 | ylabel('$\dot{\theta}$','FontSize',20,'Interpreter','latex') 103 | zlabel('$\dot{V}(x)$','FontSize',20,'Interpreter','latex') 104 | 105 | 106 | 107 | %% pendulum dynamics (uncontrolled) 108 | function f = pendulum_f(x,m,b,l,g) 109 | s = x(1); 110 | c = x(2); 111 | thetadot = x(3); 112 | f = [c*thetadot; 113 | -s*thetadot; 114 | -1/(m*l^2)*(b*thetadot + m*g*l*s)]; 115 | end 116 | 117 | function x = pendulum_xi(x_org) 118 | x = [sin(x_org(1)); cos(x_org(1)); x_org(2)]; 119 | end -------------------------------------------------------------------------------- /pendulum_saturated_control_sol.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | restoredefaultpath; 3 | 4 | m = 1; g = 9.8; l = 1; b = 0.1; 5 | umax = 2; 6 | A = [0, 1; g/l, -b/(m*l^2)]; 7 | B = [0; 1/(m*l^2)]; 8 | Q = diag([1,1]); 9 | R = 1; 10 | 11 | [K,S] = lqr(A,B,Q,R,zeros(2,1)); 12 | 13 | x1_lb = -0.2*pi; 14 | x1_ub = 0.2*pi; 15 | x2_lb = -0.2*pi; 16 | x2_ub = 0.2*pi; 17 | 18 | 19 | %% Simulated pendulum with clipped control 20 | pendulum_sat = @(t,x) pendulum_f(x,clip(-K*x,-umax,umax),m,g,l,b); 21 | T = 100; 22 | N = 1000; 23 | stable = []; 24 | unstable = []; 25 | for i = 1:N 26 | x1 = x1_lb + rand*(x1_ub - x1_lb); 27 | x2 = x2_lb + rand*(x2_ub - x2_lb); 28 | x0 = [x1;x2]; 29 | [t,y] = ode89(pendulum_sat,[0,T],x0); 30 | if norm(y(end,:)) < 1e-3 31 | stable = [stable,x0]; 32 | else 33 | unstable = [unstable,x0]; 34 | end 35 | % figure; 36 | % plot(t,y); 37 | end 38 | %% plot 39 | figure; 40 | scatter(stable(1,:),stable(2,:),80,'filled','Marker','o'); 41 | hold on 42 | scatter(unstable(1,:),unstable(2,:),80,'filled','Marker','square'); 43 | xlabel('$x_1$','Interpreter','latex','FontSize',24); 44 | ylabel('$x_2$','Interpreter','latex','FontSize',24); 45 | ax = gca; ax.FontSize = 20; 46 | hold on 47 | 48 | %% certify ROA 49 | sostoolspath = '../SOSTOOLS'; 50 | mosekpath = '../../../mosek'; 51 | addpath(genpath(sostoolspath)); 52 | addpath(genpath(mosekpath)); 53 | 54 | x = mpvar('x',[2,1]); 55 | J = x'*S*x; 56 | eps = 0.01; 57 | rho = 2; 58 | kappa = 4; 59 | prog = sosprogram(x); 60 | 61 | % case 1 62 | Jdot1 = - jacobian(J,x)*pendulum_poly(x,umax,m,g,l,b) - eps*(x'*x); 63 | set1 = [rho - J; -K*x - umax]; 64 | [prog, rhs1] = SOSonSet(prog,x,kappa,set1); 65 | prog = soseq(prog, Jdot1 - rhs1); 66 | 67 | % case 2 68 | Jdot2 = - jacobian(J,x)*pendulum_poly(x,-K*x,m,g,l,b) - eps*(x'*x); 69 | set2 = [rho - J; umax + K*x; -K*x + umax]; 70 | [prog, rhs2] = SOSonSet(prog,x,kappa,set2); 71 | prog = soseq(prog, Jdot2 - rhs2); 72 | 73 | % case 3 74 | Jdot3 = - jacobian(J,x)*pendulum_poly(x,-umax,m,g,l,b) - eps*(x'*x); 75 | set3 = [rho - J; -umax + K*x]; 76 | [prog, rhs3] = SOSonSet(prog,x,kappa,set3); 77 | prog = soseq(prog, Jdot3 - rhs3); 78 | 79 | options.solver = 'mosek'; 80 | prog = sossolve(prog, options); 81 | 82 | e = ellipse(S/rho,zeros(2,1)); 83 | plot(e(1,:),e(2,:),'green','LineWidth',4); 84 | legend('Stabilized','Non-Stabilized','Certified','FontSize',22); 85 | 86 | 87 | %% helper function 88 | function v = clip(u,a,b) 89 | v = u; 90 | if u > b 91 | v = b; 92 | end 93 | if u < a 94 | v = a; 95 | end 96 | end 97 | 98 | function zdot = pendulum_f(z,u,m,g,l,b) 99 | % pendulum true dynamics 100 | z1 = z(1); 101 | z2 = z(2); 102 | 103 | zdot = [ 104 | z2; 105 | 1/(m*l^2)*(u - b*z2 + m*g*l*sin(z1)) 106 | ]; 107 | end 108 | 109 | function zdot = pendulum_poly(z,u,m,g,l,b) 110 | % pendulum approximate polynomial dynamics 111 | z1 = z(1); 112 | z2 = z(2); 113 | 114 | zdot = [ 115 | z2; 116 | 1/(m*l^2)*(u - b*z2 + ... 117 | m*g*l*( z1 - z1^3/(factorial(3)) + z1^5/(factorial(5)))) 118 | ]; 119 | % 120 | end 121 | 122 | function [prog,rhs] = SOSonSet(prog,var,kappa,set_ineq) 123 | % a polynomial p is SOS on a set K defined by inequality constraints 124 | % relaxation order kappa 125 | v0 = monomials(var,0:kappa); 126 | [prog, sig0] = sossosvar(prog,v0); 127 | rhs = sig0; 128 | for j = 1:length(set_ineq) 129 | gj = set_ineq(j); 130 | degj = floor((2*kappa - gj.maxdeg)/2); 131 | vj = monomials(var,0:degj); 132 | [prog, sigj] = sossosvar(prog,vj); 133 | rhs = rhs + sigj*gj; 134 | end 135 | end 136 | 137 | function e = ellipse(A,center) 138 | [U,~] = eig(A); 139 | theta = linspace(0,2*pi,1000); 140 | elipoid = zeros(2,1000); 141 | for pt = 1:1000 142 | elipoid(:,pt) = U(:,1)*sin(theta(pt)) + U(:,2)*cos(theta(pt)); 143 | elipoid(:,pt) = elipoid(:,pt)/sqrt(elipoid(:,pt)'*A*elipoid(:,pt)); 144 | end 145 | e = elipoid + center; 146 | end 147 | -------------------------------------------------------------------------------- /quadrotor2d_sos_observer_simulation.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | % Load solution from SOS program 4 | sol = load('SOS-sols/quadrotor2d_sol.mat').sol; 5 | 6 | sostoolspath = "../SOSTOOLS"; 7 | addpath(genpath(sostoolspath)) 8 | 9 | % Constants 10 | g = 9.8; 11 | 12 | % Model parameters 13 | m = 1; l = 1; r = l/2; I = m*l^2/12; 14 | ns = 7; ny = 4; nu = 2; 15 | obs_idx = [1,3,5,6]; 16 | C = [ 17 | 1, 0, 0, 0, 0, 0, 0; 18 | 0, 0, 1, 0, 0, 0, 0; 19 | 0, 0, 0, 0, 1, 0, 0; 20 | 0, 0, 0, 0, 0, 1, 0; 21 | ]; 22 | 23 | num_steps = 19990; 24 | dt = 0.001; 25 | 26 | 27 | %% simulate real dynamics 28 | x0 = [1;0;1;0;1/2;1/2*sqrt(3);0]; 29 | u_traj = zeros(nu,num_steps); 30 | x_traj = zeros(ns,num_steps+1); 31 | x_traj(:,1) = x0; 32 | x = x0; 33 | for i = 1:num_steps 34 | u1 = 1 + rand; 35 | u2 = 1 + rand; 36 | % u = [u1;u2]; 37 | u = zeros(2,1); 38 | u_traj(:,i) = u; 39 | y = C*x; 40 | xdot = quadrotor2d_f(x) + quadrotor2d_psi(u,y,m,g,I,r); 41 | 42 | x = xdot * dt + x; 43 | x(5:6) = normc(x(5:6)); 44 | x_traj(:,i+1) = x; 45 | end 46 | y_traj = x_traj(obs_idx,:); 47 | theta_traj = atan2(x_traj(5,:),x_traj(6,:)); 48 | 49 | 50 | %% simulate observer 51 | % xhat0 = [0;0;0;0;0;1;0.5]; 52 | xhat0 = x0; 53 | xhat_traj = zeros(ns,num_steps+1); 54 | xhat_traj(:,1) = xhat0; 55 | xhat = xhat0; 56 | Ces = [zeros(4,1)]; 57 | for i = 1:num_steps 58 | yi = y_traj(:,i); 59 | ui = u_traj(:,i); 60 | yhati = C*xhat; 61 | Ce = yhati - yi; 62 | Ces = [Ces, Ce]; 63 | Qi = Q_func(Ce,sol,ns,obs_idx); 64 | Mi = M_func(Ce,yi,sol,ns,obs_idx); 65 | 66 | xhatdot = quadrotor2d_f(xhat) + quadrotor2d_psi(u,yi,m,g,I,r) + (Qi \ Mi)*Ce; 67 | xhat = xhat + xhatdot * dt; 68 | 69 | xhat(5:6) = normc(xhat(5:6)); 70 | xhat_traj(:,i+1) = xhat; 71 | end 72 | 73 | 74 | %% plot comparison 75 | labelsize = 20; 76 | e_norm_traj = sqrt(sum((xhat_traj - x_traj).^2,1)); 77 | 78 | t_traj = (1:(num_steps+1)) * dt; 79 | figure; 80 | tiledlayout(4,1) 81 | nexttile 82 | s_comp = [x_traj(5,:);xhat_traj(1,:)]; 83 | plot(t_traj,s_comp','LineWidth',2) 84 | ylabel('$\sin \theta$','Interpreter','latex','FontSize',labelsize) 85 | xlabel('time','FontSize',labelsize) 86 | ax = gca; 87 | ax.FontSize = 16; 88 | 89 | nexttile 90 | c_comp = [x_traj(6,:);xhat_traj(2,:)]; 91 | plot(t_traj,c_comp','LineWidth',2) 92 | ylabel('$\cos \theta$','Interpreter','latex','FontSize',labelsize) 93 | xlabel('time','FontSize',labelsize) 94 | ax = gca; 95 | ax.FontSize = 16; 96 | 97 | nexttile 98 | thdot_comp = [x_traj(7,:);xhat_traj(3,:)]; 99 | plot(t_traj,thdot_comp','LineWidth',2) 100 | ylabel('$\dot{\theta}$','Interpreter','latex','FontSize',labelsize) 101 | xlabel('time','FontSize',labelsize) 102 | ax = gca; 103 | ax.FontSize = 16; 104 | 105 | nexttile 106 | plot(t_traj,e_norm_traj','LineWidth',2) 107 | ylabel('$\Vert \hat{x} - x \Vert$','Interpreter','latex','FontSize',labelsize) 108 | xlabel('time','FontSize',labelsize) 109 | ax = gca; 110 | ax.FontSize = 16; 111 | 112 | 113 | %% helper functions 114 | function Q = Q_func(Ce,sol,ns,obs_idx) 115 | e = mpvar('e',[ns,1]); 116 | Q_tilde = double(subs(sol.Q, e(obs_idx), Ce)); 117 | Q = Q_tilde + sol.eps*eye(ns); 118 | end 119 | 120 | function M = M_func(Ce,y,sol,ns,obs_idx) 121 | e = mpvar('e',[ns,1]); 122 | x = mpvar('x',[ns,1]); 123 | M = double(subs(sol.M, [e(obs_idx); x(obs_idx)], [Ce;y])); 124 | end 125 | 126 | % State defined as x = [x; xdot; y; ydot; sin(theta); cos(theta); theta_dot] 127 | function f = quadrotor2d_f(x) 128 | f = [x(2); 129 | 0; 130 | x(4); 131 | 0; 132 | x(6)*x(7); 133 | -x(5)*x(7); 134 | 0; 135 | ]; 136 | end 137 | 138 | function psi = quadrotor2d_psi(u,y,m,g,I,r) 139 | psi = [0; 140 | -(u(1)+u(2))*y(3)/m; 141 | 0; 142 | (u(1)+u(2))*y(4)/m - g; 143 | 0; 144 | 0; 145 | 1/I*r*(u(1)-u(2)); 146 | ]; 147 | end 148 | -------------------------------------------------------------------------------- /pendulum_integrator_collocation.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | m = 1; l = 1; g = 9.8; b = 0.1; umax = 5; 4 | initial_state = [0;0]; % start from the bottom 5 | N = 10; 6 | T0 = 5; % initial guess on T 7 | x0 = zeros(2,N); % initial guess on (x1,...,x_N) 8 | u0 = zeros(N,1); % initial guess on (u1,...,u_N) 9 | x0(:,1) = initial_state; 10 | 11 | % % bang-bang for initial guess 12 | % for i = 1:N-1 13 | % u = -0.99*sign(x(2)); 14 | % xdot = [x(2);-(b*x(2)+m*g*l*sin(x(1))+u)/m/l^2]; 15 | % x = x + T0/(N-1)*xdot; 16 | % x0(:,i+1) = x; 17 | % u0(i+1,:) = u; 18 | % end 19 | 20 | v0 = [T0; x0(:); u0(:)]; 21 | lb = [0.1; 22 | -Inf*ones(2*N,1); 23 | -umax*ones(N,1)]; 24 | ub = [50; 25 | Inf*ones(2*N,1); 26 | umax*ones(N,1)]; 27 | 28 | obj = @(v) objective(v,N); 29 | nonlincon = @(v) collocation(v,N,initial_state,m,l,g,b); 30 | 31 | options = optimoptions('fmincon','Algorithm','interior-point',... 32 | 'display','iter','MaxFunctionEvaluations',1e6,'MaxIterations',1e4); 33 | 34 | [vopt,fopt,~,out] = fmincon(obj,v0,... 35 | [],[],[],[],... % no linear (in)equality constraints 36 | lb,ub,... 37 | nonlincon,options); 38 | 39 | fprintf("Maximum constraint violation: %3.2f.\n",out.constrviolation); 40 | fprintf("Objective: %3.2f.\n",fopt); 41 | 42 | %% plot solution 43 | uopt = vopt(2*N+2:end); 44 | xopt = vopt(2:2*N+1); 45 | xopt = reshape(xopt,2,N); 46 | Topt = vopt(1); 47 | t_grid = linspace(0,1,N)*Topt; 48 | figure; 49 | scatter(t_grid,uopt,100,'filled'); hold on; 50 | plot(t_grid,uopt,'LineWidth',2); 51 | xlabel('$t$','FontSize',24,'Interpreter','latex'); 52 | ylabel('$u(t)$','FontSize',24,'Interpreter','latex'); 53 | ax = gca; ax.FontSize = 20; 54 | grid on; 55 | figure; 56 | scatter(t_grid,xopt,100,'filled'); hold on; 57 | plot(t_grid,xopt,'LineWidth',2); 58 | xlabel('$t$','FontSize',24,'Interpreter','latex'); 59 | ylabel('$x(t)$','FontSize',24,'Interpreter','latex'); 60 | grid on; 61 | legend('$\theta(t)$','$\dot{\theta}(t)$','FontSize',20,'Interpreter','latex'); 62 | 63 | 64 | % integrate from the initial state using the found controls 65 | noise = 1.0; 66 | tt = linspace(0,Topt,100); 67 | [t,sol] = ode89(@(t,y) pendulum_ode(t,y,uopt,t_grid,m,l,g,b,noise),tt,initial_state); 68 | figure; 69 | plot(tt,sol,'LineWidth',2); 70 | xlabel('$t$','FontSize',24,'Interpreter','latex'); 71 | ylabel('$x(t)$','FontSize',24,'Interpreter','latex'); 72 | ax = gca; ax.FontSize = 20; 73 | legend('$\theta(t)$','$\dot{\theta}(t)$','FontSize',20,'Interpreter','latex'); 74 | grid on; 75 | 76 | 77 | %% helper functions 78 | function f = objective(v,N) 79 | T = v(1); h = T / (N-1); 80 | x = v(2:2*N+1); x = reshape(x,2,N); 81 | theta = x(1,:); 82 | thetadot = x(2,:); 83 | u = v(2*N+2:end); 84 | 85 | tmp = [cos(theta); 86 | sin(theta); 87 | thetadot]; 88 | 89 | x_cost = sum((tmp - [-1;0;0]).^2,1); 90 | 91 | u_cost = u.^2; 92 | 93 | f = sum(x_cost(:) + u_cost(:)) * h; 94 | end 95 | 96 | 97 | function dx = pendulum(x,u,m,l,g,b) 98 | dx = [x(2); 99 | -1/(m*l^2) * (-u + b*x(2)+m*g*l*sin(x(1))) ]; 100 | end 101 | 102 | function [c,ceq] = collocation(v,N,initial_state,m,l,g,b) 103 | T = v(1); 104 | h = T/(N-1); 105 | x = reshape(v(2:2*N+1),2,N); 106 | u = v(2*N+2:end); 107 | 108 | c = []; 109 | ceq = []; 110 | 111 | for k=1:N-1 112 | uk = u(k); 113 | ukp1 = u(k+1); 114 | xk = x(:,k); 115 | xkp1 = x(:,k+1); 116 | fk = pendulum(xk,uk,m,l,g,b); 117 | fkp1 = pendulum(xkp1,ukp1,m,l,g,b); 118 | 119 | % collocation points 120 | xkc = 0.5*(xk+xkp1) + h/8 * (fk - fkp1); 121 | ukc = 0.5*(uk + ukp1); 122 | dxkc = -3/(2*h) * (xk-xkp1) - 0.25*(fk + fkp1); 123 | 124 | % collocation constraint 125 | ceq = [ceq; 126 | dxkc - pendulum(xkc,ukc,m,l,g,b)]; 127 | end 128 | 129 | ceq = [ceq; 130 | x(:,1) - initial_state; % initial condition 131 | x(:,end) - [pi;0]]; % land at target point 132 | end 133 | 134 | function dx = pendulum_ode(t,states,u_grid,t_grid,m,l,g,b,noise) 135 | u_t = interp1(t_grid,u_grid,t); % piece-wise linear 136 | x = states; 137 | dx = [x(2); 138 | -1/(m*l^2) * (-u_t + b*x(2)+m*g*l*sin(x(1))) ]; 139 | dx = dx + noise*randn(2,1); 140 | end 141 | 142 | 143 | -------------------------------------------------------------------------------- /pendulum_value_iteration_barycentric.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | m = 1; gravity = 9.8; l = 1; b = 0.1; 4 | 5 | N = 50; % number of bins in discretization 6 | 7 | THETA = linspace(-pi,pi,N); % theta between -pi and pi 8 | THETADOT = linspace(-pi,pi,N); % thetadot between -pi and pi 9 | U = linspace(-4.9,4.9,N)'; % control between -4.9 and 4.9 10 | 11 | [X1,X2] = meshgrid(THETA,THETADOT); 12 | X = [X1(:),X2(:)]; 13 | 14 | nx = size(X,1); % number of states 15 | nu = length(U); % number of controls 16 | 17 | % create running cost vector 18 | g = pendulum_running_cost(X,U); 19 | g_mat = reshape(g,nx,nu); 20 | 21 | % construct transition matrix P 22 | % create a triangulation of the mesh points 23 | DT = delaunayTriangulation(X); 24 | figure; triplot(DT); 25 | dt = 0.01; % convert continuous-time dynamics to discrete-time 26 | P_rows = []; % row indices of nonzero entries in P 27 | P_cols = []; % col indices of nonzero entries in P 28 | P_vals = []; % values of nonzero entries in P 29 | count = 1; 30 | for j = 1:nu 31 | u = U(j); % control 32 | for i = 1:nx 33 | x = X(i,:); % state 34 | xp = pendulum_f_z(x(:),u,m,gravity,l,b)*dt + x(:); % next state 35 | % wrap theta between -pi and pi 36 | xp(1) = clip(xp(1),-pi,pi); % use wrapToPi also works 37 | % clip thetadot between -pi and pi 38 | xp(2) = clip(xp(2),-pi,pi); 39 | % find xp's barycentric coordinate in the triangulation 40 | [ID,B] = pointLocation(DT,xp'); 41 | % create probability matrix 42 | if length(ID) > 1 43 | error("point lies inside two triangles.") 44 | end 45 | P_rows = [P_rows,count,count,count]; 46 | P_cols = [P_cols,DT.ConnectivityList(ID,:)]; 47 | P_vals = [P_vals,B]; 48 | count = count + 1; 49 | end 50 | end 51 | P = sparse(P_rows,P_cols,P_vals,nx*nu,nx); 52 | 53 | %% start value iteration 54 | fprintf("Start value iteration ...") 55 | gamma = 0.999; 56 | Q = zeros(nx*nu,1); 57 | iter = 1; 58 | MAX_ITERS = 1e5; 59 | while iter < MAX_ITERS 60 | Q_mat = reshape(Q,nx,nu); 61 | J_Q = min(Q_mat,[],2); 62 | 63 | Q_new = g + gamma*P*J_Q; 64 | Q_diff = norm(Q_new - Q); 65 | Q = Q_new; 66 | iter = iter + 1; 67 | % check convergence 68 | if Q_diff < 1e-9 69 | fprintf("Value iteration converged in %d iterations, Q_diff = %3.2e.\n",iter,Q_diff) 70 | break 71 | end 72 | end 73 | 74 | %% compute optimal cost to go 75 | Q_mat = reshape(Q,nx,nu); 76 | J = min(Q_mat,[],2); 77 | 78 | figure; 79 | surf(reshape(X(:,1),N,N),... 80 | reshape(X(:,2),N,N),... 81 | reshape(J,N,N)); 82 | xlabel("$\theta$",'Interpreter','latex','FontSize',16); 83 | ylabel("$\dot{\theta}$",'Interpreter','latex','FontSize',16); 84 | zlabel("$J^\star$",'Interpreter','latex','FontSize',16) 85 | 86 | %% execute the controller 87 | x = [-0.4;0.3]; 88 | x_traj = x; 89 | u_traj = []; 90 | MAX_ITERS = 1e3; 91 | iter = 1; 92 | while iter < MAX_ITERS 93 | [ID,B] = pointLocation(DT,x'); 94 | vertices = DT.ConnectivityList(ID,:); 95 | u_v = zeros(length(vertices),1); 96 | for j = 1:length(vertices) 97 | v = vertices(j); 98 | [~,id] = min(Q_mat(v,:)); 99 | u_v(j) = U(id); 100 | end 101 | u = B(:)'*u_v; 102 | xp = pendulum_f_z(x(:),u,m,gravity,l,b)*dt + x(:); % next state 103 | u_traj = [u_traj;u]; 104 | 105 | if norm(x - xp) < 1e-6 106 | break; 107 | end 108 | x = xp; 109 | x_traj = [x_traj,x]; 110 | iter = iter + 1; 111 | end 112 | 113 | %% plot comparison 114 | labelsize = 20; 115 | 116 | t_traj = (1:(size(x_traj,2)-1)) * dt; 117 | figure; 118 | tiledlayout(3,1) 119 | nexttile 120 | plot(t_traj,x_traj(1,1:end-1),'LineWidth',2) 121 | ylabel('$z_1$','Interpreter','latex','FontSize',labelsize) 122 | xlabel('time','FontSize',labelsize) 123 | grid on 124 | ax = gca; 125 | ax.FontSize = 16; 126 | 127 | nexttile 128 | plot(t_traj,x_traj(2,1:end-1),'LineWidth',2) 129 | ylabel('$z_2$','Interpreter','latex','FontSize',labelsize) 130 | xlabel('time','FontSize',labelsize) 131 | grid on 132 | ax = gca; 133 | ax.FontSize = 16; 134 | 135 | nexttile 136 | plot(t_traj,u_traj(1:length(t_traj)),'LineWidth',2) 137 | ylabel('$u$','Interpreter','latex','FontSize',labelsize) 138 | xlabel('time','FontSize',labelsize) 139 | grid on 140 | ax = gca; 141 | ax.FontSize = 16; 142 | 143 | 144 | %% helper functions 145 | % pendulum continuous-time dynamics 146 | function zdot = pendulum_f_z(z,u,m,g,l,b) 147 | z1 = z(1); 148 | z2 = z(2); 149 | 150 | zdot = [ 151 | z2; 152 | 1/(m*l^2)*(u - b*z2 + m*g*l*sin(z1)) 153 | ]; 154 | end 155 | 156 | % create pendulum g vector 157 | function g = pendulum_running_cost(X,U) 158 | nx = size(X,1); 159 | nu = length(U); 160 | 161 | g_x = sum(X.^2,2); 162 | g_u = U.^2; 163 | 164 | g = repmat(g_x,1,nu) + repmat(g_u',nx,1); 165 | 166 | g = g(:); 167 | end 168 | 169 | function u_clip = clip(u,umin,umax) 170 | u_clip = min(umax,max(u,umin)); 171 | end 172 | 173 | 174 | 175 | -------------------------------------------------------------------------------- /pendulum_sos_observer_simulation.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | % Load solution from SOS program 4 | sol = load('SOS-sols/pendulum_sol.mat').sol; 5 | 6 | sostoolspath = "../SOSTOOLS"; 7 | addpath(genpath(sostoolspath)) 8 | 9 | m = 1; g = 9.8; l = 1; b = 0.1; 10 | ns = 3; ny = 2; nu =1; 11 | eps = 10^-2; 12 | 13 | num_steps = 5999; 14 | dt = 0.01; 15 | 16 | 17 | %% simulate real dynamics 18 | x0 = [0;1;0]; 19 | u_traj = zeros(1,num_steps); 20 | x_traj = zeros(3,num_steps+1); 21 | x_traj(:,1) = x0; 22 | x = x0; 23 | for i = 1:num_steps 24 | ui = -1 + 2*rand; 25 | u_traj(i) = ui; 26 | y = x(1:2); 27 | xdot = pendulum_f(x,m,b,l) + pendulum_psi(ui,y,m,g,l); 28 | 29 | x = xdot * dt + x; 30 | x(1:2) = normc(x(1:2)); 31 | x_traj(:,i+1) = x; 32 | end 33 | y_traj = x_traj(1:2,:); 34 | theta_traj = atan2(x_traj(1,:),x_traj(2,:)); 35 | 36 | 37 | %% simulate observer 38 | xhat0 = [0;1;2]; 39 | xhat_traj = zeros(3,num_steps+1); 40 | xhat_traj(:,1) = xhat0; 41 | xhat = xhat0; 42 | for i = 1:num_steps 43 | yi = y_traj(:,i); 44 | ui = u_traj(i); 45 | yhati = xhat(1:2); 46 | Ce = yhati - yi; 47 | Qi = Q_func(Ce, sol, ns); 48 | Mi = M_func(Ce,yi,sol,ns); 49 | 50 | xhatdot = pendulum_f(xhat,m,b,l) + pendulum_psi(ui,yi,m,g,l) + (Qi \ Mi)*Ce; 51 | 52 | xhat = xhat + xhatdot * dt; 53 | 54 | xhat(1:2) = normc(xhat(1:2)); 55 | xhat_traj(:,i+1) = xhat; 56 | end 57 | 58 | 59 | %% plot comparison 60 | labelsize = 20; 61 | e_norm_traj = sqrt(sum((xhat_traj - x_traj).^2,1)); 62 | 63 | t_traj = (1:(num_steps+1)) * dt; 64 | figure; 65 | tiledlayout(4,1) 66 | nexttile 67 | s_comp = [x_traj(1,:);xhat_traj(1,:)]; 68 | plot(t_traj,s_comp','LineWidth',2) 69 | ylabel('$\sin \theta$','Interpreter','latex','FontSize',labelsize) 70 | xlabel('time','FontSize',labelsize) 71 | ax = gca; 72 | ax.FontSize = 16; 73 | 74 | nexttile 75 | c_comp = [x_traj(2,:);xhat_traj(2,:)]; 76 | plot(t_traj,c_comp','LineWidth',2) 77 | ylabel('$\cos \theta$','Interpreter','latex','FontSize',labelsize) 78 | xlabel('time','FontSize',labelsize) 79 | ax = gca; 80 | ax.FontSize = 16; 81 | 82 | nexttile 83 | thdot_comp = [x_traj(3,:);xhat_traj(3,:)]; 84 | plot(t_traj,thdot_comp','LineWidth',2) 85 | ylabel('$\dot{\theta}$','Interpreter','latex','FontSize',labelsize) 86 | xlabel('time','FontSize',labelsize) 87 | ax = gca; 88 | ax.FontSize = 16; 89 | 90 | nexttile 91 | plot(t_traj,e_norm_traj','LineWidth',2) 92 | ylabel('$\Vert \hat{x} - x \Vert$','Interpreter','latex','FontSize',labelsize) 93 | xlabel('time','FontSize',labelsize) 94 | ax = gca; 95 | ax.FontSize = 16; 96 | 97 | 98 | %% Create animation 99 | 100 | filename = 'Animations/pendulum_animation.gif'; 101 | figure; 102 | 103 | for i = 1:30:num_steps 104 | clf; 105 | 106 | % Calculate pendulum position 107 | x_pendulum = l * x_traj(1,i); 108 | y_pendulum = -l * x_traj(2,i); 109 | 110 | hold on 111 | grid on 112 | 113 | h_origin = plot(0, 0, 'ko', 'MarkerSize', 5, 'MarkerFaceColor', 'k'); 114 | h_pendulum = plot(0, 0, 'bo', 'MarkerSize', 10, 'MarkerFaceColor', 'b'); 115 | line_handle = line([0, 0], [0, -l], 'LineWidth', 2); 116 | xlims = [-l-0.2, l+0.2]; 117 | xlim(xlims); 118 | ylims = [-l-0.2, l+0.2]; 119 | ylim(ylims); 120 | axis equal; 121 | xlabel('x'); 122 | ylabel('y'); 123 | 124 | % Plot the pendulum 125 | set(h_origin, 'XData', 0, 'YData', 0); 126 | set(h_pendulum, 'XData', x_pendulum, 'YData', y_pendulum); 127 | set(line_handle, 'XData', [0, x_pendulum], 'YData', [0, y_pendulum]); 128 | 129 | % Restore initial x-axis limits 130 | xlim(xlims); 131 | 132 | % Add time annotation 133 | time = i * dt; % calculate current time 134 | annotation_text = sprintf('Time: %.2f s', time); 135 | text(xlims(2)-0.5, ylims(1)+0.2, annotation_text, 'FontSize', 10); 136 | 137 | % Capture the plot as an image 138 | frame = getframe; 139 | im = frame2im(frame); 140 | [imind,cm] = rgb2ind(im,256); 141 | 142 | % Write to the GIF file 143 | if i == 1 144 | imwrite(imind,cm,filename,'gif', 'Loopcount',inf, 'DelayTime', dt); 145 | else 146 | imwrite(imind,cm,filename,'gif','WriteMode','append', 'DelayTime', dt); 147 | end 148 | end 149 | 150 | 151 | %% helper functions 152 | function Q = Q_func(Ce,sol,ns) 153 | e_1 = Ce(1); 154 | e_2 = Ce(2); 155 | e = mpvar('e',[ns,1]); 156 | Q_tilde = double(subs(sol.Q, e([1,2]), [e_1;e_2])); 157 | Q = Q_tilde + sol.eps*eye(ns); 158 | end 159 | 160 | function M = M_func(Ce,y,sol,ns) 161 | e = mpvar('e',[ns,1]); 162 | x = mpvar('x',[ns,1]); 163 | e_1 = Ce(1); 164 | e_2 = Ce(2); 165 | M = double(subs(sol.M, [e([1,2]); x([1,2])], [e_1;e_2;y])); 166 | end 167 | 168 | % State defined as x = [sin(theta), cos(theta), theta_dot] 169 | function f = pendulum_f(x,m,b,l) 170 | const = b / (m * l^2); 171 | f = [x(2)*x(3); 172 | -x(1)*x(3); 173 | -const*x(3) 174 | ]; 175 | end 176 | 177 | function psi = pendulum_psi(u,y,m,g,l) 178 | psi = [ 179 | 0; 180 | 0; 181 | (u - m*g*l*y(1))/(m*l^2) 182 | ]; 183 | end 184 | -------------------------------------------------------------------------------- /cartpole_sos_observer_simulation.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | % Load solution from SOS program 4 | sol = load('SOS-sols/cartpole_sol.mat').sol; 5 | 6 | sostoolspath = "../SOSTOOLS"; 7 | addpath(genpath(sostoolspath)) 8 | 9 | m_c = 1; m_p = 1; l = 1; g = 9.8; 10 | ns = 6; % number of states 11 | nu = 1; % number of inputs 12 | ny = 4; % number of outputs 13 | 14 | eps = 10^-2; 15 | 16 | num_steps = 59990; 17 | dt = 0.001; 18 | 19 | %% simulate real dynamics 20 | 21 | % State defined as x = [x, x_dot, sin(theta), cos(theta), theta_dot, a] 22 | % where a = 1/(m_c/m_p+sin(theta)^2) 23 | 24 | theta0 = pi/3; 25 | s0 = sin(theta0); 26 | c0 = cos(theta0); 27 | a0 = 1/(m_c/m_p + sin(theta0^2)); 28 | x0 = [1;0;s0;c0;0;a0]; 29 | u_traj = zeros(nu,num_steps); 30 | x_traj = zeros(ns,num_steps+1); 31 | x_traj(:,1) = x0; 32 | x = x0; 33 | for i = 1:num_steps 34 | u = randi([-5, 5]); 35 | % u = 0; 36 | u_traj(i) = u; 37 | y = x([1,3,4,6]); 38 | xdot = cartpole_f(x,l) + cartpole_psi(u, y, m_p, l, m_c, g); 39 | 40 | x = x + xdot * dt; 41 | x(3:4) = normc(x(3:4)); 42 | % TODO: check if a variation over time makes sense or not 43 | x_traj(:,i+1) = x; 44 | end 45 | y_traj = x_traj([1,3,4,6],:); 46 | theta_traj = atan2(x_traj(3,:),x_traj(4,:)); 47 | 48 | 49 | %% simulate observer 50 | xhat0 = [0;0;0;1;0;m_p/m_c]; 51 | xhat_traj = zeros(ns,num_steps+1); 52 | xhat_traj(:,1) = xhat0; 53 | xhat = xhat0; 54 | for i = 1:num_steps 55 | yi = y_traj(:,i); 56 | u = u_traj(i); 57 | yhati = xhat([1,3,4,6]); 58 | Ce = yhati - yi; 59 | Qi = Q_func(Ce, eps, sol, ns); 60 | Mi = M_func(Ce, yi, sol, ns); 61 | 62 | xhatdot = cartpole_f(xhat,l) + cartpole_psi(u, y, m_p, l, m_c, g) + (Qi \ Mi)*Ce; 63 | 64 | xhat = xhat + xhatdot * dt; 65 | xhat(3:4) = normc(xhat(3:4)); 66 | xhat_traj(:,i+1) = xhat; 67 | end 68 | theta_hat_traj = atan2(xhat_traj(3,:),xhat_traj(4,:)); 69 | 70 | 71 | %% plot comparison 72 | labelsize = 20; 73 | e_norm_traj = sqrt(sum((xhat_traj - x_traj).^2,1)); 74 | t_traj = (1:(num_steps+1)) * dt; 75 | 76 | figure; 77 | 78 | tiledlayout(5,1) 79 | nexttile 80 | x_comp = [x_traj(1,:);xhat_traj(1,:)]; 81 | plot(t_traj,x_comp','LineWidth',2) 82 | ylabel('$x$','Interpreter','latex','FontSize',labelsize) 83 | xlabel('time','FontSize',labelsize) 84 | ax = gca; 85 | ax.FontSize = 16; 86 | 87 | nexttile 88 | s_comp = [x_traj(3,:);xhat_traj(3,:)]; 89 | plot(t_traj,s_comp','LineWidth',2) 90 | ylabel('$\sin \theta$','Interpreter','latex','FontSize',labelsize) 91 | xlabel('time','FontSize',labelsize) 92 | ax = gca; 93 | ax.FontSize = 16; 94 | 95 | nexttile 96 | c_comp = [x_traj(4,:);xhat_traj(4,:)]; 97 | plot(t_traj,c_comp','LineWidth',2) 98 | ylabel('$\cos \theta$','Interpreter','latex','FontSize',labelsize) 99 | xlabel('time','FontSize',labelsize) 100 | ax = gca; 101 | ax.FontSize = 16; 102 | 103 | nexttile 104 | thdot_comp = [x_traj(5,:);xhat_traj(5,:)]; 105 | plot(t_traj,thdot_comp','LineWidth',2) 106 | ylabel('$\dot{\theta}$','Interpreter','latex','FontSize',labelsize) 107 | xlabel('time','FontSize',labelsize) 108 | ax = gca; 109 | ax.FontSize = 16; 110 | 111 | nexttile 112 | plot(t_traj,e_norm_traj','LineWidth',2) 113 | ylabel('$\Vert \hat{x} - x \Vert$','Interpreter','latex','FontSize',labelsize) 114 | xlabel('time','FontSize',labelsize) 115 | ax = gca; 116 | ax.FontSize = 16; 117 | 118 | %% Create animation 119 | 120 | filename = 'Animations/cartpole_animation.gif'; 121 | figure; 122 | for i = 1:30:num_steps 123 | clf; 124 | 125 | % Calculate cart and pole positions 126 | cart_pos = x_traj(1, i); 127 | pole_pos = l * [sin(theta_traj(i)); -cos(theta_traj(i))]; % Pole is attached to the cart at one end and hangs down 128 | 129 | % Plot the cart 130 | rectangle('Position', [cart_pos-0.5, -0.25, 1, 0.5], 'FaceColor', 'b'); 131 | hold on; 132 | 133 | % Plot the pole 134 | line([cart_pos, cart_pos+pole_pos(1)], [0, pole_pos(2)], 'Color', 'r', 'LineWidth', 2); 135 | 136 | xlims = [-2, 5]; 137 | xlim(xlims); 138 | ylims = [-2, 2]; 139 | ylim(ylims); 140 | axis equal; 141 | set(gca, 'YLimMode', 'manual', 'XLimMode', 'manual'); 142 | xlabel('x'); 143 | ylabel('y'); 144 | 145 | % Add time annotation 146 | time = i * dt; % calculate current time 147 | annotation_text = sprintf('Time: %.2f s', time); 148 | text(xlims(2)-1, ylims(1)+0.2, annotation_text, 'FontSize', 10); 149 | 150 | % Capture the plot as an image 151 | frame = getframe; 152 | im = frame2im(frame); 153 | [imind,cm] = rgb2ind(im,256); 154 | 155 | % Write to the GIF file 156 | if i == 1 157 | imwrite(imind,cm,filename,'gif', 'Loopcount',inf, 'DelayTime', dt); 158 | else 159 | imwrite(imind,cm,filename,'gif','WriteMode','append', 'DelayTime', dt); 160 | end 161 | end 162 | 163 | 164 | %% helper functions 165 | function Q = Q_func(Ce, eps, sol, ns) 166 | e = mpvar('e',[ns,1]); 167 | e_1 = Ce(1); 168 | e_3 = Ce(2); 169 | e_4 = Ce(3); 170 | e_6 = Ce(4); 171 | Q_tilde = double(subs(sol.Q, e([1,3,4,6]), [e_1;e_3;e_4;e_6])); 172 | Q = Q_tilde + eps*eye(ns); 173 | end 174 | 175 | function M = M_func(Ce, y, sol, ns) 176 | e = mpvar('e',[ns,1]); 177 | x = mpvar('x',[ns,1]); 178 | e_1 = Ce(1); 179 | e_3 = Ce(2); 180 | e_4 = Ce(3); 181 | e_6 = Ce(4); 182 | M = double(subs(sol.M, [e([1,3,4,6]); x([1,3,4,6])], [e_1;e_3;e_4;e_6;y])); 183 | end 184 | 185 | % State defined as x = [x, x_dot, sin(theta), cos(theta), theta_dot, a] 186 | % where a = 1/(m_c/m_p+sin(theta)^2) 187 | function f = cartpole_f(x,l) 188 | f = [x(2); 189 | x(6) * x(3)*l*x(5)^2; 190 | x(4)*x(5); 191 | -x(3)*x(5); 192 | -x(6)*x(5)^2*x(3)*x(4); 193 | -2*x(3)*x(4)*x(5)*x(6)^2; 194 | ]; 195 | end 196 | 197 | function psi = cartpole_psi(u, y, m_p, l, m_c, g) 198 | psi = [0; 199 | 1/m_p*y(4)*u + y(4)*y(2)*y(3)*g; 200 | 0; 201 | 0; 202 | -1/(l*m_p)*y(4)*(u*y(3) + (m_c+m_p)*g*y(2)); 203 | 0; 204 | ]; 205 | end 206 | -------------------------------------------------------------------------------- /acrobot_highgain_observer.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | % 3 | cvxpath = "./cvx"; 4 | addpath(genpath(cvxpath)) 5 | 6 | ns = 4; ny = 2; nu = 1; 7 | m_1 = 1; m_2 = 1; l_1 = 1; l_2 = 1; l_c1 = l_1/2; l_c2 = l_2/2; g = 9.8; 8 | I_1 = 1/3*m_1*l_1^2/3; I_2 = 1/3*m_2*l_2^2; 9 | 10 | obs_idx = [1, 2]; 11 | 12 | %% Find the optimal K that maximizes convergence rate 13 | % TODO 14 | k_1 = 1; k_2 = 0.1; L = 1000; 15 | 16 | %% simulate real dynamics 17 | dt = 0.0001; 18 | T = 10; 19 | num_steps = floor(T/dt); 20 | 21 | x0 = [pi/6; pi/6; 1; 1]; 22 | u_traj = zeros(nu,num_steps); 23 | x_traj = zeros(ns,num_steps+1); 24 | x_traj(:,1) = x0; 25 | x = x0; 26 | for i = 1:num_steps 27 | u = rand/3; 28 | u_traj(:,i) = u; 29 | y = x(obs_idx); 30 | xdot1 = x(3:4); 31 | xdot2 = acrobot_Phi(x,u,m_1,m_2,I_1,I_2,l_1,l_c1,l_c2,g); 32 | xdot = [xdot1; xdot2]; 33 | x = x + xdot * dt; 34 | x_traj(:,i+1) = x; 35 | end 36 | y_traj = x_traj(obs_idx,:); 37 | 38 | 39 | %% simulate observer 40 | xhat0 = zeros(ns,1); 41 | xhat_traj = zeros(ns,num_steps+1); 42 | xhat_traj(:,1) = xhat0; 43 | xhat = xhat0; 44 | for i = 1:num_steps 45 | y = y_traj(:,i); 46 | u = u_traj(:,i); 47 | xhatdot1 = xhat(3:4) - L*k_1*(xhat(1:2) - y); 48 | xhatdot2 = acrobot_Phi(xhat,u,m_1,m_2,I_1,I_2,l_1,l_c1,l_c2,g) - L^2*k_2*(xhat(1:2) - y); 49 | xhatdot = [xhatdot1; xhatdot2]; 50 | xhat = xhat + xhatdot * dt; 51 | xhat_traj(:,i+1) = xhat; 52 | end 53 | e_norm_traj = sqrt(sum((xhat_traj - x_traj).^2,1)); 54 | 55 | 56 | %% plot comparison 57 | labelsize = 20; 58 | 59 | PLOT = true; 60 | if PLOT 61 | t_traj = (1:(num_steps+1)) * dt; 62 | figure('Position', [300 200 900 900]); 63 | tiledlayout(5,1) 64 | nexttile 65 | plot(t_traj,x_traj(1,:),'LineWidth',2) 66 | hold on 67 | plot(t_traj,xhat_traj(1,:),'LineWidth',2) 68 | ylabel('$\theta_1$','Interpreter','latex','FontSize',labelsize) 69 | xlabel('time','FontSize',labelsize) 70 | legend('$\theta_1$','$\hat{\theta}_1$','Interpreter','latex','FontSize',labelsize) 71 | ax = gca; 72 | ax.FontSize = 16; 73 | 74 | nexttile 75 | plot(t_traj,x_traj(2,:),'LineWidth',2) 76 | hold on 77 | plot(t_traj,xhat_traj(2,:),'LineWidth',2) 78 | ylabel('$\theta_2$','Interpreter','latex','FontSize',labelsize) 79 | xlabel('time','FontSize',labelsize) 80 | legend('$\theta_2$','$\hat{\theta}_2$','Interpreter','latex','FontSize',labelsize) 81 | ax = gca; 82 | ax.FontSize = 16; 83 | 84 | nexttile 85 | plot(t_traj,x_traj(3,:),'LineWidth',2) 86 | hold on 87 | plot(t_traj,xhat_traj(3,:),'LineWidth',2) 88 | ylabel('$\dot{\theta}_1$','Interpreter','latex','FontSize',labelsize) 89 | xlabel('time','FontSize',labelsize) 90 | legend('$\dot{\theta}_1$','$\dot{\hat{\theta}}_1$','Interpreter','latex','FontSize',labelsize) 91 | ax = gca; 92 | ax.FontSize = 16; 93 | 94 | nexttile 95 | plot(t_traj,x_traj(4,:),'LineWidth',2) 96 | hold on 97 | plot(t_traj,xhat_traj(4,:),'LineWidth',2) 98 | ylabel('$\dot{\theta}_2$','Interpreter','latex','FontSize',labelsize) 99 | xlabel('time','FontSize',labelsize) 100 | legend('$\dot{\theta}_2$','$\dot{\hat{\theta}}_2$','Interpreter','latex','FontSize',labelsize) 101 | ax = gca; 102 | ax.FontSize = 16; 103 | 104 | nexttile 105 | e_norm_traj_log = log(e_norm_traj); 106 | plot(t_traj,e_norm_traj_log,'LineWidth',2) 107 | ylabel('$\log \Vert \hat{x} - x \Vert$','Interpreter','latex','FontSize',labelsize) 108 | xlabel('time','FontSize',labelsize) 109 | ax = gca; 110 | ax.FontSize = 16; 111 | end 112 | 113 | %% Create animation 114 | 115 | ANIMATE = false; 116 | if ANIMATE 117 | filename = 'Animations/acrobot_animation.gif'; 118 | figure; 119 | 120 | delay = round(10^5 * dt); 121 | for i = 1:delay:num_steps 122 | clf; 123 | 124 | % Calculate link positions 125 | link1_pos = [l_1 * sin(x_traj(1, i)), -l_1 * cos(x_traj(1, i))]; 126 | link2_pos = link1_pos + [l_2 * sin(x_traj(1, i) + x_traj(2, i)), -l_2 * cos(x_traj(1, i) + x_traj(2, i))]; 127 | 128 | % Plot the links 129 | hold on; 130 | plot([0, link1_pos(1)], [0, link1_pos(2)], 'b', 'LineWidth', 2); 131 | plot([link1_pos(1), link2_pos(1)], [link1_pos(2), link2_pos(2)], 'r', 'LineWidth', 2); 132 | plot(0, 0, 'ko', 'MarkerSize', 10, 'MarkerFaceColor', 'k'); % Origin point 133 | xlims = [-l_1-l_2-1, l_1+l_2+1]; 134 | ylims = [-l_1-l_2-1, l_1+l_2+1]; 135 | xlim(xlims); 136 | ylim(ylims); 137 | axis equal; 138 | set(gca, 'YLimMode', 'manual', 'XLimMode', 'manual'); 139 | xlabel('x'); 140 | ylabel('y'); 141 | 142 | % Add time annotation 143 | time = i * dt; % calculate current time 144 | annotation_text = sprintf('Time: %.2f s', time); 145 | text(xlims(2)-1, ylims(1)+0.2, annotation_text, 'FontSize', 10); 146 | 147 | % Capture the plot as an image 148 | frame = getframe; 149 | im = frame2im(frame); 150 | [imind,cm] = rgb2ind(im,256); 151 | 152 | % Write to the GIF file 153 | if i == 1 154 | imwrite(imind,cm,filename,'gif', 'Loopcount',inf, 'DelayTime', dt); 155 | else 156 | imwrite(imind,cm,filename,'gif','WriteMode','append', 'DelayTime', dt); 157 | end 158 | end 159 | end 160 | 161 | 162 | %% Helper functions 163 | % State defined as x = [theta_1; theta_2; thetadot_1; thetadot_2] 164 | function Phi = acrobot_Phi(x,u,m_1,m_2,I_1,I_2,l_1,l_c1,l_c2,g) 165 | M = [I_1 + I_2 + m_2*l_1^2 + 2*m_2*l_1*l_c2*cos(x(2)), I_2 + m_2*l_1*l_c2*cos(x(2)); 166 | I_2 + m_2*l_1*l_c2*cos(x(2)), I_2]; 167 | C = [-m_2*l_1*l_c2*sin(x(2))*x(4), -m_2*l_1*l_c2*sin(x(2))*x(4); 168 | m_2*l_1*l_c2*sin(x(2))*x(3), 0]; 169 | g = [-m_1*l_c1*g*sin(x(1)) - m_2*g*(l_1*sin(x(1)) + l_c2*sin(x(1)+x(2))); 170 | -m_2*l_c2*g*sin(x(1)+x(2))]; 171 | tau = [0; u]; 172 | Phi = M \ (tau - C*x(3:4) + g); 173 | end 174 | -------------------------------------------------------------------------------- /Neural-FVI/Training_batch.py: -------------------------------------------------------------------------------- 1 | import torch as th 2 | import numpy as np 3 | from torch.utils.data import DataLoader, TensorDataset 4 | import torch.optim as optim 5 | import control as ctl 6 | from tqdm import tqdm 7 | from typing import Dict, List, Tuple 8 | from torch.func import vmap, jacrev, functional_call 9 | import utils 10 | from CostToGoFunction import CostToGo 11 | from LinearDynamics import LinearSystem 12 | 13 | # Device and Seed Initialization 14 | device = th.device("cuda" if th.cuda.is_available() else "cpu") 15 | seed = th.randint(0, 10000000, (1,)) 16 | th.manual_seed(seed) 17 | if th.cuda.is_available(): 18 | th.cuda.manual_seed_all(seed) 19 | np.random.seed(seed) 20 | print(f"seed = {seed}") 21 | 22 | ######################################################################## 23 | ############################## PARAMETERS ############################## 24 | ######################################################################## 25 | 26 | batch_size = 32768 27 | epoch = 10000 28 | U_MAX = float('inf') 29 | x0max = 8 30 | x1max = 8 31 | batch_size_train = 2048 32 | pretrain = True 33 | 34 | model = CostToGo(input_size=2).to(device) 35 | if pretrain: 36 | model.load_state_dict(th.load('./CTOptimalControlExperiment/Linear/model/initial_model.pth')) 37 | optimizer = optim.Adam(model.parameters(), lr=0.00005) 38 | scheduler = StepLR(optimizer, step_size=50, gamma=0.99) 39 | system = LinearSystem() 40 | 41 | def functionalized_model( 42 | model: CostToGo, 43 | params: Dict, 44 | x: th.Tensor, 45 | ): 46 | res = functional_call(model, params, x) 47 | return res @ th.t(res) 48 | 49 | def get_fx_gx_torch( 50 | x: th.Tensor, 51 | SystemModel: system, 52 | ) -> Tuple[th.Tensor, th.Tensor]: 53 | 54 | batch_size, x_dim = x.shape 55 | fx, gx = system.dynamics_torch(x, device) 56 | 57 | fx = fx.unsqueeze(2) 58 | gx = gx.unsqueeze(2) 59 | return fx, gx 60 | 61 | def LQR_controller(xt): 62 | A = th.tensor([[0, 1], 63 | [0, 0]], dtype=th.float32, device=device) 64 | 65 | B = th.tensor([[0], 66 | [1]], dtype=th.float32, device=device) 67 | 68 | Q = th.tensor([[1, 0], 69 | [0, 1]], dtype=th.float32, device=device) 70 | 71 | R = th.tensor([[1]], dtype=th.float32, device=device) 72 | 73 | K, S, E = ctl.lqr(A.cpu(), B.cpu(), Q.cpu(), R.cpu()) 74 | th_K = th.tensor(K, dtype=th.float32, device=device) 75 | u = -th_K @ xt 76 | 77 | return u 78 | 79 | 80 | if __name__ == "__main__": 81 | x = th.tensor(utils.prepare_data(sample_number=batch_size, x0max=x0max, x1max=x1max, vector_length=2), dtype=th.float32, requires_grad=True).to(device) 82 | x_eval = th.tensor(utils.prepare_data(sample_number=16 * batch_size_train, x0max=x0max, x1max=x1max, vector_length=2), dtype=th.float32, requires_grad=True).to(device) 83 | 84 | # DataLoader Initialization 85 | train_dataset1 = TensorDataset(x) 86 | train_loader1 = DataLoader(train_dataset1, batch_size=batch_size_train, shuffle=True) 87 | eval_dataset1 = TensorDataset(x_eval) 88 | eval_loader1 = DataLoader(eval_dataset1, batch_size=batch_size_train, shuffle=True) 89 | 90 | single_dJdx_func = jacrev(functionalized_model, argnums=2) 91 | batch_dJdx_func = vmap(single_dJdx_func, in_dims=(None, None, 0)) 92 | batch_th_dot = vmap(th.dot) 93 | batch_LQR_controller = vmap(LQR_controller) 94 | 95 | # loss 96 | loss_values = [] 97 | best_val_loss = float('inf') 98 | 99 | # Training 100 | for i in range(epoch): 101 | for (batch_x,)in tqdm(train_loader1): 102 | batch_dJdx = batch_dJdx_func(model, dict(model.named_parameters()), batch_x) 103 | batch_lqr_u = th.squeeze(batch_LQR_controller(batch_x)) 104 | fx, gx = get_fx_gx_torch(batch_x, system) 105 | batch_dJdx = batch_dJdx.unsqueeze(1) 106 | 107 | LfJ = th.squeeze(batch_dJdx @ fx) # [batch_size, ] 108 | LgJ = th.squeeze(batch_dJdx @ gx) # [batch_size, ] 109 | batch_x_square = batch_th_dot(batch_x, batch_x) # [batch_size, ] 110 | 111 | batch_h1 = (batch_x_square + LfJ - 0.25 * (LgJ**2)) # [batch_size, ] 112 | 113 | sign_loss = 0.05*th.sum(th.relu(LgJ * batch_lqr_u)) 114 | wrong_number = 0.5*th.sum(th.abs(th.sign(LgJ) + th.sign(batch_lqr_u))) 115 | assert th.all(th.relu(LgJ * batch_lqr_u).ge(0)) 116 | 117 | x0 = th.tensor([0.0,0.0], dtype=th.float32, device=device) 118 | residual_loss = (th.sum(th.abs(batch_h1)))/batch_size_train + th.dot(model(x0), model(x0)) 119 | loss = sign_loss + residual_loss 120 | 121 | loss_values.append(loss.item()) 122 | 123 | optimizer.zero_grad() 124 | loss.backward() 125 | optimizer.step() 126 | scheduler.step() 127 | 128 | model.eval() 129 | with th.no_grad(): 130 | loss_eval = 0 131 | for (batch_x_eval,) in tqdm(eval_loader1): 132 | batch_dJdx_eval = batch_dJdx_func(model, dict(model.named_parameters()), batch_x_eval) 133 | fx_eval, gx_eval = get_fx_gx_torch(batch_x_eval, system) 134 | batch_lqr_u_eval = th.squeeze(batch_LQR_controller(batch_x_eval)) 135 | 136 | batch_dJdx_eval = batch_dJdx_eval.unsqueeze(1) 137 | 138 | LfJ_eval = th.squeeze(batch_dJdx_eval @ fx_eval) # [batch_size, ] 139 | LgJ_eval = th.squeeze(batch_dJdx_eval @ gx_eval) # [batch_size, ] 140 | batch_x_square_eval = batch_th_dot(batch_x_eval, batch_x_eval) # [batch_size, ] 141 | 142 | batch_h1_eval = (batch_x_square_eval + LfJ_eval - 0.25 * (LgJ_eval**2)) # [batch_size, ] 143 | 144 | wrong_number_eval = 0.5*th.sum(th.abs(th.sign(LgJ_eval) + th.sign(batch_lqr_u_eval))) 145 | sign_loss_eval = 0.01*th.sum(th.relu(LgJ_eval * batch_lqr_u_eval)) 146 | residual_loss_eval = (th.sum(th.abs(batch_h1_eval)))/batch_size_train + th.dot(model(x0), model(x0)) 147 | loss_eval += sign_loss_eval + residual_loss_eval 148 | 149 | loss_eval /= 16 150 | if loss_eval < best_val_loss: 151 | best_val_loss = loss_eval 152 | # th.save(model.state_dict(), './CTOptimalControlExperiment/Linear/model/best_model.pth') 153 | 154 | print(f'epoch: {i}, loss: {loss.item()}, loss_eval: {loss_eval}') 155 | # th.save(model.state_dict(), './CTOptimalControlExperiment/Linear/model/last_model.pth') 156 | 157 | -------------------------------------------------------------------------------- /quadrotor_highgain_observer_simulation.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | m = 1; g = 9.8; l = 1; b = 0.1; 4 | cvxpath = "../cvx"; 5 | addpath(genpath(cvxpath)) 6 | 7 | L = 200; 8 | k = [1;1]; 9 | num_steps = 5999; 10 | dt = 0.001; 11 | A = [-k(1) 1;-k(2) 0]; 12 | J = diag([1;1;1]); 13 | J_inv = inv(J); 14 | rng(1); 15 | %A = [1 0;0 1]; 16 | %% confirm lambda 17 | % lambda = 0.249; 18 | cvx_begin sdp 19 | variable P(2,2) symmetric 20 | variable lam(1,1) 21 | maximize(lam) 22 | subject to 23 | % diag(P) == 1 24 | A'*P+P*A+4*lam*P <=0 25 | P >= 0 26 | cvx_end 27 | lambda_max(P) 28 | lambda_min(P) 29 | lambda_max(A'*P + P*A + 4*lambda*P) 30 | lambda_min(A'*P + P*A + 4*lambda*P) 31 | coff_1 = -(2*lambda*L-b*sqrt(2/(L^2-1))*L*sqrt(lambda_max(P)/lambda_min(P))); 32 | %% simulate real dynamics 33 | eulerangle0 = [0;0;0]; 34 | q0 = [1;0;0;0]; 35 | tr0 = [0;0;0]; 36 | v0 = [1;0;1]; 37 | w0 = [0;0;0]; 38 | x0 = [tr0;v0;w0;q0]; 39 | rotorpos1 = [1;0;0]; 40 | rotorpos2 = [0;1;0]; 41 | u_traj = zeros(6,num_steps+1); 42 | x_traj = zeros(13,num_steps+1); 43 | rotor_traj = zeros(6,num_steps+1); 44 | %seperate x and R because R is a matrix 45 | x_traj(:,1) = x0; 46 | rotor_traj(:,1) = [rotorpos1;rotorpos2]; 47 | x = x0; 48 | for i = 1:num_steps 49 | %force we have in the body frame 50 | f = [0;0;9]; 51 | %torque we have in the body frame 52 | tor = [50;-50;0]; 53 | ui = [f;tor]; 54 | u_traj(:,i) = ui; 55 | R = (eye(3) + 2*x(10)*wHat_func(x(11:13)) + 2*wHat_func(x(11:13))*wHat_func(x(11:13))); 56 | xdot = [x(4:6);f/m-[0;0;g];-J_inv*cross(x(7:9),J*x(7:9))+tor;1/2*[-x(11:13)';x(10)*eye(3)+wHat_func(x(11:13))]*x(7:9)]; 57 | % xdot = [x(4:6);(eye(3)+2*x(10)*wHat_func(x(11:13))+2*wHat_func(x(11:13))*wHat_func(x(11:13)))*f/m-[0;0;g];-J_inv*cross(x(7:9),J*x(7:9))+tor;1/2*[-x(11:13)';x(10)*eye(3)+wHat_func(x(11:13))]*x(7:9)]; 58 | x = xdot * dt + x; 59 | x(10:13) = normalize(x(10:13),'norm',2); 60 | % Rdot = R*crossmatrix(x(7:9)); 61 | % R = Rdot * dt + R; 62 | % R = expmap(R); 63 | x_traj(:,i+1) = x; 64 | rotor_traj(:,i+1) = [R*rotorpos1;R*rotorpos2]; 65 | if(mod(i,50)==0) 66 | plot3(x_traj(1,i+1),x_traj(2,i+1),x_traj(3,i+1),'o'); 67 | hold on; 68 | plot3(x_traj(1,i+1)+rotor_traj(1,i+1),x_traj(2,i+1)+rotor_traj(2,i+1),x_traj(3,i+1)+rotor_traj(3,i+1),'o'); 69 | hold on; 70 | plot3(x_traj(1,i+1)-rotor_traj(1,i+1),x_traj(2,i+1)-rotor_traj(2,i+1),x_traj(3,i+1)-rotor_traj(3,i+1),'o'); 71 | hold on; 72 | plot3(x_traj(1,i+1)+rotor_traj(4,i+1),x_traj(2,i+1)+rotor_traj(5,i+1),x_traj(3,i+1)+rotor_traj(6,i+1),'o'); 73 | hold on; 74 | plot3(x_traj(1,i+1)-rotor_traj(4,i+1),x_traj(2,i+1)-rotor_traj(5,i+1),x_traj(3,i+1)-rotor_traj(6,i+1),'o'); 75 | axis([-10 10 -10 10 -5 5]); 76 | mov(i/50) = getframe; 77 | clf 78 | end 79 | end 80 | % movie(mov,1); 81 | %R is not in here 82 | y_traj = [x_traj(1:3,:);x_traj(10:13,:)]; 83 | %% draw simulation 84 | % plot3(x_traj(1,:),x_traj(2,:),x_traj(3,:)); 85 | % hold on; 86 | % plot3(x_traj(1,:)+rotor_traj(1,:),x_traj(2,:)+rotor_traj(2,:),x_traj(3,:)+rotor_traj(3,:)); 87 | % hold on; 88 | % plot3(x_traj(1,:)-rotor_traj(1,:),x_traj(2,:)-rotor_traj(2,:),x_traj(3,:)-rotor_traj(3,:)); 89 | % hold on; 90 | % plot3(x_traj(1,:)+rotor_traj(4,:),x_traj(2,:)+rotor_traj(5,:),x_traj(3,:)+rotor_traj(6,:)); 91 | % hold on; 92 | % plot3(x_traj(1,:)-rotor_traj(4,:),x_traj(2,:)-rotor_traj(5,:),x_traj(3,:)-rotor_traj(6,:)); 93 | % hold on; 94 | % axis equal 95 | % axis tight 96 | %% simulate observer 97 | qhat0 = [1;0;0;0]; 98 | trhat0 = [0;0;0]; 99 | vhat0 = [1;0;1]; 100 | what0 = [0;0;0]; 101 | xhat0 = [tr0;v0;w0;q0]; 102 | xhat_traj = zeros(13,num_steps+1); 103 | rotor_traj = zeros(6,num_steps+1); 104 | %seperate x and R because R is a matrix 105 | x_traj(:,1) = x0; 106 | rotor_traj(:,1) = [rotorpos1;rotorpos2]; 107 | x = x0; 108 | for i = 1:num_steps 109 | %force we have in the body frame 110 | f = [0;0;9]; 111 | %torque we have in the body frame 112 | tor = [5;-5;0]; 113 | ui = [f;tor]; 114 | u_traj(:,i) = ui; 115 | R = (eye(3)+2*x(10)*wHat_func(x(11:13))+2*wHat_func(x(11:13))*wHat_func(x(11:13))); 116 | xdot = [x(4:6);f/m-[0;0;g];-J_inv*cross(x(7:9),J*x(7:9))+tor;1/2*[-x(11:13)';x(10)*eye(3)+wHat_func(x(11:13))]*x(7:9)]; 117 | % xdot = [x(4:6);(eye(3)+2*x(10)*wHat_func(x(11:13))+2*wHat_func(x(11:13))*wHat_func(x(11:13)))*f/m-[0;0;g];-J_inv*cross(x(7:9),J*x(7:9))+tor;1/2*[-x(11:13)';x(10)*eye(3)+wHat_func(x(11:13))]*x(7:9)]; 118 | x = xdot * dt + x; 119 | x(10:13) = normalize(x(10:13),'norm',2); 120 | % Rdot = R*crossmatrix(x(7:9)); 121 | % R = Rdot * dt + R; 122 | % R = expmap(R); 123 | x_traj(:,i+1) = x; 124 | rotor_traj(:,i+1) = [R*rotorpos1;R*rotorpos2]; 125 | if(mod(i,50)==0) 126 | plot3(x_traj(1,i+1),x_traj(2,i+1),x_traj(3,i+1),'o'); 127 | hold on; 128 | plot3(x_traj(1,i+1)+rotor_traj(1,i+1),x_traj(2,i+1)+rotor_traj(2,i+1),x_traj(3,i+1)+rotor_traj(3,i+1),'o'); 129 | hold on; 130 | plot3(x_traj(1,i+1)-rotor_traj(1,i+1),x_traj(2,i+1)-rotor_traj(2,i+1),x_traj(3,i+1)-rotor_traj(3,i+1),'o'); 131 | hold on; 132 | plot3(x_traj(1,i+1)+rotor_traj(4,i+1),x_traj(2,i+1)+rotor_traj(5,i+1),x_traj(3,i+1)+rotor_traj(6,i+1),'o'); 133 | hold on; 134 | plot3(x_traj(1,i+1)-rotor_traj(4,i+1),x_traj(2,i+1)-rotor_traj(5,i+1),x_traj(3,i+1)-rotor_traj(6,i+1),'o'); 135 | axis([-10 10 -10 10 -5 5]); 136 | mov(i/50) = getframe; 137 | clf 138 | end 139 | end 140 | 141 | 142 | %% plot comparison 143 | 144 | coff_2 = log(lambda_max(P)/lambda_min(P))/2+log((xhat0(1)-x0(1))^2+((xhat0(2)-x0(2))/L)^2)/2; 145 | labelsize = 20; 146 | e_norm_traj = sqrt(sum((xhat_traj(1,:) - x_traj(1,:)).^2+(xhat_traj(2,:) - x_traj(2,:)).^2/L^2,1)); 147 | 148 | t_traj = (1:(num_steps+1)) * dt; 149 | figure; 150 | % tiledlayout(3,1) 151 | % nexttile 152 | % s_comp = [x_traj(1,:);xhat_traj(1,:)]; 153 | % plot(t_traj,s_comp','LineWidth',2) 154 | % ylabel('$\theta$','Interpreter','latex','FontSize',labelsize) 155 | % xlabel('time','FontSize',labelsize) 156 | % ax = gca; 157 | % ax.FontSize = 16; 158 | % 159 | % nexttile 160 | % c_comp = [x_traj(2,:);xhat_traj(2,:)]; 161 | % plot(t_traj,c_comp','LineWidth',2) 162 | % ylabel('$\dot{\theta}$','Interpreter','latex','FontSize',labelsize) 163 | % xlabel('time','FontSize',labelsize) 164 | % ax = gca; 165 | % ax.FontSize = 16; 166 | 167 | 168 | nexttile 169 | bound = coff_2+coff_1*t_traj; 170 | plot(t_traj,log(e_norm_traj'),'LineWidth',2) 171 | hold on; 172 | plot(t_traj,bound','LineWidth',2) 173 | ylabel('$log (\Vert \hat{x} - x \Vert)$','Interpreter','latex','FontSize',labelsize) 174 | xlabel('time','FontSize',labelsize) 175 | ax = gca; 176 | ax.FontSize = 16; 177 | 178 | %% functions 179 | function wHat = wHat_func(x) 180 | wHat = [0 -x(3) x(2);x(3) 0 -x(1);-x(2) x(1) 0]; 181 | end --------------------------------------------------------------------------------