├── Derive_EoM.m ├── MAIN_linearPlant.m ├── MAIN_trajectory.m ├── README.md ├── dynamics.m ├── eventFunc.m ├── finiteLqr.m ├── getContour.m ├── getLinSys.m ├── getLinTraj.m ├── license.txt ├── randDisk.m ├── reachableDynamics.m ├── reachableDynamicsRel.m ├── reachableForward.m ├── reachableForwardRel.m ├── reachableSet.png ├── rhs.m ├── stabilizingController.m └── trajectoryLqr.m /Derive_EoM.m: -------------------------------------------------------------------------------- 1 | %Drive Equations of Motion 2 | 3 | %Non-linear 2-dimensional system, with one input 4 | 5 | syms x y u 'real' 6 | 7 | % % dx = y*sin(x) + y*y; 8 | % % dy = x*y + x*cos(x)+ u*cos(x); 9 | 10 | dx = sin(y)+1-cos(u); 11 | dy = sin(x)+sin(u); 12 | 13 | z = [x;y]; 14 | f = [dx;dy]; 15 | 16 | A = jacobian(f,z); 17 | B = jacobian(f,u); 18 | 19 | %Write the dynamics to a file 20 | matlabFunction(f(1),f(2),'file','dynamics.m','vars',{'x','y','u'},'outputs',{'dx','dy'}); 21 | 22 | %Write the linearized dynamics to a file 23 | matlabFunction(A,B,'file','getLinSys.m','vars',{'x','y','u'},'outputs',{'A','B'}); 24 | 25 | 26 | -------------------------------------------------------------------------------- /MAIN_linearPlant.m: -------------------------------------------------------------------------------- 1 | %MAIN 2 | % 3 | % Demonstrates how to solve the finite-horizon continuous-time linear 4 | % quadratic regulator problem for a linear system 5 | % 6 | 7 | nState = 3; 8 | nInput = 1; 9 | nSoln = 100; 10 | 11 | %LTI plant 12 | A = 0.5*eye(nState) + 0.5*rand(nState); 13 | B = rand(nState,nInput); 14 | 15 | %LTI cost 16 | Q = eye(nState); 17 | R = eye(nInput); 18 | F = zeros(nState); %Terminal Cost 19 | 20 | %Solve continuous LQR problem: 21 | [K,S,E] = lqr(A,B,Q,R); 22 | 23 | %Solve the finite-horizon version 24 | tSpan = [0,30]; %Large time... 25 | tol = 1e-6; 26 | Soln = finiteLqr(tSpan,A,B,Q,R,F,nSoln,tol); 27 | 28 | %Compare the results: (Should go to zero for tSpan(2) goes to infinity) 29 | K_error = K - Soln(1).K 30 | S_error = S - Soln(1).S 31 | E_error = E - Soln(1).E 32 | 33 | %Make a plot showing what the gains look like: 34 | figure(21); clf; 35 | KK = reshape([Soln.K],nState,nSoln); 36 | t = [Soln.t]; 37 | for i=1:nState 38 | subplot(nState,1,i); hold on; 39 | plot(tSpan,[K(i), K(i)],'k--','LineWidth',2) 40 | plot(t,KK(i,:),'r-','LineWidth',2); 41 | ylabel(['K(' num2str(i) ')']); 42 | end 43 | xlabel('Time') 44 | 45 | subplot(nState,1,1); 46 | title('Finite-Horizon Gains, Compare to Infinite Horizon Soln') 47 | 48 | -------------------------------------------------------------------------------- /MAIN_trajectory.m: -------------------------------------------------------------------------------- 1 | %MAIN 2 | % 3 | % This script is used for testing the continuous, finite horizon, LQR 4 | % solver that I'm working on. 5 | % 6 | % 7 | clear; clc; 8 | 9 | 10 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 11 | % User Set Parameters % 12 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 13 | 14 | nSim = 25; 15 | testPerturbation = 0.1; %Initial position error amplitude 16 | 17 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 18 | % Draw the dynamical system % 19 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 20 | 21 | n = 13; 22 | xLim = [-4,1.5]; 23 | yLim = [-4,1]; 24 | x = linspace(xLim(1),xLim(2),n); 25 | y = linspace(yLim(1),yLim(2),n); 26 | 27 | [xx,yy] = ndgrid(x,y); 28 | 29 | [dxx, dyy] = dynamics(xx,yy,zeros(n,n)); 30 | 31 | figure(1); clf; hold on; 32 | title('Dynamical System - Simulations'); 33 | quiver(xx,yy,dxx,dyy); 34 | xlabel('x'); 35 | ylabel('y'); 36 | 37 | 38 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 39 | % Compute a reference trajectory % 40 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 41 | 42 | options = odeset(); 43 | options.Events = @(t,z)eventFunc(t,z,xLim,yLim); 44 | tSpan = [0,-10]; 45 | 46 | control = @(t) -2*sin(0.4*t-0.1)+0.6*cos(0.9*t+0.1); 47 | 48 | userFunc = @(t,z)rhs(t,z,control(t)); 49 | z0 = [0;0]; 50 | 51 | sol = ode45(userFunc,tSpan,z0,options); 52 | 53 | nTime = 70; 54 | tSol = linspace(sol.x(end),sol.x(1),nTime); 55 | zSol = deval(sol,tSol); 56 | xSol = zSol(1,:); 57 | ySol = zSol(2,:); 58 | uSol = control(tSol); 59 | tSpan = [tSol(1),tSol(end)]; 60 | 61 | plot(xSol,ySol,'r-','LineWidth',3); 62 | plot(0,0,'ro','MarkerSize',10,'LineWidth',2) 63 | axis(1.2*[xLim,yLim]); axis equal; 64 | 65 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 66 | % Store the reference trajectory % 67 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 68 | 69 | %We will store each trajectory as a polynomial. For low-order fits, say 70 | %less than 10th order, Matlab's polyval and polyfit are good. For 71 | %higher-order fit's, it is best to use different method, based on 72 | %barycentric interpolation. Google chebyfun. 73 | 74 | nFit = 5; %Order of polynomial fitting 75 | xFit = polyfit(tSol,xSol,nFit); 76 | yFit = polyfit(tSol,ySol,nFit); 77 | uFit = polyfit(tSol,uSol,nFit); 78 | 79 | figure(2); clf; 80 | subplot(3,1,1); hold on; 81 | plot(tSol,xSol,'k.','MarkerSize',15) 82 | plot(tSol,polyval(xFit,tSol),'r-') 83 | xlim(tSpan); 84 | ylabel('x') 85 | title('Polynomial approximation of trajectory') 86 | subplot(3,1,2); hold on; 87 | plot(tSol,ySol,'k.','MarkerSize',15) 88 | plot(tSol,polyval(yFit,tSol),'r-') 89 | xlim(tSpan); 90 | ylabel('y') 91 | subplot(3,1,3); hold on; 92 | plot(tSol,uSol,'k.','MarkerSize',15) 93 | plot(tSol,polyval(uFit,tSol),'r-') 94 | xlim(tSpan); 95 | ylabel('u') 96 | xlabel('t') 97 | 98 | 99 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 100 | % Compute LQR Gains along trajectory % 101 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 102 | 103 | %Function handle for getting linearized system dynamics 104 | linSys = @(t)getLinTraj(t,xFit,yFit,uFit); 105 | 106 | Q = eye(2); % Running cost on state 107 | R = 1; % Running cost on input 108 | F = eye(2); % Terminal cost on state 109 | tol = 1e-6; % Accuracy of ricatti propagation 110 | 111 | Soln = trajectoryLqr(tSol,linSys,Q,R,F,tol); 112 | 113 | K = reshape([Soln.K],2,nTime); 114 | kxFit = polyfit(tSol,K(1,:),nFit); 115 | kyFit = polyfit(tSol,K(2,:),nFit); 116 | 117 | figure(3); clf; 118 | subplot(2,1,1); hold on; 119 | plot(tSol,K(1,:),'k.','MarkerSize',15) 120 | plot(tSol,polyval(kxFit,tSol),'r-') 121 | xlim(tSpan); 122 | ylabel('Kx') 123 | title('Polynomial approximation of lqr gains') 124 | subplot(2,1,2); hold on; 125 | plot(tSol,K(2,:),'k.','MarkerSize',15) 126 | plot(tSol,polyval(kyFit,tSol),'r-') 127 | xlim(tSpan); 128 | ylabel('Ky') 129 | xlabel('t') 130 | 131 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 132 | % Compare to infinite-horizon LQR gains % 133 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 134 | 135 | ssSoln(nTime).t = 0; 136 | ssSoln(nTime).K = zeros(1,2); 137 | ssSoln(nTime).S = zeros(2); 138 | ssSoln(nTime).E = zeros(2,1); 139 | for i=1:nTime 140 | tNow = tSol(i); 141 | [A,B] = linSys(tNow); 142 | [KK,SS,EE] = lqr(A,B,Q,R); 143 | ssSoln(i).t = 0; 144 | ssSoln(i).K = KK; 145 | ssSoln(i).S = SS; 146 | ssSoln(i).E = EE; 147 | end 148 | 149 | ssK = reshape([ssSoln.K],2,nTime); 150 | figure(4); clf; 151 | subplot(2,1,1); hold on; 152 | plot(tSol,K(1,:),'k.','MarkerSize',15) 153 | plot(tSol,ssK(1,:),'bo','MarkerSize',10) 154 | legend('finite','infinite') 155 | xlim(tSpan); 156 | ylabel('Kx') 157 | title('Compare finite vs infinite horizon gains') 158 | subplot(2,1,2); hold on; 159 | plot(tSol,K(2,:),'k.','MarkerSize',15) 160 | plot(tSol,ssK(2,:),'bo','MarkerSize',10) 161 | legend('finite','infinite') 162 | xlim(tSpan); 163 | ylabel('Ky') 164 | xlabel('t') 165 | 166 | 167 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 168 | % Lyapunov Stability Analysis % 169 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 170 | 171 | % Let V(x) = x'Sx, where S is the cost-to-go matrix from LQR solution: 172 | 173 | %%%% TODO %%%% 174 | 175 | % Plan: 176 | % 177 | % 1) Represent the S matrix, which is time-varying, as a polynomial. 178 | % 179 | % 2) Compute the derivative of S wrt time. 180 | % 181 | % 3) Try the Lyapunov function: 182 | % V(x,t) = x'*S(t)*x % x = perturbation from nominal 183 | % dV(x,t) = 2*x*f(x,t)*S(t)*dS(t) %Chain Rule (check math...) 184 | % 185 | % 4) Verify stability by checking signs of lyapunov function and 186 | % derivative? 187 | % 188 | % 5) Not sure if this whole approach is correct - check against papers... 189 | % 190 | 191 | 192 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 193 | % Run a bunch of simulations % 194 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 195 | 196 | controller = @(t,z)stabilizingController(t,z(1,:),z(2,:),xFit,yFit,uFit,kxFit,kyFit); 197 | userFunc = @(t,z)rhs(t,z,controller(t,z)); 198 | zNom = [xSol(1);ySol(1)]; 199 | 200 | xSim = zeros(nTime,nSim); 201 | ySim = zeros(nTime,nSim); 202 | uSim = zeros(nTime,nSim); 203 | failFlag = false(nSim,1); 204 | for i=1:nSim 205 | try 206 | z0 = zNom + testPerturbation*randn(2,1); 207 | [~, yout] = ode45(userFunc,tSol,z0); 208 | xSim(:,i) = yout(:,1); 209 | ySim(:,i) = yout(:,2); 210 | uSim(:,i) = controller(tSol,yout'); 211 | catch ME 212 | xSim(:,i) = z0(1); 213 | ySim(:,i) = z0(2); 214 | uSim(:,i) = 0; 215 | failFlag(i) = true; 216 | end 217 | end 218 | 219 | % Plot the simulated trajectories: 220 | figure(1); hold on; %Plot on top of dynamics vector field 221 | for i=1:nSim 222 | plot(xSim(:,i),ySim(:,i),'k-','LineWidth',1); 223 | end 224 | 225 | %%%% Plot trajectories against time: 226 | figure(5); clf; 227 | 228 | for i=1:nSim 229 | if ~failFlag(i) 230 | subplot(3,1,1); hold on; 231 | plot(tSol,xSim(:,i),'k-','LineWidth',1) 232 | subplot(3,1,2); hold on; 233 | plot(tSol,ySim(:,i),'k-','LineWidth',1) 234 | subplot(3,1,3); hold on; 235 | plot(tSol,uSim(:,i),'k-','LineWidth',1) 236 | end 237 | end 238 | 239 | subplot(3,1,1); hold on; 240 | plot(tSol,xSol,'r-','LineWidth',3) 241 | xlim(tSpan); 242 | ylabel('x') 243 | title('Compare simulations against reference trajectory') 244 | subplot(3,1,2); hold on; 245 | plot(tSol,ySol,'r-','LineWidth',3) 246 | xlim(tSpan); 247 | ylabel('y') 248 | subplot(3,1,3); hold on; 249 | plot(tSol,uSol,'r-','LineWidth',3) 250 | xlim(tSpan); 251 | ylabel('u') 252 | xlabel('t') 253 | 254 | 255 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 256 | % Computate the backward reachable set -- Convex Hull Method % 257 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 258 | 259 | nSamples = 500; %How many simulations to run? 260 | nPointsPerSample = 10; %Used for computing reachable set 261 | 262 | % What is "close enough" to the final state? 263 | xRadFinal = 0.015; 264 | yRadFinal = 0.015; 265 | 266 | %Compute a random set of initial conditions 267 | IC = randDisk(nSamples,[xRadFinal,yRadFinal]); 268 | 269 | %Simulation runs backwards in time 270 | tSpanFlip = linspace(tSpan(2),tSpan(1),nPointsPerSample); %Run system backwards in time 271 | 272 | %Create a data structure for storing the point cloud: 273 | ReachCvx.t = ones(nSamples,1)*tSpanFlip; % Time 274 | ReachCvx.x = zeros(nSamples,nPointsPerSample); % x position 275 | ReachCvx.y = zeros(nSamples,nPointsPerSample); % y position 276 | ReachCvx.c(nPointsPerSample).x = []; %x contour 277 | ReachCvx.c(nPointsPerSample).y = []; %x contour 278 | 279 | %Run the simulation: 280 | for i=1:nSamples 281 | idxLow = (i-1)*nPointsPerSample+1; 282 | idxUpp= i*nPointsPerSample; 283 | [~, yout] = ode45(userFunc,tSpanFlip,IC(:,i)); 284 | ReachCvx.x(i,:) = yout(:,1); 285 | ReachCvx.y(i,:) = yout(:,2); 286 | end 287 | 288 | %Compute the convex hull at each time step 289 | for i=1:nPointsPerSample 290 | k = convhull(ReachCvx.x(:,i),ReachCvx.y(:,i)); 291 | ReachCvx.c(i).t = ReachCvx.t(k,i); 292 | ReachCvx.c(i).x = ReachCvx.x(k,i); 293 | ReachCvx.c(i).y = ReachCvx.y(k,i); 294 | end 295 | 296 | 297 | %Plot the reachable sets against time: 298 | figure(6); clf; hold on; 299 | colors = jet(nPointsPerSample); 300 | for i=1:nPointsPerSample 301 | %Convex Hull 302 | plot3(ReachCvx.c(i).x, ReachCvx.c(i).y, ReachCvx.c(i).t,'color',colors(i,:)); 303 | %Point Cloud: 304 | plot3(ReachCvx.x(:,i), ReachCvx.y(:,i), ReachCvx.t(:,i),'.','MarkerSize',10,'color',colors(i,:)); 305 | end 306 | 307 | %plot the nominal trajectory 308 | plot3(xSol,ySol,tSol,'k-','LineWidth',3); 309 | 310 | 311 | 312 | title('Convex Hull approximation of the reachable set'); 313 | xlabel('x'); ylabel('y'); zlabel('time'); 314 | view(3); 315 | 316 | 317 | 318 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 319 | % Computate the backward reachable set -- Front Propagation % 320 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 321 | 322 | nSamples = 250; %How many simulations to run? 323 | nPointsPerSample = 10; %Used for computing reachable set 324 | 325 | % What is "close enough" to the final state? 326 | xRadFinal = 0.015; 327 | yRadFinal = 0.015; 328 | 329 | %Compute initial conditions on the circle around the final set: 330 | angle = linspace(0,2*pi,nSamples+1); angle(end) = []; 331 | IC = [cos(angle); sin(angle)]; 332 | IC(1,:) = IC(1,:)*xRadFinal; 333 | IC(2,:) = IC(2,:)*yRadFinal; 334 | 335 | %Simulation runs backwards in time 336 | tSpanFlip = linspace(tSpan(2),tSpan(1),nPointsPerSample); %Run system backwards in time 337 | 338 | %Create a data structure for storing the point cloud: 339 | ReachFrt.t = ones(nSamples,1)*tSpanFlip; % Time 340 | ReachFrt.x = zeros(nSamples,nPointsPerSample); % x position 341 | ReachFrt.y = zeros(nSamples,nPointsPerSample); % y position 342 | 343 | %Run the simulation: 344 | for i=1:nSamples 345 | idxLow = (i-1)*nPointsPerSample+1; 346 | idxUpp= i*nPointsPerSample; 347 | [~, yout] = ode45(userFunc,tSpanFlip,IC(:,i)); 348 | ReachFrt.x(i,:) = yout(:,1); 349 | ReachFrt.y(i,:) = yout(:,2); 350 | end 351 | 352 | %Plot the reachable sets against time: 353 | figure(7); clf; hold on; 354 | colors = jet(nPointsPerSample); 355 | idx = [1:size(ReachFrt.x,1),1]; %Make the contours wrap around 356 | for i=1:nPointsPerSample 357 | %Front contour 358 | plot3(ReachFrt.x(idx,i), ReachFrt.y(idx,i), ReachFrt.t(idx,i),'color',colors(i,:)); 359 | %Point Cloud: 360 | plot3(ReachFrt.x(:,i), ReachFrt.y(:,i), ReachFrt.t(:,i),'k.','MarkerSize',15); 361 | %Fill the central region: 362 | h = patch(ReachFrt.x(idx,i), ReachFrt.y(idx,i), ReachFrt.t(idx,i),colors(i,:)); 363 | end 364 | plot3(xSol,ySol,tSol,'k-'); 365 | 366 | hp = findobj(gcf,'type','patch'); 367 | set(hp,'facealpha',0.7) 368 | set(hp,'edgealpha',0.1) 369 | 370 | title('Front Propagation approximation of the reachable set'); 371 | xlabel('x'); ylabel('y'); zlabel('time'); 372 | view(3); 373 | 374 | 375 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 376 | % Computation with the Level Set Toolbox % 377 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 378 | 379 | %This section of code requires the levelSetToolbox to operate: 380 | % http://www.cs.ubc.ca/~mitchell/ToolboxLS/ 381 | if exist('odeCFLset','file') 382 | %This section of code is still under development. As of 11/20/2014 the 383 | %following two scripts should run, but they are slow. It also seems 384 | %that there is some numerical dissapation, that is causing inaccurate 385 | %results for any reasonable level of discretization. 386 | 387 | %%%% reachableForward 388 | %%%% reachableForwardRel 389 | end 390 | 391 | 392 | 393 | 394 | 395 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | #README.md 2 | 3 | This directory contains a tutorial for using matlab to stabilize a trajectory of a non-linear system, using a finite-horizon continuous-time Linear Quadratic Regulator (LQR). 4 | 5 | ## Key Files: 6 | 7 | ### MAIN_trajectory: 8 | Entry-point function for the primary demo, using the full non-linear plant 9 | - Plots vector field for non-linear system 10 | - Generate a nominal trajectory 11 | - Approximate trajectory using polynomial curve fitting 12 | - Solve LQR along the trajectory 13 | - Approximate gains using polynomial curve fitting 14 | - Compare the gains obtained from finite vs infinite horizon lqr 15 | - Run simulations to demonstrate controller's stabilizing properties 16 | 17 | ### trajectoryLqr: 18 | Solves the finite-horizon continuous-time LQR problem for a time-varying plant (stabilize a non-linear trajectory) 19 | 20 | ### Derive_EoM: 21 | Derives the dynamics equations and writes function files for the non-linear plant. 22 | 23 | ### MAIN_linearPlant: 24 | A quick little demo to find the finite-horizon continuous-time LQR gains for a linear plant, and then comparing the solutions to the infinite-horizon version, generated using Matlab's `lqr` command. 25 | 26 | ### finiteLqr: 27 | Solves the finite-horizon continuous-time LQR problem for a linear time-invariant plant. 28 | -------------------------------------------------------------------------------- /dynamics.m: -------------------------------------------------------------------------------- 1 | function [dx,dy] = dynamics(x,y,u) 2 | %DYNAMICS 3 | % [DX,DY] = DYNAMICS(X,Y,U) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.0. 6 | % 19-Nov-2014 20:59:12 7 | 8 | dx = -cos(u)+sin(y)+1.0; 9 | if nargout > 1 10 | dy = sin(u)+sin(x); 11 | end 12 | -------------------------------------------------------------------------------- /eventFunc.m: -------------------------------------------------------------------------------- 1 | function [value, isterminal, direction] = eventFunc(~,z,xLim,yLim) 2 | %This function is used to stop the simulation if it goes outside of the 3 | %domain of the system. 4 | 5 | value = [... 6 | z(1,:) - xLim(1); 7 | z(1,:) - xLim(2); 8 | z(2,:) - yLim(1); 9 | z(2,:) - yLim(2)]; 10 | isterminal = true(size(value)); 11 | direction = zeros(size(value)); 12 | end -------------------------------------------------------------------------------- /finiteLqr.m: -------------------------------------------------------------------------------- 1 | function Soln = finiteLqr(tSpan,A,B,Q,R,F,nSoln,tol) 2 | 3 | %This function solves the finite-horizon continuous-time linear quadratic 4 | %regulator problem for a linear plant. 5 | 6 | nState = size(A,1); 7 | nInput = size(B,2); 8 | 9 | userFun = @(t,z)rhs(t,z,A,B,Q,R,nState); 10 | z0 = reshape(F,nState*nState,1); 11 | tSpan = fliplr(tSpan); %Integrate backwards in time 12 | 13 | options = odeset(); 14 | options.RelTol = tol; 15 | options.AbsTol = tol; 16 | sol = ode45(userFun,tSpan,z0); 17 | t = linspace(tSpan(2),tSpan(1),nSoln); 18 | z = deval(sol,t); 19 | 20 | Soln(nSoln).t = 0; 21 | Soln(nSoln).K = zeros(nState,nInput); 22 | Soln(nSoln).S = zeros(nState,nState); 23 | Soln(nSoln).E = zeros(nState,1); 24 | 25 | for i=1:nSoln 26 | zNow = z(:,i); 27 | tNow = t(i); 28 | S = reshape(zNow,nState,nState); 29 | K = R\(B'*S); 30 | Soln(i).t = tNow; 31 | Soln(i).K = K; 32 | Soln(i).S = S; 33 | Soln(i).E = eig(A-B*K); 34 | end 35 | 36 | end 37 | 38 | function dz = rhs(~,z,A,B,Q,R,nState) 39 | P = reshape(z,nState,nState); 40 | dP = ricatti(A,B,Q,R,P); 41 | dz = reshape(dP,nState*nState,1); 42 | end 43 | 44 | function [dP, K] = ricatti(A,B,Q,R,P) 45 | K = R\B'*P; 46 | dP = -(A'*P + P*A - P*B*K + Q); 47 | end -------------------------------------------------------------------------------- /getContour.m: -------------------------------------------------------------------------------- 1 | function C = getContour(g,data,level) 2 | 3 | %This function is used to obtain a set of level-curves for a surface 4 | %function. Used for computing backward reachable set. 5 | 6 | % There is some weird meshgrid vs ndgrid stuff going on here. 7 | 8 | % MESHGRID vs NDGRID -- switch x and y 9 | c = contourc(g.vs{2}, g.vs{1}, data, [level, level]); 10 | 11 | if isempty(c) 12 | C(1).x = []; 13 | C(1).y = []; 14 | else 15 | idx = 1; %Index of the next delimiter 16 | len = size(c,2); %Total number of indicies 17 | count = 0; %Check number of segments 18 | while idx 1 10 | B = [sin(u);cos(u)]; 11 | end 12 | -------------------------------------------------------------------------------- /getLinTraj.m: -------------------------------------------------------------------------------- 1 | function [A, B] = getLinTraj(t,xFit,yFit,uFit) 2 | 3 | %Linearizes the system about the nominal trajectory. This function is used 4 | %to turn a non-linear trajectory tracking problem into a time-varying 5 | %linear problem. 6 | 7 | x = polyval(xFit,t); 8 | y = polyval(yFit,t); 9 | u = polyval(uFit,t); 10 | 11 | [A,B] = getLinSys(x,y,u); 12 | 13 | end -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | Copyright (c) 2016 Matthew P. Kelly 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 5 | 6 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | 8 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 9 | -------------------------------------------------------------------------------- /randDisk.m: -------------------------------------------------------------------------------- 1 | function samples = randDisk(nSamples,radius,center) 2 | % randDisk(nSamples,radius,center) 3 | % 4 | % Compute a random distribution of points inside a unit disk. 5 | % 6 | % INPUTS: 7 | % nSamples = number of random samples to return 8 | % radius = optional radius parameter. Default = 1. 9 | % radius = R --> circle with radius R 10 | % radius = [a;b;th] --> ellipse 11 | % a = major-axis radius 12 | % b = minor-axis radius 13 | % th = angle between major-axis and x-axis 14 | % center = [cx, cy] optional center of the circle command 15 | % 16 | % OUTPUTS: 17 | % samples = [x;y] a uniform distribution within the desired region 18 | % 19 | % 20 | 21 | r = sqrt(rand(1,nSamples)); 22 | phi = 2*pi*rand(1,nSamples); 23 | [x,y] = pol2cart(phi,r); 24 | 25 | if nargin==1 %Simple unit disk 26 | samples = [x;y]; 27 | else %Then we have some scaling to do: 28 | if length(radius)==1 %The scale the circle uniformly 29 | x = radius*x; 30 | y = radius*y; 31 | samples = [x;y]; 32 | elseif length(radius)==2 %The plain ellipse 33 | a = radius(1); b = radius(2); 34 | x = a*x; 35 | y = b*y; 36 | samples = [x;y]; 37 | elseif length(radius)==3 %Then ellipse with rotation! 38 | a = radius(1); b = radius(2); th = -radius(3); 39 | x = a*x; 40 | y = b*y; 41 | R = [cos(th), sin(th); -sin(th), cos(th)]; 42 | S = [x;y]; 43 | samples = (R*S); 44 | else 45 | error('Invalid input for radius'); 46 | end 47 | 48 | if nargin>2 %Then shift the whole thing 49 | samples(1,:) = samples(1,:) + center(1); 50 | samples(2,:) = samples(2,:) + center(2); 51 | end 52 | end 53 | 54 | end -------------------------------------------------------------------------------- /reachableDynamics.m: -------------------------------------------------------------------------------- 1 | function velocity = reachableDynamics(t,~,schemeData) 2 | 3 | %This function calculates the closed-loop backwards dynamics, for use in 4 | %calculation of the backwards reachable set. 5 | 6 | % FORWARD REACHABLE 7 | 8 | xFit = schemeData.fit.x; 9 | yFit = schemeData.fit.y; 10 | uFit = schemeData.fit.u; 11 | kxFit = schemeData.fit.kx; 12 | kyFit = schemeData.fit.ky; 13 | 14 | x = schemeData.grid.xs{1}; 15 | y = schemeData.grid.xs{2}; 16 | 17 | u = stabilizingController(t,x,y,xFit,yFit,uFit,kxFit,kyFit); 18 | 19 | [dx,dy] = dynamics(x,y,u); 20 | 21 | velocity{1} = dx; 22 | velocity{2} = dy; 23 | 24 | end 25 | 26 | -------------------------------------------------------------------------------- /reachableDynamicsRel.m: -------------------------------------------------------------------------------- 1 | function velocity = reachableDynamicsRel(t,~,schemeData) 2 | 3 | %This function calculates the closed-loop backwards dynamics, for use in 4 | %calculation of the backwards reachable set. 5 | 6 | %FORWARD REACHABLE 7 | 8 | xFit = schemeData.fit.x; 9 | yFit = schemeData.fit.y; 10 | uFit = schemeData.fit.u; 11 | kxFit = schemeData.fit.kx; 12 | kyFit = schemeData.fit.ky; 13 | 14 | xNom = polyval(xFit,t); 15 | yNom = polyval(yFit,t); 16 | 17 | x = schemeData.grid.xs{1} + xNom; 18 | y = schemeData.grid.xs{2} + yNom; 19 | 20 | u = stabilizingController(t,x,y,xFit,yFit,uFit,kxFit,kyFit); 21 | 22 | [dx,dy] = dynamics(x,y,u); 23 | 24 | velocity{1} = dx; 25 | velocity{2} = dy; 26 | 27 | end 28 | 29 | -------------------------------------------------------------------------------- /reachableForward.m: -------------------------------------------------------------------------------- 1 | % reachableForward 2 | % 3 | % A script for computing the forward reachable set, using an absolute 4 | % coordinate system. It seems to give roughly reasonable reasonable 5 | % results, but there seems to be some numerical dissapation during the run. 6 | % For example, points that we know are stable appear to be unstable, which 7 | % I think is due to the feature size of the reachable set approaching the 8 | % grid-spacing. This will occur for any grid spacing, since it seems like 9 | % the reachable set becomes an very long and thin ellipsoid-like shape. 10 | % 11 | 12 | nReach = 50; %Number of times at which to display level sets 13 | if (tSpan(2)~=0), error('tSpan(2) must be equal to 0 for reachability calculations!'); end; 14 | tReach = linspace(tSpan(1),tSpan(2),nReach); 15 | 16 | radius = 0.2; 17 | 18 | nGrid = 400; %Discritization of the state space in each dimension 19 | grid.dim = 2; %Two-dimensional problem 20 | grid.min = [-4;-6]; %%%%[xLim(1);yLim(1)]-2*radius; 21 | grid.max = [0;-2]; %%%%[xLim(2);yLim(2)]+2*radius; 22 | grid.dx = (1/(nGrid-1))*(grid.max-grid.min); 23 | grid.bdry = @addGhostExtrapolate; 24 | grid = processGrid(grid); 25 | 26 | % Create initial conditions (a circle around the target) 27 | center = [ polyval(xFit,tReach(1)); polyval(yFit,tReach(1))]; 28 | data = shapeSphere(grid, center, radius); %Initialize level-set function 29 | levelSet = 0; 30 | 31 | % Set up the level-set problem: 32 | schemeFunc = @termConvection; 33 | schemeData.velocity = @reachableDynamics; 34 | schemeData.grid = grid; 35 | 36 | schemeData.fit.x = xFit; 37 | schemeData.fit.y = yFit; 38 | schemeData.fit.u = uFit; 39 | schemeData.fit.kx = kxFit; 40 | schemeData.fit.ky = kyFit; 41 | 42 | % Set up time approximation scheme. 43 | integratorOptions = odeCFLset('factorCFL', 0.5, 'stats', 'on'); 44 | 45 | % Choose approximations at appropriate level of accuracy. 46 | accuracy = 'medium'; 47 | switch(accuracy) 48 | case 'low' 49 | schemeData.derivFunc = @upwindFirstFirst; 50 | integratorFunc = @odeCFL1; 51 | case 'medium' 52 | schemeData.derivFunc = @upwindFirstENO2; 53 | integratorFunc = @odeCFL2; 54 | case 'high' 55 | schemeData.derivFunc = @upwindFirstENO3; 56 | integratorFunc = @odeCFL3; 57 | case 'veryHigh' 58 | schemeData.derivFunc = @upwindFirstWENO5; 59 | integratorFunc = @odeCFL3; 60 | otherwise 61 | error('Unknown accuracy level %s', accuracy); 62 | end 63 | 64 | %Data structure for storing the reachable set, as contour lines: 65 | Reach(nReach).C = []; 66 | 67 | %Store the initial contour line: 68 | Reach(1).C = getContour(grid,data,levelSet); 69 | 70 | %Run the backwards reachability calculation: 71 | startTime = cputime; 72 | for idx = 1:(nReach-1) 73 | 74 | %Display data to use: 75 | figure(12); clf; contour(grid.xs{1},grid.xs{2}, data); 76 | 77 | % Reshape data array into column vector for ode solver call. 78 | y0 = data(:); 79 | 80 | % How far to step? 81 | tSpanReach = [tReach(idx), tReach(idx+1)]; 82 | 83 | % Take a timestep. 84 | [t, y] = feval(integratorFunc, schemeFunc, tSpanReach, y0,... 85 | integratorOptions, schemeData); 86 | if t~=tSpanReach(2) 87 | error('Something funny happened!') 88 | end 89 | 90 | % Get back the correctly shaped data array 91 | data = reshape(y, grid.shape); 92 | 93 | if min(min(data)) > 0, break; end 94 | 95 | %Store the level curve: 96 | Reach(idx+1).C = getContour(grid,data,levelSet); 97 | 98 | end 99 | 100 | %Plot the results: 101 | figure(6); clf; hold on; 102 | colors = jet(nReach); 103 | xNomReach = polyval(xFit,tReach); 104 | yNomReach = polyval(yFit,tReach); 105 | for i=1:nReach 106 | C = Reach(i).C; 107 | for j=1:length(C) 108 | x = C(j).x; 109 | y = C(j).y ; 110 | plot(x,y,'LineWidth',3,'color',colors(i,:)); 111 | end 112 | end 113 | plot(xNomReach,yNomReach,'k.','MarkerSize',20); 114 | plot(xSol,ySol,'k') 115 | xlabel('x'); 116 | ylabel('y'); 117 | zlabel('t'); 118 | title('Forward reachable set') 119 | 120 | -------------------------------------------------------------------------------- /reachableForwardRel.m: -------------------------------------------------------------------------------- 1 | % reachableForwardRel 2 | % 3 | % A script for computing the forward reachable set, using an relative 4 | % coordinate system. It seems to give roughly reasonable reasonable 5 | % results, but there seems to be some numerical dissapation during the run. 6 | % For example, points that we know are stable appear to be unstable, which 7 | % I think is due to the feature size of the reachable set approaching the 8 | % grid-spacing. This will occur for any grid spacing, since it seems like 9 | % the reachable set becomes an very long and thin ellipsoid-like shape. 10 | % 11 | 12 | nReach = 40; %Number of times at which to display level sets 13 | if (tSpan(2)~=0), error('tSpan(2) must be equal to 0 for reachability calculations!'); end; 14 | % tReach = linspace(tSpan(1),tSpan(2),nReach); 15 | tReach = linspace(tSpan(1),-1.5,nReach); 16 | 17 | radius = 0.3; 18 | 19 | nGrid = 400; %Discritization of the state space in each dimension 20 | grid.dim = 2; %Two-dimensional problem 21 | grid.min = 8*radius*[-1;-1]; 22 | grid.max = 8*radius*[1;1]; 23 | grid.dx = (1/(nGrid-1))*(grid.max-grid.min); 24 | grid.bdry = @addGhostExtrapolate; 25 | grid = processGrid(grid); 26 | 27 | % Create initial conditions (a circle around the target) 28 | center = [0;0]; %We're working in relative coordinates here! 29 | data = shapeSphere(grid, center, radius); %Initialize level-set function 30 | levelSet = 0; 31 | 32 | 33 | % Set up the level-set problem: 34 | schemeFunc = @termConvection; 35 | schemeData.velocity = @reachableDynamicsRel; 36 | schemeData.grid = grid; 37 | 38 | schemeData.fit.x = xFit; 39 | schemeData.fit.y = yFit; 40 | schemeData.fit.u = uFit; 41 | schemeData.fit.kx = kxFit; 42 | schemeData.fit.ky = kyFit; 43 | 44 | % Set up time approximation scheme. 45 | integratorOptions = odeCFLset('factorCFL', 0.5, 'stats', 'on'); 46 | 47 | % Choose approximations at appropriate level of accuracy. 48 | accuracy = 'medium'; 49 | switch(accuracy) 50 | case 'low' 51 | schemeData.derivFunc = @upwindFirstFirst; 52 | integratorFunc = @odeCFL1; 53 | case 'medium' 54 | schemeData.derivFunc = @upwindFirstENO2; 55 | integratorFunc = @odeCFL2; 56 | case 'high' 57 | schemeData.derivFunc = @upwindFirstENO3; 58 | integratorFunc = @odeCFL3; 59 | case 'veryHigh' 60 | schemeData.derivFunc = @upwindFirstWENO5; 61 | integratorFunc = @odeCFL3; 62 | otherwise 63 | error('Unknown accuracy level %s', accuracy); 64 | end 65 | 66 | %Data structure for storing the reachable set, as contour lines: 67 | Reach(nReach).C = []; 68 | 69 | %Store the initial contour line: 70 | Reach(1).C = getContour(grid,data,levelSet); 71 | 72 | %Run the backwards reachability calculation: 73 | startTime = cputime; 74 | for idx = 1:(nReach-1) 75 | 76 | %Display data to user: 77 | figure(12); clf; contour(grid.xs{1},grid.xs{2}, data); 78 | 79 | % Reshape data array into column vector for ode solver call. 80 | y0 = data(:); 81 | 82 | % How far to step? 83 | tSpanReach = [tReach(idx), tReach(idx+1)]; 84 | 85 | % Take a timestep. 86 | [t, y] = feval(integratorFunc, schemeFunc, tSpanReach, y0,... 87 | integratorOptions, schemeData); 88 | if t~=tSpanReach(2) 89 | error('Something funny happened!') 90 | end 91 | 92 | % Get back the correctly shaped data array 93 | data = reshape(y, grid.shape); 94 | 95 | if min(min(data)) > 0, break; end 96 | 97 | %Store the level curve: 98 | Reach(idx+1).C = getContour(grid,data,levelSet); 99 | 100 | end 101 | 102 | %Plot the results: 103 | figure(7); clf; hold on; 104 | colors = jet(nReach); 105 | xNomReach = polyval(xFit,tReach); 106 | yNomReach = polyval(yFit,tReach); 107 | for i=1:nReach 108 | C = Reach(i).C; 109 | if ~isempty(Reach(i).C) 110 | for j=1:length(C) 111 | x = C(j).x + xNomReach(i); 112 | y = C(j).y +yNomReach(i); 113 | plot(x,y,'LineWidth',3,'color',colors(i,:)); 114 | end 115 | end 116 | end 117 | plot(xNomReach,yNomReach,'k.','MarkerSize',20); 118 | plot(xSol,ySol,'k') 119 | xlabel('x'); 120 | ylabel('y'); 121 | zlabel('t'); 122 | title('Forward reachable set') 123 | 124 | 125 | -------------------------------------------------------------------------------- /reachableSet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MatthewPeterKelly/Continuous_Finite_LQR/15aae85075355acc71f47b2a8b22f696b6790848/reachableSet.png -------------------------------------------------------------------------------- /rhs.m: -------------------------------------------------------------------------------- 1 | function dz = rhs(~,z,u) 2 | %This function is a wrapper for the automatically generated dynamics 3 | %function. It is designed to be called inside of ode45 4 | 5 | x = z(1,:); y = z(2,:); 6 | [dx,dy] = dynamics(x,y,u); 7 | dz = [dx;dy]; 8 | end -------------------------------------------------------------------------------- /stabilizingController.m: -------------------------------------------------------------------------------- 1 | function u = stabilizingController(t,x,y,xFit,yFit,uFit,kxFit,kyFit) 2 | 3 | %This function is a controller that is used to stabilize the system to a 4 | %nominal trajectory. 5 | 6 | xRef = polyval(xFit,t); 7 | yRef = polyval(yFit,t); 8 | uRef = polyval(uFit,t); 9 | Kx = polyval(kxFit,t); 10 | Ky = polyval(kyFit,t); 11 | 12 | u = uRef - Kx.*(x-xRef) - Ky.*(y-yRef); 13 | 14 | end -------------------------------------------------------------------------------- /trajectoryLqr.m: -------------------------------------------------------------------------------- 1 | function Soln = trajectoryLqr(t,linSys,Q,R,F,tol) 2 | % Soln = trajectoryLqr(t,linSys,Q,R,F,tol) 3 | % 4 | % This function is used to solve the finite-horizon continuout-time linear 5 | % quadratic regulator problem. 6 | % 7 | % INPUTS: 8 | % t = monotonically increasing vector of times at which you would like 9 | % the gain matrix to be calculated 10 | % linSys = function handle for time-varying linear dynamics 11 | % [A, B] = linSys(t) 12 | % Q = state cost matrix 13 | % R = input cost matrix 14 | % F = final state cost matrix 15 | % tol = accuracy of riccati equation propagation 16 | % 17 | % OUTPUTS: 18 | % Soln = struct array with solution at each point in t 19 | % Soln(i).t = t(i); 20 | % Soln(i).K = gain matrix at t(i) 21 | % Soln(i).S = Cost to go 22 | % Soln(i).E = close-loop eigen-values for system at t(i) 23 | % 24 | % NOTES: 25 | % 26 | % J = x'Fx + Integral {x'Qx + u'Ru} dt 27 | % 28 | % See Also LQR 29 | 30 | nState = size(Q,1); 31 | nInput = size(R,1); 32 | 33 | userFun = @(t,z)rhs(t,z,linSys,Q,R,nState); 34 | z0 = reshape(F,nState*nState,1); 35 | tSpan = [t(end),t(1)]; 36 | 37 | options = odeset(); 38 | options.RelTol = tol; 39 | options.AbsTol = tol; 40 | sol = ode45(userFun,tSpan,z0); 41 | z = deval(sol,t); 42 | 43 | nSoln = length(t); 44 | Soln(nSoln).t = 0; 45 | Soln(nSoln).K = zeros(nState,nInput); 46 | Soln(nSoln).S = zeros(nState,nState); 47 | Soln(nSoln).E = zeros(nState,1); 48 | 49 | for idx=1:nSoln 50 | i = nSoln-idx+1; 51 | zNow = z(:,i); 52 | tNow = t(i); 53 | S = reshape(zNow,nState,nState); 54 | [A,B] = linSys(tNow); 55 | K = R\(B'*S); 56 | Soln(i).t = tNow; 57 | Soln(i).K = K; 58 | Soln(i).S = S; 59 | Soln(i).E = eig(A-B*K); 60 | end 61 | 62 | end 63 | 64 | function dz = rhs(t,z,linSys,Q,R,nState) 65 | P = reshape(z,nState,nState); 66 | [A,B] = linSys(t); 67 | dP = ricatti(A,B,Q,R,P); 68 | dz = reshape(dP,nState*nState,1); 69 | end 70 | 71 | function [dP, K] = ricatti(A,B,Q,R,P) 72 | K = R\B'*P; 73 | dP = -(A'*P + P*A - P*B*K + Q); 74 | end 75 | 76 | 77 | --------------------------------------------------------------------------------