Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Martin Vítek
ZCU-nixie-clock
Commits
5a840f79
Commit
5a840f79
authored
Sep 23, 2020
by
Martin Vítek
Browse files
Finally working regulator
parent
f4cf941c
Changes
3
Hide whitespace changes
Inline
Side-by-side
Model/boost_model.tex
View file @
5a840f79
...
...
@@ -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
View file @
5a840f79
...
...
@@ -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.0
5
;
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
(
"d
elta
"
);
...
...
Model/my_dlqr.m
0 → 100644
View file @
5a840f79
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
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment