%% ЛР4. Вариант 1. Полный скрипт (пункты 1–6)
clc; clear; close all;

%% --- Исходные данные ---
A = [-39   -71   -23;
      23    40    13;
     -6.6 -10.7  -3.3];

B = [0.228; -0.127; 0.038];
C = [28000 84000 112000];
D = 0;

T0_list = [0.1 1 10];          % три периода дискретизации
p_cl    = [0.5 0.6 0.7];       % желаемые полюса замкнутой системы
x0      = [0;0;0];             % истинное начальное состояние
xhat0   = [0;0;0];             % начальная оценка
Nsteps  = 200;                 % число дискретных шагов моделирования

results = struct([]);

fprintf('\n=== Лабораторная работа №4. Вариант 1 ===\n');

%% ============================================================
% 1–4. Дискретизация, проверка Wc/Wo, расчёт K, k0, L
% =============================================================
for k = 1:numel(T0_list)

    T0 = T0_list(k);
    fprintf('\n==============================\n');
    fprintf('ПЕРИОД ДИСКРЕТИЗАЦИИ T0 = %.1f c\n', T0);
    fprintf('==============================\n');

    % ---- дискретизация ----
    sysd = c2d(ss(A,B,C,D), T0, 'zoh');
    [Ad,Bd,Cd,Dd] = ssdata(sysd);

    % ---- управляемость/наблюдаемость ----
    rWc = rank(ctrb(Ad,Bd));
    rWo = rank(obsv(Ad,Cd));
    fprintf('rank(Wc)=%d, rank(Wo)=%d\n', rWc, rWo);

    % ---- матрица управления K ----
    K = place(Ad, Bd, p_cl);
    fprintf('K =\n'); disp(K);

    % ---- масштаб k0 для нулевой ошибки ----
    sys_cl = ss(Ad - Bd*K, Bd, Cd, 0, T0);
    dc = dcgain(sys_cl);
    k0 = 1/dc;
    fprintf('k0 = %.6g\n', k0);

    % ---- наблюдатель L ----
    p_obs = p_cl.^2;          % полюса наблюдателя быстрее
    L = place(Ad', Cd', p_obs)';  
    fprintf('L =\n'); disp(L);

    % ---- матрицы дискретного контроллера с наблюдателем ----
    Ablock = Ad - Bd*K - L*Cd;
    Bblock = [Bd*k0, L];    % входы: w, y
    Cblock = -K;            % выход = u
    Dblock = [k0, 0];

    % сохраняем
    results(k).T0 = T0;
    results(k).Ad = Ad;  results(k).Bd = Bd;  results(k).Cd = Cd;
    results(k).K  = K;   results(k).k0 = k0;
    results(k).L  = L;
    results(k).Ablock = Ablock; 
    results(k).Bblock = Bblock;
    results(k).Cblock = Cblock;
    results(k).Dblock = Dblock;
end

%% ============================================================
% 5. Переходные характеристики замкнутой системы
% =============================================================
figure('Name','Переходные характеристики замкнутой системы');

for k = 1:numel(T0_list)

    T0 = results(k).T0;

    Ablock = results(k).Ablock;
    Bblock = results(k).Bblock;
    Cblock = results(k).Cblock;
    Dblock = results(k).Dblock;

    % создаём дискретный контроллер
    sys_ctrl = ss(Ablock, Bblock, Cblock, Dblock, T0);

    % создаём замкнутую систему вход→выход
    % входы sys_ctrl: [w; y]
    % связываем: y = Cx → линейная обратная связь
    Ad = results(k).Ad; Bd = results(k).Bd; Cd = results(k).Cd;
    K  = results(k).K;  k0 = results(k).k0;

    % комбинированная модель x[k+1] = (Ad-Bd*K)x + Bd*k0*w
    sys_closed = ss(Ad - Bd*K, Bd*k0, Cd, 0, T0);

    % моделируем
    t = (0:Nsteps)*T0;
    w = ones(size(t));    
    y = lsim(sys_closed, w, t);

    subplot(3,1,k);
    stairs(t,y,'LineWidth',1.4);
    grid on;
    xlabel('t, c'); ylabel('y');
    title(sprintf('Переходная характеристика замкнутой системы, T0 = %.1f',T0));
end

%% ============================================================
% 6. Влияние наблюдателя (быстрый/медленный, разные xhat0)
% =============================================================
T0 = 1;                                 % анализ делаем при T0 = 1 с
idx = find([results.T0] == T0);

Ad = results(idx).Ad;
Bd = results(idx).Bd;
Cd = results(idx).Cd;

K  = results(idx).K;
k0 = results(idx).k0;

% быстрый наблюдатель (как раньше)
L_fast = results(idx).L;

% медленный наблюдатель (полюса ближе к 1)
p_obs_slow = [0.9 0.92 0.95];
L_slow = place(Ad', Cd', p_obs_slow).';

figure('Name','Влияние параметров наблюдателя на переходный процесс');

cases = { ...
    struct('L', L_fast, 'xhat0', [0;0;0], 'title', 'Быстрый наблюдатель, x̂(0)=0'), ...
    struct('L', L_slow, 'xhat0', [0;0;0], 'title', 'Медленный наблюдатель, x̂(0)=0'), ...
    struct('L', L_fast, 'xhat0', [5;5;5], 'title', 'Быстрый наблюдатель, x̂(0)≠0') ...
};

x0 = [0;0;0];            % истинное начальное состояние
t  = 0:T0:60;
w  = ones(size(t));      % ступенька

for i = 1:numel(cases)
    L = cases{i}.L;
    xhat0 = cases{i}.xhat0;

    % матрицы объединённой системы "объект+наблюдатель+регулятор"
    Acl = [Ad          -Bd*K;
           L*Cd  Ad - Bd*K - L*Cd];

    Bcl = [Bd*k0;
           Bd*k0];

    Ccl = [Cd  zeros(size(Cd))];
    Dcl = 0;

    sys_obs = ss(Acl, Bcl, Ccl, Dcl, T0);

    z0 = [x0; xhat0];              % начальное состояние [x; x̂]
    [y, t_out] = lsim(sys_obs, w, t, z0);

    subplot(3,1,i);
    stairs(t_out, y, 'LineWidth', 1.4);
    grid on;
    xlabel('t, c'); ylabel('y(t)');
    title(cases{i}.title);
    fprintf('y(end) = %.6f\n', y(end));
end