├── LICENSE ├── README.md ├── codes ├── prediction_accuracy.m ├── predictive_control_koopman1.m ├── predictive_control_koopman2.m ├── predictive_control_taylor.m └── readme └── data ├── net12.mat └── xstar_np.mat /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Shengwen Xie 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Koopman_operator_predictive_control 2 | * This project contains simulation codes for the paper "Discrete System Linearization using Koopman Operators for Predictive Control and Its Applications"[2]. 3 | * All the codes are Not commented and NOT optimized, but they should be easy to follow with the paper aside. 4 | * Feel free to contact me through swxie@outlook.com if you have any questions regarding the code and paper. 5 | 6 | Data: 7 | * net12.mat : RNN model for the PEA system 8 | * xstar_np.mat : LME model mentioned in [1] 9 | * These two models are generated based on the real system input-output data with the method in [1]. 10 | 11 | Codes: 12 | * prediction_accuracy : compare the accuracy of linearization with Taylor series and Koopman operators 13 | * predictive_control_koopman1 : predictive control, linearization using Koopman operators, one linear model 14 | * predictive_control_koopman2 : predictive control, linearization using Koopman operators, two linear models 15 | * predictive_control_taylor : predictive control, linearization using Taylor series 16 | 17 | #Note that the results generated from these codes may be different from the results reported in the paper, this is due to the characteristics of randomness in the method. 18 | 19 | [1] Xie, Shengwen, and Juan Ren. "Recurrent-neural-network-based Predictive Control of Piezo Actuators for Trajectory Tracking." IEEE/ASME Transactions on Mechatronics (2019).\ 20 | [2] Xie, Shengwen, and Juan Ren. "Linearization of Recurrent-neural-network-based models for Predictive Control of Nano-positioning Systems using Data-driven Koopman Operators" IEEE Access (2020). DOI:10.1109/ACCESS.2020.3013935. 21 | -------------------------------------------------------------------------------- /codes/prediction_accuracy.m: -------------------------------------------------------------------------------- 1 | %2020.03.18 2 | % compare the prediction accuracy of linearization with Taylor series and 3 | %Koopman operator 4 | clear,clc, 5 | % 6 | load('E:\back up flash\20191230\20170814NNIdentifymodel\lib\20190605_RNN_linearization\github\net12.mat') 7 | load('E:\back up flash\20191230\20170814NNIdentifymodel\lib\20190605_RNN_linearization\github\xstar_np.mat') 8 | 9 | global W1 10 | global B1 11 | global B2 12 | global W2 13 | global B3 14 | global xkpc 15 | global ukpc 16 | global refk %col vector 17 | global Nh 18 | global Nc 19 | global Abar 20 | global Bbar 21 | global Ae 22 | global Be 23 | global Gall 24 | 25 | global Call 26 | global Cbar 27 | global scale 28 | 29 | scale=10.0; 30 | 31 | %extract rnn from matlab 32 | IW=net.IW; 33 | LW=net.LW; 34 | rnnb=net.b;%all the biaes values 35 | 36 | %nonlinear state space equation 37 | % xk=tanh( W1*xk+B2+B1*uk(k) ); 38 | % y(k)=W2*xk+B3; 39 | B1=cell2mat(IW(1)); 40 | W1=cell2mat(LW(1)); 41 | B2=cell2mat(rnnb(1)); 42 | W2=cell2mat(LW(2)); 43 | B3=cell2mat(rnnb(2)); 44 | 45 | Ns=2; 46 | Ae=reshape(xstar(1:Ns^2),Ns,Ns); 47 | Be=xstar(Ns^2+1:Ns^2+Ns,1); 48 | Ge=xstar(Ns^2+Ns+1:Ns^2+Ns*2,1); 49 | Ce=(xstar(Ns^2+Ns*2+1:Ns^2+Ns*3,1))'; 50 | xstarpara=xstar; 51 | % % 52 | 53 | fs=10000; 54 | Ts=1/fs; 55 | t=(0:Ts:5)'; 56 | amp=1.322; 57 | testU=amp*(sin(2*pi*242.6*t+1.5*pi)+1); 58 | 59 | Lseq=length(testU); 60 | ekall=zeros(Lseq,1); 61 | yPredTest=zeros(Lseq,1);%modeling error 62 | % plot(testU);hold on,plot(testY); 63 | Nrnn=20; 64 | 65 | % 66 | simN=1500; 67 | %LPF state space equation 68 | Nf=3;%order of filter 69 | [b,a]=butter(Nf,0.2); 70 | [Abar,Bbar,Cbar,Dbar]=tf2ss(b,a); 71 | bxn=zeros(Nf+Nrnn+Ns,1);%state [beta(k); x(k); eta(k)] 72 | 73 | ek=0; 74 | 75 | Gall=[zeros(Nrnn+Nf,1);Ge]; 76 | Call=[zeros(1,Nrnn+Nf) Ce]; 77 | refsig=testU;%reference signal 78 | yk=zeros(simN,1); 79 | uk=testU; 80 | betak=zeros(Nf,1); 81 | xk=zeros(Nrnn,1); 82 | etak=zeros(Ns,1); 83 | %parameter for UKF 84 | Nall=Ns+Nrnn+Nf; 85 | xkhat=zeros(Nall,1);%ukf estimated states 86 | Rv=eye(Nall)*0.000002;%process noise 87 | Rn=0.005;%measurement noise, scalar!!! 88 | Pk=Rv; 89 | %sigma points parameters alpha beta gamma 90 | alpha=0.0001; 91 | kappa=3/Nall/1000;%can be set to zero 92 | beta=2;%for guassian 2 is optimal 93 | lambda=alpha^2*(Nall+kappa)-Nall; 94 | gamma=sqrt(Nall+lambda); 95 | W0m=lambda/(Nall+lambda); 96 | W0c=lambda/(Nall+lambda)+(1-alpha^2+beta); 97 | Wim=1/2/(Nall+lambda); 98 | Wic=Wim; 99 | 100 | y_est=zeros(simN,1); 101 | 102 | WimMat=diag([W0m ones(1,2*Nall)*Wim]); 103 | WicMat=diag([W0c ones(1,2*Nall)*Wic]); 104 | Pk_=zeros(Nall); 105 | 106 | %noise 107 | measn=normrnd(0,0.01,simN,1); 108 | prn=normrnd(0,0.0002,Nall,simN); 109 | 110 | %predictive control parameter 111 | xkpc=zeros(Nall,1); 112 | ukpc=0; 113 | Nh=8;%control horizon 114 | Nc=8; 115 | refk=zeros(Nh,1); 116 | 117 | rpc=0.8; 118 | % 119 | options=optimoptions(@fmincon,'Display','off'); 120 | 121 | uoptGD=zeros(simN,1); 122 | opterror=zeros(simN,2); 123 | ek1=0; 124 | % 125 | %simN=900; 126 | %add a weight matrix We for the tracking error 127 | dwe=1-(1:Nh)/Nh+0.1; 128 | We=diag(dwe.^2); 129 | We=eye(Nh); 130 | % 131 | cnt=0; 132 | ypre_TL=zeros(simN,1); 133 | flag=1; 134 | xkTL=xk; 135 | uk=uk/scale; 136 | %generate other m-1 observalbes 137 | mobs=20;%m observales 138 | %measurement: Wm(i,:)*tanh(W1*xk+Bm(:,i)) 139 | Wm=(rand(mobs-1,size(W2,2))-0.5)*2; 140 | Bm=(rand(size(B1,1),mobs-1)-0.5)*2; 141 | B3m=(rand(mobs,1)-0.5)*5; 142 | %Ks number of samples in each epoch 143 | Ks=20; %20 144 | Kep=60; %number of epoch 20 when range is large make Kep large 145 | Kep1=0; 146 | % 147 | [b,a]=butter(6,0.15); 148 | ukSamx=(rand(Kep*Ks,1)-0.5)*4; 149 | ukSamy=filtfilt(b,a,ukSamx); 150 | % plot(y),hold on, %plot(x); 151 | % ukSam=(reshape(ukSamy,Ks,Kep))'; 152 | ukSam=(rand(Kep-Kep1,Ks)-0.5)*2; 153 | for k=1:Kep 154 | tmp=ukSam(k,1); 155 | ukSam(k,:)=ukSam(k,:)-tmp; 156 | end 157 | 158 | Np=12; 159 | % 160 | Xtmp=zeros(mobs,Ks*Kep); %used for constructing Xlift and Xlift_p 161 | ypre_Koop=zeros(simN,1); 162 | 163 | Xlift=zeros(mobs+1,(Ks-1)*Kep); 164 | Xlift_p=zeros(mobs,(Ks-1)*Kep); 165 | 166 | y_t1=zeros(simN,1); 167 | y_t2=zeros(simN,1); 168 | 169 | Wg=W1; 170 | Wslope=(rand(1,mobs-1)-0.5)*0+1; 171 | 172 | flag=1; 173 | for k=1:simN-2 174 | %Taylor linearization 175 | %xk=Apc*xk+Bpc*uk+B_c : B_c constant term 176 | %yk=W2*xk+B3 177 | if(flag==1) 178 | %update coefficients 179 | f0=W1*xk+B2+B1*uk(k); 180 | tanhf0=tanh(f0); 181 | df0=1-tanhf0.^2; 182 | Apc=W1.*repmat(df0,1,size(W1,2)); 183 | Bpc=df0.*B1; 184 | B_c=tanhf0-Apc*xk-Bpc*uk(k); 185 | flag=0; 186 | xkTL=xk; 187 | 188 | %Koopman operator 189 | for k_koop=1:Kep %Kep epochs 190 | xkKoop=xk; 191 | for k_koop1=1:Ks 192 | Xtmp(1,k_koop1+Ks*(k_koop-1))=(W2*xkKoop+B3)*scale; 193 | tmp=tanh( repmat(Wslope,20,1).*repmat(Wg*xkKoop,1,mobs-1)+Bm); 194 | % tmp=( repmat(Wg*xkKoop,1,mobs-1)+Bm); 195 | for k_koop2=2:mobs 196 | Xtmp(k_koop2,k_koop1+Ks*(k_koop-1))=(Wm(k_koop2-1,:)*tmp(:,k_koop2-1)+B3m(k_koop2))*scale; 197 | end 198 | %Radp: range adapter 199 | rng=max(uk(k:k+Np))-min(uk(k:k+Np)); 200 | Radp=rng/2; 201 | xkKoop=tanh(W1*xkKoop+B2+B1*(ukSam(k_koop,k_koop1)*Radp+uk(k))); 202 | end 203 | end 204 | %construct Xlift and Xlift_p 205 | 206 | for k_koop=1:Kep 207 | Xlift(:,1+(Ks-1)*(k_koop-1):(Ks-1)*k_koop)=[Xtmp(:,1+Ks*(k_koop-1):-1+Ks*k_koop);ukSam(k_koop,1:Ks-1)*Radp+uk(k)]; 208 | Xlift_p(:,1+(Ks-1)*(k_koop-1):(Ks-1)*k_koop)=Xtmp(:,2+Ks*(k_koop-1):Ks*k_koop); 209 | end 210 | 211 | AB=Xlift_p/Xlift; 212 | %parameters for the lifted linear system 213 | ApcK=AB(:,1:mobs); 214 | BpcK=AB(:,end); 215 | CpcK=[1 zeros(1,mobs-1)]; 216 | % disp(['loop:' num2str(k)]); 217 | % disp( num2str(AB*Xlift(:,1)-Xlift_p(:,1))); 218 | xkKoop=zeros(mobs,1); 219 | xkKoop(1)=(W2*xk+B3)*scale; 220 | tmp=tanh( repmat(Wslope,20,1).*repmat(Wg*xk,1,mobs-1)+Bm ); 221 | % tmp=( repmat(Wg*xk,1,mobs-1)+Bm ); 222 | for k_koop2=2:mobs 223 | xkKoop(k_koop2)=(Wm(k_koop2-1,:)*tmp(:,k_koop2-1)+B3m(k_koop2))*scale; 224 | end 225 | 226 | y_t1(k)=CpcK*xkKoop; 227 | 228 | xkKooptmp=ApcK*xkKoop+BpcK*uk(k); 229 | 230 | y_t2(k+1)=CpcK*xkKooptmp; 231 | cnt=0; 232 | else 233 | if(cnt==Np) %prediction horizon 234 | flag=1; 235 | % cnt=0; 236 | end 237 | end 238 | %linearized system dynamics: Taylor series 239 | ypre_TL(k)=(W2*xkTL+B3)*scale; 240 | xkTL=Apc*xkTL+Bpc*uk(k)+B_c; 241 | 242 | cnt=cnt+1; 243 | 244 | %linearization based on Koopman operator 245 | ypre_Koop(k)=CpcK*xkKoop; 246 | xkKoop=ApcK*xkKoop+BpcK*uk(k); 247 | 248 | 249 | %system dynamics 250 | yk(k)=(W2*xk+B3)*scale; 251 | xk=tanh(W1*xk+B2+B1*uk(k)); 252 | 253 | 254 | 255 | end 256 | % 257 | %prediction results 258 | ind1=100; 259 | % subplot(1,3,1), 260 | % plot(yk(ind1:simN)),hold on,plot(ypre_TL(ind1:simN)); 261 | % subplot(1,3,2), 262 | % plot(yk(ind1:simN)-ypre_TL(ind1:simN)); 263 | % 264 | % subplot(1,3,3), 265 | subplot(1,2,1), 266 | plot(yk(ind1:simN),'red','linewidth',2), 267 | hold on,plot(ypre_Koop(ind1:simN),'green--','linewidth',2); 268 | plot(ypre_TL(ind1:simN),'blue--','linewidth',2)% plot(y_t1(ind1:simN),'*'); plot(y_t2(ind1:simN),'x'); 269 | legend('ref','Koopman','Taylor'); 270 | subplot(1,2,2), 271 | plot(yk(ind1:simN)-ypre_Koop(ind1:simN),'red--','linewidth',2),hold on, 272 | plot(yk(ind1:simN)-ypre_TL(ind1:simN),'blue--','linewidth',2); 273 | legend('Koopman','Taylor'); 274 | 275 | e1=yk(ind1:simN)-ypre_Koop(ind1:simN); 276 | 277 | e2=yk(ind1:simN)-ypre_TL(ind1:simN); 278 | 279 | disp([norm(e1) norm(e2)]); 280 | disp([norm(e1,inf) norm(e2,inf)]); 281 | disp([var(e1) var(e2)]); 282 | disp('finished!'); 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | -------------------------------------------------------------------------------- /codes/predictive_control_koopman1.m: -------------------------------------------------------------------------------- 1 | %2020.03.18 2 | %linearize the nonlinear mode with Koopman operators and design predictive 3 | %controller for output tracking. 4 | clear,clc, 5 | load('E:\back up flash\20191230\20170814NNIdentifymodel\lib\20190605_RNN_linearization\github\net12.mat') 6 | load('E:\back up flash\20191230\20170814NNIdentifymodel\lib\20190605_RNN_linearization\github\xstar_np.mat') 7 | global W1 8 | global B1 9 | global B2 10 | global W2 11 | global B3 12 | global xkpc 13 | global ukpc 14 | global refk %col vector 15 | global Nh 16 | global Nc 17 | global Abar 18 | global Bbar 19 | global Ae 20 | global Be 21 | global Gall 22 | 23 | global Call 24 | global Cbar 25 | global scale 26 | 27 | scale=10.0; 28 | 29 | Vnh=12; 30 | 31 | %extract rnn from matlab 32 | IW=net.IW; 33 | LW=net.LW; 34 | rnnb=net.b;%all the biaes values 35 | 36 | %nonlinear state space equation 37 | % xk=tanh( W1*xk+B2+B1*uk(k) ); 38 | % y(k)=W2*xk+B3; 39 | B1=cell2mat(IW(1)); 40 | W1=cell2mat(LW(1)); 41 | B2=cell2mat(rnnb(1)); 42 | W2=cell2mat(LW(2)); 43 | B3=cell2mat(rnnb(2)); 44 | 45 | Ns=2; 46 | Ae=reshape(xstar(1:Ns^2),Ns,Ns); 47 | Be=xstar(Ns^2+1:Ns^2+Ns,1); 48 | Ge=xstar(Ns^2+Ns+1:Ns^2+Ns*2,1); 49 | Ce=(xstar(Ns^2+Ns*2+1:Ns^2+Ns*3,1))'; 50 | xstarpara=xstar; 51 | 52 | fs=10000; 53 | Ts=1/fs; 54 | t=(0:Ts:5)'; 55 | amp=1.5; 56 | testU=amp*(sin(2*pi*50*t+1.5*pi)+1); 57 | % testU=rand(length(t),1)*amp; 58 | % 59 | Lseq=length(testU); 60 | ekall=zeros(Lseq,1); 61 | yPredTest=zeros(Lseq,1);%modeling error 62 | % plot(testU);hold on,plot(testY); 63 | Nrnn=20; 64 | 65 | % 66 | simN=3000; 67 | %LPF state space equation 68 | Nf=3;%order of filter 69 | [b,a]=butter(Nf,0.2); 70 | [Abar,Bbar,Cbar,Dbar]=tf2ss(b,a); 71 | bxn=zeros(Nf+Nrnn+Ns,1);%state [beta(k); x(k); eta(k)] 72 | 73 | ek=0; 74 | 75 | Gall=[zeros(Nrnn+Nf,1);Ge]; 76 | Call=[zeros(1,Nrnn+Nf) Ce]; 77 | refsig=testU;%reference signal 78 | yk=zeros(simN,1); 79 | uk=testU; 80 | betak=zeros(Nf,1); 81 | xk=zeros(Nrnn,1); 82 | etak=zeros(Ns,1); 83 | %parameter for UKF 84 | Nall=Ns+Nrnn+Nf; 85 | xkhat=zeros(Nall,1);%ukf estimated states 86 | Rv=eye(Nall)*0.000002;%process noise 87 | Rn=0.005;%measurement noise, scalar!!! 88 | Pk=Rv; 89 | %sigma points parameters alpha beta gamma 90 | alpha=0.0001; 91 | kappa=3/Nall/1000;%can be set to zero 92 | beta=2;%for guassian 2 is optimal 93 | lambda=alpha^2*(Nall+kappa)-Nall; 94 | gamma=sqrt(Nall+lambda); 95 | W0m=lambda/(Nall+lambda); 96 | W0c=lambda/(Nall+lambda)+(1-alpha^2+beta); 97 | Wim=1/2/(Nall+lambda); 98 | Wic=Wim; 99 | 100 | y_est=zeros(simN,1); 101 | 102 | WimMat=diag([W0m ones(1,2*Nall)*Wim]); 103 | WicMat=diag([W0c ones(1,2*Nall)*Wic]); 104 | Pk_=zeros(Nall); 105 | 106 | %noise 107 | measn=normrnd(0,0.01,simN,1); 108 | prn=normrnd(0,0.0002,Nall,simN); 109 | 110 | %predictive control parameter 111 | xkpc=zeros(Nall,1); 112 | ukpc=0; 113 | Nh=Vnh;%control horizon 114 | 115 | Nc=8; 116 | refk=zeros(Nh,1); 117 | 118 | rpc=0.1; 119 | % 120 | options=optimoptions(@fmincon,'Display','off'); 121 | 122 | uoptGD=zeros(simN,1); 123 | opterror=zeros(simN,2); 124 | ek1=0; 125 | % 126 | %simN=900; 127 | %add a weight matrix We for the tracking error 128 | dwe=1-(1:Nh)/Nh+0.1; 129 | We=diag(dwe.^2); 130 | We=eye(Nh); 131 | 132 | cnt=0; 133 | ypre_TL=zeros(simN,1); 134 | flag=1; 135 | xkTL=xk; 136 | uk=uk/scale; 137 | %generate other m-1 observalbes 138 | mobs=8;%m observales 139 | %measurement: Wm(i,:)*tanh(W1*xk+Bm(:,i)) 140 | Wm=(rand(mobs-1,size(W2,2))-0.5)*2; 141 | Bm=(rand(size(B1,1),mobs-1)-0.5)*2; 142 | B3m=(rand(mobs,1)-0.5)*2; 143 | %Ks number of samples in each epoch 144 | Ks=Vnh; 145 | Kep=50; %number of epoch 146 | ukSam=(rand(Kep,Ks)-0.5)*2; 147 | ukSam(:,1)=0; 148 | 149 | Xtmp=zeros(mobs,Ks*Kep); %used for constructing Xlift and Xlift_p 150 | ypre_Koop=zeros(simN,1); 151 | 152 | y_t1=zeros(simN,1); 153 | y_t2=zeros(simN,1); 154 | % 155 | xk=zeros(Nrnn,1); 156 | % Ks=Vnh; 157 | % Kep=20; %number of epoch 158 | Xlift=zeros(mobs+1,(Ks-1)*Kep); 159 | Xlift_p=zeros(mobs,(Ks-1)*Kep); 160 | for k=1:simN-2 161 | %Taylor linearization 162 | %xk=Apc*xk+Bpc*uk+B_c : B_c constant term 163 | %yk=W2*xk+B3 164 | %Koopman operator 165 | for k_koop=1:Kep %Kep epochs 166 | xkKoop=xk; 167 | for k_koop1=1:Ks 168 | Xtmp(1,k_koop1+Ks*(k_koop-1))=(W2*xkKoop+B3)*scale; 169 | tmp=tanh( repmat(W1*xkKoop,1,mobs-1)+Bm); 170 | for k_koop2=2:mobs 171 | Xtmp(k_koop2,k_koop1+Ks*(k_koop-1))=(Wm(k_koop2-1,:)*tmp(:,k_koop2-1)+B3)*scale; 172 | end 173 | rng=max(refsig(k:k+Nh+5))-min(refsig(k:k+Nh+5)); 174 | Radp=rng/10/3; 175 | xkKoop=tanh(W1*xkKoop+B2+B1*(ukSam(k_koop,k_koop1)*Radp+uk(k))); 176 | end 177 | end 178 | %construct Xlift and Xlift_p 179 | 180 | for k_koop=1:Kep 181 | Xlift(:,1+(Ks-1)*(k_koop-1):(Ks-1)*k_koop)=[Xtmp(:,1+Ks*(k_koop-1):-1+Ks*k_koop);ukSam(k_koop,1:Ks-1)*Radp+uk(k)]; 182 | Xlift_p(:,1+(Ks-1)*(k_koop-1):(Ks-1)*k_koop)=Xtmp(:,2+Ks*(k_koop-1):Ks*k_koop); 183 | end 184 | 185 | AB=Xlift_p/Xlift; 186 | %parameters for the lifted linear system 187 | ApcK=AB(:,1:mobs); 188 | BpcK=AB(:,end); 189 | CpcK=[1 zeros(1,mobs-1)]; 190 | % disp(['loop:' num2str(k)]); 191 | % disp( num2str(AB*Xlift(:,1)-Xlift_p(:,1))); 192 | xkKoop=zeros(mobs,1); 193 | xkKoop(1)=(W2*xk+B3)*scale; 194 | tmp=tanh( repmat(W1*xk,1,mobs-1)+Bm ); 195 | for k_koop2=2:mobs 196 | xkKoop(k_koop2)=(Wm(k_koop2-1,:)*tmp(:,k_koop2-1)+B3)*scale; 197 | end 198 | 199 | % y_t1(k)=CpcK*xkKoop; 200 | % xkKooptmp=ApcK*xkKoop+BpcK*uk(k); 201 | % y_t2(k+1)=CpcK*xkKooptmp; % 202 | %linearization based on Koopman operator 203 | 204 | %predictive control with 205 | %ypre_Koop(k)=CpcK*xkKoop; 206 | %xkKoop=ApcK*xkKoop+BpcK*uk(k); 207 | Apc=ApcK; 208 | Bpc=BpcK; 209 | Cpc=CpcK; 210 | 211 | xkpc=xkKoop; 212 | ukpc=uk(k); 213 | %predictive control setup 214 | Gpc=zeros(size(Cpc,1)*Nh,size(Apc,2)); 215 | Hpc=zeros(Nh,Nh); 216 | Fpc=zeros(Nh,1); 217 | hpc=zeros(Nh+1,1); 218 | hpc(Nh+1)=Cpc*Apc^(Nh-1)*Bpc; 219 | 220 | Dpc=eye(Nh); 221 | Dpc(Nh+1:Nh+1:(Nh-1)*Nh+Nh-1)=-1; 222 | Dpc(end)=0; 223 | 224 | %MPC parameters setup 225 | for k2=1:Nh 226 | if(k2==1) 227 | hpc(k2)=0; 228 | else 229 | hpc(k2)=Cpc*Apc^(k2-2)*Bpc; %Apc^(k2-2) 230 | end 231 | Gpc( (k2-1)*size(Cpc,1)+1:k2*size(Cpc,1),: )=Cpc*Apc^k2; 232 | Hpc(k2,1:k2)=(hpc(k2:-1:1,1))'; 233 | end 234 | 235 | Fpc=hpc(2:end); 236 | Qpc=rpc*(Dpc'*Dpc)+Hpc'*Hpc; 237 | fpc=Hpc'*(Gpc*xkpc+Fpc*ukpc-refsig(k+1:k+Nh,1)); 238 | xopt=-Qpc\fpc; 239 | uk(k+1)=xopt(1); 240 | 241 | %system dynamics 242 | yk(k)=(W2*xk+B3)*scale; 243 | xk=tanh(W1*xk+B2+B1*uk(k)); 244 | end 245 | disp('finished!'); 246 | % 247 | %plot the tracking results and the tracking errors 248 | subplot(1,2,1), 249 | plot(yk(1:simN)),hold on,plot(refsig(1:simN)); 250 | subplot(1,2,2), 251 | plot(yk(1:simN)-refsig(1:simN)); 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | -------------------------------------------------------------------------------- /codes/predictive_control_koopman2.m: -------------------------------------------------------------------------------- 1 | %2020.03.18 2 | %use two linear models to approxmiate the nonlinear model 3 | clear,clc, 4 | load('E:\back up flash\20191230\20170814NNIdentifymodel\lib\20190605_RNN_linearization\github\net12.mat') 5 | load('E:\back up flash\20191230\20170814NNIdentifymodel\lib\20190605_RNN_linearization\github\xstar_np.mat') 6 | global W1 7 | global B1 8 | global B2 9 | global W2 10 | global B3 11 | global xkpc 12 | global ukpc 13 | global refk %col vector 14 | global Nh 15 | global Nc 16 | global Abar 17 | global Bbar 18 | global Ae 19 | global Be 20 | global Gall 21 | global Call 22 | global Cbar 23 | global scale 24 | 25 | scale=10.0; 26 | 27 | %extract rnn from matlab 28 | IW=net.IW; 29 | LW=net.LW; 30 | rnnb=net.b;%all the biaes values 31 | 32 | %nonlinear state space equation 33 | % xk=tanh( W1*xk+B2+B1*uk(k) ); 34 | % y(k)=W2*xk+B3; 35 | B1=cell2mat(IW(1)); 36 | W1=cell2mat(LW(1)); 37 | B2=cell2mat(rnnb(1)); 38 | W2=cell2mat(LW(2)); 39 | B3=cell2mat(rnnb(2)); 40 | 41 | Ns=2; 42 | Ae=reshape(xstar(1:Ns^2),Ns,Ns); 43 | Be=xstar(Ns^2+1:Ns^2+Ns,1); 44 | Ge=xstar(Ns^2+Ns+1:Ns^2+Ns*2,1); 45 | Ce=(xstar(Ns^2+Ns*2+1:Ns^2+Ns*3,1))'; 46 | xstarpara=xstar; 47 | % 48 | % testU=(y1range(2)-y1range(1))/(x1range(2)-x1range(1))*(Xtr-x1range(1))+y1range(1); 49 | % testU=Xtr*scale; 50 | % testY=Ttr*scale;% to be predicted 51 | fs=10000; 52 | Ts=1/fs; 53 | t=(0:Ts:5)'; 54 | amp=1.2; 55 | testU=amp*(sin(2*pi*250*t+1.5*pi)+1); 56 | 57 | Lseq=length(testU); 58 | ekall=zeros(Lseq,1); 59 | yPredTest=zeros(Lseq,1);%modeling error 60 | % plot(testU);hold on,plot(testY); 61 | Nrnn=20; 62 | 63 | simN=3000; 64 | %LPF state space equation 65 | Nf=3;%order of filter 66 | [b,a]=butter(Nf,0.2); 67 | [Abar,Bbar,Cbar,Dbar]=tf2ss(b,a); 68 | bxn=zeros(Nf+Nrnn+Ns,1);%state [beta(k); x(k); eta(k)] 69 | 70 | ek=0; 71 | 72 | Gall=[zeros(Nrnn+Nf,1);Ge]; 73 | Call=[zeros(1,Nrnn+Nf) Ce]; 74 | refsig=testU;%reference signal 75 | yk=zeros(simN,1); 76 | uk=testU; 77 | betak=zeros(Nf,1); 78 | xk=zeros(Nrnn,1); 79 | etak=zeros(Ns,1); 80 | %parameter for UKF 81 | Nall=Ns+Nrnn+Nf; 82 | xkhat=zeros(Nall,1);%ukf estimated states 83 | Rv=eye(Nall)*0.000002;%process noise 84 | Rn=0.005;%measurement noise, scalar!!! 85 | Pk=Rv; 86 | %sigma points parameters alpha beta gamma 87 | alpha=0.00001; 88 | kappa=3/Nall/1000;%can be set to zero 89 | beta=2;%for guassian 2 is optimal 90 | lambda=alpha^2*(Nall+kappa)-Nall; 91 | gamma=sqrt(Nall+lambda); 92 | W0m=lambda/(Nall+lambda); 93 | W0c=lambda/(Nall+lambda)+(1-alpha^2+beta); 94 | Wim=1/2/(Nall+lambda); 95 | Wic=Wim; 96 | 97 | y_est=zeros(simN,1); 98 | 99 | WimMat=diag([W0m ones(1,2*Nall)*Wim]); 100 | WicMat=diag([W0c ones(1,2*Nall)*Wic]); 101 | Pk_=zeros(Nall); 102 | 103 | %noise 104 | measn=normrnd(0,0.01,simN,1); 105 | prn=normrnd(0,0.0002,Nall,simN); 106 | 107 | %predictive control parameter 108 | xkpc=zeros(Nall,1); 109 | ukpc=0; 110 | Nh=14;%control horizon 111 | Nc=8; 112 | refk=zeros(Nh,1); 113 | 114 | rpc=0.8; 115 | % 116 | options=optimoptions(@fmincon,'Display','off'); 117 | 118 | uoptGD=zeros(simN,1); 119 | opterror=zeros(simN,2); 120 | ek1=0; 121 | % 122 | 123 | %add a weight matrix We for the tracking error 124 | dwe=1-(1:Nh)/Nh+0.1; 125 | We=diag(dwe.^2); 126 | We=eye(Nh); 127 | for k=1:simN 128 | %system dynamics 129 | betak=bxn(1:Nf); 130 | xk=bxn(Nf+1:Nf+Nrnn); 131 | etak=bxn(Nf+Nrnn+1:end); 132 | 133 | % uk(k)=(y1range(2)-y1range(1))/(x1range(2)-x1range(1))*(uk(k)-x1range(1))+y1range(1); 134 | tmp=Cbar*betak/scale; 135 | bxn=[Abar*betak+Bbar*uk(k); 136 | tanh(W1*xk+B2+B1*tmp); 137 | Ae*etak+scale*Be*(W2*xk+B3)]+Gall*ek1; 138 | yk(k)=Call*bxn+ek1; 139 | ek1=measn(k)*0; 140 | 141 | %state estimation 142 | %Sigma points 143 | sqrtPk=chol(Pk,'lower');%cholesky factorization 144 | xi=[xkhat repmat(xkhat,1,Nall)+gamma*sqrtPk repmat(xkhat,1,Nall)-gamma*sqrtPk]; 145 | %Time update 146 | uk_1=uk(k); 147 | betaki=xi(1:Nf,:); 148 | xki=xi(Nf+1:Nf+Nrnn,:); 149 | etaki=xi(Nf+Nrnn+1:end,:); 150 | tmp=Cbar*betaki/scale; 151 | xikk_1=[Abar*betaki+repmat(Bbar*uk_1,1,2*Nall+1); 152 | tanh(W1*xki+repmat(B2,1,2*Nall+1)+B1*tmp); 153 | Ae*etaki+scale*Be*(W2*xki+B3)]+repmat(Gall*ek,1,2*Nall+1); 154 | xkhat_=sum(xikk_1*WimMat,2); 155 | Pk_=Rv; 156 | for k1=1:2*Nall+1 157 | Pk_=Pk_+WicMat(k1,k1)*( (xikk_1(:,k1)-xkhat_)*(xikk_1(:,k1)-xkhat_)' ); 158 | end 159 | ykk_1=Call*xikk_1;%row vector 160 | ykhat_=sum(ykk_1*WimMat);%scalar 161 | %Measurement update 162 | Pykayka=Rn;%yka means yk with ~ above 163 | for k1=1:2*Nall+1 164 | Pykayka=Pykayka+WicMat(k1,k1)*( (ykk_1(:,k1)-ykhat_)*(ykk_1(:,k1)-ykhat_)' ); 165 | end 166 | Pxkyk=zeros(Nall,1); 167 | for k1=1:2*Nall+1 168 | Pxkyk=Pxkyk+WicMat(k1,k1)*( (xikk_1(:,k1)-xkhat_)*(ykk_1(:,k1)-ykhat_)' ); 169 | end 170 | Kk=Pxkyk/Pykayka;%Pxkyk*inv(Pykayka) 171 | xkhat=xkhat_+Kk*(yk(k)-ykhat_); 172 | ek=yk(k)-ykhat_; 173 | Pk=Pk_-Kk*Pykayka*Kk'; 174 | y_est(k)=Call*xkhat; 175 | %predictive control: fmincon------------------------------------------- 176 | xkpc=xkhat; 177 | % xkpc=bxn; 178 | ukpc=uk(k); 179 | refk(1:Nc)=(refsig(k+1:k+Nc,1)); %col vector 180 | if(Nh>Nc) 181 | refk(Nc+1:Nh)=refk(Nc); 182 | end 183 | % lb=-ones(4,1); 184 | % ub=ones(4,1); 185 | lb=[];ub=[]; 186 | x0=ones(Nc,1)*uk(k); 187 | % xstar=fmincon(@preError_1_V3,x0,[],[],[],[],lb,ub,[],options);%dont forget @ !! 188 | % uk(k+1)=xstar(1); 189 | 190 | %second method:linearization at each point 20190606 191 | %current state is xkpc, current input is ukpc 192 | %convert to M_k+1=Apc*M_k+Bpc*uk+Mc where Mc is constant 193 | %output: y_k=Call*M_k 194 | Npc=Nf+Nrnn+Ns;%order of the model 195 | Apc=zeros(Npc,Npc); 196 | Bpc=zeros(Npc,1); 197 | Cpc=Call; 198 | Apc(1:Nf,1:Nf)=Abar; 199 | Apc(Nf+Nrnn+1:end,Nf+1:end)=[scale*Be*W2 Ae]; 200 | Bpc(1:Nf)=Bbar; 201 | %compute f0 where the linearization happens 202 | Mc=zeros(Npc,1); 203 | betak_0=xkpc(1:Nf); 204 | xk_0=xkpc(Nf+1:Nf+Nrnn); 205 | f0=W1*xk_0+B2+B1*Cbar*betak_0/scale; 206 | tanhf0=tanh(f0); 207 | df0=1-tanhf0.^2; 208 | tmpB1CW=[B1*Cbar/scale W1]; 209 | dfdbetax=repmat(df0,1,size(tmpB1CW,2)).*tmpB1CW; 210 | Mc(Nf+1:Nf+Nrnn)=-dfdbetax*xkpc(1:Nf+Nrnn)+tanhf0; 211 | Mc(Nf+Nrnn+1:end)=scale*Be*B3; 212 | Apc(Nf+1:Nf+Nrnn,1:Nf+Nrnn)=dfdbetax; 213 | %predictive control setup 214 | Gpc=zeros(size(Cpc,1)*Nh,size(Apc,2)); 215 | Hpc=zeros(Nh,Nh); 216 | Fpc=zeros(Nh,1); 217 | hpc=zeros(Nh+1,1); 218 | hpc(Nh+1)=Cpc*Apc^(Nh-1)*Bpc; 219 | Lpc=zeros(Nh,1); 220 | 221 | Dpc=eye(Nh); 222 | Dpc(Nh+1:Nh+1:(Nh-1)*Nh+Nh-1)=-1; 223 | Dpc(end)=0; 224 | [V_c,D_c] = eig(Apc); 225 | % V_c=real(V_c); 226 | % D_c=real(D_c); 227 | %MPC parameters setup 228 | for k2=1:Nh 229 | if(k2==1) 230 | hpc(k2)=0; 231 | Lpc(1)=Cpc*Mc; 232 | else 233 | hpc(k2)=Cpc*real( V_c*D_c^(k2-2)/V_c )*Bpc; %Apc^(k2-2) 234 | Lpc(k2)=Lpc(k2-1)+Cpc*real(V_c*D_c^(k2-1)/V_c)*Mc; 235 | end 236 | Gpc( (k2-1)*size(Cpc,1)+1:k2*size(Cpc,1),: )=Cpc*real(V_c*D_c^k2/V_c); 237 | Hpc(k2,1:k2)=(hpc(k2:-1:1,1))'; 238 | end 239 | 240 | Fpc=hpc(2:end); 241 | Qpc=rpc*(Dpc'*We*Dpc)+Hpc'*We*Hpc; 242 | fpc=Hpc'*We*(Gpc*xkpc+Fpc*ukpc-refsig(k+1:k+Nh,1)+Lpc); 243 | xopt=-Qpc\fpc; 244 | uk(k+1)=xopt(1); 245 | %set up new linearization for better approximation 246 | %compute linearization point 247 | xkSL=xkpc; 248 | lenSL=5; 249 | ukSL=[ukpc; xopt(1:lenSL-1)]; 250 | for kSL=1:lenSL %nonlinear forward 251 | % xkSL=Apc*xkSL+Bpc*ukSL(kSL)+Mc; 252 | betakSL=xkSL(1:Nf); 253 | xk_sl=xkSL(Nf+1:Nf+Nrnn); 254 | etakSL=xkSL(Nf+Nrnn+1:end); 255 | tmpSL=Cbar*betakSL/scale; 256 | xkSL=[Abar*betakSL+Bbar*ukSL(kSL); 257 | tanh(W1*xk_sl+B2+B1*tmpSL); 258 | Ae*etakSL+scale*Be*(W2*xk_sl+B3)]; 259 | end 260 | %linearize at xkSL 261 | %new linear model 262 | Apc2=Apc; 263 | %compute f0 where the linearization happens 264 | Mc2=Mc; 265 | betak2_0=xkSL(1:Nf); 266 | xk2_0=xkSL(Nf+1:Nf+Nrnn); 267 | f20=W1*xk2_0+B2+B1*Cbar*betak2_0/scale; 268 | tanhf20=tanh(f20); 269 | df20=1-tanhf20.^2; 270 | tmpB1CW=[B1*Cbar/scale W1]; 271 | dfdbetax2=repmat(df20,1,size(tmpB1CW,2)).*tmpB1CW; 272 | Mc2(Nf+1:Nf+Nrnn)=-dfdbetax2*xkSL(1:Nf+Nrnn)+tanhf20; 273 | Apc2(Nf+1:Nf+Nrnn,1:Nf+Nrnn)=dfdbetax2; 274 | lenSL=7; 275 | for k2=lenSL+1:Nh 276 | % hpc(k2)=Cpc*real( V_c*D_c^(lenSL-2)/V_c )*Apc2^(k2-lenSL)*Bpc; %Apc^(k2-2) 277 | % Lpc(k2)=Lpc(k2-1)+Cpc*real(V_c*D_c^(lenSL-1)/V_c)*Apc2^(k2-lenSL)*Mc2; 278 | % 279 | % Gpc( (k2-1)*size(Cpc,1)+1:k2*size(Cpc,1),: )=Cpc*real(V_c*D_c^lenSL/V_c)*Apc2^(k2-lenSL); 280 | 281 | hpc(k2)=Cpc*Apc2^(k2-lenSL)*real( V_c*D_c^(lenSL-2)/V_c )*Bpc; %Apc^(k2-2) 282 | Lpc(k2)=Lpc(k2-1)+Cpc*Apc2^(k2-lenSL)*real(V_c*D_c^(lenSL-1)/V_c)*Mc2; 283 | 284 | Gpc( (k2-1)*size(Cpc,1)+1:k2*size(Cpc,1),: )=Cpc*Apc2^(k2-lenSL)*real(V_c*D_c^lenSL/V_c); 285 | Hpc(k2,1:k2)=(hpc(k2:-1:1,1))'; 286 | end 287 | hpc(Nh+1)=Cpc*Apc^(lenSL-2)*Apc2^(Nh+1-lenSL)*Bpc; 288 | Fpc=hpc(2:end); 289 | Qpc=rpc*(Dpc'*Dpc)+Hpc'*Hpc; 290 | fpc=Hpc'*(Gpc*xkpc+Fpc*ukpc-refsig(k+1:k+Nh,1)+Lpc); 291 | xopt=-Qpc\fpc; 292 | uk(k+1)=xopt(1); 293 | 294 | end 295 | 296 | % 297 | %tracking results and tracking errors 298 | subplot(1,2,1), 299 | plot(yk(1:simN)),hold on,plot(refsig(1:simN)); 300 | subplot(1,2,2), 301 | plot(yk(1:simN)-refsig(1:simN)); 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | -------------------------------------------------------------------------------- /codes/predictive_control_taylor.m: -------------------------------------------------------------------------------- 1 | %2020/03/18 2 | %use Taylor series to linearize the system and apply predictive control 3 | clear,clc, 4 | load('E:\back up flash\20191230\20170814NNIdentifymodel\lib\20190605_RNN_linearization\github\net12.mat') 5 | load('E:\back up flash\20191230\20170814NNIdentifymodel\lib\20190605_RNN_linearization\github\xstar_np.mat') 6 | global W1 7 | global B1 8 | global B2 9 | global W2 10 | global B3 11 | global xkpc 12 | global ukpc 13 | global refk %col vector 14 | global Nh 15 | global Nc 16 | global Abar 17 | global Bbar 18 | global Ae 19 | global Be 20 | global Gall 21 | global Call 22 | global Cbar 23 | global scale 24 | 25 | scale=10.0; 26 | 27 | %extract rnn from matlab 28 | IW=net.IW; 29 | LW=net.LW; 30 | rnnb=net.b;%all the biaes values 31 | 32 | %nonlinear state space equation 33 | % xk=tanh( W1*xk+B2+B1*uk(k) ); 34 | % y(k)=W2*xk+B3; 35 | B1=cell2mat(IW(1)); 36 | W1=cell2mat(LW(1)); 37 | B2=cell2mat(rnnb(1)); 38 | W2=cell2mat(LW(2)); 39 | B3=cell2mat(rnnb(2)); 40 | 41 | Ns=2; 42 | Ae=reshape(xstar(1:Ns^2),Ns,Ns); 43 | Be=xstar(Ns^2+1:Ns^2+Ns,1); 44 | Ge=xstar(Ns^2+Ns+1:Ns^2+Ns*2,1); 45 | Ce=(xstar(Ns^2+Ns*2+1:Ns^2+Ns*3,1))'; 46 | xstarpara=xstar; 47 | % 48 | % testU=(y1range(2)-y1range(1))/(x1range(2)-x1range(1))*(Xtr-x1range(1))+y1range(1); 49 | % testU=Xtr*scale; 50 | % testY=Ttr*scale;% to be predicted 51 | fs=10000; 52 | Ts=1/fs; 53 | t=(0:Ts:5)'; 54 | amp=1.5; 55 | testU=amp*(sin(2*pi*352.6*t+1.5*pi)+1); 56 | 57 | Lseq=length(testU); 58 | ekall=zeros(Lseq,1); 59 | yPredTest=zeros(Lseq,1);%modeling error 60 | % plot(testU);hold on,plot(testY); 61 | Nrnn=20; 62 | 63 | % 64 | simN=3000; 65 | %LPF state space equation 66 | Nf=3;%order of filter 67 | [b,a]=butter(Nf,0.2); 68 | [Abar,Bbar,Cbar,Dbar]=tf2ss(b,a); 69 | bxn=zeros(Nf+Nrnn+Ns,1);%state [beta(k); x(k); eta(k)] 70 | 71 | ek=0; 72 | 73 | Gall=[zeros(Nrnn+Nf,1);Ge]; 74 | Call=[zeros(1,Nrnn+Nf) Ce]; 75 | refsig=testU;%reference signal 76 | yk=zeros(simN,1); 77 | uk=testU; 78 | betak=zeros(Nf,1); 79 | xk=zeros(Nrnn,1); 80 | etak=zeros(Ns,1); 81 | %parameter for UKF 82 | Nall=Ns+Nrnn+Nf; 83 | xkhat=zeros(Nall,1);%ukf estimated states 84 | Rv=eye(Nall)*0.000002;%process noise 85 | Rn=0.005;%measurement noise, scalar!!! 86 | Pk=Rv; 87 | %sigma points parameters alpha beta gamma 88 | alpha=0.00001; 89 | kappa=3/Nall/1000;%can be set to zero 90 | beta=2;%for guassian 2 is optimal 91 | lambda=alpha^2*(Nall+kappa)-Nall; 92 | gamma=sqrt(Nall+lambda); 93 | W0m=lambda/(Nall+lambda); 94 | W0c=lambda/(Nall+lambda)+(1-alpha^2+beta); 95 | Wim=1/2/(Nall+lambda); 96 | Wic=Wim; 97 | 98 | y_est=zeros(simN,1); 99 | 100 | WimMat=diag([W0m ones(1,2*Nall)*Wim]); 101 | WicMat=diag([W0c ones(1,2*Nall)*Wic]); 102 | Pk_=zeros(Nall); 103 | 104 | %noise 105 | measn=normrnd(0,0.01,simN,1); 106 | prn=normrnd(0,0.0002,Nall,simN); 107 | 108 | %predictive control parameter 109 | xkpc=zeros(Nall,1); 110 | ukpc=0; 111 | Nh=25;%control horizon 112 | Nc=8; 113 | refk=zeros(Nh,1); 114 | 115 | rpc=0.5; 116 | % 117 | options=optimoptions(@fmincon,'Display','off'); 118 | 119 | uoptGD=zeros(simN,1); 120 | opterror=zeros(simN,2); 121 | ek1=0; 122 | % 123 | %simN=900; 124 | %add a weight matrix We for the tracking error 125 | dwe=1-(1:Nh)/Nh+0.1; 126 | We=diag(dwe.^2); 127 | We=eye(Nh); 128 | for k=1:simN 129 | %system dynamics 130 | betak=bxn(1:Nf); 131 | xk=bxn(Nf+1:Nf+Nrnn); 132 | etak=bxn(Nf+Nrnn+1:end); 133 | 134 | % uk(k)=(y1range(2)-y1range(1))/(x1range(2)-x1range(1))*(uk(k)-x1range(1))+y1range(1); 135 | tmp=Cbar*betak/scale; 136 | bxn=[Abar*betak+Bbar*uk(k); 137 | tanh(W1*xk+B2+B1*tmp); 138 | Ae*etak+scale*Be*(W2*xk+B3)]+Gall*ek1; 139 | yk(k)=Call*bxn+ek1; 140 | ek1=measn(k)*0; 141 | 142 | %state estimation 143 | %Sigma points 144 | sqrtPk=chol(Pk,'lower');%cholesky factorization 145 | xi=[xkhat repmat(xkhat,1,Nall)+gamma*sqrtPk repmat(xkhat,1,Nall)-gamma*sqrtPk]; 146 | %Time update 147 | uk_1=uk(k); 148 | betaki=xi(1:Nf,:); 149 | xki=xi(Nf+1:Nf+Nrnn,:); 150 | etaki=xi(Nf+Nrnn+1:end,:); 151 | tmp=Cbar*betaki/scale; 152 | xikk_1=[Abar*betaki+repmat(Bbar*uk_1,1,2*Nall+1); 153 | tanh(W1*xki+repmat(B2,1,2*Nall+1)+B1*tmp); 154 | Ae*etaki+scale*Be*(W2*xki+B3)]+repmat(Gall*ek,1,2*Nall+1); 155 | xkhat_=sum(xikk_1*WimMat,2); 156 | Pk_=Rv; 157 | for k1=1:2*Nall+1 158 | Pk_=Pk_+WicMat(k1,k1)*( (xikk_1(:,k1)-xkhat_)*(xikk_1(:,k1)-xkhat_)' ); 159 | end 160 | ykk_1=Call*xikk_1;%row vector 161 | ykhat_=sum(ykk_1*WimMat);%scalar 162 | %Measurement update 163 | Pykayka=Rn;%yka means yk with ~ above 164 | for k1=1:2*Nall+1 165 | Pykayka=Pykayka+WicMat(k1,k1)*( (ykk_1(:,k1)-ykhat_)*(ykk_1(:,k1)-ykhat_)' ); 166 | end 167 | Pxkyk=zeros(Nall,1); 168 | for k1=1:2*Nall+1 169 | Pxkyk=Pxkyk+WicMat(k1,k1)*( (xikk_1(:,k1)-xkhat_)*(ykk_1(:,k1)-ykhat_)' ); 170 | end 171 | Kk=Pxkyk/Pykayka;%Pxkyk*inv(Pykayka) 172 | xkhat=xkhat_+Kk*(yk(k)-ykhat_); 173 | ek=yk(k)-ykhat_; 174 | Pk=Pk_-Kk*Pykayka*Kk'; 175 | y_est(k)=Call*xkhat; 176 | %predictive control: fmincon------------------------------------------- 177 | xkpc=xkhat; 178 | xkpc=bxn; 179 | ukpc=uk(k); 180 | refk(1:Nc)=(refsig(k+1:k+Nc,1)); %col vector 181 | if(Nh>Nc) 182 | refk(Nc+1:Nh)=refk(Nc); 183 | end 184 | % lb=-ones(4,1); 185 | % ub=ones(4,1); 186 | lb=[];ub=[]; 187 | x0=ones(Nc,1)*uk(k); 188 | % xstar=fmincon(@preError_1_V3,x0,[],[],[],[],lb,ub,[],options);%dont forget @ !! 189 | % uk(k+1)=xstar(1); 190 | 191 | %second method:linearization at each point 20190606 192 | %current state is xkpc, current input is ukpc 193 | %convert to M_k+1=Apc*M_k+Bpc*uk+Mc where Mc is constant 194 | %output: y_k=Call*M_k 195 | Npc=Nf+Nrnn+Ns;%order of the model 196 | Apc=zeros(Npc,Npc); 197 | Bpc=zeros(Npc,1); 198 | Cpc=Call; 199 | Apc(1:Nf,1:Nf)=Abar; 200 | Apc(Nf+Nrnn+1:end,Nf+1:end)=[scale*Be*W2 Ae]; 201 | Bpc(1:Nf)=Bbar; 202 | %compute f0 where the linearization happens 203 | Mc=zeros(Npc,1); 204 | betak_0=xkpc(1:Nf); 205 | xk_0=xkpc(Nf+1:Nf+Nrnn); 206 | f0=W1*xk_0+B2+B1*Cbar*betak_0/scale; 207 | tanhf0=tanh(f0); 208 | df0=1-tanhf0.^2; 209 | tmpB1CW=[B1*Cbar/scale W1]; 210 | dfdbetax=repmat(df0,1,size(tmpB1CW,2)).*tmpB1CW; 211 | Mc(Nf+1:Nf+Nrnn)=-dfdbetax*xkpc(1:Nf+Nrnn)+tanhf0; 212 | Mc(Nf+Nrnn+1:end)=scale*Be*B3; 213 | Apc(Nf+1:Nf+Nrnn,1:Nf+Nrnn)=dfdbetax; 214 | %predictive control setup 215 | Gpc=zeros(size(Cpc,1)*Nh,size(Apc,2)); 216 | Hpc=zeros(Nh,Nh); 217 | Fpc=zeros(Nh,1); 218 | hpc=zeros(Nh+1,1); 219 | hpc(Nh+1)=Cpc*Apc^(Nh-1)*Bpc; 220 | Lpc=zeros(Nh,1); 221 | 222 | Dpc=eye(Nh); 223 | Dpc(Nh+1:Nh+1:(Nh-1)*Nh+Nh-1)=-1; 224 | Dpc(end)=0; 225 | % [V_c,D_c] = eig(Apc); 226 | % V_c=real(V_c); 227 | % D_c=real(D_c); 228 | %MPC parameters setup 229 | for k2=1:Nh 230 | if(k2==1) 231 | hpc(k2)=0; 232 | Lpc(1)=Cpc*Mc; 233 | else 234 | hpc(k2)=Cpc*Apc^(k2-2)*Bpc; %Apc^(k2-2) 235 | Lpc(k2)=Lpc(k2-1)+Cpc*Apc^(k2-1)*Mc; 236 | end 237 | Gpc( (k2-1)*size(Cpc,1)+1:k2*size(Cpc,1),: )=Cpc*Apc^k2; 238 | Hpc(k2,1:k2)=(hpc(k2:-1:1,1))'; 239 | end 240 | 241 | Fpc=hpc(2:end); 242 | Qpc=rpc*(Dpc'*We*Dpc)+Hpc'*We*Hpc; 243 | fpc=Hpc'*We*(Gpc*xkpc+Fpc*ukpc-refsig(k+1:k+Nh,1)+Lpc); 244 | xopt=-Qpc\fpc; 245 | uk(k+1)=xopt(1); 246 | % if k<100 247 | % if uk(k+1)<0 248 | % uk(k+1)=-4; 249 | % elseif uk(k+1)>4 250 | % uk(k+1)=4; 251 | % end 252 | % end 253 | end 254 | 255 | 256 | %tracking results and tracking error 257 | subplot(1,2,1), 258 | plot(yk(1:simN)),hold on,plot(refsig(1:simN)); 259 | subplot(1,2,2), 260 | plot(yk(1:simN)-refsig(1:simN)); 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | -------------------------------------------------------------------------------- /codes/readme: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /data/net12.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nopaddleboat/Koopman_operator_predictive_control/0be6c5579523bc5ed189c71736ec78d15f0febd3/data/net12.mat -------------------------------------------------------------------------------- /data/xstar_np.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nopaddleboat/Koopman_operator_predictive_control/0be6c5579523bc5ed189c71736ec78d15f0febd3/data/xstar_np.mat --------------------------------------------------------------------------------