PROS - Ergasia 1 - Thema 1 - sim.m

clear all
clc

% Παραμετροι
m = 15;
k = 2;
b = 0.2;
% Function Handle
u = @(t) 5 * sin(2* t) + 10.5;

% Initial State, all zero
x0(1) = 0;
x0(2) = 0;
tspan = 0 : 0.1: 10;

% Simulation
[t, state] = ode15s(@(t, state)msd(t, state, m, b, k, u), tspan, x0);
t;
display(' ');
y = state(:,1);
display(' ');
velocity = state(:,2);
display(' ');

% Plot
figure(1);
plot(t, y);                        % Θεση
figure(2);
plot(t, velocity);                % Ταχυτητα
figure(3);
plot(y, velocity);
display(' ');
display(' ');

% ========================================================================
% ========================= Filters with lsim ============================
% ========================================================================
sys1 = tf([-1 0], [1 3 2]);
sys2 = tf([-1], [1 3 2]);
sys3 = tf([1], [1 3 2]);
z = [];
z(1,:) = lsim(sys1, y, t);
z(2,:) = lsim(sys2, y, t);
% Make u a 101-elements array
counter = 0;
uList = [];
for i = 0: 0.1: 10
    counter = counter + 1;
    uList(counter) = u(i);
end
z(3,:) = lsim(sys3, uList, t)

z(1,3)
z(2,3)
z(3,3)



A = zeros(3)
N = 101;
for i = 1:N
    % Το i θα είναι η στήλη μας
    stili = z(:,i);
    A = A + stili * stili';
end
A = A / N
Ainv = inv(A)

B = zeros(3,1)
for i = 1:N
    stili = z(:,i);
    B = B + stili * y(i);
end
B = B / N

answer = Ainv * B
M = 1 / answer(3)
K = (2 + answer(2)) * M
B = (3 + answer(1)) * M