%Podgrupa 1
clear;
%Na poczatku przypisuje dana transmitancje do G
s = tf('s');
G = (s+0.5)/((s+0.1)*(s^2+0.2*s+1.01));

%uzywam funkcji rlocus i rlocfind aby znalezc K krytyczne
%znajdujace sie na przecieciu linii krytycznej z osia urojona
rlocus(G);
[Kkr, parametry] = rlocfind(G);

%Przypisuje parametry
%Tu juz na stale przypisalem to Kkr znalezione za pierwszym razem
Kkr = 1.1334;
Pkr = 5; % 2PI / freq
K = 0.6*Kkr;
Ti = Pkr/2;
Td = Pkr/8;

%Tworze regulator
Sys = pidstd(K, Ti, Td);

%Rysuje odpowiedz skokowa transmitancji
step(G);
title('Transmitancja - odpowiedz skokowa');

%Lacze regulator z transmitancja szeregowo otrzymujac system otwarty
sys_otw = series(G, Sys);

%Tworze system zamkniety
sys_zamk = feedback(sys_otw, 1);

%Rysuje wykresy odpowiedzi skokowej systemu otwartego
%i zamknietego
figure;
step(sys_otw);
title('System otwarty - odpowiedz skokowa');
figure;
step(sys_zamk);
title('System zamkniety - odpowiedz skokowa');

----------------------------------------------------------------------------------------
%Szymon Sadowski podgrupa 2
clear
%model inercyjny II ma postać: 1/(T^2*s^2+T*ksi*s+1)

%załadowanie pomiarów do przestrzeni roboczej - zmienna y
load('obiekt.mat');

%tworzę znormalizowany wektor czasowy od 0 do 1 z 700 elementami
t = linspace(0,1,600);

%wyrysowanie danych zmierzonych
figure()
plot(t,y)

%"Poczatkowe zgadnięte parametry"
T = 0.1;
ksi = 1;
k = 0.2;
theta = 0.1;

%Utworzenie systemu o zgadniętych parametrach
s = tf('s');
G = k/(T*T*s*s+T*ksi*s+1);
set(G, 'OutputDelay', theta);

%Wyrysowanie odpowiedzi impulsowej systemu ze zgadniętymi parametrami na czerwono
hold on
impulse(G,t,'r')

%Optymalizacja funkcji błędu i znalezienie optymalnych parametrów modelu
opt_params = fminsearch('ident', [T ksi k theta])

%"Nowe - Optymalne parametry"
T = opt_params(1);
ksi = opt_params(2);
k = opt_params(3);
theta = opt_params(4);

%Utworzenie systemu z wyznaczonymi optymalnymi parametrami
s = tf('s');
optimal_sys = k/(T*T*s*s+T*ksi*s+1);
set(optimal_sys, 'OutputDelay', theta);

%Wyrysowanie odpowiedzi impulsowej systemu z wyznaczonymi optymalnymi
%parametrami na zielono
impulse(optimal_sys, t, 'g')

title('Odp. impulsowa: niebieski - rzeczywiste pomiary, czerwony - dobrane poczatkowe parametry, zielony - optymalizacja')

%Wnioski:
%Odpowiedź impulsowa systemu z parametrami dopasowanymi przez funkcję fminsearch dobrze
%pokrywa wykres rzeczywistych pomiarów.
%Z tego względu możemy przypuszczać, że zmierzony obiekt dynamiczny
%faktycznie jest obiektem inercyjnym II rzędu z opóźnieniem.
%Końcowy błąd zasygnalizowany przez funkcję fminsearch jest rzędu 0.2464 co
%potwierdza, że obiekt został zidentyfikowany dobrze

%funkcja obliczajaca blad dla danych parametrow w stosunku do rzeczywistych
%pomiarow
function [error] = ident(params)
    %zaladowanie pomiarow
    global y
    load('obiektA.mat');

    %wyodrębnienie parametrów z argumentu
    param_T = params(1);
    param_ksi = params(2);
    param_k = params(3);
    param_theta = params(4);

    %utworzenie systemu na podstawie parametrów z argumentu
    s = tf('s');
    iner_II = param_k/(param_T*param_T*s*s+param_T*param_ksi*s+1);
    set(iner_II, 'OutputDelay', param_theta);

    %zbadanie odpowiedzi impulsowej systemu z dobranymi parametrami
    t = linspace(0,1,700);
    [Y_model,T] = impulse(iner_II, t);

    %roznica w odpowiedzi impulsowej miedzy rzeczywistymi pomiarami a
    %systemem z dobranymi parametrami
    diff = Y_model-y;
    %suma kwadratów roznic w odpowiedzi impulsowej miedzy rzeczywistymi pomiarami a
    %systemem z dobranymi parametrami
    error = sum((Y_model-y).^2)

 ---------------------------------------------------------------------------------

Gdy trzeba usunac opoznienie:

s = tf('s');
G_in = (6*(s+5))/((s+1)*(s+2)*(s+3)*(s+4));
sys = exp(-s);
sysx = pade(sys, 3);
G = series(G_in, sysx);


-------------------------------------------

s = tf('s');
K=1;
G = K*(s+0.3)/((s+0.1)*(s^2+0.2*s+1.01));

rlocus(G);
sgrid();
axis('equal');
K = 2.98;
G = K*(s+0.3)/((s+0.1)*(s^2+0.2*s+1.01));

% step(G);
% figure();
% impulse(G);


-------------------------------------------------------------------