Регрессионная идентификация линейных непрерывных систем управления

function LAB122;
clc,close all
%% Параметры инерционных звеньев
k1 = 1; T1 = 1;
k2 = 2; T2 = 2;
k3 = 3; T3 = 0.5;
%%%------------------------------
%% Матрицы непрерывной системы
global A B 
A = [-1/T1, 0, 0;
k2/T2, -1/T2, 0;
0, k3/T3, -1/T3];
B = [k1/T1; 0; 0];
C = [0, 0, 1]
D = 0;
%%%------------------------------
%% Размерность системы управления
n = length(A);
%% Размерность управления
r = size(B, 2);
%% Размерность выхода
m = size(C, 1);
 
%% Преобразование к дискретной системе
Ts = 0.01; % шаг квантования
%% Расчет матрицы Ad
Ad = expm(A*Ts);
%% Расчет матрицы Bd
if abs(det(A)) < 1e-10
Bd = inv(A)*(Ad - eye(size(A)))*B;
 
else
syms tau
 
Bd2 = int(expm(-A*tau)*B, tau, 0, Ts);
Bd = double(Bd2);
end
%%%------------------------------
 
%% Время наблюдений дискретной системы
Kend = 100; %% число шагов квантования
%% Вид входного воздействия
% % U(t) = exp(-0.5*t).*cos(2*t); 
 
%% Решение разностного уравнения
X0 = zeros(length(Ad),1);
Xk = zeros(length(Ad),Kend);

for k = 1 : Kend
sm = zeros(length(Ad), 1);
   for J = 0 : k-1
sm = sm + Ad^(k-1-J)*Bd*exp(-0.5*J*Ts)*cos(2*J*Ts);
   end
Xk(:,k) = (Ad^k)*X0 + sm;
Uk(1:r,k) = exp(-0.5*k*Ts)*cos(2*k*Ts);
end

%% Массив выходных значений системы 
Ykd = C*Xk; %% как результат измерения

%% Хранение массива KSI
KSI = [Xk(:, 2 : end)]';
 
%% Хранение массива Wk
Wk = [(Xk(:, 1 : end-1))',(Uk(1 : end-1))'];
%% Регрессионная оценка параметров дискретной системы
if abs(det(Wk'*Wk)) < 1e-10
F = (pinv(Wk'*Wk)*Wk'*KSI)'; 
 
else
F = (inv(Wk'*Wk)*Wk'*KSI)' ;   
end

%% Идентифицированные матрицы
Adr = F(:, 1 : n);
Bdr = F(:, n+1 : end);
 
%% Хранение массива Yk
Yk = Ykd(:, 2:end);
 
%% Хранение массива YWk
YWk = Xk(:, 1:end-1);
 
%% Регрессионная оценка матриц дискретной системы
if abs(det(YWk*YWk')) < 1e-10
FY = (pinv(YWk*YWk')*YWk*Yk'); 
 
else
FY = (inv(YWk*YWk')*YWk*Yk'); 
end
 
%% Идентифицированная матрица выхода
Cdr = FY' %% для дискретной системы
Creg = Cdr %% для непрерывной системы
 
%% Обратное преобразование - оценка матриц А, В
global Areg Breg
Areg = logm(Adr)/Ts;
 
if Ts <= 1e-1 
Breg = Bdr/Ts;
 
elseif Ts > 1e-1 & abs(det(Areg)) <= 1e-10 
Breg = inv(inv(Areg)*(Adr - eye(size(Adr))))*Bdr;    
 
else
    sd2 = ss(Adr, Bdr, C, D, Ts);
        if abs(min(eig(Adr)) ) < 1e-3
    sreg = d2c(sd2,'tustin');
        else
sreg = d2c(sd2,'zox');        
        end
 [Areg, Breg, C, D] = ssdata(sreg);    
end
 
%%% Верификация модели
%%% Анализ систем при ступенчатом воздействии
global Um
Um = 12;
Xreg0 = zeros(length(Areg), 1);
X0 = zeros(length(A), 1);
T = [0, 12];
[treg, Xreg] = ode23(@fun, T, Xreg0);
Yreg = Xreg*Creg'; %% выход системы
 
[t, X] = ode23(@fun0, T, X0);
Y = X*C'; %% выход системы
 
h12 = figure(1);
set(h12, 'name','Верификация при ступенчатом воздействии');
line(treg, Yreg, 'linew', 2);
line(t, Y, 'marker', 'o','color','r');
str1 = ...
 '\bf\fontsize{11}\fontname{times}\itY_r_e_g\rm\bf(\itt\rm\bf)';
str2 ='\bf\fontsize{11}\fontname{times}\itY\rm\bf(\itt\rm\bf)';
legend(str1, str2, 'location','best');
 
title('\bf\fontsize{10}\fontname{times}Результат верификации двух моделей по выходу');
xlabel('\bf\fontsize{12}\fontname{times}\it -  -  -  -  -  -  -  t -  -  -  -  -  -  -');
ylabel('\bf\fontsize{12}\fontname{times}\it Y\rm\bf(\itt\rm\bf) '); grid on

 
%%%------------------------------
function f = fun0(t,X)
%% Для заданной системы
global A B Um
f = A*X + B*Um;
 
function f = fun(tr, Xr)
%% Для идентифицированной системы 
global Areg Breg Um
f = Areg*Xr + Breg*Um;

C =
     0     0     1
Cdr =
    0.0003    0.0592    0.9802
Creg =
    0.0003    0.0592    0.9802