Teljes Matlab script kiegészítő függvényekkel.
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.
% 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);
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 ];
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
Problem
Name :
Objective sense : min
Type : CONIC (conic optimization problem)
Constraints : 14
Cones : 0
Scalar variables : 0
Matrix variables : 3
Integer variables : 0
Optimizer started.
Presolve started.
Linear dependency checker started.
Linear dependency checker terminated.
Eliminator - tries : 0 time : 0.00
Lin. dep. - tries : 1 time : 0.00
Lin. dep. - number : 0
Presolve terminated. Time: 0.00
Problem
Name :
Objective sense : min
Type : CONIC (conic optimization problem)
Constraints : 14
Cones : 0
Scalar variables : 0
Matrix variables : 3
Integer variables : 0
Optimizer - threads : 4
Optimizer - solved problem : the primal
Optimizer - Constraints : 14
Optimizer - Cones : 0
Optimizer - Scalar variables : 0 conic : 0
Optimizer - Semi-definite variables: 3 scalarized : 30
Factor - setup time : 0.00 dense det. time : 0.00
Factor - ML order time : 0.00 GP order time : 0.00
Factor - nonzeros before factor : 101 after factor : 105
Factor - dense dim. : 0 flops : 3.96e+03
ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME
0 1.8e+00 1.0e+00 1.0e+00 0.00e+00 -4.000000000e-03 0.000000000e+00 1.0e+00 0.00
1 4.2e-01 2.3e-01 4.7e-01 9.96e-01 -2.669752533e-03 0.000000000e+00 2.3e-01 0.01
2 7.4e-02 4.1e-02 1.9e-01 9.81e-01 -2.424807185e-03 0.000000000e+00 4.1e-02 0.01
3 1.5e-02 8.5e-03 7.7e-02 8.90e-01 -2.466832262e-03 0.000000000e+00 8.4e-03 0.01
4 1.6e-03 9.0e-04 2.4e-02 8.06e-01 -2.570715608e-04 0.000000000e+00 9.0e-04 0.01
5 2.7e-06 1.5e-06 9.4e-04 9.79e-01 -6.755147482e-07 0.000000000e+00 1.5e-06 0.01
6 1.4e-14 8.4e-15 7.8e-15 1.00e+00 -1.249225875e-15 0.000000000e+00 7.9e-15 0.01
Optimizer terminated. Time: 0.02
Interior-point solution summary
Problem status : PRIMAL_AND_DUAL_FEASIBLE
Solution status : OPTIMAL
Primal. obj: -1.2492258747e-15 nrm: 1e-12 Viol. con: 4e-13 barvar: 0e+00
Dual. obj: 0.0000000000e+00 nrm: 5e+00 Viol. con: 0e+00 barvar: 1e-14
Optimizer summary
Optimizer - time: 0.02
Interior-point - iterations : 6 time: 0.01
Basis identification - time: 0.00
Primal - iterations : 0 time: 0.00
Dual - iterations : 0 time: 0.00
Clean primal - iterations : 0 time: 0.00
Clean dual - iterations : 0 time: 0.00
Simplex - time: 0.00
Primal simplex - iterations : 0 time: 0.00
Dual simplex - iterations : 0 time: 0.00
Mixed integer - relaxations: 0 time: 0.00
sol =
struct with fields:
yalmiptime: 0.3347
solvertime: 0.0291
info: 'Successfully solved (MOSEK)'
problem: 0
K =
-1.7482 -7.6865 -56.9980 -19.5788
Eigenvalues of A(0.0)-B*K: [ -5.1, -5.1,-0.27, -2.2]
Eigenvalues of A(0.1)-B*K: [ -5.2, -5.2, -2.2,-0.28]
Eigenvalues of A(0.2)-B*K: [ -5.3, -5.3, -2.1,-0.28]
Eigenvalues of A(0.3)-B*K: [ -5.4, -5.4, -2.1,-0.29]
Eigenvalues of A(0.4)-B*K: [ -5.5, -5.5, -2.1,-0.29]
Eigenvalues of A(0.5)-B*K: [ -5.7, -5.7, -2, -0.3]
Eigenvalues of A(0.6)-B*K: [ -5.8, -5.8, -2,-0.31]
Eigenvalues of A(0.7)-B*K: [ -5.9, -5.9, -2,-0.31]
Eigenvalues of A(0.8)-B*K: [ -6, -6, -1.9,-0.32]
Eigenvalues of A(0.9)-B*K: [ -6.1, -6.1, -1.9,-0.33]
Eigenvalues of A(1.0)-B*K: [ -6.9, -5.5, -1.8,-0.34]