├── AsymPotentialWell_FIG.m ├── AsymmetricPotentialWell.m ├── AutonomDoubleGyre.m ├── AutonomDoubleGyre_FIG.m ├── DiscoverDuffing.m ├── DiscoverDuffing_Convergence.m ├── DiscoverDuffing_KRONICvsEDMDc.m ├── FIGURES ├── EXAMPLES.png ├── KRONIC.png └── SparseRegression.png ├── HamiltonianDuffing.m ├── HamiltonianDuffing_FIG.m ├── LICENSE ├── NonAutonomDoubleGyre.m ├── NonAutonomDoubleGyre_FIG.m ├── Pendulum.m ├── Pendulum_FIG.m ├── QuarticPotentialWell.m ├── QuarticPotentialWell_FIG.m ├── README.md ├── SlowManifold.m └── utils ├── AsymmetricDoubleWellFUN.m ├── DoubleGyre_ensemble.m ├── Duffing_ensemble.m ├── ObjectiveFCN_EDMDc.m ├── ObjectiveFCN_KRONIC.m ├── QuarticPotentialWell_ensemble.m ├── SlowManifold_PhasePlot_Control.m ├── SlowManifold_ensemble.m ├── applyPeriodicBC.m ├── buildGamma.m ├── buildTheta.m ├── buildThetaGradient.m ├── color_line3.m ├── evalCostFun.m ├── evalCostFun_AsymPotentialWell_PhaseController.m ├── evalCostFun_Hamiltonian.m ├── evalCostFun_KoopEfun.m ├── evalCostFun_TimeDependentKoopEfun.m ├── getBRSys.m ├── getBRTrafo.m ├── poolData.m ├── poolDataLIST.m ├── rk4singlestep.m ├── rk4u.m ├── runMPC.m ├── showPendulumResults.m ├── showResults_SlowManifold.m ├── sparseKoopmanEfunSys.m └── sparsifyDynamics.m /AsymPotentialWell_FIG.m: -------------------------------------------------------------------------------- 1 | path2data = '../Data/'; ModelName1 = ['AsymPotentialWell_','B11_']; 2 | load([path2data,[ModelName1,'Data.mat']]) 3 | path2figs = '../Figures/ASYM_POT_WELL/'; mkdir(path2figs) 4 | 5 | 6 | %% Show Potential function 7 | V = @(x)(x.^4/4 - x.^2/2 - (a/3).*x.^3 + a*x); 8 | figure,hold on, box on 9 | x = [-3:0.1:3]; 10 | plot(x,x.^4/4 - x.^2/2 - (a/3).*x.^3 + a*x,'-k','LineWidth',3); 11 | xlim([-2.5 2.5]),ylim([-0.5 1]) 12 | plot(x0(1,1),V(x0(1,1)),'o','Color',[0,0.5,1],'MarkerFaceColor',[0,0.5,1]); 13 | plot(x0(2,1),V(x0(2,1)),'o','Color',[0,1,1],'MarkerFaceColor',[0,1,1]); 14 | plot(-1,V(-1),'or','MarkerFaceColor','r'); 15 | plot(x,V(a).*ones(length(x)),'--k') 16 | plot(a,V(a),'or','MarkerFaceColor','r'); 17 | plot(1,V(1),'or','MarkerFaceColor','r'); 18 | xlabel('x1'), ylabel('V') 19 | set(gca,'ytick',[0,1]) 20 | set(gca,'FontSize',14, 'LineWidth',1) 21 | set(gcf,'Position',[100 100 250 120]) 22 | set(gcf,'PaperPositionMode','auto'), 23 | print('-painters','-depsc2', '-loose', [path2figs,ModelName1,'Potential','.eps']); 24 | 25 | 26 | %% Show phase plot with controlled trajectories 27 | cmap = gray(2*11); 28 | cmap = cmap(1:2:end,:); 29 | LineWidth = 2; 30 | clear ph 31 | 32 | figure; 33 | box on 34 | hold on 35 | sh = surf(x,y,-0.4.*ones(size(Hgrid)));shading interp, view(2), hold on 36 | sh.FaceColor = [0 0 0]; 37 | contourf(x,y,log(Hgrid+0.42),[log([-0.2,-0.1,0,0.25:0.5:4]+0.42)],'LineColor','none'), colormap(cmap) 38 | % for i = 1:3 39 | % ph(1) = plot3(y0(1:2000,1,i),y0(1:2000,2,i),ones(size(y0(1:2000,2,i))),'--','Color',[0.8,0.8,0],'LineWidth',1); 40 | % end 41 | ph(1) = plot3(y0(1:10000,1,1),y0(1:10000,2,1),ones(size(y0(1:10000,2,1))),'--','Color',[0.8,0.8,0],'LineWidth',1); 42 | ph(1) = plot3(y0(1:5500,1,2),y0(1:5500,2,2),ones(size(y0(1:5500,2,2))),'--','Color',[0.8,0.8,0],'LineWidth',1); 43 | ph(1) = plot3(y0(1:40000,1,3),y0(1:40000,2,3),ones(size(y0(1:40000,2,3))),'--','Color',[0.8,0.8,0],'LineWidth',1); 44 | ph(2) = plot3(y1(:,1,1),y1(:,2,1),ones(size(y1(:,2,1))),'-','Color',[0,0.5,1],'LineWidth',LineWidth+1); 45 | ph(3) = plot3(y1(:,1,2),y1(:,2,2),ones(size(y1(:,2,2))),'-','Color',[0,1,1],'LineWidth',1); 46 | plot3(x0(1,1),x0(1,2),2,'o','Color',[0.8,0.8,0],'MarkerFaceColor',[0.8,0.8,0]); 47 | % plot3(x0(2,1),x0(2,2),2,'o','Color',[0.8,0.8,0],'MarkerFaceColor',[0.8,0.8,0],'MarkerSize',4); 48 | % plot3(xREF(1),xREF(2),2,'ok','MarkerFaceColor','k'); 49 | plot3(-1,0,2,'ok','MarkerFaceColor','r'); 50 | plot3(a,0,2,'ok','MarkerFaceColor','r'); 51 | plot3(1,0,2,'ok','MarkerFaceColor','r'); 52 | axis equal 53 | plot([-2.5 -2.5],[-1.5 1.5],'-k') 54 | plot([-2.5 2.5],[1.5 1.5],'-k') 55 | xlim([-2.5 2.5]), ylim([-1.5 1.5]) 56 | % legend(ph,'Unforced','Controlled') 57 | xlabel('x1'), ylabel('x2') 58 | set(gca,'FontSize',14, 'LineWidth',1) 59 | set(gcf,'Position',[100 100 250 180]) 60 | set(gcf,'PaperPositionMode','auto') 61 | print('-depsc2', '-loose', [path2figs,ModelName1,'PhasePlot','.eps']); 62 | 63 | %% Show cost, actuation etc. 64 | LineWidth = 2; 65 | fhandle = figure; hold on, box on, grid on 66 | plot(tspan,Hvals0,'-','Color',[0.8,0.8,0],'LineWidth',2); 67 | plot(tspan,Hvals1(:,1),'-','Color',[.1,.1,1],'LineWidth',2) 68 | plot(tspan,Hvals1(:,2),'--','Color',[0,1,1],'LineWidth',2) 69 | xlabel('t'), ylabel('H'), ylim([-0.41 0.81]), set(gca,'ytick',[-0.4:0.4:0.8]) 70 | set(gca,'FontSize',16) 71 | axis tight 72 | set(gca,'xtick',[0,20,40]) 73 | set(gcf,'Position',[100 100 225 200]) 74 | set(gcf,'PaperPositionMode','auto') 75 | print('-painters','-depsc2', '-loose', [path2figs,ModelName1,'H','.eps']); 76 | 77 | 78 | figure; hold on, box on, grid on 79 | plot(tspan,Jvals0(:,1),'-','Color',[0.8,0.8,0],'LineWidth',2); 80 | plot(tspan,Jvals1(:,1),'-','Color',[.1,.1,1],'LineWidth',2); 81 | plot(tspan,Jvals1(:,2),'--','Color',[0,1,1],'LineWidth',2); 82 | xlabel('t'), ylabel('J') 83 | set(gca,'FontSize',16) 84 | axis tight 85 | set(gca,'xtick',[0,20,40]) 86 | set(gcf,'Position',[100 100 225 200]) 87 | set(gcf,'PaperPositionMode','auto') 88 | print('-painters','-depsc2', '-loose', [path2figs,ModelName1,'J','.eps']); 89 | 90 | figure; hold on 91 | plot([0,1],[0,0],'-','Color',[0.8,0.8,0],'LineWidth',6); 92 | plot([0,1],[0,0],'-','Color',[.1,.1,1],'LineWidth',6); 93 | plot([0,1],[0,0],'-','Color',[0,1,1],'LineWidth',6); 94 | pl = legend('Unforced', 'Controlled1','Controlled2'); pl.Position = [0.3 0.35 0.5 0.2]; 95 | xlim([-1 2]), axis off 96 | set(gca,'FontSize',16) 97 | set(gca,'xtick',[0,20,40]) 98 | set(gcf,'Position',[100 100 225 200]) 99 | set(gcf,'PaperPositionMode','auto') 100 | print('-painters','-depsc2', '-loose', [path2figs,ModelName1,'legend','.eps']); 101 | 102 | clear ph 103 | fhandle = figure; hold on, box on, grid on 104 | % plot(tspan,uvals0,'-','Color',[0.8,0.8,0],'LineWidth',2); 105 | plot(tspan,uvals1(1,:,1),'-','Color',[.1,.1,1],'LineWidth',2); 106 | plot(tspan,uvals1(2,:,1),'-','Color',[.1,0.7,.1],'LineWidth',2); 107 | xlabel('t'), ylabel('u'),ylim([-2.1 2.1]), xlim([0 50]) 108 | set(gca,'FontSize',16) 109 | legend('u1','u2') 110 | set(gca,'xtick',[0,20,40]) 111 | set(gcf,'Position',[100 100 225 200]) 112 | set(gcf,'PaperPositionMode','auto') 113 | print('-painters','-depsc2', '-loose', [path2figs,ModelName1,'_u','.eps']); 114 | 115 | 116 | 117 | -------------------------------------------------------------------------------- /AsymmetricPotentialWell.m: -------------------------------------------------------------------------------- 1 | clear all, close all, clc 2 | addpath('./utils/'); 3 | path2figs = '../Figures/ASYM_POT_WELL/'; mkdir(path2figs) 4 | path2data = '../Data/'; mkdir(path2data) 5 | ModelName = 'AsymPotentialWell_'; 6 | 7 | % Parameters 8 | ode_options = odeset('RelTol',1e-10, 'AbsTol',1e-11); 9 | a = -0.25; 10 | B = [0; 0]; 11 | tspan = .001:.001:50; 12 | x0 = [-1.50984, -1; 13 | -1.1, 0; 14 | a, 0.001]; 15 | Q = 1; 16 | R = 1; 17 | xREF = [1;0]; 18 | 19 | % Define functions 20 | f = @(t,x,u)([x(2); -(x(1).^3 - x(1) - a.*x(1).^2 + a)]+B*u); 21 | H = @(x)(x(2).^2/2 + x(1).^4/4 - x(1).^2/2 - (a/3).*x(1).^3 + a*x(1)); 22 | REF = H(xREF); 23 | 24 | % Hamiltonian function 25 | x = [-3:0.01:3]; 26 | y = [-3:0.01:3]; 27 | [X,Y] = meshgrid(x,y); 28 | Hgrid = zeros(size(X)); 29 | for i = 1:size(X,1) 30 | for j = 1:size(X,2) 31 | Hgrid(i,j) = H([X(i,j); Y(i,j)]); 32 | end 33 | end 34 | 35 | %% Trajectory integration of unforced system for different initial 36 | % conditions 37 | y0 = zeros(length(tspan),2,size(x0,1)); 38 | Hvals0 = zeros(length(tspan),size(x0,1)); 39 | Jvals0 = zeros(length(tspan),size(x0,1)); 40 | for i = 1:size(x0,1) 41 | [t,y0tmp] = ode45(@(t,x)f(t,x,0),tspan,x0(i,:),ode_options); 42 | [Hvals0(:,i),Jvals0(:,i)] = evalCostFun_Hamiltonian(H,y0tmp,zeros(1,size(y0tmp,1)),Q,R,REF); 43 | y0(:,:,i) = y0tmp; 44 | end 45 | 46 | %% Controlled system // Parameters 47 | gradH = @(x)([(x(1).^3 - x(1) - a.*x(1).^2 + a); x(2)]); 48 | 49 | % B = [0; 1]; R = 1; 50 | % ModelName1 = [ModelName, 'B01_']; 51 | % B = [1; 0]; R= 1; 52 | % ModelName1 = [ModelName, 'B10_']; 53 | B = eye(2); R = 0.05.*eye(2);%0.05.*eye(2); 54 | ModelName1 = [ModelName, 'B11_']; 55 | Bc = B; 56 | 57 | f = @(t,x,u)([x(2); -(x(1).^3 - x(1) - a.*x(1).^2 + a)]+B*u); 58 | gain = @(x)(lqr(0,(gradH(x)'*B),Q,R)); 59 | 60 | %% 3-phase controller 61 | [t,ytmp] = ode45(@(t,x)AsymmetricDoubleWellFUN(t,x,gain,H,a,B),tspan,x0(1,:),ode_options); 62 | y1(:,:,1) = ytmp; 63 | [t,ytmp] = ode45(@(t,x)AsymmetricDoubleWellFUN(t,x,gain,H,a,B),tspan,x0(2,:),ode_options); 64 | y1(:,:,2) = ytmp; 65 | 66 | %% 67 | Hvals1 = zeros(length(t),size(y1,3)); 68 | Jvals1 = zeros(length(t),size(y1,3)); 69 | uvals1 = zeros(size(B,2),length(t),size(y1,3)); 70 | for i = 1:size(y1,3) 71 | [Hvals_tmp,Jvals_tmp,uvals_tmp] = evalCostFun_AsymPotentialWell_PhaseController(y1(:,:,i),B,R,Q,H,gain,a,REF); 72 | Hvals1(:,i) = Hvals_tmp; 73 | Jvals1(:,i) = Jvals_tmp; 74 | uvals1(:,:,i) = uvals_tmp; 75 | end 76 | 77 | %% SAVE RESULTS 78 | save([path2data,[ModelName1,'Data.mat']]) 79 | 80 | 81 | -------------------------------------------------------------------------------- /AutonomDoubleGyre.m: -------------------------------------------------------------------------------- 1 | clear all, close all, clc 2 | 3 | addpath('./utils/'); 4 | path2figs = '../Figures/DOUBLE_GYRE/'; mkdir(path2figs) 5 | path2data = '../Data/'; mkdir(path2data) 6 | ModelName = 'AutonomDoubleGyre_'; 7 | 8 | % Parameters double gyre 9 | tspan = 0:.01:50; 10 | epsilon = 0; 11 | omega = 2*pi; 12 | A = 0.25; 13 | 14 | % Streamfunction / Hamiltonian 15 | x = [0:0.01:2]; y = [0:0.01:1]; 16 | [X,Y] = meshgrid(x,y); 17 | StreamFun = A*sin(pi*X).*sin(pi*Y); 18 | Hfield = -StreamFun; 19 | 20 | % Parameters control 21 | x0 = [1.3; 0.5]; % initial state 22 | Q = 1; % weighing efun/state in cost function 23 | R = 1; % penalize control 24 | REF = 0.2; % reference value / level set 25 | ode_options = odeset('RelTol',1e-10, 'AbsTol',1e-11); 26 | 27 | % Define functions 28 | Psi = @(x)(A*sin(pi*x(1)).*sin(pi*x(2))); % Streamfunction 29 | gradPsi = @(x)([A*pi*cos(pi*x(1))*sin(pi*x(2)); % Gradient of Psi 30 | A*pi*sin(pi*x(1))*cos(pi*x(2))]); 31 | 32 | %% UNFORCED SYSTEM 33 | B = [0; 0]; 34 | f = @(t,x,u)([-A*pi*sin(pi*x(1))*cos(pi*x(2));A*pi*cos(pi*x(1))*sin(pi*x(2))] + B*u); 35 | [t,y0] = ode45(@(t,x)f(t,x,0),tspan,x0,ode_options); 36 | [Psivals0,Jvals0] = evalCostFun_KoopEfun(Psi,y0,zeros(1,size(y0,1)),Q,R,REF); 37 | 38 | %% Controlled system: different actuation choice 39 | % B = [0; 1]; R = 1; 40 | % ModelName1 = [ModelName, 'B01_']; 41 | % B = [1; 0]; R = 1; 42 | % ModelName1 = [ModelName, 'B10_']; % not gain comp 43 | B = [1, 0; 0, 1]; R = eye(2); 44 | ModelName1 = [ModelName, 'B11_']; 45 | Bc = B; 46 | 47 | % KRONIC using stream function 48 | f = @(t,x,u)([-A*pi*sin(pi*x(1))*cos(pi*x(2)); A*pi*cos(pi*x(1))*sin(pi*x(2))] + B*u); % need to be updated with B 49 | gain = @(x)(lqr(0,(gradPsi(x)'*B),Q,R)); 50 | [~,y1] = ode45(@(t,x)f(t,x,-gain(x)*(Psi(x)-REF)),tspan,x0,ode_options); 51 | uvals1 = zeros(size(B,2),length(y1)); for k=1:length(y1), uvals1(:,k) = - gain(y1(k,:))*(Psi(y1(k,:))-REF); end 52 | [Psivals1,Jvals1] = evalCostFun_KoopEfun(Psi,y1,uvals1,Q,R,REF); 53 | 54 | %% Do the same for an ensemble of drifters 55 | % using rk4 with constant timestep as it is faster 56 | 57 | % Ensemble of initial conditions 58 | y0ic = [0 0]; 59 | xvec = 0.01:0.25:2; xvec = [xvec,1.98]; %0.5 60 | yvec = 0.01:0.25:1; yvec = [yvec,0.98]; %0.5 61 | dt = 0.005; 62 | Ny = length(yvec); 63 | Nx = length(xvec); 64 | yIC = zeros(2,Ny,Nx); 65 | [x0,y0] = meshgrid(xvec+y0ic(1),yvec+y0ic(2)); 66 | yIC(1,:,:) = x0; 67 | yIC(2,:,:) = y0; 68 | 69 | % UNFORCED SYSTEM 70 | B = [0; 0]; 71 | duration = 25; 72 | tspan =[0,duration]; 73 | L = duration/dt; 74 | yin = yIC; 75 | yout = zeros(2,Ny,Nx,L); 76 | yout(:,:,:,1) = yin; 77 | for step = 1:L-1 78 | time = step*dt; 79 | yout(:,:,:,step+1) = rk4singlestep(@(t,y,u)DoubleGyre_ensemble(t,y,A,omega,epsilon,B,u),dt,time,yin,0); 80 | yin = yout(:,:,:,step); 81 | end 82 | 83 | % CONTROL 84 | % B = Bc; 85 | % B = Bc; 86 | B = Bc; Ncontrol = size(B,2); 87 | gain = @(x)(lqr(0,(gradPsi(x)'*B),Q,R)); 88 | 89 | yin = yIC; 90 | uin = zeros(Ncontrol,Ny,Nx); 91 | for iy = 1:Ny 92 | for ix = 1:Nx 93 | uin(:,iy,ix) = -gain(yin(:,iy,ix))*(Psi(yin(:,iy,ix))-REF); 94 | end 95 | end 96 | yout_ctrl = zeros(2,Ny,Nx,L); 97 | yout_ctrl(:,:,:,1) = yin; 98 | uout = zeros(Ncontrol,Ny,Nx,L); 99 | uout(:,:,:,1) = uin; 100 | for step = 1:L-1 101 | time = step*dt; 102 | yout_ctrl(:,:,:,step+1) = rk4singlestep(@(t,y,u)DoubleGyre_ensemble(t,y,A,omega,epsilon,B,u),dt,time,yin,uin); 103 | yin = yout_ctrl(:,:,:,step+1); 104 | uin = zeros(Ncontrol,Ny,Nx); 105 | for iy = 1:Ny 106 | for ix = 1:Nx 107 | uin(:,iy,ix) = -gain(yin(:,iy,ix))*(Psi(yin(:,iy,ix))-REF); 108 | end 109 | end 110 | uout(:,:,:,step+1) = uin; 111 | end 112 | 113 | %% SAVE RESULTS 114 | save([path2data,[ModelName1,'Ensemble.mat']]) 115 | 116 | -------------------------------------------------------------------------------- /AutonomDoubleGyre_FIG.m: -------------------------------------------------------------------------------- 1 | path2data = '../Data/'; ModelName1 = ['AutonomDoubleGyre_', 'B11_']; 2 | load([path2data,[ModelName1,'Ensemble.mat']]) 3 | path2figs = '../Figures/DOUBLE_GYRE/'; mkdir(path2figs) 4 | 5 | %% Phase plot with ensemble trajectories 6 | cmap = gray(30); cmap=flipud(cmap(1:end,:)); 7 | figure,surf(x,y,StreamFun-10), shading interp, hold on, box on, view(2) 8 | caxis([min(min(StreamFun))-10 max(max(StreamFun))-10]) 9 | colormap(cmap) 10 | 11 | for iy = 1:Ny 12 | for ix = 1:Nx 13 | tmp = squeeze(yout_ctrl(:,iy,ix,:)); 14 | [ynew,StartIDX] = applyPeriodicBC(tmp',[0,2],[0,1],Psi); 15 | for i = 1:length(StartIDX)-1 16 | plot(ynew(StartIDX(i):StartIDX(i+1)-1,1),ynew(StartIDX(i):StartIDX(i+1)-1,2),'-','Color',[.1,.1,1]) 17 | end 18 | plot(ynew(1,1),ynew(1,2),'.','Color',[1 0 0],'MarkerSize',8) 19 | end 20 | end 21 | plot(ynew(1,1),ynew(1,2),'.','Color',[1 0 0],'MarkerSize',10) 22 | contour(X,Y,StreamFun, [-1:0.05:1],'k','ShowText','on') 23 | contour3(X,Y,StreamFun+20, [0.2+20, 0.2+20],'--','Color',0.8.*ones(1,3),... %[1,0,0] 24 | 'ShowText','off','LineWidth',2) 25 | % text(0.45,0.75,'0.2','FontSize',14,'Color',0.7.*ones(1,3))%[1,0,0] 26 | box on 27 | axis tight 28 | 29 | xlabel('x1'), ylabel('x2') 30 | set(gca,'FontSize',14, 'LineWidth',1) 31 | axis equal 32 | set(gcf,'Position',[100 100 600 350]) 33 | set(gcf,'PaperPositionMode','auto') 34 | print('-depsc2', '-loose', [path2figs,ModelName1,'PhasePlot','.eps']); 35 | -------------------------------------------------------------------------------- /DiscoverDuffing.m: -------------------------------------------------------------------------------- 1 | clear all, close all, clc 2 | addpath('./utils'); 3 | % addpath('../../../Sources/Code/psv-master') 4 | path2figs = './../Figures/DUFFING/'; 5 | mkdir(path2figs) 6 | ModelName = 'DiscoverDuffing'; 7 | 8 | % Parameters 9 | dt = 0.001; 10 | tspan = dt:dt:10; 11 | x0 = [0; -2.8]; 12 | 13 | f = @(t,x)([x(2); x(1) - x(1)^3]); 14 | H = @(x)( (1/2)*x(2).^2-(1/2)*x(1).^2 + (1/4)*x(1).^4 ); 15 | ode_options = odeset('RelTol',1e-10, 'AbsTol',1e-11); 16 | 17 | usesine = 0; 18 | polyorder = 4; 19 | nvar = 2; 20 | 21 | %% Trajectory data 22 | ModelName_tmp = [ModelName, '_trajectory_']; 23 | 24 | [t,y] = ode45(f,tspan,x0,ode_options); 25 | 26 | Hy = zeros(length(y),1); 27 | dy = zeros(size(y)); 28 | for k=1:length(y) 29 | dy(k,:) = f(0,y(k,:))'; 30 | Hy(k) = H(y(k,:)); 31 | end 32 | 33 | figure; hold on, box on 34 | plot(t,y(:,1),'-','Color',[0,0,0.7],'LineWidth',2) 35 | plot(t,y(:,2),'-','Color',[0,0.7,0],'LineWidth',2) 36 | legend('x1','x2') 37 | xlabel('t'), ylabel('xi') 38 | set(gca,'xtick',[0:2:10]) 39 | set(gca,'FontSize',16) 40 | set(gcf,'Position',[100 100 225 200]) 41 | set(gcf,'PaperPositionMode','auto') 42 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'Hamiltonian_Trajectory','.eps']); 43 | 44 | % Construct libraries 45 | Theta = buildTheta(y,nvar,polyorder,usesine); 46 | Gamma = buildGamma(y,dy,nvar,polyorder,usesine); 47 | 48 | % Compute SVD 49 | [U,S,V] = svd(0*Theta - Gamma,'econ'); 50 | 51 | % Least-squares Koopman 52 | K = pinv(Theta)*Gamma; 53 | K(abs(K)<1e-12) = 0; 54 | [T,D] = eig(K); 55 | D = diag(D); 56 | [~,IX] = sort(abs(D),'ascend'); 57 | 58 | % Compute eigenfunction 59 | xi0 = V(:,end); % from SVD 60 | xi0(abs(xi0)<1e-12) = 0; 61 | 62 | D(IX(1)) 63 | xi1 = T(:,IX(1));%+Tls(:,IX(2)); % from least-squares fit 64 | xi1(abs(xi1)<1e-12) = 0; 65 | 66 | % Print coefficients 67 | poolDataLIST({'x','y'},xi0,nvar,polyorder,usesine); 68 | poolDataLIST({'x','y'},xi1,nvar,polyorder,usesine); 69 | 70 | % Plot evolution of eigenfunction = Hamiltonian 71 | if length(Hy)~=length(t) 72 | t = 1:length(Hy); 73 | end 74 | 75 | figure; hold on, box on 76 | ph(1) = plot(t,Hy./norm(Hy),'-k', 'LineWidth',18,'Color',[0.7,0.7,0.7]); 77 | ph(2) = plot(t,(Theta)*(xi0)./norm((Theta)*(xi0)),'-b', 'LineWidth',8,'Color',[0,0,1]); 78 | ph(3) = plot(t,-(Theta)*(xi1)./norm((Theta)*(xi1)),'--', 'LineWidth',8,'Color',[0,0.5,0]); 79 | xlabel('t'), ylabel('E') 80 | ylim([0.01-1e-4 0.01+2e-4]), xlim([min(t) max(t)]) 81 | set(gca,'xtick',[0:2:10]) 82 | legend(ph,'True place', 'SVD place', 'LS place') 83 | set(gca,'FontSize',16) 84 | set(gcf,'Position',[100 100 260 200]) 85 | set(gcf,'PaperPositionMode','auto') 86 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'HamiltonianReconstruction','.eps']); 87 | 88 | % Plot error 89 | dstep = 5; 90 | clear ph 91 | figure; hold on, box on 92 | tmp = Gamma*xi0; 93 | ph(1) = plot(t(1:dstep:end),tmp(1:dstep:end),'-k', 'LineWidth',2);%,'Color',[0,0,1]); 94 | tmp = -Gamma*xi1; 95 | ph(2) = plot(t(1:dstep:end),tmp(1:dstep:end),'-r', 'LineWidth',2);%,'Color',[0,0.5,0]); 96 | xlabel('t'), ylabel('Gamma xi') 97 | xlim([min(t) max(t)]) 98 | ylim([min(Gamma*xi1)+0.2*min(Gamma*xi1) max(Gamma*xi1)+0.5*max(Gamma*xi1)]) 99 | legend(ph,'SVD p', 'LS p') 100 | set(gca,'xtick',[0:2:10]) 101 | set(gca,'FontSize',16) 102 | set(gcf,'Position',[100 100 225 200]) 103 | set(gcf,'PaperPositionMode','auto') 104 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'HamiltonianError','.eps']); 105 | 106 | err0 = (Hy./norm(Hy)-(Theta)*(xi0)./norm((Theta)*(xi0)))./(Hy./norm(Hy)); 107 | err1 = (Hy./norm(Hy)-(Theta)*(xi1)./norm((Theta)*(xi1)))./(Hy./norm(Hy)); 108 | figure, plot(err0,'-r'),hold on, plot(err1,'--b') 109 | axis tight 110 | 111 | return 112 | %% Ensemble data 113 | close all 114 | ModelName_tmp = [ModelName, '_ensemble_']; 115 | 116 | % Collect data 117 | clear y dy Hy 118 | [Xgrid,Vgrid] = meshgrid([-2.45:.05:2.5],[-2.45:.05:2.5]); 119 | Nx = size(Xgrid,1), Ny = size(Vgrid,2); 120 | y(:,1) = reshape(Xgrid,[Nx*Ny 1]); 121 | y(:,2) = reshape(Vgrid,[Nx*Ny 1]); 122 | Hy = zeros(length(y),1); 123 | Hdy = zeros(length(y),1); 124 | dy = zeros(size(y)); 125 | for k=1:length(y) 126 | dy(k,:) = f(0,y(k,:))'; 127 | Hy(k) = H(y(k,:)); 128 | Hdy(k) = H(dy(k,:)); 129 | end 130 | 131 | cmap = gray(2*11); 132 | cmap = cmap(1:2:end,:); 133 | xH = [-2.4:0.01:2.4]; 134 | [X,V] = meshgrid(xH,xH); 135 | Hfield = (1/2)*V.^2-(X.^2)/2 + (1/4)*X.^4; 136 | 137 | figure; box on, hold on 138 | sh = surf(xH,xH,zeros(size(Hfield)));shading interp, view(2) 139 | sh.FaceColor = [0 0 0]; 140 | contourf(xH,xH,log(Hfield+(0.2500001)),[log([-0.2,-0.1,0,0.25:0.5:4]+(0.2500001))],'LineColor','none'), colormap(cmap) 141 | hold on 142 | sh = scatter(Xgrid(:),Vgrid(:),'.b'); 143 | sh.SizeData = 5; 144 | x1 = -sqrt(2):0.01:sqrt(2); 145 | plot(x1,x1.*sqrt(1-0.5.*(x1).^2),'--y') 146 | plot(x1,-x1.*sqrt(1-0.5.*(x1).^2),'--y') 147 | xlabel('x1'), ylabel('x2') 148 | drawnow 149 | set(gca,'FontSize',16) 150 | set(gcf,'Position',[100 100 220 200]) 151 | set(gcf,'PaperPositionMode','auto') 152 | % print('-painters','-depsc2', '-loose', [path2figs,ModelName_tmp,'Hamiltonian_SamplingPoints','.eps']); 153 | print('-opengl','-depsc2', '-loose', [path2figs,ModelName_tmp,'Hamiltonian_SamplingPoints','.eps']); 154 | 155 | % Construct libraries 156 | Theta = buildTheta(y,nvar,polyorder,usesine); 157 | Gamma = buildGamma(y,dy,nvar,polyorder,usesine); 158 | 159 | % Compute SVD 160 | [U,S,V] = svd(0*Theta - Gamma,'econ'); 161 | 162 | % Least-squares Koopman 163 | K = pinv(Theta)*Gamma; 164 | K(abs(K)<1e-12) = 0; 165 | [T,D] = eig(K); 166 | D = diag(D); 167 | [~,IX] = sort(abs(D),'ascend'); 168 | 169 | % Compute eigenfunction 170 | xi0 = V(:,end); % from SVD 171 | xi0(abs(xi0)<1e-12) = 0; 172 | 173 | D(IX(1)) 174 | xi1 = T(:,IX(1)); % from least-squares fit 175 | xi1(abs(xi1)<1e-12) = 0; 176 | 177 | % Print coefficients 178 | poolDataLIST({'x','y'},xi0,nvar,polyorder,usesine); 179 | poolDataLIST({'x','y'},xi1,nvar,polyorder,usesine); 180 | 181 | % Plot evolution of eigenfunction = Hamiltonian 182 | if length(Hy)~=length(t) 183 | t = 1:length(Hy); 184 | end 185 | 186 | %% Show results for ensemble of points 187 | close all 188 | figure; hold on, box on 189 | ph(1) = plot(t,Hy./norm(Hy),'-k', 'LineWidth',5,'Color',[0.7,0.7,0.7]); 190 | ph(2) = plot(t,(Theta)*(xi0)./norm((Theta)*(xi0)),'-k', 'LineWidth',2);%,'Color',[0,0,1]); 191 | ph(3) = plot(t,-(Theta)*(xi1)./norm((Theta)*(xi1)),'-r', 'LineWidth',2);%,'Color',[0,0.5,0]); 192 | xlabel('sampling point'), ylabel('E') 193 | ylim([-0.005 0.05]), 194 | xlim([-0 10]) 195 | legend(ph,'True place', 'SVD place', 'LS place','location','north') 196 | set(gca,'FontSize',16) 197 | set(gcf,'Position',[100 100 240 200]) 198 | set(gcf,'PaperPositionMode','auto') 199 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'HamiltonianReconstruction','.eps']); 200 | 201 | % Plot error 202 | dstep = 5; 203 | clear ph 204 | figure; hold on, box on 205 | tmp = Gamma*xi1; 206 | ph(2) = plot([1:dstep:Nx*Ny],tmp(1:dstep:end),'-r', 'LineWidth',2);%,'Color',[0,0.5,0]); 207 | tmp = Gamma*xi0; 208 | ph(1) = plot([1:dstep:Nx*Ny],tmp(1:dstep:end),'-k', 'LineWidth',2);%,'Color',[0,0,1]); 209 | xlabel('sampling point'), ylabel('Gamma xi') 210 | axis tight 211 | xlim([0,Nx*Ny+10]) 212 | set(gca, 'xtick',[5000 10000],'xticklabel',{'5k', '10k'}) 213 | set(gca,'FontSize',16) 214 | set(gcf,'Position',[100 100 225 200]) 215 | set(gcf,'PaperPositionMode','auto') 216 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'HamiltonianError','.eps']); 217 | -------------------------------------------------------------------------------- /DiscoverDuffing_Convergence.m: -------------------------------------------------------------------------------- 1 | clear all, close all, clc 2 | addpath('./utils'); 3 | path2figs = './../Figures/DUFFING/'; mkdir(path2figs) 4 | ModelName = 'DiscoverDuffingCVG'; 5 | 6 | % Parameters 7 | dt = 0.001; 8 | tspan = dt:dt:10; 9 | x0 = [0; -2.8]; 10 | 11 | % Function definitions 12 | f = @(t,x)([x(2); x(1) - x(1)^3]); 13 | H = @(x)( (1/2)*x(2).^2-(1/2)*x(1).^2 + (1/4)*x(1).^4 ); 14 | ode_options = odeset('RelTol',1e-10, 'AbsTol',1e-11); 15 | 16 | % Parameters for KRONIC 17 | usesine = 0; 18 | polyorder = 4; 19 | nvar = 2; 20 | 21 | %% Collect trajectory data 22 | ModelName_tmp = [ModelName, '_trajectory_']; 23 | [t,y] = ode45(f,tspan,x0,ode_options); 24 | 25 | figure; hold on, box on 26 | plot(t,y(:,1),'-','Color',[0,0,0.7],'LineWidth',2) 27 | plot(t,y(:,2),'-','Color',[0,0.7,0],'LineWidth',2) 28 | legend('x1','x2') 29 | xlabel('t'), ylabel('xi') 30 | set(gca,'xtick',[0:2:10]) 31 | set(gca,'FontSize',16) 32 | set(gcf,'Position',[100 100 225 200]) 33 | set(gcf,'PaperPositionMode','auto') 34 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'Hamiltonian_Trajectory','.eps']); 35 | 36 | %% Compute derivatives 37 | % using fourth order central difference 38 | dy = zeros(length(y)-5,nvar); 39 | for i=3:length(y)-3 40 | for k=1:nvar 41 | dy(i-2,k) = (1/(12*dt))*(-y(i+2,k)+8*y(i+1,k)-8*y(i-1,k)+y(i-2,k)); 42 | end 43 | end 44 | x = y(3:end-3,1:nvar); 45 | dx = dy; 46 | tx = t(3:end-3); 47 | 48 | %% Identify Koopman eigenfunction with lambda=0 (Hamiltonian) from full trajectory 49 | % Truth/reference 50 | Hx = zeros(length(x),1); 51 | for k=1:length(x) 52 | Hx(k) = H(x(k,:)); 53 | end 54 | 55 | % Construct libraries 56 | Theta = buildTheta(x,nvar,polyorder,usesine); 57 | Gamma = buildGamma(x,dx,nvar,polyorder,usesine); 58 | 59 | % Compute SVD 60 | [U,S,V] = svd(0*Theta - Gamma,'econ'); 61 | 62 | % Least-squares Koopman 63 | K = pinv(Theta)*Gamma; 64 | K(abs(K)<1e-12) = 0; 65 | [T,D] = eig(K); 66 | D = diag(D); 67 | [~,IX] = sort(abs(D),'ascend'); 68 | 69 | % Compute eigenfunction 70 | xi0 = V(:,end); % from SVD 71 | % xi0(abs(xi0)<1e-12) = 0; 72 | 73 | D(IX(1)) 74 | xi1 = T(:,IX(1));%+Tls(:,IX(2)); % from least-squares fit 75 | % xi1(abs(xi1)<1e-12) = 0; 76 | 77 | % Corrections for scaling 78 | xi0 = 1/2*xi0/xi0(3); % energy 79 | xi1 = 1/2*xi1/xi1(3); 80 | 81 | % Corrections for sign 82 | if all((Theta)*(xi0)<0), xi0 = -xi0; end; 83 | if all((Theta)*(xi1)<0), xi1 = -xi1; end; 84 | 85 | % Print coefficients 86 | poolDataLIST({'x','y'},xi0,nvar,polyorder,usesine); 87 | poolDataLIST({'x','y'},xi1,nvar,polyorder,usesine); 88 | 89 | % Plot evolution of eigenfunction = Hamiltonian 90 | clear ph 91 | figure; hold on, box on 92 | ph(1) = semilogy(tx,((Theta)*(xi0))-Hx,'-b', 'LineWidth',2,'Color',[0,0,1]); 93 | ph(2) = semilogy(tx,((Theta)*(xi1))-Hx,'-', 'LineWidth',2,'Color',[0,0.5,0]); 94 | xlabel('t'), ylabel('E') 95 | legend(ph,'SVD place', 'LS place') 96 | set(gca,'FontSize',16) 97 | set(gcf,'Position',[100 100 260 200]) 98 | set(gcf,'PaperPositionMode','auto') 99 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'HamiltonianReconstructionError','.eps']); 100 | 101 | clear ph 102 | figure; hold on, box on 103 | ph(1) = semilogy(tx,Hx,'-k', 'LineWidth',18,'Color',[0.7,0.7,0.7]); 104 | ph(2) = semilogy(tx,(Theta)*(xi0),'-b', 'LineWidth',8,'Color',[0,0,1]); 105 | ph(3) = semilogy(tx,(Theta)*(xi1),'-', 'LineWidth',8,'Color',[0,0.5,0]); 106 | xlabel('t'), ylabel('E') 107 | legend(ph,'True place', 'SVD place', 'LS place') 108 | set(gca,'FontSize',16) 109 | set(gcf,'Position',[100 100 260 200]) 110 | set(gcf,'PaperPositionMode','auto') 111 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'HamiltonianReconstruction','.eps']); 112 | 113 | % Plot error 114 | clear ph 115 | figure; hold on, box on 116 | ph(1) = plot(tx,Gamma*xi0,'-k', 'LineWidth',2);%,'Color',[0,0,1]); 117 | ph(2) = plot(tx,Gamma*xi1,'--r', 'LineWidth',2);%,'Color',[0,0.5,0]); 118 | xlabel('t'), ylabel('Gamma xi') 119 | xlim([min(t) max(t)]) 120 | ylim([min(Gamma*xi1)+0.2*min(Gamma*xi1) max(Gamma*xi1)+0.5*max(Gamma*xi1)]) 121 | legend(ph,'SVD p', 'LS p') 122 | set(gca,'xtick',[0:2:10]) 123 | set(gca,'FontSize',16) 124 | set(gcf,'Position',[100 100 225 200]) 125 | set(gcf,'PaperPositionMode','auto') 126 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'HamiltonianError','.eps']); 127 | 128 | %% Error for increasing number of measurements 129 | M = length(tx); 130 | Theta0 = Theta; 131 | Gamma0 = Gamma; 132 | 133 | mset = [7,10,15,21,28,42,56,86,112,224,448,896, 1792, 3584, M]; 134 | Nm = length(mset); 135 | HRerr_SVD = zeros(Nm,1); 136 | HRerr_LSE = zeros(Nm,1); 137 | HRerr_rel_SVD = zeros(Nm,1); 138 | HRerr_rel_LSE = zeros(Nm,1); 139 | Times_SVD = zeros(Nm,1); 140 | Times_LSE = zeros(Nm,1); 141 | HR_SVD = zeros(size(Theta0,1),Nm); 142 | HR_LSE = zeros(size(Theta0,1),Nm); 143 | NS_SVD = zeros(Nm,1); 144 | NS_LSE = zeros(Nm,1); 145 | Cost_SVD = zeros(Nm,1); 146 | Cost_LSE = zeros(Nm,1); 147 | Xc = zeros(size(y,1),size(y,2),Nm); 148 | Hc = zeros(size(y,1),Nm); 149 | uc = zeros(size(y,1),Nm); 150 | xi_SVD = zeros(14,Nm); 151 | xi_LSE = zeros(14,Nm); 152 | %% 153 | for iM = 1:length(mset) 154 | m = mset(iM); 155 | t0 = tic; 156 | 157 | % Data 158 | noise = 0*randn(m,nvar); 159 | ydat = y(1:m,:)+noise; 160 | 161 | tpre0 = tic; 162 | % Time derivative 163 | dy = zeros(length(ydat)-5,nvar); 164 | for i=3:length(ydat)-3 165 | for k=1:nvar 166 | dy(i-2,k) = (1/(12*dt))*(-ydat(i+2,k)+8*ydat(i+1,k)-8*ydat(i-1,k)+ydat(i-2,k)); 167 | end 168 | end 169 | x = ydat(3:end-3,1:nvar); 170 | dx = dy; 171 | 172 | % Construct libraries 173 | Theta = buildTheta(x,nvar,polyorder,usesine); 174 | Gamma = buildGamma(x,dx,nvar,polyorder,usesine); 175 | tpre = toc(tpre0); 176 | 177 | tSVD = tic; 178 | % Compute SVD 179 | [U,S,V] = svd(0*Theta - Gamma,'econ'); 180 | 181 | % Compute eigenfunction 182 | xi0 = V(:,end); % from SVD 183 | xi0(abs(xi0)<1e-8) = 0; xi0 = xi0./norm(xi0); 184 | 185 | % Time to estimate 186 | Times_SVD(iM) = toc(tSVD)+tpre; 187 | 188 | tLSE = tic; 189 | % Least-squares Koopman 190 | K = pinv(Theta)*Gamma; 191 | % K(abs(K)<1e-8) = 0; 192 | [T,D] = eig(K); 193 | D = diag(D); 194 | [~,IX] = sort(abs(D),'ascend'); 195 | 196 | if D(IX(1))==conj(D(IX(2))) 197 | xi1 = T(:,IX(1))+T(:,IX(2)); 198 | else 199 | D(IX(1)); 200 | xi1 = T(:,IX(1)); 201 | end 202 | xi1(abs(xi1)<1e-8) = 0; 203 | xi1 = xi1./norm(xi1); 204 | 205 | Times_LSE(iM) = toc(tLSE)+tpre; 206 | 207 | xi_SVD(:,iM) = xi0; 208 | xi_LSE(:,iM) = xi1; 209 | 210 | % Print coefficients 211 | % poolDataLIST({'x','y'},xi0,nvar,polyorder,usesine); 212 | % poolDataLIST({'x','y'},xi1,nvar,polyorder,usesine); 213 | 214 | % Scaling corrections: Not necessary for control, just for estimating error 215 | % Corrections for sign 216 | if all((Theta0)*(xi0)<0), xi0 = -xi0; end; 217 | if all((Theta0)*(xi1)<0), xi1 = -xi1; end; 218 | % Corrections for scaling 219 | xi0 = 1/2*xi0/xi0(3); % energy 220 | 221 | % Reconstruction Error 222 | HRerr_SVD(iM) = max(abs( (Theta0*(xi0))-Hx )); 223 | HRerr_rel_SVD(iM) = max(abs( ((Theta0*(xi0))-Hx)./Hx)); 224 | HRerr_LSE(iM) = max(abs( (Theta0*(xi1))-Hx )); 225 | HRerr_rel_LSE(iM) = max(abs( ((Theta0*(xi1))-Hx)./Hx)); 226 | 227 | % Null solution error 228 | NS_SVD(iM) = max(abs(Gamma0*xi0)); 229 | NS_LSE(iM) = max(abs(Gamma0*xi1)); 230 | 231 | % Eigenfunction time series 232 | HR_SVD(:,iM) = (Theta0)*(xi0); 233 | HR_LSE(:,iM) = (Theta0)*(xi1); 234 | 235 | % Control with KRONIC 236 | Q = 1; 237 | R = 1; 238 | REF = 0; 239 | A = 0; 240 | B = [0; 1]; 241 | Aphi = 0; % Koopman eigenvalue = 0 242 | Bphi = @(x) [ [(buildThetaGradient(x',1,nvar,polyorder,usesine)*xi0); (buildThetaGradient(x',2,nvar,polyorder,usesine)*xi0)]' * B]; 243 | gain = @(x)(lqr(A,Bphi(x),Q,R)); 244 | Hfun = @(x) [buildTheta(x',nvar,polyorder,usesine)*xi0]; 245 | fun = @(t,x,u)([x(2); x(1) - x(1)^3] + B*u); 246 | ode_options = odeset('RelTol',1e-10, 'AbsTol',1e-11); 247 | [~,y1] = ode45(@(t,x)fun(t,x,-gain(x)*(Hfun(x)-REF)),tspan,x0); %ode_options 248 | 249 | for i = 1:size(y1,1) % Calculate eigenfunction value and applied control 250 | Hc(i,iM) = H(y1(i,:)); 251 | uc(i,iM) = -gain(y1(i,:)')*(Hfun(y1(i,:)')-REF); 252 | end 253 | J = cumsum(Q*(Hc(:,iM)-REF).^2 + R*uc(:,iM).^2); 254 | Cost_SVD(iM) = J(end); 255 | 256 | Xc(:,:,iM) = y1; 257 | 258 | 259 | % Control using LSE solution 260 | % Bphi = @(x) [ [(buildThetaGradient(x',1,nvar,polyorder,usesine)*xi1); (buildThetaGradient(x',2,nvar,polyorder,usesine)*xi1)]' * B]; 261 | % gain = @(x)(lqr(A,Bphi(x),Q,R)); 262 | % Hfun = @(x) [buildTheta(x',nvar,polyorder,usesine)*xi1]; 263 | % fun = @(t,x,u)([x(2); x(1) - x(1)^3] + B*u); 264 | % [~,y1] = ode45(@(t,x)fun(t,x,-gain(x)*(Hfun(x)-REF)),tspan,x0); %ode_options 265 | % 266 | % for i = 1:size(y1,1) 267 | % Hc(i,iM) = H(y1(i,:)); 268 | % uc(i,iM) = -gain(y1(i,:)')*(Hfun(y1(i,:)')-REF); 269 | % end 270 | % J = cumsum(Q*(Hc(:,iM)-REF).^2 + R*uc(:,iM).^2); 271 | % Cost_LSE(iM) = J(end); 272 | 273 | 274 | disp(['FINI: ',num2str(iM), ' of ',num2str(Nm)]) 275 | toc(t0) % one run 276 | end 277 | 278 | %% Kink in curve 279 | mset(13) 280 | xi_SVD(:,13) 281 | NS_SVD(13) 282 | 283 | 284 | %% Show results 285 | clear ph 286 | figure; 287 | semilogy(mset,HRerr_SVD,'-k', 'LineWidth',2);hold on, box on, axis tight 288 | % semilogy(mset,HRerr_LSE,'--r', 'LineWidth',2); 289 | % semilogy(mset(13),HRerr_SVD(13),'or') 290 | % xlim([0,10^4]) 291 | xlabel('Points'), ylabel('RecErr') 292 | set(gca,'FontSize',16,'ytick',[10^0,10^1,10^2,10^3])%,'xtick',[1000,5000,10000]) 293 | set(gcf,'Position',[100 100 225 100]) 294 | set(gcf,'PaperPositionMode','auto') 295 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'DataPoints_Error','.eps']); 296 | 297 | clear ph 298 | figure; 299 | semilogy(mset,NS_SVD,'-k', 'LineWidth',2);hold on, box on, axis tight, 300 | % semilogy(mset(13),NS_SVD(13),'or') 301 | %ph(2) = 302 | % semilogy(mset,NS_LSE,'--r', 'LineWidth',2); 303 | xlim([0,10^4]) 304 | xlabel('Points'), ylabel('NullSolErr') 305 | set(gca,'FontSize',16,'ytick',[10^-9,10^-6,10^0],'xtick',[1000,5000,10000]) 306 | set(gcf,'Position',[100 100 240 105]) 307 | set(gcf,'PaperPositionMode','auto') 308 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'DataPoints_NullError','.eps']); 309 | 310 | clear ph 311 | figure; 312 | ph(1) = plot(mset,Times_SVD,'-k', 'LineWidth',2);hold on, box on,axis tight 313 | % ph(2) = plot(7:M,NS_LSE(7:M),'--r', 'LineWidth',2); 314 | xlim([0,10^4]) 315 | xlabel('Points'), ylabel('Time') 316 | set(gca,'FontSize',16,'ytick',[2*1e-3, 6*1e-3],'xtick',[1000,5000,10000]) 317 | set(gcf,'Position',[100 100 225 115]) 318 | set(gcf,'PaperPositionMode','auto') 319 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'DataPoints_Times','.eps']); 320 | 321 | clear ph 322 | figure; 323 | semilogy(mset,Cost_SVD,'-k', 'LineWidth',2);hold on, box on, axis tight 324 | xlabel('Points'), ylabel('Cost') 325 | xlim([0,10^4]) 326 | set(gca,'FontSize',16,'ytick',[10^4,10^6],'xtick',[1000,5000,10^4]) 327 | set(gcf,'Position',[100 100 305 120]) 328 | set(gcf,'PaperPositionMode','auto') 329 | print('-depsc2', '-loose', [path2figs,ModelName_tmp,'DataPoints_ControlCost','.eps']); 330 | 331 | %% Show controlled trajectories 332 | figure,hold on,box on 333 | cmap = jet(Nm); 334 | x1 = -sqrt(2):0.01:sqrt(2); 335 | plot(x1,x1.*sqrt(1-0.5.*(x1).^2),'--k') 336 | plot(x1,-x1.*sqrt(1-0.5.*(x1).^2),'--k') 337 | iM = Nm; plot(Xc(:,1,iM),Xc(:,2,iM),'-','Color',cmap(iM,:),'LineWidth',4); 338 | iM = Nm-1; plot(Xc(:,1,iM),Xc(:,2,iM),'-','Color',cmap(iM,:),'LineWidth',3); 339 | iM = Nm-2; plot(Xc(:,1,iM),Xc(:,2,iM),'-','Color',cmap(iM,:),'LineWidth',2); 340 | for iM = Nm:-1:1 341 | plot(Xc(:,1,iM),Xc(:,2,iM),'-','Color',cmap(iM,:),'LineWidth',3-(Nm-iM)*0.2); 342 | end 343 | set(gcf,'Position',[100 100 305 120]) 344 | set(gcf,'PaperPositionMode','auto') 345 | 346 | 347 | -------------------------------------------------------------------------------- /DiscoverDuffing_KRONICvsEDMDc.m: -------------------------------------------------------------------------------- 1 | clear all, close all, clc 2 | addpath('./utils'); 3 | path2figs = './../Figures/DUFFING/'; mkdir(path2figs) 4 | ModelName = 'DiscoverDuffing_KRONICvsEDMDc'; 5 | 6 | % Parameters 7 | dt = 0.01; 8 | tspan = dt:dt:10; 9 | x0 = [0; -2.8]; 10 | 11 | % Function definitions 12 | f = @(t,x)([x(2); x(1) - x(1)^3]); 13 | H = @(x)( (1/2)*x(2).^2-(1/2)*x(1).^2 + (1/4)*x(1).^4 ); 14 | ode_options = odeset('RelTol',1e-10, 'AbsTol',1e-11); 15 | nvar = 2; 16 | 17 | %% Collect trajectory data 18 | % Unforced 19 | [t,y] = ode45(f,tspan,x0,ode_options); 20 | 21 | % Forced 22 | B = [0; 1]; 23 | forcing = @(x,t) [(2*(sin(2*pi*.1*t).*sin(2*pi*1*t)))]; 24 | f = @(t,x) [x(2); x(1) - x(1)^3] + B*forcing(x,t); 25 | [tF,yF] = ode45(f,tspan,x0,ode_options); 26 | 27 | uF = forcing(yF,tspan)'; 28 | U = uF'; 29 | 30 | figure; hold on, box on 31 | plot3(y(:,1),y(:,2),zeros(size(y(:,2))),'-','Color',[0,0,0.7],'LineWidth',2) 32 | color_line3(yF(:,1),yF(:,2),zeros(size(yF(:,2))),uF,'LineWidth',2);%,'--','Color','r','LineWidth',2) 33 | xlabel('x1'), ylabel('x2') 34 | 35 | figure,plot(uF) 36 | 37 | %% EDMDc 38 | % Parameters 39 | pEDMDc.usesine = 0; 40 | pEDMDc.polyorder = 4; 41 | Y = poolData(yF,2,pEDMDc.polyorder,pEDMDc.usesine)'; 42 | Nstates = size(Y,1); 43 | 44 | G = [Y(:,1:end-1);U(:,1:end-1)]; 45 | [Ug,Sg,Vg] = svd(G,'econ'); 46 | AB = Y(:,2:end)*Vg*Sg^(-1)*Ug'; 47 | Am = AB(1:Nstates,1:Nstates); 48 | Bm = AB(1:Nstates,end); 49 | Cm = eye(Nstates,Nstates); 50 | Dm = zeros(Nstates,1); 51 | sysmodel_EDMDc = ss(Am,Bm,Cm,Dm,dt); 52 | 53 | % yout = poolDataLIST(yin,ahat,nVars,polyorder,usesine) 54 | yEDMDc = lsim(sysmodel_EDMDc,uF,tspan,poolData(x0',2,pEDMDc.polyorder,pEDMDc.usesine)'); 55 | yEDMDc = yEDMDc(:,1:2); 56 | 57 | figure; hold on, box on 58 | plot3(y(:,1),y(:,2),zeros(size(y(:,2))),'-','Color',[0,0,0.7],'LineWidth',2) 59 | color_line3(yF(:,1),yF(:,2),zeros(size(yF(:,2))),uF,'LineWidth',2);%,'--','Color','r','LineWidth',2) 60 | plot3(yEDMDc(:,1),yEDMDc(:,2),zeros(size(yEDMDc(:,2))),'--','Color','r','LineWidth',2) 61 | xlabel('x1'), ylabel('x2') 62 | legend('Unforced','Forced','EDMDc') 63 | %% KRONIC 64 | % Parameters 65 | pKRONIC.usesine = 0; 66 | pKRONIC.polyorder = 4; 67 | 68 | % Compute derivatives using fourth order central difference 69 | dy = zeros(length(y)-5,nvar); 70 | for i=3:length(y)-3 71 | for k=1:nvar 72 | dy(i-2,k) = (1/(12*dt))*(-y(i+2,k)+8*y(i+1,k)-8*y(i-1,k)+y(i-2,k)); 73 | end 74 | end 75 | x = y(3:end-3,1:nvar); 76 | dx = dy; 77 | tx = t(3:end-3); 78 | 79 | % Construct libraries 80 | Theta = buildTheta(x,nvar,pKRONIC.polyorder,pKRONIC.usesine); 81 | Gamma = buildGamma(x,dx,nvar,pKRONIC.polyorder,pKRONIC.usesine); 82 | 83 | % Compute SVD 84 | [U,S,V] = svd(0*Theta - Gamma,'econ'); 85 | KRONIC.phi = V(:,end); 86 | 87 | % Remove values below small threshold 88 | KRONIC.phi(abs(KRONIC.phi)<1e-6) = 0; 89 | 90 | % Print coefficients 91 | poolDataLIST({'x','y'},KRONIC.phi,nvar,pKRONIC.polyorder,pKRONIC.usesine); 92 | 93 | %% Control parameters & functions 94 | x_REF = [0; 0]; 95 | H_REF = H(x_REF); 96 | 97 | f = @(t,x,u,p)([x(2); x(1)-x(1)^3] + B*u); 98 | Hc = @(x)((1/2)*x(2).^2-(x(1).^2)/2 + (1/4)*x(1).^4 - H_REF); 99 | gradH = @(x)([-x(1) + x(1)^3; x(2)]); 100 | 101 | % Parameters MPC 102 | options = optimoptions('fmincon','Algorithm','sqp','Display','none', ... 103 | 'MaxIterations',100); 104 | Duration = 10; % Run for 'Duration' time units 105 | N = 5; % Prediction horizon (number of iterations) 106 | Nu = N; % Control horizon (number of iterations) 107 | Q = 5*eye(2); % x weights 108 | Ru = 1; % u weights 109 | R = 0; % du weights 110 | QH = 5; % phi(=H) weights 111 | 112 | %% EDMDc 113 | % MPC 114 | p_EDMDc.sys = sysmodel_EDMDc; 115 | p_EDMDc.nvar = 2; 116 | p_EDMDc.polyorder = pEDMDc.polyorder; 117 | p_EDMDc.usesine = pEDMDc.usesine; 118 | p_EDMDc.Hfun = H; 119 | tic 120 | [xHistory_EDMDc_MPC, uHistory_EDMDc_MPC, tHistory_EDMDc_MPC, rHistory_EDMDc_MPC] = runMPC(f,Duration,dt,N,Nu,x0,@ObjectiveFCN_EDMDc,[],QH,R,Ru,options,x_REF,p_EDMDc); 121 | toc 122 | 123 | % EDMDc // LQR fails 124 | % Qlarge = zeros(Nstates,Nstates); Qlarge(1:nvar,1:nvar) = Q; 125 | % K = lqr(sysmodel_EDMDc.A,sysmodel_EDMDc.B,Qlarge,Ru); 126 | % [~,yEDMDc] = ode45(@(t,x)f(t,x,-K*poolData(x',2,polyorder,usesine)'),tspan,x0); 127 | 128 | %% KRONIC (analytic) 129 | 130 | % MPC 131 | Hsys = @(t,x,u,p) [gradH(x)'*B*u]; 132 | % p_KRONIC.sys = Hsys; 133 | p_analyticKRONIC.A = 0; 134 | p_analyticKRONIC.B = @(x) [gradH(x)'*B]; 135 | p_analyticKRONIC.H = H; 136 | p_analyticKRONIC.Ts = dt; 137 | tic 138 | [xHistory_analyticKRONIC_MPC, uHistory_analyticKRONIC_MPC, tHistory_analyticKRONIC_MPC, rHistory_analyticKRONIC_MPC] = runMPC(f,Duration,dt,N,Nu,x0,@ObjectiveFCN_KRONIC,[],QH,R,Ru,options,x_REF,p_analyticKRONIC); 139 | toc 140 | 141 | % LQR 142 | tic 143 | gain = @(x)(lqr(0,(gradH(x)'*B),QH,Ru)); 144 | [tHistory_analyticKRONIC_LQR,xHistory_analyticKRONIC_LQR] = ode45(@(t,x)f(t,x,-gain(x)*Hc(x)),tspan,x0); 145 | xHistory_analyticKRONIC_LQR = xHistory_analyticKRONIC_LQR'; 146 | uHistory_analyticKRONIC_LQR = zeros(size(tHistory_analyticKRONIC_LQR)); 147 | for i = 1:length(xHistory_analyticKRONIC_LQR) 148 | uHistory_analyticKRONIC_LQR(i) = -gain(xHistory_analyticKRONIC_LQR(:,i))*Hc(xHistory_analyticKRONIC_LQR(:,i)); 149 | end 150 | toc 151 | 152 | %% KRONIC (data) 153 | 154 | % MPC 155 | pKRONIC.phi = KRONIC.phi; 156 | pKRONIC.A = 0; 157 | pKRONIC.B = @(x) [(buildThetaGradient(x',1,nvar,pKRONIC.polyorder,pKRONIC.usesine)*pKRONIC.phi);(buildThetaGradient(x',2,nvar,pKRONIC.polyorder,pKRONIC.usesine)*pKRONIC.phi)]'*B; 158 | pKRONIC.H = @(x) buildTheta(x',nvar,pKRONIC.polyorder,pKRONIC.usesine)*pKRONIC.phi; 159 | pKRONIC.Ts = dt; 160 | tic 161 | [xHistory_KRONIC_MPC, uHistory_KRONIC_MPC, tHistory_KRONIC_MPC, rHistory_KRONIC_MPC] = runMPC(f,Duration,dt,N,Nu,x0,@ObjectiveFCN_KRONIC,[],QH,R,Ru,options,x_REF,pKRONIC); 162 | toc 163 | 164 | 165 | % LQR 166 | tic 167 | pKRONIC.Hc = @(x) [buildTheta(x',nvar,pKRONIC.polyorder,pKRONIC.usesine)*KRONIC.phi - pKRONIC.H(x_REF)]; 168 | gain = @(x)(lqr(0,pKRONIC.B(x),QH,Ru)); 169 | [tHistory_KRONIC_LQR,xHistory_KRONIC_LQR] = ode45(@(t,x)f(t,x,-gain(x)*pKRONIC.Hc(x)),tspan,x0); 170 | xHistory_KRONIC_LQR = xHistory_KRONIC_LQR'; 171 | uHistory_KRONIC_LQR = zeros(size(tHistory_KRONIC_LQR)); 172 | for i = 1:length(xHistory_KRONIC_LQR) 173 | uHistory_KRONIC_LQR(i) = -gain(xHistory_KRONIC_LQR(:,i))*pKRONIC.Hc(xHistory_KRONIC_LQR(:,i)); 174 | end 175 | toc 176 | 177 | 178 | %% 179 | figure; hold on, box on 180 | plot3(y(:,1),y(:,2),zeros(size(y(:,2))),'-','Color',[0,0,0.7],'LineWidth',2) 181 | % color_line3(yF(:,1),yF(:,2),zeros(size(yF(:,2))),uF,'LineWidth',2);%,'--','Color','r','LineWidth',2) 182 | % plot3(yEDMDc(:,1),yEDMDc(:,2),zeros(size(yEDMDc(:,2))),'--','Color','r','LineWidth',2) 183 | % color_line3(xHistory(1,:),xHistory(2,:),zeros(size(xHistory(1,:))),uHistory','LineWidth',2);%,'--','Color','r','LineWidth',2) 184 | plot3(xHistory_EDMDc_MPC(1,:),xHistory_EDMDc_MPC(2,:),zeros(size(xHistory_EDMDc_MPC(1,:))),'--','Color','g','LineWidth',2) 185 | plot3(xHistory_analyticKRONIC_MPC(1,:),xHistory_analyticKRONIC_MPC(2,:),zeros(size(xHistory_analyticKRONIC_MPC(1,:))),'-','Color','b','LineWidth',2) 186 | plot3(xHistory_KRONIC_MPC(1,:),xHistory_KRONIC_MPC(2,:),zeros(size(xHistory_KRONIC_MPC(1,:))),'--','Color','cyan','LineWidth',2) 187 | 188 | plot3(xHistory_analyticKRONIC_LQR(1,:),xHistory_analyticKRONIC_LQR(2,:),zeros(size(xHistory_analyticKRONIC_LQR(1,:))),'-','Color','r','LineWidth',2) 189 | plot3(xHistory_KRONIC_LQR(1,:),xHistory_KRONIC_LQR(2,:),zeros(size(xHistory_KRONIC_LQR(1,:))),'--','Color','m','LineWidth',2) 190 | 191 | % color_line3(xHistory2(1,:),xHistory2(2,:),zeros(size(xHistory2(1,:))),uHistory2','LineWidth',2);%,'--','Color','r','LineWidth',2) 192 | x1 = -sqrt(2):0.01:sqrt(2); 193 | plot(x1,x1.*sqrt(1-0.5.*(x1).^2),'--k') 194 | plot(x1,-x1.*sqrt(1-0.5.*(x1).^2),'--k') 195 | xlabel('x'),ylabel('y') 196 | legend('Unforced','EDMD-MPC','analytic KRONIC-MPC', 'KRONIC-MPC','analytic KRONIC-SDRE', 'KRONIC-SDRE','H=0') 197 | axis tight 198 | set(gca,'FontSize',16) 199 | set(gcf,'Position',[100 100 600 400]) 200 | set(gcf,'PaperPositionMode','auto') 201 | %% Show Actuation 202 | figure,plot(tHistory_EDMDc_MPC,uHistory_EDMDc_MPC,'-g','LineWidth',2), hold on, 203 | plot(tHistory_analyticKRONIC_MPC,uHistory_analyticKRONIC_MPC,'-b','LineWidth',2), 204 | plot(tHistory_KRONIC_MPC,uHistory_KRONIC_MPC,'--','Color','cyan','LineWidth',2), 205 | plot(tHistory_analyticKRONIC_LQR,uHistory_analyticKRONIC_LQR,'-r','LineWidth',2), 206 | plot(tHistory_KRONIC_LQR,uHistory_KRONIC_LQR,'--m','LineWidth',2), 207 | xlabel('time'),ylabel('u') 208 | legend('EDMD-MPC','analytic KRONIC-MPC', 'KRONIC-MPC','analytic KRONIC-SDRE', 'KRONIC-SDRE','H=0') 209 | set(gca,'FontSize',16) 210 | set(gcf,'Position',[100 100 600 400]) 211 | set(gcf,'PaperPositionMode','auto') 212 | %% Show eigenfunction time series (=energy/Hamiltonian) 213 | Nt = min([length(tHistory_EDMDc_MPC),length(tHistory_KRONIC_MPC),length(tHistory_KRONIC_LQR)]); 214 | H_EDMDc_MPC = zeros(Nt,1); 215 | H_KRONIC_MPC = zeros(Nt,1); 216 | H_KRONIC_LQR = zeros(Nt,1); 217 | for i = 1:Nt 218 | H_EDMDc_MPC(i) = pKRONIC.H(xHistory_EDMDc_MPC(:,i)); 219 | H_KRONIC_MPC(i) = pKRONIC.H(xHistory_KRONIC_MPC(:,i)); 220 | H_KRONIC_LQR(i) = pKRONIC.H(xHistory_KRONIC_LQR(:,i)); 221 | end 222 | 223 | 224 | figure,plot(H_EDMDc_MPC,'-k','LineWidth',2), hold on, plot(H_KRONIC_MPC,'--r','LineWidth',2), plot(H_KRONIC_LQR,':b','LineWidth',2) 225 | xlabel('time'),ylabel('H') 226 | legend('EDMD-MPC','KRONIC-MPC','KRONIC-LQR') 227 | set(gca,'FontSize',16) 228 | set(gcf,'Position',[100 100 600 400]) 229 | set(gcf,'PaperPositionMode','auto') 230 | %% Show cumulative cost in terms of state 231 | LineWidth = 3; 232 | [Jvals] = evalCostFun(xHistory_EDMDc_MPC(:,1:end-1),uHistory_EDMDc_MPC(:,1:end-1),Q,Ru,x_REF); 233 | [Jvals2] = evalCostFun(xHistory_KRONIC_MPC(:,1:end-1),uHistory_KRONIC_MPC(:,1:end-1),Q,Ru,x_REF); 234 | [JvalsKRONIC] = evalCostFun(xHistory_KRONIC_LQR,uHistory_KRONIC_LQR',Q,Ru,x_REF); 235 | figure,box on 236 | plot(tHistory_KRONIC_LQR,cumsum(Jvals),'--r','LineWidth',LineWidth), hold on, 237 | plot(tHistory_KRONIC_LQR,cumsum(Jvals2),':k','LineWidth',LineWidth), 238 | plot(tHistory_KRONIC_LQR,cumsum(JvalsKRONIC),'-b','LineWidth',LineWidth) 239 | ylim([0 2.5e4]) 240 | ylabel('J'), xlabel('t') 241 | set(gca,'FontSize',16) 242 | % axis tight 243 | set(gcf,'Position',[100 100 225 200]) 244 | set(gcf,'PaperPositionMode','auto') 245 | print('-painters','-depsc2', '-loose', [path2figs,ModelName,'_PerformanceComparisonWithKRONIC_Jx','.eps']); 246 | 247 | %% Show cumulative cost in terms of eigenfunction 248 | clear ph 249 | [Jvals_H] = evalCostFun(H_EDMDc_MPC',uHistory_EDMDc_MPC(:,1:end-1),QH,Ru,H(x_REF)); 250 | [Jvals2_H] = evalCostFun(H_KRONIC_MPC',uHistory_KRONIC_MPC(:,1:end-1),QH,Ru,H(x_REF)); 251 | [JvalsKRONIC_H] = evalCostFun(H_KRONIC_LQR',uHistory_KRONIC_LQR',QH,Ru,H(x_REF)); 252 | figure,box on 253 | ph(1) = plot(tHistory_KRONIC_LQR,cumsum(Jvals_H),'--r','LineWidth',LineWidth); hold on, 254 | ph(2) = plot(tHistory_KRONIC_LQR,cumsum(Jvals2_H),':k','LineWidth',LineWidth); 255 | ph(3) = plot(tHistory_KRONIC_LQR,cumsum(JvalsKRONIC_H),'-b','LineWidth',LineWidth); 256 | ylim([0 2.5e4]) 257 | ylabel('J'), xlabel('t') 258 | set(gca,'FontSize',16) 259 | % axis tight 260 | set(gcf,'Position',[100 100 225 200]) 261 | set(gcf,'PaperPositionMode','auto') 262 | print('-painters','-depsc2', '-loose', [path2figs,ModelName,'_PerformanceComparisonWithKRONIC_Jphi','.eps']); 263 | 264 | axis off 265 | ylim([5e4 1e5]) 266 | legend(ph,{'EDMDc-MPC','KRONIC-MPC', 'KRONIC-SDRE00'}) 267 | print('-painters','-depsc2', '-loose', [path2figs,ModelName,'_PerformanceComparisonWithKRONIC_legend','.eps']); 268 | 269 | -------------------------------------------------------------------------------- /FIGURES/EXAMPLES.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eurika-kaiser/KRONIC/1d0893f2b39f83e3faf86c65acf68ebfc0af816a/FIGURES/EXAMPLES.png -------------------------------------------------------------------------------- /FIGURES/KRONIC.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eurika-kaiser/KRONIC/1d0893f2b39f83e3faf86c65acf68ebfc0af816a/FIGURES/KRONIC.png -------------------------------------------------------------------------------- /FIGURES/SparseRegression.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/eurika-kaiser/KRONIC/1d0893f2b39f83e3faf86c65acf68ebfc0af816a/FIGURES/SparseRegression.png -------------------------------------------------------------------------------- /HamiltonianDuffing.m: -------------------------------------------------------------------------------- 1 | clear all, close all, clc 2 | 3 | path2figs = '../Figures/'; 4 | path2data = '../Data/'; 5 | ModelName = 'HamiltonianDuffing_'; 6 | 7 | % Parameters 8 | Q = 1; 9 | R = 1; 10 | REF = 0; 11 | a = 0;%sqrt(1/5); 12 | V = 1; 13 | 14 | % Hamiltonian 15 | [X,Y] = meshgrid([-2.5:0.01:2.5], [-2.5:0.01:2.5]); 16 | Hfield = (1/2)*Y.^2-(X.^2)/2 + (V/4)*X.^4;% 17 | 18 | % Initial condition 19 | y0ic = [0 0]; 20 | xvec = -1; 21 | yvec = -2.5:0.25:-0.25; 22 | x = [-4:0.01:4]; 23 | dt = 0.001; 24 | Ny = length(yvec); 25 | Nx = length(xvec); 26 | yIC = zeros(2,Ny,Nx); 27 | [x0,y0] = meshgrid(xvec+y0ic(1),yvec+y0ic(2)); 28 | yIC(1,:,:) = x0; 29 | yIC(2,:,:) = y0; 30 | yIC = [yIC, [1; 0.5], [1; 0.25]]; Ny = Ny+2; 31 | 32 | %% UNFORCED 33 | B = [0; 0]; 34 | a = 0; 35 | V = 1; 36 | duration = 25; 37 | tspan =[0,duration]; 38 | L = duration/dt; 39 | yin = yIC; 40 | yout = zeros(2,Ny,Nx,L); 41 | yout(:,:,:,1) = yin; 42 | for step = 1:L-1 43 | time = step*dt; 44 | yout(:,:,:,step+1) = rk4singlestep(@(t,y,u)Duffing_ensemble(t,y,0,1,1,0,0,B,u),dt,time,yin,0); 45 | yin = yout(:,:,:,step); 46 | end 47 | 48 | 49 | %% KRONIC 50 | B = [0; 1]; 51 | ModelName1 = [ModelName, 'B01_']; 52 | % B = [1; 0]; 53 | % ModelName = [ModelName, 'B10_']; 54 | Bc = B; 55 | 56 | B = Bc; 57 | f = @(t,x,u)([x(2); x(1)-V*x(1)^3]+B*u); 58 | H = @(x)((1/2)*x(2).^2-(x(1).^2)/2 + (V/4)*x(1).^4); 59 | gradH = @(x)([-x(1) + V*x(1)^3; x(2)]); 60 | gain = @(x)(lqr(0,(gradH(x)'*B),Q,R)); 61 | 62 | % yIC = squeeze(yout(1:2,1,1,1:1000:8000)); 63 | a = 2.5; 64 | yIC = [-2 0 2 -1 1 -2 0 2 -1 1 -a -a -a a a a; ... 65 | a a a a a -a -a -a -a -a -2 0.001 2 -2 0.001 2]; 66 | Nic = size(yIC,2); 67 | yin = yIC; 68 | uin = zeros(1,Nic); 69 | for i = 1:Nic 70 | uin(i) = -gain(yin(:,i))*H(yin(:,i)); 71 | end 72 | yout_ctrl = zeros(2,Nic,L); 73 | yout_ctrl(:,:,1) = yin; 74 | for step = 1:L-1 75 | time = step*dt; 76 | yout_ctrl(:,:,step+1) = rk4singlestep(@(t,y,u)Duffing_ensemble(t,y,0,1,1,0,0,B,u),dt,time,yin,uin); 77 | yin = yout_ctrl(:,:,step+1); 78 | uin = zeros(1,Nic); 79 | for i = 1:Nic 80 | uin(i) = -gain(yin(:,i))*H(yin(:,i)); 81 | end 82 | end 83 | 84 | % Inner circles 85 | yIC = [squeeze(yout(1:2,end,1,1:3000:10000)),squeeze(yout(1:2,end-2,1,1:3000:10000))]; 86 | Ny = size(yIC,2); 87 | yIC(1,1:Ny/2) = 1+0.1*(yIC(1,1:Ny/2)-1); 88 | yIC(2,1:Ny/2) = 0.1*yIC(2,1:Ny/2); 89 | yIC(1,Ny/2+1:end) = -1+0.1*(yIC(1,Ny/2+1:end)+1); 90 | yIC(2,Ny/2+1:end) = 0.1*yIC(2,Ny/2+1:end); 91 | Nic = size(yIC,2); 92 | yin = yIC; 93 | uin = zeros(1,Nic); 94 | for i = 1:Nic 95 | uin(i) = -gain(yin(:,i))*H(yin(:,i)); 96 | end 97 | yout_ctrl_in = zeros(2,Nic,L); 98 | yout_ctrl_in(:,:,1) = yin; 99 | for step = 1:L-1 100 | time = step*dt; 101 | yout_ctrl_in(:,:,step+1) = rk4singlestep(@(t,y,u)Duffing_ensemble(t,y,0,1,1,0,0,B,u),dt,time,yin,uin); 102 | yin = yout_ctrl_in(:,:,step+1); 103 | uin = zeros(1,Nic); 104 | for i = 1:Nic 105 | uin(i) = -gain(yin(:,i))*H(yin(:,i)); 106 | end 107 | end 108 | 109 | 110 | 111 | %% SAVE RESULTS 112 | save([path2data,[ModelName1,'Ensemble.mat']]) 113 | 114 | -------------------------------------------------------------------------------- /HamiltonianDuffing_FIG.m: -------------------------------------------------------------------------------- 1 | path2data = '../Data/'; ModelName1 = ['HamiltonianDuffing_','B01_']; 2 | load([path2data,[ModelName1,'Ensemble.mat']]) 3 | path2figs = '../Figures/HAMILTONIAN_DUFFING/'; mkdir(path2figs) 4 | 5 | %% Show Phase plot 6 | [X,Y] = meshgrid([-2.4:0.01:2.4], [-2.4:0.01:2.4]); 7 | Hfield = (1/2)*Y.^2-(X.^2)/2 + (V/4)*X.^4;% 8 | 9 | cmap = gray(2*11); 10 | cmap = cmap(1:2:end,:); 11 | figure; 12 | box on 13 | hold on 14 | % contourf([-2.5:0.01:2.5], [-2.5:0.01:2.5],Hfield,[-1,-0.2,-0.1,0,0.1,0.5,1.5,2.5],'LineColor','none'), colormap(gray) 15 | % sh = surf([-2.4:0.01:2.4], [-2.4:0.01:2.4],-ones(size(Hfield)),ones(size(Hfield)));shading interp,view(2) 16 | % sh.FaceColor = [0 0 0]; 17 | sh = surf([-2.4:0.01:2.4], [-2.4:0.01:2.4],zeros(size(Hfield)));shading interp, view(2) 18 | sh.FaceColor = [0 0 0]; 19 | contourf([-2.4:0.01:2.4], [-2.4:0.01:2.4],log(Hfield+0.2500001),[log([-0.2,-0.1,0,0.25:0.5:4]+0.25)],'LineColor','none'), colormap(cmap) 20 | hold on 21 | 22 | % Nx = size(yout,3); 23 | % Ny = size(yout,2); 24 | % for iy = 1:Ny 25 | % for ix = 1:Nx 26 | % plot(squeeze(yout(1,iy,ix,:)),squeeze(yout(2,iy,ix,:)),'-','Color',0.7*ones(1,3),'LineWidth',2,'MarkerSize',10) 27 | % end 28 | % end 29 | 30 | Nic = size(yout_ctrl,2); 31 | cmap = jet(Nic); 32 | for iIC = 1:Nic 33 | hold on 34 | if any([1,2,3,6,7,8] == iIC); 35 | % plot3(squeeze(yout_ctrl(1,iIC,:)),squeeze(yout_ctrl(2,iIC,:)),squeeze(ones(size(yout_ctrl(2,iIC,:)))),'-','Color','b','LineWidth',1.2) 36 | plot(squeeze(yout_ctrl(1,iIC,:)),squeeze(yout_ctrl(2,iIC,:)),'-','Color',cmap(3,:),'LineWidth',1.2) 37 | end 38 | end 39 | 40 | Nic = size(yout_ctrl,2); 41 | % cmap = jet(Nic); 42 | for iIC = 1:Nic 43 | hold on 44 | if any([1,2,4,5,7] == iIC); 45 | plot(squeeze(yout_ctrl_in(1,iIC,:)),squeeze(yout_ctrl_in(2,iIC,:)),'-','Color',cmap(3,:),'LineWidth',1.2) 46 | end 47 | end 48 | 49 | x1 = -sqrt(2):0.01:sqrt(2); 50 | plot(x1,x1.*sqrt(1-0.5.*(x1).^2),'--y') 51 | plot(x1,-x1.*sqrt(1-0.5.*(x1).^2),'--y') 52 | drawnow 53 | 54 | xlabel('x_1'), ylabel('x_2') 55 | set(gca,'FontSize',14, 'LineWidth',1) 56 | axis equal 57 | axis([-2.5 2.5 -2.5 2.5]) 58 | set(gcf,'Position',[100 100 250 250]) 59 | set(gcf,'PaperPositionMode','auto') 60 | print('-depsc2', '-loose', [path2figs,ModelName1,'PhasePlot','.eps']); 61 | print('-dpng', '-loose', [path2figs,ModelName1,'PhasePlot','.png']); -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | CiteMe License for Open Source Software, Version 0 draft 2016-06-24. (CiteMe OSS v0 draft) 2 | 3 | Copyright (c) 2017, Eurika Kaiser, eurika.kaiser@gmail.com 4 | 5 | Permission to use, copy, modify, and/or distribute this software for any purpose 6 | with or without fee is hereby granted, provided that the above copyright notice 7 | and this permission notice appear in all copies. 8 | 9 | 10 | If this software is used to make a contribution to the findings published in an 11 | article, then the author(s) of that article are requested to include a citation 12 | using one of the references provided below. 13 | 14 | Scientific articles shall include: 15 | 16 | Eurika Kaiser, J. Nathan Kutz, Steven L. Brunton, “Data-driven discovery of Koopman eigenfunctions for control”, arXiv preprint arXiv:707.01146 17 | 18 | Articles of any other type shall include: 19 | 20 | Eurika Kaiser, J. Nathan Kutz, Steven L. Brunton, “Data-driven discovery of Koopman eigenfunctions for control”, arXiv preprint arXiv:707.01146 21 | 22 | https://github.com/eurika-kaiser/KRONIC 23 | 24 | 25 | If this software is used as a component of a CiteMe OSS licensed third-party 26 | software that requests a citation which supersedes the reference provided in 27 | this license in regard to its relevance to the findings published in the 28 | article, then the citation requested in this license shall be optional. 29 | 30 | If a large portion of this software is modified, then the author of the 31 | modifications may replace the references provided in this license by references 32 | that have a higher scientific relevance in regard to contributions the modified 33 | software may make to publishable findings. 34 | 35 | 36 | THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH 37 | REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND 38 | FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, 39 | INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS 40 | OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER 41 | TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF 42 | THIS SOFTWARE. -------------------------------------------------------------------------------- /NonAutonomDoubleGyre.m: -------------------------------------------------------------------------------- 1 | clear all, close all, clc 2 | 3 | path2figs = '../Figures/DOUBLE_GYRE/'; mkdir(path2figs) 4 | path2data = '../Data/'; mkdir(path2data) 5 | ModelName = 'NonAutonomDoubleGyre_'; 6 | 7 | % Parameters double gyre 8 | tspan = 0:.01:50; 9 | epsilon = 0.25; 10 | omega = 2*pi; 11 | A = 0.25; 12 | 13 | % Forcing in x 14 | % x = 1+ (sqrt(1+4*epsilon^2*sin(omega*tspan).^2)-1)./(2*epsilon*sin(omega*tspan)); 15 | % figure,plot(tspan,x) 16 | 17 | % Streamfunction / Hamiltonian 18 | PSIsteady = @(x)(A*sin(pi*x(1)).*sin(pi*x(2))); 19 | Hsteady = @(x)(-A*sin(pi*x(1)).*sin(pi*x(2))); 20 | x = [0:0.01:2]; y = [0:0.01:1]; 21 | [X,Y] = meshgrid(x,y); 22 | force = zeros(size(X,1),size(X,2),length(tspan)); 23 | PSIfield = zeros(size(X,1),size(X,2),length(tspan)); 24 | for it = 1:length(tspan) 25 | force(:,:,it) = epsilon*sin(omega.*tspan(it))*X.^2 + (1-2*epsilon*sin(omega.*tspan(it))).*X; 26 | PSIfield(:,:,it) = A*sin(pi*force(:,:,it)).*sin(pi*Y); 27 | end 28 | StreamFun = A*sin(pi*X).*sin(pi*Y); 29 | 30 | % Parameters control 31 | % x0 = [1.3; 0.5]; 32 | x0 = [1.02; 0.01]; 33 | Q = 1; 34 | R = 1; 35 | REF = 0.2; 36 | ode_options = odeset('RelTol',1e-10, 'AbsTol',1e-11); 37 | % ode_options = odeset('RelTol',1e-6, 'AbsTol',1e-7); 38 | 39 | % Define functions 40 | force = @(x,t)(epsilon*sin(omega*t)*x(1).^2 + (1-2*epsilon*sin(omega*t)).*x(1)); 41 | dfdx = @(x,t)(2*epsilon*sin(omega*t)*x(1) + (1-2*epsilon*sin(omega*t))); 42 | Psi = @(x,t)(A*sin(pi*force(x,t)).*sin(pi*x(2))); 43 | gradPsi = @(x,t)([A*pi*cos(pi*force(x,t))*sin(pi*x(2))*dfdx(x,t); ... 44 | A*pi*sin(pi*force(x,t))*cos(pi*x(2))]); 45 | 46 | 47 | %% UNFORCED SYSTEM 48 | B = [0; 0]; 49 | f = @(t,x,u)([-A*pi*sin(pi*force(x,t))*cos(pi*x(2)); ... 50 | A*pi*cos(pi*force(x,t))*sin(pi*x(2)).*dfdx(x,t)]+B*u); %TODO, check again, dfdx 51 | [t,y0] = ode45(@(t,x)f(t,x,0),tspan,x0,ode_options); 52 | [Psivals0,Jvals0] = evalCostFun_TimeDependentKoopEfun(Psi,y0,zeros(size(y0,1),1),Q,R,REF,tspan); 53 | 54 | %% KRONIC using stream function 55 | % B = [0; 1]; R = 1; 56 | % ModelName = [ModelName, 'B01_']; 57 | % B = [1; 0]; R = 1; 58 | % ModelName = [ModelName, 'B10_']; 59 | B = eye(2); R = eye(2); 60 | ModelName1 = [ModelName, 'B11_']; 61 | Bc = B; 62 | 63 | % Koopman eigenfunction control 64 | f = @(t,x,u)([-A*pi*sin(pi*force(x,t))*cos(pi*x(2)); ... 65 | A*pi*cos(pi*force(x,t))*sin(pi*x(2))]+B*u); 66 | gain = @(x,t)(lqr(0,(gradPsi(x,t)'*B),Q,R)); 67 | [~,y1] = ode45(@(t,x)f(t,x,-gain(x,t)*(Psi(x,t)-REF)),tspan,x0,ode_options); 68 | 69 | uvals1 = zeros(length(y1),size(B,2)); for k=1:length(y1), uvals1(k,:) = - gain(y1(k,:),tspan(k))*(Psi(y1(k,:),tspan(k))-REF); end 70 | [Psivals1,Jvals1] = evalCostFun_TimeDependentKoopEfun(Psi,y1,uvals1,Q,R,REF,tspan); 71 | 72 | 73 | %% SAVE RESULTS 74 | save([path2data,[ModelName1,'Data.mat']]) -------------------------------------------------------------------------------- /NonAutonomDoubleGyre_FIG.m: -------------------------------------------------------------------------------- 1 | 2 | path2data = '../Data/'; ModelName1 = ['NonAutonomDoubleGyre_', 'B11_']; 3 | load([path2data,[ModelName1,'Data.mat']]) 4 | path2figs = '../Figures/DOUBLE_GYRE/'; mkdir(path2figs) 5 | 6 | %% Phase plot with single trajectory 7 | clear ph 8 | [y1_corrected,StartIDX,outside_vec,switched_domain_vec,Hvec] = applyPeriodicBC(y1,[0,2],[0,1],Hsteady); 9 | cmap = gray(30); cmap=flipud(cmap(1:end,:)); 10 | 11 | figure,surf(x,y,StreamFun-10), shading interp, hold on, box on, view(2) 12 | caxis([min(min(StreamFun))-10 max(max(StreamFun))-10]) 13 | colormap(cmap) 14 | caxis([min(min(StreamFun))-10 max(max(StreamFun))-10]) 15 | contour(X,Y,StreamFun, [-1:0.05:1],'k','ShowText','on') 16 | 17 | ph(1) = plot(y0(:,1),y0(:,2),'-','Color',[0.8,0.8,0],'LineWidth',2); 18 | plot(y1_corrected(1,1),y1_corrected(1,2),'.','Color',[1 0 0],'MarkerSize',20) 19 | for i = 1:length(StartIDX)-1 20 | ph(2) = plot(y1_corrected(StartIDX(i):StartIDX(i+1)-1,1),y1_corrected(StartIDX(i):StartIDX(i+1)-1,2),'-','Color',[.1,.1,1],'LineWidth',2); 21 | end 22 | contour3(X,Y,PSIfield(:,:,end)+20, [0.2+20, 0.2+20],'--','Color',0.8.*ones(1,3), ... 23 | 'ShowText','off','LineWidth',2) 24 | % text(0.45,0.75,'-0.2','FontSize',14,'Color',[0.8,0.8,1]) 25 | box on 26 | % legend(ph,'Unforced', 'Controlled') 27 | xlabel('x1'), ylabel('x2') 28 | set(gca,'FontSize',14, 'LineWidth',1) 29 | axis equal 30 | set(gcf,'Position',[100 100 600 350]) 31 | set(gcf,'PaperPositionMode','auto') 32 | print('-opengl','-depsc2', '-loose', [path2figs,ModelName1,'singleIC_PhasePlot','.eps']); 33 | 34 | %% Cost etc. for single trajectory 35 | LineWidth = 2; 36 | fhandle = figure; hold on, box on, grid on 37 | plot(tspan,Psivals0,'-','Color',[0.8,0.8,0],'LineWidth',2); 38 | plot(tspan,Psivals1,'-','Color',[.1,.1,1],'LineWidth',2) 39 | xlabel('t'), ylabel('Psi') 40 | set(gca,'FontSize',16) 41 | axis tight 42 | set(gca,'xtick',[0,20,40]) 43 | set(gcf,'Position',[100 100 225 200]) 44 | set(gcf,'PaperPositionMode','auto') 45 | print('-painters','-depsc2', '-loose', [path2figs,ModelName1,'singleIC_Psi','.eps']); 46 | 47 | clear ph 48 | fhandle = figure; hold on, box on, grid on 49 | ph(1) = plot(tspan,Jvals0,'-','Color',[0.8,0.8,0],'LineWidth',2); 50 | ph(2) = plot(tspan,Jvals1,'-','Color',[.1,.1,1],'LineWidth',2); 51 | xlabel('t'), ylabel('J') 52 | set(gca,'FontSize',16) 53 | axis tight 54 | set(gca,'xtick',[0,20,40]) 55 | set(gcf,'Position',[100 100 225 200]) 56 | set(gcf,'PaperPositionMode','auto') 57 | print('-painters','-depsc2', '-loose', [path2figs,ModelName1,'singleIC_J','.eps']); 58 | 59 | figure; hold on 60 | ph(1) = plot([0,1],[0,0],'-','Color',[0.8,0.8,0],'LineWidth',6); 61 | ph(2) = plot([0,1],[0,0],'-','Color',[.1,.1,1],'LineWidth',6); 62 | pl = legend('Unforced', 'Controlled'); pl.Position = [0.3 0.35 0.5 0.2]; 63 | xlim([-1 2]), axis off 64 | set(gca,'FontSize',16) 65 | set(gca,'xtick',[0,20,40]) 66 | set(gcf,'Position',[100 100 225 200]) 67 | set(gcf,'PaperPositionMode','auto') 68 | print('-painters','-depsc2', '-loose', [path2figs,ModelName1,'singleIC_legend','.eps']); 69 | 70 | 71 | clear ph 72 | fhandle = figure; hold on, box on, grid on 73 | % plot(tspan,uvals0,'-','Color',[0.8,0.8,0],'LineWidth',2); 74 | ph(1) = plot(tspan,uvals1(:,1),'-','Color',[.1,.1,1],'LineWidth',2); 75 | ph(2) = plot(tspan,uvals1(:,2),'-','Color',[.1,0.7,.1],'LineWidth',2); 76 | xlabel('t'), ylabel('u') 77 | set(gca,'FontSize',16) 78 | axis tight 79 | legend('u1','u2') 80 | set(gca,'xtick',[0,20,40]) 81 | set(gcf,'Position',[100 100 225 200]) 82 | set(gcf,'PaperPositionMode','auto') 83 | print('-painters','-depsc2', '-loose', [path2figs,ModelName1,'singleIC_u','.eps']); 84 | -------------------------------------------------------------------------------- /Pendulum.m: -------------------------------------------------------------------------------- 1 | clear all, close all, clc 2 | 3 | ModelName = 'Pendulum_'; 4 | path2data = '../Data/'; mkdir(path2data) 5 | path2figs = '../Figures/PENDULUM/'; mkdir(path2figs) 6 | 7 | % Parameters 8 | tspan = .001:.001:100; 9 | x0 = [0.5;0.5];[.5; .5]; 10 | Q = 1; 11 | R = 1; 12 | H = @(x)([(1/2)*x(2)^2-cos(x(1))]); 13 | gradH = @(x)([sin(x(1)); x(2)]); 14 | [X,Y] = meshgrid([-2*pi:0.01:2*pi], [-4:0.01:4]); 15 | Hfield = (1/2)*Y.^2-cos(X); 16 | 17 | %% Unforced 18 | REF = 0; 19 | B = [0; 0]; 20 | f = @(t,x,u)([x(2); -sin(x(1))]+B*u); 21 | ode_options = odeset('RelTol',1e-10, 'AbsTol',1e-11); 22 | [t,y0] = ode45(@(t,x)f(t,x,0),tspan,x0,ode_options); 23 | [Hvals0,Jvals0] = evalCostFun_Hamiltonian(H,y0,zeros(1,size(y0,1)),Q,R,REF); 24 | 25 | %% Controlled system 26 | % B = [0; 1]; 27 | % ModelName1 = [ModelName, 'B01_']; 28 | B = [1; 0]; 29 | ModelName1 = [ModelName, 'B10_']; 30 | f = @(t,x,u)([x(2); -sin(x(1))]+B*u); 31 | 32 | 33 | % KRONIC: Case 1 34 | REF = 1;%H([pi;0]); 35 | Href = @(x)([(1/2)*x(2)^2-cos(x(1))]-REF); 36 | gain = @(x)(lqr(0,(gradH(x)'*B),Q,R)); 37 | [~,y1] = ode45(@(t,x)f(t,x,-gain(x)*Href(x)),tspan,x0,ode_options); 38 | uvals1 = zeros(1,length(y1)); for k=1:length(y1), uvals1(1,k) = - gain(y1(k,:))*Href(y1(k,:)); end 39 | [Hvals1,Jvals1] = evalCostFun_Hamiltonian(H,y1,uvals1,Q,R,REF); 40 | 41 | % Store results 42 | DataStore.y1 = y1; 43 | DataStore.u1 = uvals1; 44 | DataStore.H1 = Hvals1; 45 | DataStore.J1 = Jvals1; 46 | DataStore.tspan1 = tspan; 47 | 48 | 49 | % KRONIC: Case 2 50 | REF = 0; 51 | Href = @(x)([(1/2)*x(2)^2-cos(x(1))]-REF); 52 | gain = @(x)(lqr(0,(gradH(x)'*B),Q,R)); 53 | [~,y1] = ode45(@(t,x)f(t,x,-gain(x)*Href(x)),tspan,x0,ode_options); 54 | uvals1 = zeros(1,length(y1)); for k=1:length(y1), uvals1(1,k) = - gain(y1(k,:))*Href(y1(k,:)); end 55 | [Hvals1,Jvals1] = evalCostFun_Hamiltonian(H,y1,uvals1,Q,R,REF); 56 | 57 | % Store results 58 | DataStore.y2 = y1; 59 | DataStore.u2 = uvals1; 60 | DataStore.H2 = Hvals1; 61 | DataStore.J2 = Jvals1; 62 | DataStore.tspan2 = tspan; 63 | 64 | % KRONIC: Case 3 65 | REF = 2; 66 | Href = @(x)([(1/2)*x(2)^2-cos(x(1))]-REF); 67 | gain = @(x)(lqr(0,(gradH(x)'*B),Q,R)); 68 | [~,y1] = ode45(@(t,x)f(t,x,-gain(x)*Href(x)),tspan,x0,ode_options); 69 | uvals1 = zeros(1,length(y1)); for k=1:length(y1), uvals1(1,k) = - gain(y1(k,:))*Href(y1(k,:)); end 70 | [Hvals1,Jvals1] = evalCostFun_Hamiltonian(H,y1,uvals1,Q,R,REF); 71 | 72 | Htest = zeros(size(Hvals1)); 73 | for k=1:length(y1), Htest(k) = H([mod(y1(k,1),2*pi), y1(k,2)]); end % seems ok, =2 74 | 75 | % Store results 76 | DataStore.y3 = y1; 77 | DataStore.u3 = uvals1; 78 | DataStore.H3 = Hvals1; 79 | DataStore.J3 = Jvals1; 80 | DataStore.tspan3 = tspan; 81 | 82 | 83 | % KRONIC: Case 4 84 | REF = H([0,0]); 85 | Href = @(x)([(1/2)*x(2)^2-cos(x(1))]-REF); 86 | gain = @(x)(lqr(0,(gradH(x)'*B),Q,R)); 87 | [~,y1] = ode45(@(t,x)f(t,x,-gain(x)*Href(x)),tspan,x0,ode_options); 88 | uvals1 = zeros(1,length(y1)); for k=1:length(y1), uvals1(1,k) = - gain(y1(k,:))*Href(y1(k,:)); end 89 | [Hvals1,Jvals1] = evalCostFun_Hamiltonian(H,y1,uvals1,Q,R,REF); 90 | 91 | % Store results 92 | DataStore.y4 = y1; 93 | DataStore.u4 = uvals1; 94 | DataStore.H4 = Hvals1; 95 | DataStore.J4 = Jvals1; 96 | DataStore.tspan4 = tspan; 97 | 98 | %% SAVE RESULTS 99 | save([path2data,[ModelName1,'Data.mat']]) 100 | 101 | -------------------------------------------------------------------------------- /Pendulum_FIG.m: -------------------------------------------------------------------------------- 1 | path2data = '../Data/'; ModelName1='Pendulum_B10_'; 2 | load([path2data,[ModelName1,'Data.mat']]) 3 | path2figs = '../Figures/PENDULUM/'; mkdir(path2figs) 4 | 5 | 6 | %% Show phase plot 7 | figure; hold on, box on 8 | imagesc(X(1,:),Y(:,1),Hfield), shading interp, view(2), colormap(gray(10)) 9 | plot(y0(:,1),y0(:,2),'-','Color',0.9*ones(1,3),'LineWidth',1); 10 | hold on , grid on 11 | plot(DataStore.y1(1:end,1),DataStore.y1(1:end,2),'-','Color',[1,0,0],'LineWidth',1.2); 12 | plot(DataStore.y2(1:end,1),DataStore.y2(1:end,2),'-','Color',[1,1,0],'LineWidth',1.2); 13 | plot(DataStore.y3(1:end,1),DataStore.y3(1:end,2),'-','Color',[0,0,1],'LineWidth',1.2); 14 | plot(DataStore.y4(1:end,1),DataStore.y4(1:end,2),'-','Color',[0,1,0],'LineWidth',1.2); 15 | xlabel('x_1'), ylabel('x_2') 16 | hold on 17 | 18 | % Separatrices 19 | plot(2*atan(sinh([-pi:0.1:pi])), 2*sech([-pi:0.1:pi]),'--','Color',0.7*ones(1,3), 'LineWidth',1) % center 20 | plot(-2*atan(sinh([-pi:0.1:pi])), -2*sech([-pi:0.1:pi]),'--','Color',0.7*ones(1,3), 'LineWidth',1) 21 | plot(2*atan(sinh([-pi:0.1:pi]))-2*pi, 2*sech([-pi:0.1:pi]),'--','Color',0.7*ones(1,3), 'LineWidth',1) %left 22 | plot(-2*atan(sinh([-pi:0.1:pi]))-2*pi, -2*sech([-pi:0.1:pi]),'--','Color',0.7*ones(1,3), 'LineWidth',1) 23 | plot(2*atan(sinh([-pi:0.1:pi]))+2*pi, 2*sech([-pi:0.1:pi]),'--','Color',0.7*ones(1,3), 'LineWidth',1) %right 24 | plot(-2*atan(sinh([-pi:0.1:pi]))+2*pi, -2*sech([-pi:0.1:pi]),'--','Color',0.7*ones(1,3), 'LineWidth',1) 25 | axis([-2*pi-0.02 2*pi+0.02 -4-0.03 4+0.03]) 26 | 27 | set(gca,'xtick',[-2*pi,-pi,0,pi,2*pi],'xticklabel',{'-2\pi', '-\pi', '0', '\pi', '-2\pi'}) 28 | set(gca,'FontSize',14, 'LineWidth',1) 29 | set(gcf,'Position',[100 100 350 250]) 30 | set(gcf,'PaperPositionMode','auto') 31 | print('-depsc2', '-loose', [path2figs,ModelName,'PhasePlot','.eps']); 32 | print('-dpng', '-loose', [path2figs,ModelName,'PhasePlot','.png']); 33 | 34 | 35 | -------------------------------------------------------------------------------- /QuarticPotentialWell.m: -------------------------------------------------------------------------------- 1 | clear all, close all, clc 2 | figpath = '../Figures/QUARTIC_POT_WELL/'; mkdir(figpath) 3 | path2data = '../Data/'; mkdir(path2data) 4 | addpath('./utils'); 5 | 6 | % Parameters 7 | n = 2; 8 | options = odeset('RelTol',1e-12,'AbsTol',1e-12*ones(1,n)); 9 | ModelName = 'QuarticPotentialWell'; 10 | dt = .025; 11 | tspan = 0:dt:10; 12 | 13 | % Define functions 14 | f = @(t,x,u)([x(2); -x(1)^3]); 15 | H = @(x)((1/2)*x(2)^2 + (1/4)*x(1)^4); 16 | 17 | %% Hamiltonian Fuction Schematic 18 | [t,y1] = ode45(f,tspan,[-2 0],options); H1 = H([-2 0]); 19 | [t,y2] = ode45(f,tspan,[-3 0],options); H2 = H([-3 0]); 20 | [t,y3] = ode45(f,tspan,[-4 0],options); H3 = H([-4 0]); 21 | 22 | y0 = [-4; -10]; 23 | [t,yb] = ode45(f,tspan,y0,options); 24 | Hb = H(y0); 25 | 26 | x = -5:.02:5; Nx = length(x); 27 | y = -20:.02:20; Ny = length(y); 28 | [X,Y] = meshgrid(x,y); 29 | Hmat = (1/2)*Y.^2 + (1/4)*X.^4; 30 | Hmat(Hmat(:)>Hb) = NaN; 31 | Hmat = reshape(Hmat,[Nx*Ny, 1]); 32 | XY = [reshape(X, [Nx*Ny, 1]), reshape(Y, [Nx*Ny, 1])]; 33 | TF = isnan(Hmat); 34 | XY(TF,:) = []; 35 | 36 | tri = delaunay(XY(:,1),XY(:,2)); 37 | Hxy = (1/2)*XY(:,2).^2 + (1/4)*XY(:,1).^4; 38 | 39 | figure, 40 | trihandle = trisurf(tri, XY(:,1), XY(:,2),Hxy); hold on, shading interp 41 | colormap([0 0 0.7]) 42 | % surf(x,y,Hmat), shading interp, hold on 43 | plot3(yb(:,1),yb(:,2),Hb.*ones(size(yb)),'-k','LineWidth',1) 44 | plot3(y1(:,1),y1(:,2),H1.*ones(size(yb)),'-k','LineWidth',1) 45 | plot3(y2(:,1),y2(:,2),H2.*ones(size(yb)),'-k','LineWidth',1) 46 | plot3(y3(:,1),y3(:,2),H3.*ones(size(yb)),'-k','LineWidth',1) 47 | plot3(0,0,H([0,0]),'ok','MarkerSize',5,'MarkerFaceColor','k') 48 | % plot3(y1(:,1),y1(:,2),0.*ones(size(yb)),'-k','LineWidth',1) 49 | % plot3(y2(:,1),y2(:,2),0.*ones(size(yb)),'-k','LineWidth',1) 50 | % plot3(y3(:,1),y3(:,2),0.*ones(size(yb)),'-k','LineWidth',1) 51 | xlabel('x') 52 | ylabel('y') 53 | zlabel('z') 54 | trihandle.FaceAlpha = 0.4; 55 | axis off 56 | set(gcf,'Position',[100 100 400 286]); 57 | set(gcf,'PaperPositionMode','auto') 58 | print('-depsc2', '-loose', [figpath, ModelName,'_Energy.eps']); 59 | 60 | 61 | %% Unforced system 62 | ModelName = 'QuarticPotentialWell_REF002_'; 63 | 64 | dt = 0.01; 65 | tspan = .001:dt:100; 66 | Q = 1; 67 | R = eye(2); 68 | REF = 0.15; 69 | Ni = size(R,2); 70 | 71 | % Unforced 72 | B = zeros(2); 73 | f = @(t,x,u)([x(2); -x(1)^3]+B*u); 74 | H = @(x)((1/2)*x(2)^2 + (1/4)*x(1)^4 - REF); 75 | % y = +/- sqrt(2*REF- 1/2 *x^4) 76 | gradH = @(x)([x(1)^3; x(2)]); 77 | 78 | %% KRONIC 79 | B = eye(2); 80 | ModelName1 = [ModelName, 'B11_']; 81 | f = @(t,x,u)([x(2); -x(1)^3] + B*u); 82 | 83 | % Koopman eigenfunction control 84 | ode_options = odeset('RelTol',1e-10, 'AbsTol',1e-11); 85 | gain = @(x)(lqr(0,(gradH(x)'*B),Q,R)); 86 | [~,y1] = ode45(@(t,x)f(t,x,-gain(x)*H(x)),tspan,[.001; .001],ode_options); 87 | gain = @(x)(lqr(0,(gradH(x)'*B),Q,R)); 88 | [~,y2] = ode45(@(t,x)f(t,x,-gain(x)*H(x)),tspan,[1; 1],ode_options); 89 | 90 | %% SAVE RESULTS 91 | save([path2data,[ModelName1,'Data.mat']]) -------------------------------------------------------------------------------- /QuarticPotentialWell_FIG.m: -------------------------------------------------------------------------------- 1 | path2data = '../Data/'; ModelName1 = ['QuarticPotentialWell_REF002_','B11_']; 2 | load([path2data,[ModelName1,'Data.mat']]) 3 | path2figs = '../Figures/QUARTIC_POT_WELL/'; mkdir(path2figs) 4 | 5 | %% Show phase plot 6 | cmap = jet(10); 7 | cmap = cmap(1:2:end,:); 8 | 9 | figure; 10 | x = -0.88:0.01:0.88; hold on, box on 11 | % y = +/- sqrt(2*REF- 1/2 *x^4) 12 | trihandle = trisurf(tri, XY(:,1), XY(:,2),(Hxy)); hold on, shading interp, 13 | caxis([0 2]) 14 | colormap(gray(8)) 15 | view(2) 16 | plot3(y1(:,1),y1(:,2),(max(Hxy)+1).*ones(size(y1(:,1))),'-r', 'LineWidth',2,'Color',[0,0.5,1]) 17 | plot3(y2(:,1),y2(:,2),(max(Hxy)+1).*ones(size(y2(:,1))),'-', 'LineWidth',2,'Color',[0,0.5,1]) 18 | plot3([-1.6 1.6],[-1.6 -1.6],[(max(Hxy)+1) (max(Hxy)+1)],'-k', 'LineWidth',1) %b 19 | plot3([-1.6 1.6],[1.6 1.6],[(max(Hxy)+1) (max(Hxy)+1)],'-k', 'LineWidth',1) %t 20 | plot3([-1.6 -1.6],[-1.6 1.6],[(max(Hxy)+1) (max(Hxy)+1)],'-k', 'LineWidth',1) %l 21 | plot3([1.6 1.6],[-1.6 1.6],[(max(Hxy)+1) (max(Hxy)+1)],'-k', 'LineWidth',1) %r 22 | plot3(x,sqrt(2*REF- 1/2 *x.^4),(max(Hxy)+1).*ones(size(x)),'--', 'LineWidth',2,'Color',[0.9 0.9 0.9]) 23 | plot3(x,-sqrt(2*REF- 1/2 *x.^4),(max(Hxy)+1).*ones(size(x)),'--', 'LineWidth',2,'Color',[0.9 0.9 0.9]) 24 | xlabel('x_1'), ylabel('x_2') 25 | axis equal 26 | axis([-1.6 1.6 -1.6 1.6]) 27 | set(gca,'FontSize',16,'LineWidth',1,'ytick',[-1 0 1]) 28 | set(gcf,'Position',[100 100 300 300]) 29 | set(gcf,'PaperPositionMode','auto') 30 | print('-depsc2', '-loose', [figpath,ModelName1,'PhasePlot','.eps']); -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Koopman Reduced-Order Nonlinear Identification and Control (KRONIC) 2 | A Matlab library that implements a system identification framework for nonlinear dynamics -- referred to as Koopman Reduced-Order Nonlinear Identification and Control (KRONIC). For details see **Kaiser et. al (2017) [[arXiv](https://arxiv.org/abs/1707.01146)]**. 3 | 4 | ![](FIGURES/KRONIC.png) 5 | 6 | The Koopman operator has emerged as a principled linear embedding of nonlinear dynamics, and its eigenfunctions establish intrinsic coordinates along which the dynamics behave linearly. KRONIC aims to identify Koopman eigenfunctions using sparse regression from data, and then derives the controller in these intrinsic coordinates. 7 | 8 | ![](FIGURES/SparseRegression.png) 9 | 10 | Some examples on which KRONIC is demonstrated: 11 | ![](FIGURES/EXAMPLES.png) 12 | 13 | ## Installation 14 | 15 | 1. Clone this repository to your desktop. 16 | 2. Add path to `KRONIC/utils` folder to Matlab search path using `addpath('/KRONIC/utils')`. 17 | 18 | ## Dependencies 19 | There are no dependencies. 20 | 21 | ## Getting Started 22 | 23 | See examples in the main folder `KRONIC` for demonstrating the approach on various dynamical systems. Just execute this file in MatLab and it will generate the plot files in `KRONIC/FIGURES`. 24 | 25 | ## File description 26 | KRONIC/ 27 | ``` 28 | AsymmetricPotentialWell.m -- Well hopping in an asymetric potential double well 29 | AutonomDoubleGyre.m -- Control of drifters in a double gyre flow, a simple model for ocean mixing 30 | NonAutonomDoubleGyre.m -- Similarly for the non-autonomous case 31 | DiscoverDuffing.m -- Discovery of Koopman eigenfunctions/conserved quantities from data 32 | DiscoverDuffing_Convergence.m -- Convergence analysis (error, estimation, control) [data and computational efficiency] 33 | DiscoverDuffing_KRONICvsEDMDc.m -- Comparison of KRONIC with extended dynamic mode decomposition (EDMD) for control 34 | SlowManifold.m -- Control of system with slow manifold (analytical example) 35 | ... several other examples 36 | ``` 37 | 38 | 39 | ## License ([CiteMe OSS](https://github.com/cite-me/oss)) 40 | 41 | The code is published under the [CiteMe OSS license](https://github.com/cite-me/oss). See the [LICENSE file](LICENSE) for details. 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /SlowManifold.m: -------------------------------------------------------------------------------- 1 | clear all , close all , clc 2 | path2figs = './../Figures/SLOW_MANIFOLD/'; mkdir(path2figs) 3 | addpath('./utils'); 4 | 5 | %% System 6 | mu = -0.1; 7 | lambda = -1; 8 | A = [mu 0 0; 0 lambda -lambda; 0 0 2*mu]; % Koopman linear dynamics 9 | [T,D] = eig(A); 10 | slope_stab_man = T(3,3)/T(2,3); % slope of stable subspace (green) 11 | 12 | %% Nonlinear phase plot with ensemble of trajectories // Case I 13 | % STABLE 14 | 15 | % Initial condition 16 | y0ic = [0 0]; 17 | xvec = -4:1:4; 18 | yvec = [-5,20]; 19 | x = [-4:0.01:4]; 20 | dt = 0.01; 21 | Ny0 = length(yvec); 22 | Nx0 = length(xvec); 23 | yIC = zeros(2,Ny0,Nx0); 24 | [x0,y0] = meshgrid(xvec+y0ic(1),yvec+y0ic(2)); 25 | yIC(1,:,:) = x0; 26 | yIC(2,:,:) = y0; 27 | 28 | % System 29 | mu = -.1; 30 | lambda = -1; 31 | duration = 50; % for stable 32 | tspan = [0,duration]; 33 | L = duration/dt; 34 | yin = yIC; 35 | 36 | yout = zeros(2,Ny0,Nx0,L); 37 | yout(:,:,:,1) = yin; 38 | for step = 1:L-1 39 | time = step*dt; 40 | yout(:,:,:,step+1) = rk4singlestep(@(t,y,u)SlowManifold_ensemble(t,y,mu,lambda),dt,time,yin,0); 41 | yin = yout(:,:,:,step); 42 | end 43 | 44 | % UNSTABLE 45 | 46 | % Initial condition 47 | xvec = -4:1:4; 48 | Ny = length(yvec); 49 | Nx = length(xvec); 50 | yIC = zeros(2,Ny,Nx); 51 | [x0,y0] = meshgrid(xvec+y0ic(1),yvec+y0ic(2)); 52 | yIC(1,:,:) = x0; 53 | yIC(2,:,:) = y0; 54 | 55 | % Add control 56 | mu = -.1; 57 | lambda = 1; 58 | ModelName = 'SlowManifold_B01_'; 59 | ModelName = [path2figs,ModelName]; 60 | B = [0; 1]; 61 | A = [mu 0; 0 lambda]; 62 | R = 1; 63 | b = lambda/(lambda-2*mu); 64 | T = [1 0 0; 0 1 -b; 0 0 1]; 65 | Aphi = [mu 0 0; 0 lambda 0; 0 0 2*mu]; 66 | Bphi = [B; 0]; 67 | Qphi = [1 0 0; 0 1 b; 0 b b^2]; 68 | Cphi = lqr(Aphi,Bphi,Qphi,R)*T; 69 | vf3 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*(Cphi(1:2)*x + Cphi(3)*x(1)^2); 70 | 71 | SlowManifold_PhasePlot_Control 72 | 73 | % Add control 74 | mu = -mu; 75 | lambda = -lambda; 76 | ModelName = [path2figs,'SlowManifold_B10_']; 77 | subindex = @(A,r,c) A(r,c); 78 | B = [1; 0]; 79 | A = [mu 0; 0 lambda]; 80 | R = 1; 81 | Q = eye(2); 82 | b = lambda/(lambda-2*mu); 83 | T = [1 0 0; 0 1 -b]; 84 | Aphi = [mu 0; 0 lambda]; 85 | gradphi = @(x) ([1 0; -b*x(1) 1]); 86 | gain = @(x)(lqr(Aphi,(gradphi(x)*B),Q,R)*T); 87 | vf3 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*(subindex(gain(x),1,1:2)*x + subindex(gain(x),1,3)*x(1)^2); 88 | 89 | SlowManifold_PhasePlot_Control 90 | 91 | %% Koopman eigenfunction optimal control 92 | clear all , close all , clc 93 | path2figs = './../Figures/SLOW_MANIFOLD/'; 94 | dt = 0.01; 95 | tspan = 0:dt:50; 96 | x0 = [-5; 5]; 97 | subindex = @(A,r,c) A(r,c); %# An anonymous function to index a matrix 98 | 99 | %% OPTIMAL CONTROL B = [0; 1] 100 | ModelName = [path2figs, 'SlowManifold_B01_']; 101 | B = [0; 1]; 102 | mu = -.1; 103 | lambda = 1; 104 | b = lambda/(lambda-2*mu); 105 | 106 | % LQR on linearized system 107 | A = [mu 0; 0 lambda]; % system matrix from linearization 108 | Q = eye(2); % state penalization 109 | R = 1; % control penalization 110 | C = lqr(A,B,Q,R); % gain 111 | vf = @(t,x) A*x + [0; -lambda*x(1)^2] - B*C*x; % apply gain in real system 112 | [t,xLQR] = ode45(vf,tspan,x0); 113 | uLQR = (C*xLQR')'; % recalculate control input 114 | JLQR_x = cumsum(xLQR(:,1).^2 + xLQR(:,2).^2 + uLQR.^2)'; % standard LQR cost 115 | % JLQR_phi = cumsum(xLQR(:,1).^2 + xLQR(:,2).^2 + uLQR.^2 + ... % cost in eigenfun, JLQR_x part 116 | % (xLQR(:,2)-b*xLQR(:,1).^2).^2); % via phi 117 | JLQR_phi = cumsum(xLQR(:,1).^2 + xLQR(:,1).^2 + uLQR.^2 + ... % cost in eigenfun, 118 | (xLQR(:,2)-b*xLQR(:,1).^2).^2); % via phi 119 | 120 | 121 | % Koopman operator optimal control (KOOC); i.e., LQR on Koopman operator 122 | A2 = [mu 0 0; 0 lambda -lambda; 0 0 2*mu]; % system matrix in (nonlinear) observables 123 | B2 = [B; 0]; % new control vector in observables 124 | Q2 = [1 0 0; 0 1 0; 0 0 0]; % weight matrix only penalizing x1,x2 125 | R = 1; 126 | C2 = lqr(A2,B2,Q2,R); 127 | % note that controller becomes nonlinear in the state x1 128 | vf2 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*(C2(1:2)*x + C2(3)*x(1)^2); 129 | [t,xKOOC] = ode45(vf2,tspan,x0); 130 | uKOOC = (C2(1:2)*xKOOC')'+C2(3)*xKOOC(:,1).^2; 131 | JKOOC_x = cumsum(xKOOC(:,1).^2 + xKOOC(:,2).^2 + uKOOC.^2)'; 132 | JKOOC_phi = cumsum( xKOOC(:,1).^2 + (xKOOC(:,1).^2).^2 + uKOOC.^2 + ... % J_x part 133 | (xKOOC(:,2)-b*xKOOC(:,1).^2).^2); % via phi 134 | 135 | % Check 136 | [Vy,Dy] = eig(A2'); 137 | Vy^(-1)*A2'*Vy 138 | 139 | % "Analytic" 140 | H2 = [A2 -B2*R^(-1)*B2'; 141 | -Q2 -A2']; 142 | [V2,D2] = eig(H2); 143 | [D2,IX2] = sort(diag(D2),'ascend'); 144 | V2 = V2(:,IX2); D2 = D2(IX2); 145 | P2 = V2(4:6,1:3)*V2(1:3,1:3)^(-1); 146 | K2 = R^(-1)*B2'*P2 147 | 148 | % KRONIC with eigenfunctions 149 | b = lambda/(lambda-2*mu); 150 | T = [1 0 0; 0 1 -b; 0 0 1]; % Transformation between y and phi 151 | Aphi = [mu 0 0; 0 lambda 0; 0 0 2*mu]; 152 | Bphi = [B; 0]; 153 | Qphi = [1 0 0; 0 1 b; 0 b b^2]; 154 | %Qphi = [1 0 0; 0 1 0; 0 0 1]; 155 | Cphi = lqr(Aphi,Bphi,Qphi,R)*T; 156 | vf3 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*(Cphi(1:2)*x + Cphi(3)*x(1)^2); 157 | [t,xKOOC2] = ode45(vf3,tspan,x0); 158 | uKOOC2 = (Cphi(1:2)*xKOOC2')' + Cphi(3)*xKOOC2(:,1).^2; 159 | JKOOC2_x = cumsum(xKOOC2(:,1).^2 + xKOOC2(:,2).^2 + uKOOC2.^2 )'; 160 | JKOOC2_phi = cumsum(xKOOC2(:,1).^2 + (xKOOC2(:,1).^2).^2 + uKOOC2.^2 + ... % J_x part 161 | (xKOOC2(:,2)-b*xKOOC2(:,1).^2).^2); % via phi 162 | 163 | % "Analytic" 164 | N = size(Aphi,1); 165 | H3 = [Aphi -Bphi*R^(-1)*Bphi'; 166 | -Qphi -Aphi']; 167 | [V3,D3] = eig(H3); 168 | [D3,IX3] = sort(diag(D3),'ascend'); 169 | V3 = V3(:,IX3); D3 = D3(IX3); 170 | P3 = V3(N+1:2*N,1:N)*V3(1:N,1:N)^(-1); 171 | K3 = R^(-1)*Bphi'*P3*T % equal to Cphi 172 | % P*Aphi + Aphi'*P +Q - P*Bphi*R^(-1)*Bphi'*P 173 | 174 | % Nonlinear Control 175 | vf4 = @(t,x) [A*x(1:2) + [0; -lambda*x(1)^2] - B*R^(-1)*B'*x(3:4); ... 176 | -Q*x(1:2) - A'*x(3:4) - [0; lambda*x(1)]]; 177 | vf4_bc = @(xa,xb) [xa(1:2)-x0; xb(3:4)-eye(2)*xb(1:2)]; 178 | vf4_init = @(t) [x0; 0; 0]; 179 | solinit = bvpinit(tspan, vf4_init); 180 | sol = bvp4c(vf4,vf4_bc,solinit); 181 | xNLC = sol.y(1:2,:)'; 182 | tNLC = sol.x; 183 | uNLC = R^(-1)*B'*sol.y(3:4,:); uNLC = uNLC'; 184 | JNLC_x = cumsum(xNLC(:,1).^2 + xNLC(:,2).^2 + (uNLC).^2)'; 185 | JNLC_phi = cumsum(xNLC(:,1).^2 + (xNLC(:,1).^2).^2 + (uNLC).^2 + ... % J_x part 186 | (xNLC(:,2)-b*xNLC(:,1).^2).^2); % via phi 187 | 188 | %A*sol.y(1:2,1) + [0; -lambda*sol.y(1,1)^2] - B*R^(-1)*B'*sol.y(3:4,1) 189 | 190 | 191 | % Feedback linearization 192 | A5 = [mu 0; 0 lambda]; 193 | Q5 = eye(2); 194 | R5 = 1; 195 | C5 = lqr(A5,B,Q5,R5); 196 | vf5 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*(-lambda*x(1)^2 + C5*x); 197 | [t,xFL] = ode45(vf5,tspan,x0); 198 | uFL = (-lambda*(xFL(:,1).^2)' + C5*xFL')'; 199 | JFL_x = cumsum(xFL(:,1).^2 + xFL(:,2).^2 + uFL.^2)'; 200 | JFL_phi = cumsum(xFL(:,1).^2 + (xFL(:,1).^2).^2 + uFL.^2 + ... % JLQR_x part 201 | (xFL(:,2)-b*xFL(:,1).^2).^2); % via phi 202 | 203 | % Plot figures 204 | showThree = 1; 205 | ylim_vals = [-15 10]; 206 | axis_lim = [0 50 0 600000]; 207 | showResults_SlowManifold 208 | 209 | 210 | %% Balanced model 211 | % sys_y = ss(A2,B2,eye(3),0); 212 | % tf_y = tf(sys_y); 213 | % [sys_y_bal,g] = balreal(sys_y); 214 | % rsys = balred(sys_y,2); 215 | % 216 | % sys_phi = ss(Aphi,Bphi,eye(3),0); 217 | % [sys_phi_bal,g_phi] = balreal(sys_phi); 218 | % rsys_phi = balred(sys_phi,2); 219 | % 220 | % [sysr,u] = minreal(sys_phi); 221 | 222 | % LQR on minreal 223 | % Q = eye(3); 224 | % C = lqr(u*sys_phi.A*u',u*sys_phi.B,Q,R); % gain 225 | % vf = @(t,x) A*x + [0; -lambda*x(1)^2] - B*C*x; % apply gain in real system 226 | % [t,xKD] = ode45(vf,tspan,x0); 227 | 228 | %% Cost function Q = eye 229 | % Koopman operator optimal control (KOOC) 230 | A2 = [mu 0 0; 0 lambda -lambda; 0 0 2*mu]; 231 | B2 = [B; 0]; 232 | Q2 = [1 0 0; 0 1 0; 0 0 1]; 233 | R = 1; 234 | C2 = lqr(A2,B2,Q2,R); % CONSTANT 235 | vf2 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*(C2(1:2)*x + C2(3)*x(1)^2); 236 | [t,xKOOC] = ode45(vf2,tspan,x0); 237 | uKOOC = (C2(1:2)*xKOOC')'+C2(3)*xKOOC(:,1).^2; 238 | JKOOC_x = cumsum(xKOOC(:,1).^2 + xKOOC(:,2).^2 + uKOOC.^2)'; 239 | JKOOC_phi = cumsum( xKOOC(:,1).^2 + (xKOOC(:,1).^2).^2 + uKOOC.^2 + ... % J_x part 240 | (xKOOC(:,2)-b*xKOOC(:,1).^2).^2); % via phi 241 | 242 | % KC with eigenfunctions 243 | b = lambda/(lambda-2*mu); 244 | T = [1 0 0; 0 1 -b; 0 0 1]; % Transformation between y and phi 245 | Aphi = [mu 0 0; 0 lambda 0; 0 0 2*mu]; 246 | Bphi = [B; 0]; 247 | Qphi = [1 0 0; 0 1 0; 0 0 1]; 248 | % T = [1 0 0; 0 1 -b]; 249 | % Aphi = [mu 0 ; 0 lambda]; 250 | % Bphi = [B]; 251 | % Qphi = [1 0; 0 1]; 252 | Cphi = lqr(Aphi,Bphi,Qphi,R)*T; 253 | vf3 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*(Cphi(1:2)*x + Cphi(3)*x(1)^2); 254 | [t,xKOOC2] = ode45(vf3,tspan,x0); 255 | uKOOC2 = (Cphi(1:2)*xKOOC2')' + Cphi(3)*xKOOC2(:,1).^2; 256 | JKOOC2_x = cumsum(xKOOC2(:,1).^2 + xKOOC2(:,2).^2 + uKOOC2.^2 )'; 257 | JKOOC2_phi = cumsum(xKOOC2(:,1).^2 + (xKOOC2(:,1).^2).^2 + uKOOC2.^2 + ... % J_x part 258 | (xKOOC2(:,2)-b*xKOOC2(:,1).^2).^2); % via phi 259 | 260 | % Show comparison 261 | figure,hold on, box on 262 | plot(tspan,JLQR_x,'-','Color',colors(1,:),'LineWidth',2) 263 | plot(tspan,JKOOC_x,'-','Color',colors(2,:),'LineWidth',2) 264 | plot(tspan,JKOOC2_x,'--','Color',colors(3,:),'LineWidth',2) 265 | % plot(tspan,JNLC_x,':','Color','r') 266 | xlabel('t'), ylabel('Jx') 267 | axis([0 50 0 5*10^5]) 268 | set(gca,'FontSize',16) 269 | set(gcf,'Position',[100 100 225 200]) 270 | set(gcf,'PaperPositionMode','auto') 271 | print('-painters','-depsc2', '-loose', [ModelName,'Cost_Jx','.eps']); 272 | 273 | figure,hold on, box on 274 | plot(tspan,JLQR_phi,'-','Color',colors(1,:),'LineWidth',2) 275 | plot(tspan,JKOOC_phi,'-','Color',colors(2,:),'LineWidth',2) 276 | plot(tspan,JKOOC2_phi,'--','Color',colors(3,:),'LineWidth',2) 277 | % plot(tspan,JNLC_phi,':','Color','r') 278 | xlabel('t'), ylabel('Jp') 279 | axis([0 50 0 8*10^5]) 280 | set(gca,'FontSize',16) 281 | set(gcf,'Position',[100 100 225 200]) 282 | set(gcf,'PaperPositionMode','auto') 283 | print('-painters','-depsc2', '-loose', [ModelName,'Cost_Jphi','.eps']); 284 | 285 | %% Control 286 | clear all , close all , clc 287 | dt = 0.01; 288 | tspan = 0:dt:50;%50; 289 | x0 = [-5; 5]; 290 | subindex = @(A,r,c) A(r,c); %# An anonymous function to index a matrix 291 | 292 | path2figs = './../Figures/SLOW_MANIFOLD/'; 293 | %% OPTIMAL CONTROL B = [1; 0] 294 | axis_lim = [0 50 0 20000]; 295 | 296 | ModelName = [path2figs,'SlowManifold_B10_']; 297 | B = [1; 0]; 298 | mu = 0.1; 299 | lambda = -1; 300 | b = lambda/(lambda-2*mu); 301 | R = 1; 302 | RKRONIC = 4.0; 303 | phifun3 = @(x) [x(1); x(2)-b*x(1)^2; x(1)^2]; 304 | 305 | % LQR on linearized system 306 | A = [mu 0; 0 lambda]; 307 | Q = eye(2); 308 | C = lqr(A,B,Q,R); 309 | vf = @(t,x) A*x + [0; -lambda*x(1)^2] - B*C*x; 310 | [t,xLQR] = ode45(vf,tspan,x0); 311 | uLQR = (C*xLQR')'; 312 | phiLQR = zeros(length(t),3); 313 | for i = 1:length(t) 314 | phiLQR(i,:) = phifun3(xLQR(i,:)); 315 | end 316 | JLQR_x = cumsum(xLQR(:,1).^2 + xLQR(:,2).^2 + R*uLQR.^2)'; 317 | JLQR_phi = cumsum(phiLQR(:,1).^2 + phiLQR(:,2).^2 + RKRONIC*uLQR.^2); 318 | 319 | % Control with truncated Koopman system; i.e., LQR on Koopman "matrix" (fails) 320 | A2 = [mu 0 0; 0 lambda -lambda; 0 0 2*mu]; 321 | Q2 = [1 0 0; 0 1 0; 0 0 0]; 322 | 323 | % Minreal 324 | % B2 = [B; 2*x0(1)]; 325 | % sys_y = ss(A2,B2,eye(3),0); 326 | % [sys_balreal,g,T,Ti] = balreal(sys_y); 327 | % sys_balred = balred(sys_y,2); 328 | % [sys_minreal,u_mr] = minreal(sys_y); 329 | % tf_y = tf(sys_y); 330 | 331 | try 332 | C2 = @(x) lqr(A2,[B; 2*x(1)],Q2,0.01*R); 333 | vf2 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*(subindex(C2(x),1,1:2)*x + subindex(C2(x),1,3)*x(1)^2); 334 | [t,xKOOC] = ode45(vf2,tspan,x0); % fails at some point 335 | 336 | % B2 = @(x) [[B; 2*x(1)], [0;1;0]]; 337 | % C2 = @(x) lqr(A2,B2(x),Q2,0.05*eye(2)); 338 | % vf2 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*(subindex(C2(x),1,1:2)*x + subindex(C2(x),1,3)*x(1)^2); 339 | % [t,xKOOC] = ode45(vf2,tspan,x0); % fails at some point 340 | % 341 | % B2 = @(x) [[B; 2*x(1)], [0;1;0]]; 342 | % C2 = @(x) lqr(A2,B2(x),Q2,eye(2)); % getBRSys(sys_y,B2(x)),R, 343 | % T = @(x) getBRTrafo(sys_y,B2(x)); 344 | % vf2 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*C2(x)*T(x)*[x;x(1).^2]; 345 | % [t,xKOOC] = ode45(vf2,tspan,x0); % fails at some point 346 | uKOOC = zeros(length(t),1); 347 | for i = 1:length(t) 348 | uKOOC(i) = subindex(C2(xKOOC(i,:)),1,1:2)*xKOOC(i,:)' + subindex(C2(xKOOC(i,:)),1,3)*xKOOC(i,1)^2; 349 | end 350 | catch 351 | warning('Truncated Koopman system not stabilizable.') 352 | xKOOC = zeros(length(tspan),2); 353 | C2 = zeros(1,3); 354 | uKOOC = (C2(1:2)*xKOOC')'+C2(3)*xKOOC(:,1).^2; 355 | end 356 | 357 | phiKOOC = zeros(length(t),3); 358 | for i = 1:length(t) 359 | phiKOOC(i,:) = phifun3(xKOOC(i,:)); 360 | end 361 | JKOOC_x = cumsum(xKOOC(:,1).^2 + xKOOC(:,2).^2 + R*uKOOC.^2)'; 362 | JKOOC_phi = cumsum(phiKOOC(:,1).^2 + phiKOOC(:,2).^2 + RKRONIC*uKOOC.^2); 363 | 364 | % KRONIC 365 | % Truncated 2D 366 | b = lambda/(lambda-2*mu); 367 | T = [1 0 0; 0 1 -b]; % y -> phi 368 | Aphi = [mu 0; 0 lambda]; 369 | gradphi = @(x) ([1 0; -2*b*x(1) 1]); 370 | Qphi = eye(2); 371 | gain = @(x)(lqr(Aphi,(gradphi(x)*B),Qphi,RKRONIC)*T); 372 | vf3 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*(subindex(gain(x),1,1:2)*x + subindex(gain(x),1,3)*x(1)^2); 373 | % gain = @(x)(lqr(Aphi,(gradphi(x)*B),Qphi,R)); 374 | % vf3 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*gain(x)*phifun2(x); % above are identical to this 375 | [t,xKOOC2] = ode45(vf3,tspan,x0); 376 | 377 | % 3D 378 | % b = lambda/(lambda-2*mu); 379 | % T = [1 0 0; 0 1 -b; 0, 0, 1]; 380 | % Aphi = [mu 0 0; 0 lambda 0; 0 0 2*mu]; 381 | % gradphi = @(x) ([1 0; -2*b*x(1) 1; 2*x(1) 0]); 382 | % % Qphi = eye(3); 383 | % Qphi = [1 0 0; 0 1 b; 0 b b^2]; 384 | % gain = @(x)(lqr(Aphi,(gradphi(x)*B),Qphi,0.01*RKRONIC)*T); %RKRONIC 385 | % vf3 = @(t,x) A*x + [0; -lambda*x(1)^2] - B*(subindex(gain(x),1,1:2)*x + subindex(gain(x),1,3)*x(1)^2); 386 | % [t,xKOOC2] = ode45(vf3,tspan(1:10),x0); 387 | 388 | 389 | uKOOC2 = zeros(length(t),1); 390 | phiKOOC2 = zeros(length(t),3); 391 | for i = 1:length(t) 392 | uKOOC2(i) = subindex(gain(xKOOC2(i,:)),1,1:2)*xKOOC2(i,:)' + subindex(gain(xKOOC2(i,:)),1,3)*xKOOC2(i,1)^2; 393 | % uKOOC2(i) = gain(xKOOC2(i,:))*phifun2(xKOOC2(i,:)); 394 | phiKOOC2(i,:) = phifun3(xKOOC2(i,:)); 395 | end 396 | JKOOC2_x = cumsum(xKOOC2(:,1).^2 + xKOOC2(:,2).^2 + R*uKOOC2.^2 )'; 397 | JKOOC2_phi = cumsum(phiKOOC2(:,1).^2 + phiKOOC2(:,2).^2 + RKRONIC*uKOOC2.^2); 398 | 399 | 400 | % Nonlinear Control 401 | vf4 = @(t,x) [A*x(1:2) + [0; -lambda*x(1)^2] - B*R^(-1)*B'*x(3:4); ... 402 | -Q*x(1:2) - A'*x(3:4) - [0; lambda*x(1)]]; 403 | vf4_bc = @(xa,xb) [xa(1:2)-x0; xb(3:4)-eye(2)*xb(1:2)]; 404 | vf4_init = @(t) [x0; 0; 0]; 405 | solinit = bvpinit(tspan, vf4_init); 406 | sol = bvp4c(vf4,vf4_bc,solinit); 407 | xNLC = sol.y(1:2,:)'; 408 | tNLC = sol.x; 409 | uNLC = R^(-1)*B'*sol.y(3:4,:); uNLC = uNLC'; 410 | phiNLC = zeros(length(t),3); 411 | for i = 1:length(t) 412 | phiNLC(i,:) = phifun3(xNLC(i,:)); 413 | end 414 | JNLC_x = cumsum(xNLC(:,1).^2 + xNLC(:,2).^2 + R*(uNLC).^2)'; 415 | JNLC_phi = cumsum(phiNLC(:,1).^2 + phiNLC(:,2).^2 + RKRONIC*uNLC.^2); 416 | 417 | % Feedback linearization (fails, set to zero) 418 | % A5 = [0 1; -2*mu*lambda lambda+2*mu]; 419 | % B5 = [0;1]; 420 | % Q5 = eye(2); 421 | % R5 = 1; 422 | % C5 = lqr(A5,B5,Q5,R5); 423 | % vf5 = @(t,x) A*x + [0; -lambda*x(1)^2] + B*(1/(2*lambda*x(1))*C5*x); 424 | % [t,xFL] = ode45(vf5,tspan,x0); 425 | xFL = zeros(length(tspan),2); % feedback linear. doesn't work, set to zero for plotting 426 | C5 = zeros(1,2); 427 | uFL = ((1./(2*lambda*xFL(:,1)')).*(C5*xFL'))'; 428 | JFL_x = cumsum(xFL(:,1).^2 + xFL(:,2).^2 + R*uFL.^2)'; 429 | phiFL = zeros(length(t),3); 430 | for i = 1:length(t) 431 | phiFL(i,:) = phifun3(phiFL(i,:)); 432 | end 433 | JFL_phi = cumsum(phiFL(:,1).^2 + phiFL(:,2).^2 + RKRONIC*uFL.^2); 434 | 435 | ylim_vals = [-5 10]; 436 | showThree = 1; 437 | showResults_SlowManifold 438 | 439 | return 440 | %% Different plots 441 | figure, box on 442 | plot(xNLC(:,1),xNLC(:,2),'-k','LineWidth',2), hold on 443 | plot(xLQR(:,1),xLQR(:,2),'--r','LineWidth',2) 444 | % plot(xKOOC(:,1),xKOOC(:,2),'-.m','LineWidth',2) 445 | plot(xKOOC2(:,1),xKOOC2(:,2),':b','LineWidth',2) 446 | title('Phase Plot') 447 | 448 | figure, box on 449 | plot(xNLC(:,1),'-k','LineWidth',2), hold on 450 | plot(xNLC(:,2),'-k','LineWidth',2), 451 | plot(xLQR(:,1),'--r','LineWidth',2) 452 | plot(xLQR(:,2),'--r','LineWidth',2) 453 | plot(xKOOC2(:,1),':b','LineWidth',2) 454 | plot(xKOOC2(:,2),':b','LineWidth',2) 455 | % plot(xKOOC(:,1),'-.m','LineWidth',2) 456 | % plot(xKOOC(:,2),'-.m','LineWidth',2) 457 | title('Time series') 458 | 459 | figure, box on 460 | semilogy((JNLC_x),'-k','LineWidth',2), hold on 461 | semilogy((JLQR_x),'--r','LineWidth',2) 462 | semilogy((JKOOC2_x),':b','LineWidth',2) 463 | semilogy((JKOOC_x),'-.m','LineWidth',2) 464 | title('Jx') 465 | 466 | figure, box on 467 | semilogy((JNLC_phi),'-k','LineWidth',2), hold on 468 | semilogy((JLQR_phi),'--r','LineWidth',2) 469 | semilogy((JKOOC2_phi),':b','LineWidth',2) 470 | semilogy((JKOOC_phi),'-.m','LineWidth',2) 471 | title('Jphi') 472 | 473 | figure, hold on, box on 474 | plot(cumsum((uNLC).^2),'-k','LineWidth',2) 475 | plot(cumsum((uLQR).^2),'--r','LineWidth',2) 476 | plot(cumsum((uKOOC2).^2),':b','LineWidth',2) 477 | plot(cumsum((uKOOC).^2),'-.m','LineWidth',2) 478 | title('sum u^2') 479 | 480 | % figure, hold on, box on 481 | % plot(cumsum((uNLC).^2),'-k','LineWidth',2) 482 | % plot(cumsum((uLQR).^2),'--r','LineWidth',2) 483 | % plot(cumsum((uKOOC2).^2),':b','LineWidth',2) 484 | %% 485 | figure, 486 | subplot(3,1,1) 487 | hold on, box on, title('LQR') 488 | plot(cumsum((uLQR).^2),'-k','LineWidth',2) 489 | plot(cumsum(xLQR(:,1).^2),'--r','LineWidth',2) 490 | plot(cumsum((xLQR(:,2)-b*xLQR(:,1).^2).^2),':b','LineWidth',2) 491 | plot(JLQR_phi,'-g','LineWidth',2) 492 | ylim([0 15000]) 493 | subplot(3,1,2) 494 | hold on, box on, title('KRONIC') 495 | plot(cumsum(RKRONIC*(uKOOC2).^2),'-k','LineWidth',2) 496 | plot(cumsum(xKOOC2(:,1).^2),'--r','LineWidth',2) 497 | plot(cumsum((xKOOC2(:,2)-b*xKOOC2(:,1).^2).^2),':b','LineWidth',2) 498 | plot(JKOOC2_phi,'-g','LineWidth',2) 499 | ylim([0 15000]) 500 | subplot(3,1,3) 501 | hold on, box on, title('NLC') 502 | plot(cumsum((uNLC).^2),'-k','LineWidth',2) 503 | plot(cumsum(xNLC(:,1).^2),'--r','LineWidth',2) 504 | plot(cumsum((xNLC(:,2)-b*xNLC(:,1).^2).^2),':b','LineWidth',2) 505 | plot(JNLC_phi,'-g','LineWidth',2) 506 | legend('u','x1','x2-bx1^2','J') 507 | ylim([0 15000]) 508 | 509 | %% 510 | xspace = [-5:0.1:5]; Nx = length(xspace); 511 | [X,Y] = meshgrid(xspace,xspace); 512 | PHI = zeros(Nx,Nx,3); 513 | for ix = 1:Nx 514 | for iy = 1:Nx 515 | PHI(iy,ix,1:3) = phifun3([X(iy,ix); Y(iy,ix)]); 516 | end 517 | end 518 | figure, 519 | subplot(1,3,1) 520 | imagesc(xspace,xspace,PHI(:,:,1)) 521 | title('phi1') 522 | subplot(1,3,2) 523 | imagesc(xspace,xspace,PHI(:,:,2)) 524 | title('phi2') 525 | subplot(1,3,3) 526 | imagesc(xspace,xspace,PHI(:,:,3)) 527 | title('phi3') 528 | 529 | 530 | -------------------------------------------------------------------------------- /utils/AsymmetricDoubleWellFUN.m: -------------------------------------------------------------------------------- 1 | function dx = AsymmetricDoubleWellFUN(t,x,gain,H,a,B) 2 | Nc = size(B,2); 3 | 4 | % PHASE 1: control on in left well to drive to homoclinic orbit 5 | if H(x) ~= H([a,0]) && x(1) <= a 6 | u = -gain(x)*(H(x)-H([a,0])); 7 | disp('1') 8 | 9 | % PHASE 2: control off on homoclinic orbit while x <= a 10 | elseif H(x) == H([a,0]) && x(1) <= a 11 | u = zeros(Nc,1); 12 | disp('2') 13 | 14 | % PHASE 3: control on in right well to drive to energy minimum 15 | elseif x(1) > a 16 | u = -gain(x)*(H(x)-H([1;0])); 17 | disp('3') 18 | end 19 | 20 | dx = [x(2); -(x(1).^3 - x(1) - a.*x(1).^2 + a)]+B*u; 21 | -------------------------------------------------------------------------------- /utils/DoubleGyre_ensemble.m: -------------------------------------------------------------------------------- 1 | function dy = DoubleGyre_ensemble(t,y,A,omega,epsilon,B,u) 2 | % y is a two dimensional state-vector 3 | f = epsilon*sin(omega*t)*y(1,:,:).^2 + (1-2*epsilon*sin(omega*t)).*y(1,:,:); 4 | dfdx = 2*epsilon*sin(omega*t)*y(1,:,:) + (1-2*epsilon*sin(omega*t)); 5 | 6 | if size(B,2) < 2 7 | dy = [-A*pi*sin(pi*f).*cos(pi*y(2,:,:)) + B(1,:)*u(:,:,:); 8 | A*pi*cos(pi*f).*sin(pi*y(2,:,:)).*dfdx + B(2,:)*u(:,:,:)]; 9 | elseif size(B,2) >= 2 10 | dy = [-A*pi*sin(pi*f).*cos(pi*y(2,:,:)); 11 | A*pi*cos(pi*f).*sin(pi*y(2,:,:)).*dfdx]; 12 | 13 | db = zeros(size(dy)); 14 | for i = 1:size(B,2) 15 | db = db + [B(1,i)*u(i,:,:); B(2,i)*u(i,:,:)]; 16 | end 17 | 18 | dy = dy+db; 19 | 20 | end 21 | 22 | 23 | -------------------------------------------------------------------------------- /utils/Duffing_ensemble.m: -------------------------------------------------------------------------------- 1 | function dy = Duffing_ensemble(t,y,d,a,b,g,w,B,u) 2 | % y is a two dimensional state-vector 3 | dy = [ y(2,:,:) + +B(1)*u; 4 | - d*y(2,:,:) + a*y(1,:,:) - b*y(1,:,:).^3 + g*cos(w*t) + B(2)*u]; 5 | -------------------------------------------------------------------------------- /utils/ObjectiveFCN_EDMDc.m: -------------------------------------------------------------------------------- 1 | function J = ObjectiveFCN_EDMDc(uopt,x,N,Nu,xref,u0,p,Q,R,Ru) 2 | %% Cost function of nonlinear MPC 3 | % 4 | % Inputs: 5 | % u: optimization variable, from time k to time k+N-1 6 | % x: current state at time k 7 | % Ts: controller sample time 8 | % N: prediction horizon 9 | % xref: state references, constant from time k+1 to k+N 10 | % u0: previous controller output at time k-1 11 | % 12 | % Output: 13 | % J: objective function cost 14 | % 15 | 16 | %% Nonlinear MPC design parameters 17 | u = uopt; 18 | 19 | %% Integrate system 20 | [xk,~] = lsim(p.sys,[u' 0],[0:N].*p.sys.Ts,poolData(x',2,p.polyorder,p.usesine)'); 21 | xk = xk'; 22 | xk = xk(1:p.nvar,:); 23 | 24 | %% Cost Calculation 25 | % Set initial plant states, controller output and cost. 26 | uk = u(1); 27 | J = 0; 28 | % Loop through each prediction step. 29 | for ct=1:N 30 | % Obtain plant state at next prediction step. 31 | xk1 = xk(:,ct); 32 | 33 | % accumulate state tracking cost from x(k+1) to x(k+N). 34 | J = J + (p.Hfun(xk1)-p.Hfun(xref))'*Q*(p.Hfun(xk1)-p.Hfun(xref)); 35 | % accumulate MV rate of change cost from u(k) to u(k+N-1). 36 | if ct==1 37 | J = J + (uk-u0)'*R*(uk-u0) + uk'*Ru*uk; 38 | else 39 | J = J + (uk-u(ct-1))'*R*(uk-u(ct-1)) + uk'*Ru*uk; 40 | end 41 | % Update uk for the next prediction step. 42 | if ct 0 && (ynew(i,2)>ylim(2)) % top right 14 | ynew(i,:) = ynew(i,:) + [-(xlim(2)-xlim(1))/2,-1]; 15 | elseif (Psi(ynew(i,:))) < 0 && (ynew(i,2)>ylim(2)) % top left 16 | ynew(i,:) = ynew(i,:) + [+(xlim(2)-xlim(1))/2,-1]; 17 | end 18 | Hvec(i) = Psi(ynew(i,:)); 19 | 20 | % Check whether outside or inside domain of interest 21 | if (y(i,1)>xlim(1)) && (y(i,1)ylim(1)) && (y(i,2)0.1 48 | StartIDX = [StartIDX,i]; 49 | end 50 | 51 | end 52 | StartIDX = [StartIDX,N+1]; 53 | -------------------------------------------------------------------------------- /utils/buildGamma.m: -------------------------------------------------------------------------------- 1 | function yout = buildGamma(yin,ydotin,nVars,polyorder,usesine) 2 | % Copyright 2016, All Rights Reserved 3 | % Code by Steven L. Brunton 4 | 5 | n = size(yin,1); 6 | 7 | ind = 1; 8 | % poly order 0 9 | % yout(:,ind) = ones(n,1); 10 | % ind = ind+1; 11 | 12 | % poly order 1 13 | for i=1:nVars 14 | yout(:,ind) = ydotin(:,i); 15 | ind = ind+1; 16 | end 17 | 18 | if(polyorder>=2) 19 | % poly order 2 20 | for i=1:nVars 21 | for j=i:nVars 22 | yout(:,ind) = ydotin(:,i).*yin(:,j) + yin(:,i).*ydotin(:,j); 23 | ind = ind+1; 24 | end 25 | end 26 | end 27 | 28 | if(polyorder>=3) 29 | % poly order 3 30 | for i=1:nVars 31 | for j=i:nVars 32 | for k=j:nVars 33 | yout(:,ind) = ydotin(:,i).*yin(:,j).*yin(:,k) + yin(:,i).*ydotin(:,j).*yin(:,k) + yin(:,i).*yin(:,j).*ydotin(:,k); 34 | ind = ind+1; 35 | end 36 | end 37 | end 38 | end 39 | 40 | if(polyorder>=4) 41 | % poly order 4 42 | for i=1:nVars 43 | for j=i:nVars 44 | for k=j:nVars 45 | for l=k:nVars 46 | % yout(:,ind) = zeros(size(yin,1),1); 47 | % ijkl = [i j k l]; 48 | % for k=1:4 49 | % yout(:,ind) = yout(:,ind) + ydotin(:,ijkl(1)).*yin(:,ijkl(2)).*yin(:,ijkl(3)).*yin(:,ijkl(4)); 50 | % ijkl = [ijkl(2:end) ijkl(1)]; 51 | % end 52 | yout(:,ind) = ydotin(:,i).*yin(:,j).*yin(:,k).*yin(:,l) + yin(:,i).*ydotin(:,j).*yin(:,k).*yin(:,l) +yin(:,i).*yin(:,j).*ydotin(:,k).*yin(:,l) +yin(:,i).*yin(:,j).*yin(:,k).*ydotin(:,l); 53 | ind = ind+1; 54 | end 55 | end 56 | end 57 | end 58 | end 59 | 60 | if(polyorder>=5) 61 | % poly order 5 62 | for i=1:nVars 63 | for j=i:nVars 64 | for k=j:nVars 65 | for l=k:nVars 66 | for m=l:nVars 67 | yout(:,ind) = ydotin(:,i).*yin(:,j).*yin(:,k).*yin(:,l).*yin(:,m) + yin(:,i).*ydotin(:,j).*yin(:,k).*yin(:,l).*yin(:,m) + yin(:,i).*yin(:,j).*ydotin(:,k).*yin(:,l).*yin(:,m) + yin(:,i).*yin(:,j).*yin(:,k).*ydotin(:,l).*yin(:,m) + yin(:,i).*yin(:,j).*yin(:,k).*yin(:,l).*ydotin(:,m); 68 | ind = ind+1; 69 | end 70 | end 71 | end 72 | end 73 | end 74 | end 75 | 76 | if(usesine) 77 | for k=1:10; 78 | yout = [yout sin(k*yin) cos(k*yin)]; 79 | end 80 | end -------------------------------------------------------------------------------- /utils/buildTheta.m: -------------------------------------------------------------------------------- 1 | function yout = buildTheta(yin,nVars,polyorder,usesine) 2 | % Copyright 2015, All Rights Reserved 3 | % Code by Steven L. Brunton 4 | % For Paper, "Discovering Governing Equations from Data: 5 | % Sparse Identification of Nonlinear Dynamical Systems" 6 | % by S. L. Brunton, J. L. Proctor, and J. N. Kutz 7 | 8 | 9 | n = size(yin,1); 10 | % yout = zeros(n,1+nVars+(nVars*(nVars+1)/2)+(nVars*(nVars+1)*(nVars+2)/(2*3))+11); 11 | % yout = zeros(n,sum(nVars.^([1:polyorder]))); 12 | 13 | ind = 1; 14 | % poly order 0 15 | % yout(:,ind) = ones(n,1); 16 | % ind = ind+1; 17 | 18 | % poly order 1 19 | for i=1:nVars 20 | yout(:,ind) = yin(:,i); 21 | ind = ind+1; 22 | end 23 | 24 | if(polyorder>=2) 25 | % poly order 2 26 | for i=1:nVars 27 | for j=i:nVars 28 | yout(:,ind) = yin(:,i).*yin(:,j); 29 | ind = ind+1; 30 | end 31 | end 32 | end 33 | 34 | if(polyorder>=3) 35 | % poly order 3 36 | for i=1:nVars 37 | for j=i:nVars 38 | for k=j:nVars 39 | yout(:,ind) = yin(:,i).*yin(:,j).*yin(:,k); 40 | ind = ind+1; 41 | end 42 | end 43 | end 44 | end 45 | 46 | if(polyorder>=4) 47 | % poly order 4 48 | for i=1:nVars 49 | for j=i:nVars 50 | for k=j:nVars 51 | for l=k:nVars 52 | yout(:,ind) = yin(:,i).*yin(:,j).*yin(:,k).*yin(:,l); 53 | ind = ind+1; 54 | end 55 | end 56 | end 57 | end 58 | end 59 | 60 | if(polyorder>=5) 61 | % poly order 5 62 | for i=1:nVars 63 | for j=i:nVars 64 | for k=j:nVars 65 | for l=k:nVars 66 | for m=l:nVars 67 | yout(:,ind) = yin(:,i).*yin(:,j).*yin(:,k).*yin(:,l).*yin(:,m); 68 | ind = ind+1; 69 | end 70 | end 71 | end 72 | end 73 | end 74 | end 75 | 76 | if(usesine) 77 | for k=1:10; 78 | yout = [yout sin(k*yin) cos(k*yin)]; 79 | end 80 | end -------------------------------------------------------------------------------- /utils/buildThetaGradient.m: -------------------------------------------------------------------------------- 1 | function yout = buildThetaGradient(yin,iDenVar,nVars,polyorder,usesine) 2 | n = size(yin,1); 3 | 4 | ind = 1; 5 | % poly order 0 6 | % yout(:,ind) = ones(n,1); 7 | % ind = ind+1; 8 | 9 | % poly order 1 10 | try 11 | for i=1:nVars 12 | if i == iDenVar 13 | yout(:,ind) = ones(size(yin(:,i))); 14 | else 15 | yout(:,ind) = zeros(size(yin(:,i))); 16 | end 17 | ind = ind+1; 18 | end 19 | catch 20 | keyboard 21 | end 22 | if(polyorder>=2) 23 | % poly order 2 24 | for i=1:nVars 25 | for j=i:nVars 26 | if i == iDenVar && j == iDenVar 27 | yout(:,ind) = 2*yin(:,i); 28 | elseif i == iDenVar && j ~= iDenVar 29 | yout(:,ind) = yin(:,j); 30 | elseif i ~= iDenVar && j == iDenVar 31 | yout(:,ind) = yin(:,i); 32 | else 33 | yout(:,ind) = zeros(size(yin(:,i))); 34 | end 35 | ind = ind+1; 36 | end 37 | end 38 | end 39 | 40 | if(polyorder>=3) 41 | % poly order 3 42 | for i=1:nVars 43 | for j=i:nVars 44 | for k=j:nVars 45 | if i == iDenVar && j == iDenVar && k == iDenVar 46 | yout(:,ind) = 3*yin(:,i).*yin(:,j); 47 | elseif i == iDenVar && j == iDenVar && k ~= iDenVar 48 | yout(:,ind) = 2*yin(:,i).*yin(:,k); 49 | elseif i == iDenVar && j ~= iDenVar && k == iDenVar 50 | yout(:,ind) = 2*yin(:,i).*yin(:,j); 51 | elseif i ~= iDenVar && j == iDenVar && k == iDenVar 52 | yout(:,ind) = 2*yin(:,i).*yin(:,j); 53 | elseif i == iDenVar && j ~= iDenVar && k ~= iDenVar 54 | yout(:,ind) = yin(:,j).*yin(:,k); 55 | elseif i ~= iDenVar && j == iDenVar && k ~= iDenVar 56 | yout(:,ind) = yin(:,i).*yin(:,k); 57 | elseif i ~= iDenVar && j ~= iDenVar && k == iDenVar 58 | yout(:,ind) = yin(:,i).*yin(:,j); 59 | else 60 | yout(:,ind) = zeros(size(yin(:,k))); 61 | end 62 | ind = ind+1; 63 | end 64 | end 65 | end 66 | end 67 | 68 | if(polyorder>=4) 69 | % poly order 4 70 | for i=1:nVars 71 | for j=i:nVars 72 | for k=j:nVars 73 | for l=k:nVars 74 | if i == iDenVar && j == iDenVar && k == iDenVar && l == iDenVar 75 | yout(:,ind) = 4*yin(:,i).*yin(:,j).*yin(:,k); 76 | elseif i == iDenVar && j == iDenVar && k == iDenVar && l ~= iDenVar 77 | yout(:,ind) = 3*yin(:,i).*yin(:,j).*yin(:,l); 78 | elseif i == iDenVar && j == iDenVar && k ~= iDenVar && l == iDenVar 79 | yout(:,ind) = 3*yin(:,i).*yin(:,j).*yin(:,k); 80 | elseif i == iDenVar && j ~= iDenVar && k == iDenVar && l == iDenVar 81 | yout(:,ind) = 3*yin(:,i).*yin(:,j).*yin(:,k); 82 | elseif i ~= iDenVar && j == iDenVar && k == iDenVar && l == iDenVar 83 | yout(:,ind) = 3*yin(:,i).*yin(:,j).*yin(:,k); 84 | elseif i == iDenVar && j == iDenVar && k ~= iDenVar && l ~= iDenVar 85 | yout(:,ind) = 2*yin(:,i).*yin(:,k).*yin(:,l); 86 | elseif i == iDenVar && j ~= iDenVar && k == iDenVar && l ~= iDenVar 87 | yout(:,ind) = 2*yin(:,i).*yin(:,j).*yin(:,l); 88 | elseif i ~= iDenVar && j == iDenVar && k == iDenVar && l ~= iDenVar 89 | yout(:,ind) = 2*yin(:,i).*yin(:,j).*yin(:,l); 90 | elseif i == iDenVar && j ~= iDenVar && k ~= iDenVar && l == iDenVar 91 | yout(:,ind) = 2*yin(:,i).*yin(:,j).*yin(:,k); 92 | elseif i ~= iDenVar && j == iDenVar && k ~= iDenVar && l == iDenVar 93 | yout(:,ind) = 2*yin(:,i).*yin(:,j).*yin(:,k); 94 | elseif i ~= iDenVar && j ~= iDenVar && k == iDenVar && l == iDenVar 95 | yout(:,ind) = 2*yin(:,i).*yin(:,j).*yin(:,k); 96 | elseif i == iDenVar && j ~= iDenVar && k ~= iDenVar && l ~= iDenVar 97 | yout(:,ind) = yin(:,j).*yin(:,k).*yin(:,l); 98 | elseif i ~= iDenVar && j == iDenVar && k ~= iDenVar && l ~= iDenVar 99 | yout(:,ind) = yin(:,i).*yin(:,k).*yin(:,l); 100 | elseif i ~= iDenVar && j ~= iDenVar && k == iDenVar && l ~= iDenVar 101 | yout(:,ind) = yin(:,i).*yin(:,j).*yin(:,l); 102 | elseif i ~= iDenVar && j ~= iDenVar && k ~= iDenVar && l == iDenVar 103 | yout(:,ind) = yin(:,i).*yin(:,j).*yin(:,k); 104 | else 105 | yout(:,ind) = zeros(size(yin(:,i))); 106 | end 107 | ind = ind+1; 108 | end 109 | end 110 | end 111 | end 112 | end -------------------------------------------------------------------------------- /utils/color_line3.m: -------------------------------------------------------------------------------- 1 | function h = color_line3(x, y, z, c, varargin) 2 | % color_line3 plots a 3-D "line" with c-data as color 3 | % 4 | % h = color_line(x, y, z, c) 5 | % by default: 'LineStyle','-' and 'Marker','none' 6 | % 7 | % or 8 | % h = color_line(x, y, z, c, mark) 9 | % or 10 | % h = color_line(x, y, z, c, 'Property','value'...) 11 | % with valid 'Property','value' pairs for a surface object 12 | % 13 | % in: x x-data 14 | % y y-data 15 | % z z-data 16 | % c 4th dimension for colouring 17 | % mark for scatter plots with no connecting line 18 | % 19 | % out: h handle of the surface object 20 | 21 | 22 | h = surface(... 23 | 'XData',[x(:) x(:)],... 24 | 'YData',[y(:) y(:)],... 25 | 'ZData',[z(:) z(:)],... 26 | 'CData',[c(:) c(:)],... 27 | 'FaceColor','none',... 28 | 'EdgeColor','flat',... 29 | 'Marker','none'); 30 | 31 | if nargin ==5 32 | switch varargin{1} 33 | case {'+' 'o' '*' '.' 'x' 'square' 'diamond' 'v' '^' '>' '<' 'pentagram' 'p' 'hexagram' 'h'} 34 | set(h,'LineStyle','none','Marker',varargin{1}) 35 | otherwise 36 | error(['Invalid marker: ' varargin{1}]) 37 | end 38 | 39 | elseif nargin > 5 40 | set(h,varargin{:}) 41 | end -------------------------------------------------------------------------------- /utils/evalCostFun.m: -------------------------------------------------------------------------------- 1 | function [Jvals] = evalCostFun(y,u,Q,R,yREF) 2 | % y: NxM 3 | try 4 | M = size(y,2); 5 | Jvals = zeros(length(y),1); 6 | for k=1:M 7 | Jvals(k) = (y(:,k)-yREF)'*Q*(y(:,k)-yREF) + u(:,k)'*R*u(:,k); 8 | end 9 | catch 10 | keyboard 11 | end 12 | 13 | -------------------------------------------------------------------------------- /utils/evalCostFun_AsymPotentialWell_PhaseController.m: -------------------------------------------------------------------------------- 1 | function [Hvals1,Jvals1,uvals1] = evalCostFun_AsymPotentialWell_PhaseController(y1,B,R,Q,H,gain,a,REF) 2 | 3 | Nc = size(B,2); Nt = size(y1,1); 4 | uvals1 = zeros(Nc,Nt); 5 | Hvals1 = zeros(Nt,1); 6 | Jvals1 = zeros(Nt,1); 7 | 8 | for i = 1:Nt 9 | Hvals1(i) = H(y1(i,:)); 10 | % PHASE 1: control on in left well to drive to homoclinic orbit 11 | if Hvals1(i) ~= H([a,0]) && y1(i,1) <= a 12 | uvals1(:,i) = -gain(y1(i,:))*(Hvals1(i)-H([a,0])); 13 | % HREF = H([a,0]); 14 | 15 | % PHASE 2: control off on homoclinic orbit while x <= a 16 | elseif Hvals1(i) == H([a,0]) && y1(i,1) <= a 17 | uvals1(:,i) = zeros(Nc,1); 18 | % HREF = H([a,0]); 19 | 20 | % PHASE 3: control on in right well to drive to energy minimum 21 | elseif y1(i,1) > a 22 | uvals1(:,i) = -gain(y1(i,:))*(Hvals1(i)-H([1;0])); 23 | end 24 | Jvals1(i) = Q*(Hvals1(i)-REF)^2 + uvals1(:,i)'*R*uvals1(:,i); 25 | end -------------------------------------------------------------------------------- /utils/evalCostFun_Hamiltonian.m: -------------------------------------------------------------------------------- 1 | function [Hvals,Jvals] = evalCostFun_Hamiltonian(H,y,u,Q,R,REF) 2 | 3 | Hvals = zeros(length(y),1); 4 | Jvals = zeros(length(y),1); 5 | for k=1:length(y) 6 | Hvals(k) = H(y(k,:)); 7 | Jvals(k) = Q*(Hvals(k)-REF)^2 + u(:,k)'*R*u(:,k); 8 | end 9 | 10 | 11 | -------------------------------------------------------------------------------- /utils/evalCostFun_KoopEfun.m: -------------------------------------------------------------------------------- 1 | function [Efunvals,Jvals] = evalCostFun_KoopEfun(Efun,y,u,Q,R,REF) 2 | 3 | Efunvals = zeros(length(y),1); 4 | Jvals = zeros(length(y),1); 5 | for k=1:length(y) 6 | Efunvals(k) = Efun(y(k,:)); 7 | Jvals(k) = Q*(Efunvals(k)-REF)^2 + u(:,k)'*R*u(:,k); 8 | end 9 | 10 | 11 | -------------------------------------------------------------------------------- /utils/evalCostFun_TimeDependentKoopEfun.m: -------------------------------------------------------------------------------- 1 | function [Efunvals,Jvals] = evalCostFun_TimeDependentKoopEfun(Efun,y,u,Q,R,REF,t) 2 | 3 | Efunvals = zeros(length(y),1); 4 | Jvals = zeros(length(y),1); 5 | for k=1:length(y) 6 | Efunvals(k) = Efun(y(k,:),t(k)); 7 | Jvals(k) = Q*(Efunvals(k)-REF)^2 + u(k,:)*R*u(k,:)'; 8 | end 9 | 10 | 11 | -------------------------------------------------------------------------------- /utils/getBRSys.m: -------------------------------------------------------------------------------- 1 | function sysr = getBRSys(sys,B) 2 | sys.B = B; 3 | [sysr,g,T,Ti] = balreal(sys); 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /utils/getBRTrafo.m: -------------------------------------------------------------------------------- 1 | function T = getBRTrafo(sys,B) 2 | sys.B = B; 3 | [sysr,g,T,Ti] = balreal(sys); 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /utils/poolData.m: -------------------------------------------------------------------------------- 1 | function yout = poolData(yin,nVars,polyorder,usesine) 2 | % Copyright 2015, All Rights Reserved 3 | % Code by Steven L. Brunton 4 | % For Paper, "Discovering Governing Equations from Data: 5 | % Sparse Identification of Nonlinear Dynamical Systems" 6 | % by S. L. Brunton, J. L. Proctor, and J. N. Kutz 7 | 8 | n = size(yin,1); 9 | % yout = zeros(n,1+nVars+(nVars*(nVars+1)/2)+(nVars*(nVars+1)*(nVars+2)/(2*3))+11); 10 | 11 | ind = 1; 12 | % poly order 0 13 | % yout(:,ind) = ones(n,1); 14 | % ind = ind+1; 15 | 16 | % poly order 1 17 | for i=1:nVars 18 | yout(:,ind) = yin(:,i); 19 | ind = ind+1; 20 | end 21 | 22 | if(polyorder>=2) 23 | % poly order 2 24 | for i=1:nVars 25 | for j=i:nVars 26 | yout(:,ind) = yin(:,i).*yin(:,j); 27 | ind = ind+1; 28 | end 29 | end 30 | end 31 | 32 | if(polyorder>=3) 33 | % poly order 3 34 | for i=1:nVars 35 | for j=i:nVars 36 | for k=j:nVars 37 | yout(:,ind) = yin(:,i).*yin(:,j).*yin(:,k); 38 | ind = ind+1; 39 | end 40 | end 41 | end 42 | end 43 | 44 | if(polyorder>=4) 45 | % poly order 4 46 | for i=1:nVars 47 | for j=i:nVars 48 | for k=j:nVars 49 | for l=k:nVars 50 | yout(:,ind) = yin(:,i).*yin(:,j).*yin(:,k).*yin(:,l); 51 | ind = ind+1; 52 | end 53 | end 54 | end 55 | end 56 | end 57 | 58 | if(polyorder>=5) 59 | % poly order 5 60 | for i=1:nVars 61 | for j=i:nVars 62 | for k=j:nVars 63 | for l=k:nVars 64 | for m=l:nVars 65 | yout(:,ind) = yin(:,i).*yin(:,j).*yin(:,k).*yin(:,l).*yin(:,m); 66 | ind = ind+1; 67 | end 68 | end 69 | end 70 | end 71 | end 72 | end 73 | 74 | if(usesine) 75 | for k=1:10; 76 | yout = [yout sin(k*yin) cos(k*yin)]; 77 | end 78 | end -------------------------------------------------------------------------------- /utils/poolDataLIST.m: -------------------------------------------------------------------------------- 1 | function yout = poolDataLIST(yin,ahat,nVars,polyorder,usesine) 2 | % Copyright 2015, All Rights Reserved 3 | % Code by Steven L. Brunton 4 | % For Paper, "Discovering Governing Equations from Data: 5 | % Sparse Identification of Nonlinear Dynamical Systems" 6 | % by S. L. Brunton, J. L. Proctor, and J. N. Kutz 7 | 8 | n = size(yin,1); 9 | 10 | ind = 1; 11 | % poly order 0 12 | % yout{ind,1} = ['1']; 13 | % ind = ind+1; 14 | 15 | % poly order 1 16 | for i=1:nVars 17 | yout(ind,1) = yin(i); 18 | ind = ind+1; 19 | end 20 | 21 | if(polyorder>=2) 22 | % poly order 2 23 | for i=1:nVars 24 | for j=i:nVars 25 | yout{ind,1} = [yin{i},yin{j}]; 26 | ind = ind+1; 27 | end 28 | end 29 | end 30 | 31 | if(polyorder>=3) 32 | % poly order 3 33 | for i=1:nVars 34 | for j=i:nVars 35 | for k=j:nVars 36 | yout{ind,1} = [yin{i},yin{j},yin{k}]; 37 | ind = ind+1; 38 | end 39 | end 40 | end 41 | end 42 | 43 | if(polyorder>=4) 44 | % poly order 4 45 | for i=1:nVars 46 | for j=i:nVars 47 | for k=j:nVars 48 | for l=k:nVars 49 | yout{ind,1} = [yin{i},yin{j},yin{k},yin{l}]; 50 | ind = ind+1; 51 | end 52 | end 53 | end 54 | end 55 | end 56 | 57 | if(polyorder>=5) 58 | % poly order 5 59 | for i=1:nVars 60 | for j=i:nVars 61 | for k=j:nVars 62 | for l=k:nVars 63 | for m=l:nVars 64 | yout{ind,1} = [yin{i},yin{j},yin{k},yin{l},yin{m}]; 65 | ind = ind+1; 66 | end 67 | end 68 | end 69 | end 70 | end 71 | end 72 | 73 | if(usesine) 74 | for k=1:10; 75 | yout{ind,1} = ['sin(',num2str(k),'*yin)']; 76 | ind = ind + 1; 77 | yout{ind,1} = ['cos(',num2str(k),'*yin)']; 78 | ind = ind + 1; 79 | end 80 | end 81 | 82 | 83 | output = yout; 84 | newout(1) = {''}; 85 | for k=1:size(ahat,2)%length(yin) 86 | newout{1,1+k} = [yin{k},'dot']; 87 | end 88 | % newout = {'','xdot','ydot','udot'}; 89 | for k=1:size(ahat,1) 90 | newout(k+1,1) = output(k); 91 | for j=1:size(ahat,2)%length(yin) 92 | newout{k+1,1+j} = ahat(k,j); 93 | end 94 | end 95 | newout -------------------------------------------------------------------------------- /utils/rk4singlestep.m: -------------------------------------------------------------------------------- 1 | function yout = rk4singlestep(fun,dt,t0,y0,u0) 2 | 3 | f1 = fun(t0,y0,u0); 4 | f2 = fun(t0+dt/2,y0+(dt/2)*f1,u0); 5 | f3 = fun(t0+dt/2,y0+(dt/2)*f2,u0); 6 | f4 = fun(t0+dt,y0+dt*f3,u0); 7 | 8 | yout = y0 + (dt/6)*(f1+2*f2+2*f3+f4); -------------------------------------------------------------------------------- /utils/rk4u.m: -------------------------------------------------------------------------------- 1 | function x = rk4u(v,x,u,h,n,t,p) 2 | 3 | % RK4U Runge-Kutta scheme of order 4 for control system 4 | % rk4u(v,X,U,h,n) performs n steps of the scheme for the vector field v 5 | % using stepsize h on each row of the matrix X 6 | % 7 | % v(X,U) maps an (m x d)-matrix X and an (m x p)-matrix U 8 | % to an (m x d)-matrix 9 | 10 | for i = 1:n 11 | k1 = v(t,x,u,p); 12 | k2 = v(t,x + h/2*k1,u,p); 13 | k3 = v(t,x + h/2*k2,u,p); 14 | k4 = v(t,x + h*k3,u,p); 15 | x = x + h*(k1 + 2*k2 + 2*k3 + k4)/6; 16 | end -------------------------------------------------------------------------------- /utils/runMPC.m: -------------------------------------------------------------------------------- 1 | function [xHistory, uHistory, tHistory, rHistory] = runMPC(syshandle, Duration,Ts,N,Nu,x0, ObjectiveFCN, ConstraintFCN, Q, R, Ru, options, xref, pest) 2 | % Prepare variables 3 | Nvar = length(xref); 4 | Nt = (Duration/Ts)+1; 5 | uopt0 = 0; 6 | xhat = x0; 7 | uopt = uopt0.*ones(Nu,1); 8 | xHistory = zeros(Nvar,Nt); xHistory(:,1) = xhat; 9 | uHistory = zeros(1,Nt); uHistory(1) = uopt(1); 10 | tHistory = zeros(1,Nt); tHistory(1) = 0; 11 | rHistory = zeros(Nvar,Nt); 12 | 13 | % Start simulation 14 | fprintf('Simulation started. It might take a while...\n') 15 | tic 16 | for ct = 1:Nt-1 17 | 18 | % NMPC with full-state feedback 19 | COSTFUN = @(u) ObjectiveFCN(u,xhat,N,Nu,xref,uHistory(:,ct),pest,Q,R,Ru); 20 | % CONSFUN = @(u) ConstraintFCN(u,uHistory(:,ct),xhat,N,LBo,UBo,LBdu,UBdu,pest); 21 | uopt = fmincon(COSTFUN,uopt,[],[],[],[],[],[],[],options); 22 | % uopt = fmincon(COSTFUN,uopt,[],[],[],[],LB,UB,CONSFUN,option); 23 | 24 | % Integrate system 25 | xhat = rk4u(syshandle,xhat,uopt(1),Ts/10,10,[],0); %10, 2 26 | xHistory(:,ct+1) = xhat; 27 | uHistory(:,ct+1) = uopt(1); 28 | tHistory(:,ct+1) = ct*Ts; 29 | rHistory(:,ct+1) = xref; 30 | 31 | if mod(ct,10) == 0 32 | disp(['PROGRESS: ',num2str(100*ct/(Duration/Ts)),'%']) 33 | end 34 | end -------------------------------------------------------------------------------- /utils/showPendulumResults.m: -------------------------------------------------------------------------------- 1 | figure (1) 2 | ax1 = subplot('position', [0.08 0.41 0.15 0.5]); 3 | plot(y0(:,1),y0(:,2),'-','Color',0.7*ones(1,3),'LineWidth',1.2); 4 | hold on , grid on 5 | plot(y1(:,1),y1(1:end,2),'b-','LineWidth',1.2); 6 | xlabel('x_1'), ylabel('x_2') 7 | set(gca,'FontSize',14) 8 | axis([-4 4 -2.5 2.5]) 9 | % set(gca,'xtick',[-0.4:0.4:0.4]) 10 | 11 | 12 | valmin = min([min(y0,[],1),min(y1,[],1)]); 13 | valmax = max([max(y0,[],1),max(y1,[],1)]); 14 | % tmp = [mod(y1(:,1),2*pi),y1(:,2)]; 15 | % valmin = min([min(y0,[],1),min(tmp,[],1)]); 16 | % valmax = max([max(y0,[],1),max(tmp,[],1)]); 17 | dval = 0.1*(valmax-valmin); 18 | ax2 = subplot('position', [0.31 0.41 0.15 0.5]); 19 | plot(tspan,y0,'-','Color',0.7*ones(1,3),'LineWidth',1.2); hold on , grid on 20 | plot(tspan,y1(:,1),'b-','LineWidth',1.2); 21 | % plot(tspan,tmp(:,1),'b-','LineWidth',1.2); 22 | plot(tspan,y1(:,2),'b-','LineWidth',1.2); 23 | xlabel('t'), ylabel('x_k') 24 | set(gca,'FontSize',14) 25 | axis([0 maxt valmin-dval valmax+dval]) 26 | % axis([0 maxt -4 8]) 27 | % axis([0 maxt -4 4]) 28 | 29 | ax3 = subplot('position', [0.55 0.41 0.15 0.5]); 30 | valmin = min([min(Hvals0),min(Hvals1)]); 31 | valmax = max([max(Hvals0),max(Hvals1)]); 32 | dval = 0.1*(valmax-valmin); 33 | plot(tspan,Hvals0,'-','Color',0.7*ones(1,3),'LineWidth',1.2); 34 | hold on , grid on 35 | plot(tspan,Hvals1,'b-','LineWidth',1.2); 36 | xlabel('t'), ylabel('H') 37 | lhandle = legend('Unforced dynamics', ['KOOC of Hamiltonian with H_{ref}=',num2str(REF)],'location','none'); 38 | posvals = get(lhandle,'pos'); 39 | set(lhandle,'pos',[0.4 0.06 0.2 0.1]) 40 | set(gca,'FontSize',14) 41 | axis([0 maxt valmin-dval valmax+dval]) 42 | % axis ([0 maxt -1 1.2])%1.2 43 | % axis ([0 maxt -1.2 -.5]) 44 | 45 | ax4 = subplot('position', [0.82 0.41 0.15 0.5]); 46 | valmin = 0; 47 | valmax = max(max(cumsum(Jvals1))); 48 | dval = 0.1*(valmax-valmin); 49 | plot(tspan,cumsum(Jvals0),'-','Color',0.7*ones(1,3),'LineWidth',1.2); 50 | hold on , grid on 51 | plot(tspan,cumsum(Jvals1),'-','Color',[0 0 1],'LineWidth',1.2); 52 | xlabel('t'), ylabel('J') 53 | set(gca,'FontSize',14) 54 | % axis([0 maxt 0 2/3*10^4])%2, 2/3*10^4 55 | axis([0 maxt 0 valmax+2*dval]) 56 | 57 | %linkaxes([ax2,ax3],'x') 58 | set(gcf,'Position',[100 100 600 200]) 59 | set(gcf,'PaperPositionMode','auto') 60 | print('-depsc2', '-loose', [path2figs,ModelName_case,'Results','.eps']); 61 | 62 | figure; 63 | hold on, box on 64 | plot(tspan,y1(:,1),'b-','LineWidth',1.2); %mod(y1(:,1),(2*pi)), y1(:,1) 65 | % plot(tspan,mod(y1(:,1),(2*pi)),'b-','LineWidth',1.2); 66 | plot(tspan,y1(:,2),'-','Color',[0 0.7 0],'LineWidth',1.2); 67 | xlabel('t'), ylabel('x_k') 68 | %xlim ([0 50]) 69 | legend('x_1','x_2') 70 | set(gca,'FontSize',14) 71 | % axis([0 200 -0.1 0.1]) 72 | set(gcf,'Position',[100 100 600 200]) 73 | set(gcf,'PaperPositionMode','auto') 74 | print('-depsc2', '-loose', [path2figs,ModelName_case,'Results_TS','.eps']); -------------------------------------------------------------------------------- /utils/showResults_SlowManifold.m: -------------------------------------------------------------------------------- 1 | dt = 1; 2 | 3 | colors = [0,0,0; 4 | 1,0,0; 5 | 0,0,0.7; 6 | 0.9,0.5,0.2; 7 | 0,0.7,0]; 8 | 9 | %% Show results 10 | LineWidth = 3; 11 | 12 | fhandle = figure; 13 | plot(xLQR(:,1),xLQR(:,2),'-','Color',colors(1,:),'LineWidth',LineWidth); 14 | hold on , grid on 15 | if all(xKOOC(1:dt:end,1)==0)==0 16 | plot(xKOOC(1:dt:end,1),xKOOC(1:dt:end,2),'-','Color',colors(2,:),'LineWidth',LineWidth); 17 | end 18 | plot(xKOOC2(1:dt:end,1),xKOOC2(1:dt:end,2),'--','Color',colors(3,:),'LineWidth',LineWidth); 19 | if all(xFL(1:dt:end,1)==0)==0 20 | plot(xFL(1:dt:end,1),xFL(1:dt:end,2),':','Color',colors(4,:),'LineWidth',LineWidth); 21 | end 22 | plot(xNLC(1:dt:end,1),xNLC(1:dt:end,2),'-.','Color',colors(5,:),'LineWidth',LineWidth); 23 | xlabel('x1'), ylabel('x2') 24 | set(gca,'FontSize',16) 25 | axis tight 26 | ylim(ylim_vals) 27 | xlim([-5 0.001]) 28 | set(gcf,'Position',[100 100 225 200]) 29 | set(gcf,'PaperPositionMode','auto') 30 | print('-painters','-depsc2', '-loose', [ModelName,'Results_PhasePlot','.eps']); 31 | close(fhandle); 32 | 33 | fhandle = figure; 34 | plot(tspan,xLQR,'-','Color',colors(1,:),'LineWidth',LineWidth); hold on , grid on 35 | if all(xKOOC(1:dt:end,1)==0)==0 36 | plot(tspan,xKOOC,'-','Color',colors(2,:),'LineWidth',LineWidth); 37 | end 38 | plot(tspan,xKOOC2,'--','Color',colors(3,:),'LineWidth',LineWidth); 39 | if all(xFL(1:dt:end,1)==0)==0 40 | plot(tspan,xFL(1:dt:end,:),':','Color',colors(4,:),'LineWidth',LineWidth); 41 | end 42 | plot(tNLC,xNLC,'-.','Color',colors(5,:),'LineWidth',LineWidth); 43 | xlabel('t'), ylabel('xk') 44 | xlim([0 50]) 45 | set(gca,'xtick',[0,20,40]) 46 | set(gca,'FontSize',16) 47 | ylim(ylim_vals) 48 | set(gcf,'Position',[100 100 225 200]) 49 | set(gcf,'PaperPositionMode','auto') 50 | print('-painters','-depsc2', '-loose', [ModelName,'Results_TimeSeries','.eps']); 51 | close(fhandle); 52 | 53 | 54 | clear ph 55 | fhandle = figure; 56 | ph(1) = plot(tspan,JLQR_x,'-','Color',colors(1,:),'LineWidth',LineWidth); 57 | % ph(1) = plot(tspan,JLQR_x/10^3,'-','Color',colors(1,:),'LineWidth',LineWidth); 58 | hold on , grid on 59 | if all(xKOOC(1:dt:end,1)==0)==0 60 | ph(2) = plot(tspan,JKOOC_x,'-','Color',colors(2,:),'LineWidth',LineWidth); 61 | end 62 | ph(3) = plot(tspan,JKOOC2_x,'--','Color',colors(3,:),'LineWidth',LineWidth); 63 | % ph(3) = plot(tspan,JKOOC2_x/10^3,'--','Color',colors(3,:),'LineWidth',LineWidth); 64 | if all(xFL(1:dt:end,1)==0)==0 65 | ph(4) = plot(tspan,JFL_x,':','Color',colors(4,:),'LineWidth',LineWidth); 66 | end 67 | ph(5) = plot(tNLC,JNLC_x,'-.','Color',colors(5,:),'LineWidth',LineWidth); 68 | % ph(5) = plot(tNLC,JNLC_x/10^3,'-.','Color',colors(5,:),'LineWidth',LineWidth); 69 | xlabel('t'), ylabel('Jx') 70 | axis(axis_lim) 71 | % axis([axis_lim(1),axis_lim(2),0,15]) 72 | set(gca,'xtick',[0,20,40]) 73 | set(gca,'FontSize',16) 74 | set(gcf,'Position',[100 100 225 200]) 75 | set(gcf,'PaperPositionMode','auto') 76 | print('-painters','-depsc2', '-loose', [ModelName,'Results_Cost','.eps']); 77 | 78 | 79 | % Legend 80 | clear ph 81 | fhandle = figure; 82 | ph(1) = plot([10,20],[3,3],'-','Color',colors(1,:),'LineWidth',2); 83 | hold on , grid on 84 | if all(xKOOC(1:dt:end,1)==0)==0 85 | ph(2) = plot([10,20],[3,3],'-','Color',colors(2,:),'LineWidth',2); 86 | end 87 | ph(3) = plot([10,20],[3,3],'--','Color',colors(3,:),'LineWidth',2); 88 | if all(xFL(1:dt:end,1)==0)==0 89 | ph(4) = plot([10,20],[3,3],':','Color',colors(4,:),'LineWidth',2); 90 | end 91 | ph(5) = plot([10,20],[3,3],'-.','Color',colors(5,:),'LineWidth',2); 92 | if all(xKOOC(1:50:end,1)==0)==0 93 | lhandle = legend('Linearized system','KOOC', 'KRONIC', 'Feedback linearization', 'Numer. TPBV','location','none'); 94 | else 95 | lhandle = legend('Linearized system', 'KRONIC', 'Numer. TPBV','location','none'); 96 | end 97 | posvals = get(lhandle,'pos'); 98 | set(lhandle,'pos',[0.418 0.5 0.2 0.1]) 99 | axis([axis_lim(1:2),0,6]) 100 | axis off 101 | % ph(1).Color = 0.94.*ones(1,3); 102 | set(gca,'FontSize',16) 103 | set(gcf,'Position',[100 100 225 200]) 104 | set(gcf,'PaperPositionMode','auto') 105 | print('-painters','-depsc2', '-loose', [ModelName,'Results_Legend','.eps']); 106 | % close(fhandle); 107 | 108 | clear ph 109 | fhandle = figure; 110 | ph(1) = plot(tspan,JLQR_phi,'-','Color',colors(1,:),'LineWidth',LineWidth); 111 | % ph(1) = plot(tspan,JLQR_x/10^3,'-','Color',colors(1,:),'LineWidth',LineWidth); 112 | hold on , grid on 113 | if all(xKOOC(1:dt:end,1)==0)==0 114 | ph(2) = plot(tspan,JKOOC_phi,'-','Color',colors(2,:),'LineWidth',LineWidth); 115 | end 116 | ph(3) = plot(tspan,JKOOC2_phi,'--','Color',colors(3,:),'LineWidth',LineWidth); 117 | % ph(3) = plot(tspan,JKOOC2_x/10^3,'--','Color',colors(3,:),'LineWidth',LineWidth); 118 | if all(xFL(1:dt:end,1)==0)==0 119 | ph(4) = plot(tspan,JFL_phi,':','Color',colors(4,:),'LineWidth',LineWidth); 120 | end 121 | ph(5) = plot(tNLC,JNLC_phi,'-.','Color',colors(5,:),'LineWidth',LineWidth); 122 | % ph(5) = plot(tNLC,JNLC_x/10^3,'-.','Color',colors(5,:),'LineWidth',LineWidth); 123 | xlabel('t'), ylabel('Jphi') 124 | axis(axis_lim) 125 | % axis([axis_lim(1),axis_lim(2),0,15]) 126 | set(gca,'xtick',[0,20,40]) 127 | set(gca,'FontSize',16) 128 | set(gcf,'Position',[100 100 225 200]) 129 | set(gcf,'PaperPositionMode','auto') 130 | print('-painters','-depsc2', '-loose', [ModelName,'Results_CostPhi','.eps']); -------------------------------------------------------------------------------- /utils/sparseKoopmanEfunSys.m: -------------------------------------------------------------------------------- 1 | function dphi = sparseKoopmanEfunSys(t,y,u,p) 2 | % Delete again? 3 | 4 | nvar = length(y); 5 | polyorder = p.polyorder; 6 | usesine = p.usesine; 7 | xi = p.xi; 8 | phiPool = buildTheta(y,nvar,polyorder,usesine); 9 | Gamma = buildGamma(x,dx,nvar,polyorder,usesine); 10 | dphi = (phiPool*xi)'; -------------------------------------------------------------------------------- /utils/sparsifyDynamics.m: -------------------------------------------------------------------------------- 1 | function Xi = sparsifyDynamics(Theta,dXdt,lambda,n) 2 | % Copyright 2015, All Rights Reserved 3 | % Code by Steven L. Brunton 4 | % For Paper, "Discovering Governing Equations from Data: 5 | % Sparse Identification of Nonlinear Dynamical Systems" 6 | % by S. L. Brunton, J. L. Proctor, and J. N. Kutz 7 | 8 | % compute Sparse regression: sequential least squares 9 | Xi = Theta\dXdt; % initial guess: Least-squares 10 | 11 | % lambda is our sparsification knob. 12 | for k=1:10 13 | smallinds = (abs(Xi)