Finally working regulator

Model/boost_model.tex

@@ -11,6 +11,7 @@
 \usepackage{physics}
 \usepackage{amsmath}
 \usepackage{nicematrix}
+\usepackage[normalem]{ulem}
 %\usepackage{showframe}
 
 \sisetup{range-phrase = \text{--}, group-separator = \text{\,}, group-four-digits = true, output-decimal-marker = {,}}
@@ -33,6 +34,8 @@ fontsize=\footnotesize,
 linenos=false,
 }
 
+\newcommand{\msout}[1]{\text{\sout{\ensuremath{#1}}}}
+
 
 \begin{document}
 
@@ -272,13 +275,13 @@ $A_1$, $B_1$ - Q24 v uzavřeném stavu, $A_2$, $B_2$ - Q24 v otevřeném stavu.
     \end{bmatrix}
     +
     \begin{bmatrix}
-        \boldsymbol{B}_{1,1} & 0 & 0 & 0 \\
-        \boldsymbol{B}_{2,1} & 0 & 0 & 0 \\
-        \boldsymbol{B}_{3,1} & 0 & 0 & 0 \\
-        0 & 0 & 0 & -\mathrm{d}t
+        \boldsymbol{B}_{1,1} & 0  \\
+        \boldsymbol{B}_{2,1} & 0  \\
+        \boldsymbol{B}_{3,1} & 0  \\
+        0 & -\mathrm{d}t
     \end{bmatrix}
     \begin{bmatrix}
-        D \\ 0 \\ 0 \\ u_{c,t}^*
+        D \\ u_{c,t}^*
     \end{bmatrix}
 \end{align}
 
@@ -290,14 +293,11 @@ $A_1$, $B_1$ - Q24 v uzavřeném stavu, $A_2$, $B_2$ - Q24 v otevřeném stavu.
     \begin{bmatrix}
         i_{l,t} \\ u_{c,t} \\ U_{in,t} \\ \Delta_{t}
     \end{bmatrix}
-    - \boldsymbol{K}u_{c,t}^* \\
-    \boldsymbol{L} &= dlqr(\boldsymbol{A}, \boldsymbol{B}, \boldsymbol{Q}, \boldsymbol{N}, \boldsymbol{R}) \\
-    \boldsymbol{K} &= \{-\boldsymbol{C_w}[\boldsymbol{I}-(\boldsymbol{A}-\boldsymbol{BL})]^{-1}\boldsymbol{B}\}^{-1} \\
+    - K\cdot u_{c,t}^* \\
+    [\boldsymbol{L},K] &= dlqr(\boldsymbol{A}, \boldsymbol{B}, \boldsymbol{Q}, \boldsymbol{N}, \boldsymbol{R}) \\
+    \msout{K} &= \{-\boldsymbol{C_w}[\boldsymbol{I}-(\boldsymbol{A}-\boldsymbol{BL})]^{-1}\boldsymbol{B}\}^{-1} \\
     \boldsymbol{C}_w &= \begin{bmatrix}
-        1 & 0 & 0 & 0 \\
-        0 & 1 & 0 & 0 \\
-        0 & 0 & 1 & 0 \\
-        0 & 0 & 0 & 1
+        0 & 1 & 0 & 0
     \end{bmatrix}
 \end{align}
 

Model/model_sim.m

@@ -50,46 +50,53 @@ dsys.A = [dssm.A(1,1), dssm.A(1,2), dssm.A(1,3), 0;
           dssm.A(2,1), dssm.A(2,2), dssm.A(2,3), 0;
           dssm.A(3,1), dssm.A(3,2), dssm.A(3,3), 0;
           0, Ts, 0, 1];
-dsys.B = [dssm.B(1,1);
-          dssm.B(2,1);
-          dssm.B(3,1);
-          0];
+dsys.B = [dssm.B(1,1), 0;
+          dssm.B(2,1), 0;
+          dssm.B(3,1), 0;
+          0, -Ts];
 dsys.C = eye(size(dsys.B, 1));
 dsys.D = zeros(size(dsys.B));
 
 
 %Simulate system (open loop)
-i = 1;
-i0 = 0.75;
-u0 = Uin;
-delta0 = 0.9;
-tmax = 0.1;
-
-sim.vars(:,1) = [i0, u0, Uin, delta0];
-sim.t(i) = 0;
-    
-for t=dssm.Ts:dssm.Ts:tmax
-    sim.vars(:, i+1) = dsys.A*sim.vars(:, i) + dsys.B*(1-d);
-    sim.t(i+1) = t;
-
-    i = i+1;
-end
+% i = 1;
+% i0 = 0.75;
+% u0 = Uin;
+% delta0 = 0.9;
+% tmax = 0.1;
+% 
+% sim.vars(:,1) = [i0, u0, Uin, delta0];
+% sim.t(i) = 0;
+%     
+% for t=dssm.Ts:dssm.Ts:tmax
+%     sim.vars(:, i+1) = dsys.A*sim.vars(:, i) + dsys.B*(1-d);
+%     sim.t(i+1) = t;
+% 
+%     i = i+1;
+% end
 
 %Plot it
-plot_sim(sim, "Open loop response", 1);
+% plot_sim(sim, "Open loop response", 1);
 
 
 % Regulator
 % Q = diag([0, 0.5, 0, 0.5]);
-Q = 1e-5*eye(4);
+% Q = eye(4);
+Q = diag([100, 0.5, 1e-8, 10]);
 R = 1;
 N = zeros(size(Q, 1),1);
-L = dlqr(dsys.A, dsys.B, Q, R, N);
+% L = dlqr(dsys.A, dsys.B, Q, R, N);
+[Lfull,V,LL] = my_dlqr(dsys.A, dsys.B(:,1), Q, R, N, 0, 0);
+
+Cw = [0,1,0,0];
+% p = inv(0.99*eye(4) - (dsys.A - dsys.B*L));
+% K = inv(-0.99*Cw * p * dsys.B);
 
-Cw = eye(4);
-p = inv(eye(4) - (dsys.A - dsys.B*L));
-K = inv(-Cw * p * dsys.B);
+% p = eye(4) - (dsys.A - dsys.B*L);
+% K = inv(-Cw * (p\dsys.B));
 
+L = Lfull(1:4);
+K = Lfull(6);
 
 % Simulate system (regulator)
 i = 1;
@@ -98,14 +105,14 @@ u0 = 12;
 delta0 = 0;
 delta_max = 180;
 d0 = 0.95;
-tmax = 5;
+tmax = 0.05;
 
-sim_reg.vars(:,1) = [i0, u0, delta0];
+sim_reg.vars(:,1) = [i0, u0, Uin, delta0];
 sim_reg.t(i) = 0;
-sim_reg.d(:,i) = [1-d0; Uin; uc_req];
+sim_reg.d(i) = 1-d0;
     
 for t=Ts:Ts:tmax
-    sim_reg.d(:,i+1) = -L*sim_reg.vars(:, i) - K*[sim_reg.d(1,i); Uin; uc_req];
+    sim_reg.d(:,i+1) = -L*sim_reg.vars(:, i) - K*uc_req;
 
     if(sim_reg.d(1,i+1) > 0.99)
         sim_reg.d(1,i+1) = 0.99;
@@ -114,11 +121,10 @@ for t=Ts:Ts:tmax
         sim_reg.d(1,i+1) = 0;
     end
     
-    %sim_reg.vars(:, i+1) = reg.A*sim_reg.vars(:, i) + reg.B*[1-d; Uin; uc_req];
-    sim_reg.vars(:, i+1) = dsys.A*sim_reg.vars(:, i) + dsys.B*[sim_reg.d(1,i+1); Uin; uc_req];
+    sim_reg.vars(:, i+1) = dsys.A*sim_reg.vars(:, i) + dsys.B*[sim_reg.d(i+1); uc_req];
     
-    if (abs(sim_reg.vars(3, i+1)) > delta_max)
-        sim_reg.vars(3, i+1) = sign(sim_reg.vars(3, i+1)) * delta_max;
+    if (abs(sim_reg.vars(4, i+1)) > delta_max)
+        sim_reg.vars(4, i+1) = sign(sim_reg.vars(4, i+1)) * delta_max;
     end
     
     sim_reg.t(i+1) = t;
@@ -141,11 +147,11 @@ title("uc");
     
 subplot(1,4,3);
 plot(sim_reg.t, sim_reg.vars(3,:));
-title("delta");
+title("Uin");
 
 subplot(1,4,4);
-plot(sim_reg.t, sim_reg.d(1,:));
-title("d");
+plot(sim_reg.t, sim_reg.vars(4,:));
+title("delta");
 
 
 

Model/my_dlqr.m

+function [K,V,LL]  = my_dlqr(A, B, Q, R, N, h, sqV0)
+% function computes LQ controller for tracking of the reference state z
+
+nargin = 0;
+dx=size(A,1);
+du=size(B,2); 
+if nargin<7
+    sqV=1e-5*eye(dx+dx);
+else
+    sqV = sqV0;
+end
+if nargin<6
+    h = 1000;
+end
+sqQ = chol(Q);
+sqR = chol(R);
+
+LL = zeros((dx+dx)*du,h);
+
+for i=h:-1:1
+M=[sqR, zeros(du,dx), zeros(du,dx);...
+    zeros(dx,du), sqQ, -sqQ;...
+    sqV*[B A zeros(dx,dx); zeros(dx,du+dx), eye(dx) ]];
+     
+%    Y = qr(M,0); broken qr?
+    Y = chol(M'*M);
+    Yuu = Y(1:du,1:du);
+    Yxu = Y(1:du,du+1:end);
+    Yxx = Y(du+1:du+2*dx, du+1:du+2*dx);
+    
+    L=-inv(Yuu)*Yxu;
+    sqV = Yxx;
+    LL(:,i) = L(:);
+end
+
+Vf = sqV'*sqV;
+
+K = -L;
+V = Vf(1:dx,1:dx);
\ No newline at end of file