Tartalomjegyzék

ccs_nonline_Polcz_Ex1c_feedbacklin

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

File: ccs_nonline_Polcz_Ex1c_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)
    -x2*sin(x1)
    x1+x3
    x1*x2/(x4+1)
    ];

g = [
    1/(x4+1)
    0
    x2/(x4+1)
    0
    ];

h = x3-x1*x2;

phi3 = -x1*x2^2+x3*x2;
phi4 = x4;

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

ueq = 1;
f = f + g*ueq;

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

% xeq = [
%     1
%     0
%     0
%     -1/sin(1)-1
%     ];

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

\begin{align}\Phi(x) = \left(\begin{array}{c} x_{3}-x_{1}\,x_{2}\\ x_{1}+x_{3}\\ x_{2}\,x_{3}-x_{1}\,{x_{2}}^2\\ 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 = struct2cell(solve(z-Phi, x));
Phi_inv = [ Phi_inv{:} ]';
Phi_inv_fh = matlabFunction(Phi_inv, 'vars', {z});

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

Coordinate transformation

b = simplify(Lie(f, Lie(f,h)));
a = simplify(Lie(g, Lie(f,h)));
q3 = simplify(Lie(f, Phi(3)));
q4 = simplify(Lie(f, Phi(4)));

\begin{align}b(x) = x_{1}+x_{3}+\frac{x_{2}}{x_{4}+1}+\frac{1}{x_{4}+1}+x_{1}\,\sin\left(x_{1}\right),~ a(x) = \frac{x_{2}+1}{x_{4}+1},~ q_3(x) = x_{2}\,\left(x_{1}+x_{3}-x_{3}\,\sin\left(x_{1}\right)+x_{1}\,x_{2}\,\sin\left(x_{1}\right)\right),~ 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) = \frac{1}{z_{4}+1}+\frac{{z_{1}}^2+z_{2}\,z_{3}}{z_{1}+z_{3}}-\frac{z_{1}\,\left(z_{1}-z_{2}\right)}{z_{1}+z_{3}}+\frac{z_{3}}{z_{1}\,\left(z_{4}+1\right)}+\frac{z_{1}\,\sin\left(\frac{z_{1}\,\left(z_{1}-z_{2}\right)}{z_{1}+z_{3}}\right)\,\left(z_{1}-z_{2}\right)}{z_{1}+z_{3}},~ a(z) = \frac{\frac{z_{3}}{z_{1}}+1}{z_{4}+1},~ q_3(z) = \frac{z_{3}\,\left(z_{2}+z_{1}\,\sin\left(\frac{z_{1}\,\left(z_{1}-z_{2}\right)}{z_{1}+z_{3}}\right)\right)}{z_{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}\\ \frac{\frac{z_{3}}{z_{1}}+1}{z_{4}+1}\,u+\frac{1}{z_{4}+1}+\frac{{z_{1}}^2+z_{2}\,z_{3}}{z_{1}+z_{3}}-\frac{z_{1}\,\left(z_{1}-z_{2}\right)}{z_{1}+z_{3}}+\frac{z_{3}}{z_{1}\,\left(z_{4}+1\right)}+\frac{z_{1}\,\sin\left(\frac{z_{1}\,\left(z_{1}-z_{2}\right)}{z_{1}+z_{3}}\right)\,\left(z_{1}-z_{2}\right)}{z_{1}+z_{3}}\\ \frac{z_{3}\,\left(z_{2}+z_{1}\,\sin\left(\frac{z_{1}\,\left(z_{1}-z_{2}\right)}{z_{1}+z_{3}}\right)\right)}{z_{1}}\\ -\frac{z_{3}\,\left(z_{1}-z_{2}\right)}{\left(z_{1}+z_{3}\right)\,\left(z_{4}+1\right)} \end{array}\right)\end{align}

Linearizing feedback design

u_fdb = a \ (-b + v);

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

Closed loop

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

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

Zero dynamics

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

The zero dynamics is stable in the origin.

\begin{align}\dot z_3 = z_{3}\,\sin\left(\frac{1}{z_{3}-1}\right)\end{align} \begin{align}\dot z_4 = \frac{z_{3}}{\left(z_{3}-1\right)\,\left(z_{4}+1\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)