clc
close all
clear

sim('model_wahadlo')
t=0.01
l=str2num(get_param('model_wahadlo/Subsystem','l'))
m=str2num(get_param('model_wahadlo/Subsystem','m'))


%fi=out.fi;
%fi1=out.fi1;
%fi2=out.fi2;


X1 = [fi1 fi2]';
X = [fi fi1]';

A = X1 * X' * inv(X * X')'

B=[0;0];
C=[1 0; 0 1];
D=[0;0];

for i=1:length(fi)
    x=-l*sin(fi(i));
    y=-l*cos(fi(i));
    %hold off
    %plot([0 -l*sin(fi(i))],[0 -l*cos(fi(i))],'Color','r','LineWidth',2);
    %hold on
    %plot(-l*sin(fi(i)), -l*cos(fi(i)), 'y.','MarkerSize',5*m);
    %raxis([-1.1*l 1.1*l -1.1*l 1.1*l])
    pause(t)
end