%% Inverted pendulum with uncertain frictional coefficient
%  
%  File: ccs_nonlin_ipend_local_LPV.m
%  Directory: 4_gyujtemegy/11_CCS/Modellek/inverse_pendulum/2018b_nonlin_pannon
%  Author: Peter Polcz (ppolcz@gmail.com) 
%  
%  Created on 2018. August 26.
%

%% Nonlinear model

% Time
syms t real

% State variables
syms position v phi omega real

% Input
syms u real

% Frictional coefficient
syms b real
b_lims = [0,1];

% State vector
x = [
    position
    v
    phi
    omega
    ];

n = numel(x);

% Known model parameters
M = 0.5;
m = 0.2;
l = 1;
g = 9.8;

q = 4*(M+m) - 3*m*cos(phi)^2;

f_sym = [
    v
    (4*m*l*sin(phi)*omega^2 - 1.5*m*g*sin(2*phi) -4*b*v) / q
    omega
    3*(-m*l*sin(2*phi)*omega^2 / 2 + (M+m)*g*sin(phi) + b*cos(phi)*v) / (l*q)
    ];

g_sym = [
    0
    4*l
    0
    -3*cos(phi)
    ] / (l*q);

%% Linearized (LPV) model around the unstable equilibrium point

A_sym = subs(jacobian(f_sym,x), x, [0;0;0;0]);
A_fh = matlabFunction(A_sym);

% Matrix A is an affine parameter dependent matrix: A = A0 + A1*b, where
A0 = A_fh(0);
A1 = A_fh(1) - A0;

B = double(subs(g_sym, x, [0;0;0;0]));

C = [
    1 0 0 0 
    0 0 1 0
    ];

D = [ 0 ; 0 ];

%% Static state feedback design with LMIs

Q = sdpvar(n);   % Q = inv(P)
N = sdpvar(1,n); % N = K*Q

AkQ = @(b) A_fh(b)*Q - B*N;

CONS = [
    AkQ(b_lims(1))' + AkQ(b_lims(1)) <= 0
    AkQ(b_lims(2))' + AkQ(b_lims(2)) <= 0
    Q >= eye(n)*0.001
    ];

sol = optimize(CONS)

N = value(N);
Q = value(Q);

K = N/Q

for b = linspace(b_lims(1),b_lims(2),11)
    Ak = A_fh(b) - B*K;
    fprintf('Eigenvalues of A(%3.1f)-B*K: [%5.2g,%5.2g,%5.2g,%5.2g]\n', b, eig(Ak))
end

