% Hinf - Filter (Zhaojian Li et al.)
clear all
clc
% Parameter
m_c = 290; % Aufbaumasse in kg
m_w = 60; % Radmasse in kg
c_w = 19000; % Radsteifigkeit in N/m
c_c = 16800; % Aufbausteifigkeit in N/m
d_c = 200; % Aufbaudaempfung in Ns/m
%% state space
% state 1: z_t
% state 2: z_w_dot
% state 3: z_cw
% state 4: z_c_dot
alpha = 0.1;
A = [0 1 0 0;...
-c_w/m_w -d_c/m_w c_c/m_w d_c/m_w;...
0 -1 0 1;...
0 d_c/m_c -c_c/m_c -d_c/m_c];
B = [0; 1/m_w; 0; -1/m_c];
B_r = [-1; 0; 0; 0];
B_w = [-alpha; 0; 0; 0];
C_0 = [0 1 0 0;...
0 0 1 0;...
0 0 0 1];
D_0 = [0.001; 0.001; 0.001];
D_1 = 0.001;
D_r = 0.0001;
% augmented system
A_a = [A, B_r; zeros(1,4), zeros(1,1)];
B_a = [B_w; D_r];
C_a0 = [C_0, zeros(3,1); zeros(1,5)];
C_a1 = [zeros(3,5); zeros(1,4), 1];
D_a = [D_0; D_1];
E_a = [eye(4), zeros(4,1)];
%% LMI
yalmip('clear');
X = sdpvar(5,5,'symmetric');
Y = sdpvar(5,5,'symmetric');
AA = sdpvar(5,5,'full');
BB = sdpvar(5,4,'full');
CC = sdpvar(4,5,'full');
Q2 = sdpvar(1,1,'symmetric');
Q1 = sdpvar(5,5,'symmetric');
Q1inv = sdpvar(5,5,'symmetric');
gama = 0.5;
tauM = 0.5;
taum = 0.05;
% LMI (34)
T1 = [A_a*X + X*A_a', A_a + AA';...
A_a' + AA, Y*A_a + A_a'*Y + BB*(C_a0+C_a1) + (C_a0+C_a1)'*BB'];
T2 = [B_a; Y*B_a + BB*D_a];
T3 = [X*E_a' - CC'; E_a'];
T4 = [zeros(5,5), zeros(5,1), X; BB*C_a1*A_a, BB*C_a1*B_a, eye(5,5)];
DD = [Q1, zeros(5,6); zeros(1,5),Q2,zeros(1,5); zeros(5,6), Q1inv];
L1 = [T1, T2, T3, T4;...
T2', -gama^2+tauM*Q2, zeros(1,4), zeros(1,11);...
T3', zeros(4,1), -eye(4,4), zeros(4,11);...
T4', zeros(11,1), zeros(11,4), -(1/tauM)*DD]
% LMI (32)
L2 = [X, eye(5); eye(5), Y];
constraints = [L1<=0, -L2<=0, -X<=0, -Y<=0, -Q1<=0, -Q2<=0, Q1*Q1inv==eye(5)];
options = sdpsettings('verbose',1,'solver','sedumi');
solution = optimize(constraints,[],options)
X = double(X)
Y = double(Y)
AA = double(AA)
BB = double(BB)
CC = double(CC)