Commit 5a840f79 authored by Martin Vítek's avatar Martin Vítek

Finally working regulator

parent f4cf941c
...@@ -11,6 +11,7 @@ ...@@ -11,6 +11,7 @@
\usepackage{physics} \usepackage{physics}
\usepackage{amsmath} \usepackage{amsmath}
\usepackage{nicematrix} \usepackage{nicematrix}
\usepackage[normalem]{ulem}
%\usepackage{showframe} %\usepackage{showframe}
\sisetup{range-phrase = \text{--}, group-separator = \text{\,}, group-four-digits = true, output-decimal-marker = {,}} \sisetup{range-phrase = \text{--}, group-separator = \text{\,}, group-four-digits = true, output-decimal-marker = {,}}
...@@ -33,6 +34,8 @@ fontsize=\footnotesize, ...@@ -33,6 +34,8 @@ fontsize=\footnotesize,
linenos=false, linenos=false,
} }
\newcommand{\msout}[1]{\text{\sout{\ensuremath{#1}}}}
\begin{document} \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. ...@@ -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} \end{bmatrix}
+ +
\begin{bmatrix} \begin{bmatrix}
\boldsymbol{B}_{1,1} & 0 & 0 & 0 \\ \boldsymbol{B}_{1,1} & 0 \\
\boldsymbol{B}_{2,1} & 0 & 0 & 0 \\ \boldsymbol{B}_{2,1} & 0 \\
\boldsymbol{B}_{3,1} & 0 & 0 & 0 \\ \boldsymbol{B}_{3,1} & 0 \\
0 & 0 & 0 & -\mathrm{d}t 0 & -\mathrm{d}t
\end{bmatrix} \end{bmatrix}
\begin{bmatrix} \begin{bmatrix}
D \\ 0 \\ 0 \\ u_{c,t}^* D \\ u_{c,t}^*
\end{bmatrix} \end{bmatrix}
\end{align} \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. ...@@ -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} \begin{bmatrix}
i_{l,t} \\ u_{c,t} \\ U_{in,t} \\ \Delta_{t} i_{l,t} \\ u_{c,t} \\ U_{in,t} \\ \Delta_{t}
\end{bmatrix} \end{bmatrix}
- \boldsymbol{K}u_{c,t}^* \\ - K\cdot u_{c,t}^* \\
\boldsymbol{L} &= dlqr(\boldsymbol{A}, \boldsymbol{B}, \boldsymbol{Q}, \boldsymbol{N}, \boldsymbol{R}) \\ [\boldsymbol{L},K] &= 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} \\ \msout{K} &= \{-\boldsymbol{C_w}[\boldsymbol{I}-(\boldsymbol{A}-\boldsymbol{BL})]^{-1}\boldsymbol{B}\}^{-1} \\
\boldsymbol{C}_w &= \begin{bmatrix} \boldsymbol{C}_w &= \begin{bmatrix}
1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0
0 & 1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix} \end{bmatrix}
\end{align} \end{align}
......
...@@ -50,46 +50,53 @@ dsys.A = [dssm.A(1,1), dssm.A(1,2), dssm.A(1,3), 0; ...@@ -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(2,1), dssm.A(2,2), dssm.A(2,3), 0;
dssm.A(3,1), dssm.A(3,2), dssm.A(3,3), 0; dssm.A(3,1), dssm.A(3,2), dssm.A(3,3), 0;
0, Ts, 0, 1]; 0, Ts, 0, 1];
dsys.B = [dssm.B(1,1); dsys.B = [dssm.B(1,1), 0;
dssm.B(2,1); dssm.B(2,1), 0;
dssm.B(3,1); dssm.B(3,1), 0;
0]; 0, -Ts];
dsys.C = eye(size(dsys.B, 1)); dsys.C = eye(size(dsys.B, 1));
dsys.D = zeros(size(dsys.B)); dsys.D = zeros(size(dsys.B));
%Simulate system (open loop) %Simulate system (open loop)
i = 1; % i = 1;
i0 = 0.75; % i0 = 0.75;
u0 = Uin; % u0 = Uin;
delta0 = 0.9; % delta0 = 0.9;
tmax = 0.1; % tmax = 0.1;
%
sim.vars(:,1) = [i0, u0, Uin, delta0]; % sim.vars(:,1) = [i0, u0, Uin, delta0];
sim.t(i) = 0; % sim.t(i) = 0;
%
for t=dssm.Ts:dssm.Ts:tmax % for t=dssm.Ts:dssm.Ts:tmax
sim.vars(:, i+1) = dsys.A*sim.vars(:, i) + dsys.B*(1-d); % sim.vars(:, i+1) = dsys.A*sim.vars(:, i) + dsys.B*(1-d);
sim.t(i+1) = t; % sim.t(i+1) = t;
%
i = i+1; % i = i+1;
end % end
%Plot it %Plot it
plot_sim(sim, "Open loop response", 1); % plot_sim(sim, "Open loop response", 1);
% Regulator % Regulator
% Q = diag([0, 0.5, 0, 0.5]); % 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; R = 1;
N = zeros(size(Q, 1),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 = eye(4) - (dsys.A - dsys.B*L);
p = inv(eye(4) - (dsys.A - dsys.B*L)); % K = inv(-Cw * (p\dsys.B));
K = inv(-Cw * p * dsys.B);
L = Lfull(1:4);
K = Lfull(6);
% Simulate system (regulator) % Simulate system (regulator)
i = 1; i = 1;
...@@ -98,14 +105,14 @@ u0 = 12; ...@@ -98,14 +105,14 @@ u0 = 12;
delta0 = 0; delta0 = 0;
delta_max = 180; delta_max = 180;
d0 = 0.95; 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.t(i) = 0;
sim_reg.d(:,i) = [1-d0; Uin; uc_req]; sim_reg.d(i) = 1-d0;
for t=Ts:Ts:tmax 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) if(sim_reg.d(1,i+1) > 0.99)
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 ...@@ -114,11 +121,10 @@ for t=Ts:Ts:tmax
sim_reg.d(1,i+1) = 0; sim_reg.d(1,i+1) = 0;
end 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(i+1); uc_req];
sim_reg.vars(:, i+1) = dsys.A*sim_reg.vars(:, i) + dsys.B*[sim_reg.d(1,i+1); Uin; uc_req];
if (abs(sim_reg.vars(3, i+1)) > delta_max) if (abs(sim_reg.vars(4, i+1)) > delta_max)
sim_reg.vars(3, i+1) = sign(sim_reg.vars(3, i+1)) * delta_max; sim_reg.vars(4, i+1) = sign(sim_reg.vars(4, i+1)) * delta_max;
end end
sim_reg.t(i+1) = t; sim_reg.t(i+1) = t;
...@@ -141,11 +147,11 @@ title("uc"); ...@@ -141,11 +147,11 @@ title("uc");
subplot(1,4,3); subplot(1,4,3);
plot(sim_reg.t, sim_reg.vars(3,:)); plot(sim_reg.t, sim_reg.vars(3,:));
title("delta"); title("Uin");
subplot(1,4,4); subplot(1,4,4);
plot(sim_reg.t, sim_reg.d(1,:)); plot(sim_reg.t, sim_reg.vars(4,:));
title("d"); title("delta");
......
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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment