Tartalomjegyzék

ccs_nonline_Polcz_Ex1d_feedbacklin

Teljes Matlab script kiegészítő függvényekkel.

File: ccs_nonline_Polcz_Ex1d_feedbacklin.m
Directory: 4_gyujtemegy/11_CCS/_2_nonlin-pannon/2018
Author: Peter Polcz (ppolcz@gmail.com)

Created on 2018. August 13.

Model description

syms t u v real
syms x1 x2 x3 x4 real
syms z1 z2 z3 z4 real

x = [x1 ; x2 ; x3 ; x4];
z = [z1 ; z2 ; z3 ; z4];

d = @(g) jacobian(g,x);
Lie = @(f,g) d(g)*f;
br = @(f,g) d(g)*f - d(f)*g;

f = [
    x1*sin(x1)
    x1+x4
    x1+x3
    x1*x2/(x4+1)
    ];

g = [
    1
    0
    exp(x4)-1
    0
    ];

h = x2;

phi3 = x1-exp(x4)*x1+x3;
phi4 = x4;

Phi = [
    h
    Lie(f,h)
    phi3
    phi4
    ];
Phi_fh = matlabFunction(Phi, 'vars', {x});

ueq = 0;
f = f + g*ueq;

sol = struct2cell(solve(f,x));
xeq = [ sol{:} ]';
zeq = Phi_fh(xeq);

% Quick check: should be zero
% Lie(g,phi3)

\begin{align}\Phi(x) = \left(\begin{array}{c} x_{2}\\ x_{1}+x_{4}\\ x_{1}+x_{3}-x_{1}\,{\mathrm{e}}^{x_{4}}\\ x_{4} \end{array}\right)\end{align}

Quick check

\begin{align}L_g \phi_3(x) = 0\end{align}

$\phi_3(x)$ is given by Isidori, but anyone can check that this is indeed a solution for the PDE. This solution can also be computed by Mathematica.

Phi_inv = [
    z2-z4
    z1
    -z2+exp(z4)*z2+z3+z4-exp(z4)*z4
    z4
    ];

simplify(subs(Phi,x,Phi_inv))
simplify(subs(Phi_inv,z,Phi))

Phi_inv_fh = matlabFunction(Phi_inv, 'vars', {z});

Output:

ans =
 z1
 z2
 z3
 z4
ans =
 x1
 x2
 x3
 x4

\begin{align}\Phi^{-1}(z) = \left(\begin{array}{c} z_{2}-z_{4}\\ z_{1}\\ z_{3}-z_{2}+z_{4}+z_{2}\,{\mathrm{e}}^{z_{4}}-z_{4}\,{\mathrm{e}}^{z_{4}}\\ z_{4} \end{array}\right)\end{align} \begin{align}b(x) = x_{1}\,\sin\left(x_{1}\right)+\frac{x_{1}\,x_{2}}{x_{4}+1},~ a(x) = 1,~ q_3(x) = x_{1}+x_{3}-x_{1}\,\sin\left(x_{1}\right)\,\left({\mathrm{e}}^{x_{4}}-1\right)-\frac{{x_{1}}^2\,x_{2}\,{\mathrm{e}}^{x_{4}}}{x_{4}+1},~ q_4(x) = \frac{x_{1}\,x_{2}}{x_{4}+1}\end{align}

b = simplify(subs(b, x, Phi_inv));
a = simplify(subs(a, x, Phi_inv));
q3 = simplify(subs(q3, x, Phi_inv));
q4 = simplify(subs(q4, x, Phi_inv));

\begin{align}b(z) = \sin\left(z_{2}-z_{4}\right)\,\left(z_{2}-z_{4}\right)+\frac{z_{1}\,\left(z_{2}-z_{4}\right)}{z_{4}+1},~ a(z) = 1,~ q_3(z) = z_{3}+z_{2}\,{\mathrm{e}}^{z_{4}}-z_{4}\,{\mathrm{e}}^{z_{4}}-\sin\left(z_{2}-z_{4}\right)\,\left({\mathrm{e}}^{z_{4}}-1\right)\,\left(z_{2}-z_{4}\right)-\frac{z_{1}\,{\mathrm{e}}^{z_{4}}\,{\left(z_{2}-z_{4}\right)}^2}{z_{4}+1}\end{align}

dz = simplify([ z2 ; collect(b+a*u,u) ; q3 ; q4 ]);

System dynamics in the new coordinates

\begin{align}\dot z = \left(\begin{array}{c} z_{2}\\ u+\sin\left(z_{2}-z_{4}\right)\,\left(z_{2}-z_{4}\right)+\frac{z_{1}\,\left(z_{2}-z_{4}\right)}{z_{4}+1}\\ z_{3}+z_{2}\,{\mathrm{e}}^{z_{4}}-z_{4}\,{\mathrm{e}}^{z_{4}}-\sin\left(z_{2}-z_{4}\right)\,\left({\mathrm{e}}^{z_{4}}-1\right)\,\left(z_{2}-z_{4}\right)-\frac{z_{1}\,{\mathrm{e}}^{z_{4}}\,{\left(z_{2}-z_{4}\right)}^2}{z_{4}+1}\\ \frac{z_{1}\,\left(z_{2}-z_{4}\right)}{z_{4}+1} \end{array}\right)\end{align}

Linearizing feedback design

u_fdb = a \ (-b + v);

\begin{align}u = v-\sin\left(z_{2}-z_{4}\right)\,\left(z_{2}-z_{4}\right)-\frac{z_{1}\,\left(z_{2}-z_{4}\right)}{z_{4}+1}\end{align}

Closed loop

dz_fdb = simplify(subs(dz, u, u_fdb));

\begin{align}\dot z = \left(\begin{array}{c} z_{2}\\ v\\ z_{3}+z_{2}\,{\mathrm{e}}^{z_{4}}-z_{4}\,{\mathrm{e}}^{z_{4}}-\sin\left(z_{2}-z_{4}\right)\,\left({\mathrm{e}}^{z_{4}}-1\right)\,\left(z_{2}-z_{4}\right)-\frac{z_{1}\,{\mathrm{e}}^{z_{4}}\,{\left(z_{2}-z_{4}\right)}^2}{z_{4}+1}\\ \frac{z_{1}\,\left(z_{2}-z_{4}\right)}{z_{4}+1} \end{array}\right)\end{align}

Zero dynamics

Zero_dynamics = simplify(subs(dz_fdb(3:4), [z1 ; z2 ; v], [zeq(1:2) ; 0]));

J = jacobian(Zero_dynamics, [z3;z4]);
Az = subs(J,z,zeq);
eig(Az)

Output:

ans =
 0
 1

The zero dynamics is stable in the origin.

\begin{align}\dot \zeta = \left(\begin{array}{c} z_{3}-z_{4}\,{\mathrm{e}}^{z_{4}}-z_{4}\,\sin\left(z_{4}\right)\,\left({\mathrm{e}}^{z_{4}}-1\right)\\ 0 \end{array}\right)\end{align}

Linearized dynamics

Overall dynamics without the zero dynamics.

A = [
    0 1
    0 0
    ];

B = [
    0
    1
    ];

C = [ 1 0 ];

sys = ss(A,B,C,0);

Pole placement controller

Download model here: Polcz_Ex1b_state_feedback.slx

% State feedback gain designed by LQR
K = lqr(A,B,0.1*eye(2),1);

EXTRACT-IO

pcz_print_model('Polcz_Ex1b_state_feedback', 'simulate', true)

Published with MATLAB® R2017b, and sanitarized by BeautifulSoup (Python script using BSoup is written by Polcz).