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

Finally working regulator

parent f4cf941c
......@@ -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}
......
......@@ -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");
......
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