clc;
clear;
clf;
q1=[1 1 1 1 1];q2=[1 1 1 1 1];
Q0=5;
k1=1;k2=3;
m=3;
g=9.8;
l=1;
I=0.5;
k=[k1 k2]';
p12=Q0/(2*k(1));
p22=Q0/(2*k(2))*(1+1/k(1));
p11=k(1)*p22+k(2)*p12;
P=[p11 p12;p12 p22];
%C=[0 1];
b=1/(m*l^2+I);
nn=30;
TN=600;
T=2*pi;
TT=nn*T;
h=2*pi/TN;
N=TT/h;
N=round(N);
dim=2;
for i=1:TN
x1(i)=0;
x2(i)=0;
for j=1:dim+3
thetaEst(j,i)=0;
end
end
omega=[];
e1=[];e2=[];
u=[];
x10=0;x20=0;
omega0=zeros(5,1);
for i=0:N
t=i*h;
theta1=[(sin(t))^3 -1];
ksi_1=x10^2;
ksi_2=cos(x10);
theta2=[5*b b*g*l]';
d=round(-(1-0.5*sin(t)^2)/h+i+TN+1);
ksi0=exp(-sin(t)*(x1(d)^2+x2(d)^2));
xr=sin(t);xr1=cos(t);xr2=-sin(t);
e0=[x10-xr x20-xr1]';
bc=[0 1]';
namada(1)=ksi_1;
namada(2)=ksi_2;
namada(3)=k'*e0-xr2;
namada(4)=1;
namada(5)=0.5*e0'*P*bc;
for j=1:dim+3
% if(i+TN<TN&&i+TN>=0)
% thetaEst(j,i+TN+1)=0;
if(i+TN<2*TN)
thetaEst(j,i+TN+1)=t/T*q1(j)*e0'*P*bc*namada(j);
elseif(i+TN>=2*TN)
thetaEst(j,i+TN+1)=thetaEst(j,i+1)+q1(j)*e0'*P*bc*namada(j);
end
end
for j=1:dim+3
omega1(j)=h*q2(j)*e0'*P*bc*namada(j)*thetaEst(j,i+TN+1)+omega0(j);
end
u0=0;
for j=1:dim+3
u0=u0+thetaEst(j,i+TN+1)*namada(j)*omega1(j);
end
u0=-u0;
eta1=5*x10^2*(sin(t))^3+1/b*ksi0;
x11=h*x20+x10;
x21=h*b*(u0-g*l*cos(x10)+eta1)+x20;
x10=x11;x20=x21;
omega0=omega1;
x1(TN+1+i)=x11;x2(TN+1+i)=x21;
omega=[omega omega1'];
e1=[e1 x11-xr];e2=[e2 x21-xr1];
u=[u u0];
end
t1=0:N;
t1=h*t1;
figure(1);
plot(t1,e1,t1,e2);
axis tight
%title('e(t)');
xlabel('time(s)');
ylabel('tracking error e(t)');
figure(2);
t2=(nn-1)*T:h:nn*T;
para=23/40
for i=1:TN+1
nerror1(i)=e1(N-TN+i-1)*para;
nerror2(i)=e2(N-TN+i-1)*para;
end
plot(t2,nerror1,t2,nerror2);
axis tight
%title('the 30 error');
xlabel('time(s)');
ylabel('final tracking error e(t)');
figure(3);
plot(t1,u);
axis tight
%title('u(t)');
xlabel('time(s)');
ylabel('input u(t)');
figure(4);
size(t1)
size(omega)
plot(t1,omega);
%axis tight
axis([0 188.4 0 9]);
%title('time invariable');
xlabel('time(s)');
ylabel('estimated \Omega');
thet=[];
for i=0:N
thet=[thet thetaEst(:,i+TN+1)];
end
figure(5);
plot(t1,thet);
axis tight
%title('time varing');
xlabel('time(s)');
ylabel('estimated \Theta(t)');
figure(6);
perror1=[];
perror2=[];
for i=0:nn-1
temp1=[];temp2=[];
for j=1:TN
temp1=[temp1 abs(e1(i*TN+j))];
temp2=[temp2 abs(e2(i*TN+j))];
end
max1=max(temp1);
max2=max(temp2);
perror1=[perror1 max1];
perror2=[perror2 max2];
% perror1=[perror1 e1(i*TN+1)];
%perror2=[perror2 e2(i*TN+1)];
end
i=1:nn;
plot(i,perror1,i,perror2)
xlabel('Period Number');
ylabel('|e|_{sup}');
figure(7)
temp1=[];temp2=[];
for i=0:nn-1
temp1=[temp1 (e1(i*TN+1))];
temp2=[temp2 (e2(i*TN+1))];
% perror1=[perror1 e1(i*TN+1)];
%perror2=[perror2 e2(i*TN+1)];
end
i=1:nn;
plot(i,temp1,i,temp2)
xlabel('Period Number');
ylabel('tracking error e(t)');
for i=1:nn
array(i)=0;
for j=1:TN
array(i)=array(i)+h*((e1((i-1)*TN+j))^2+(e2((i-1)*TN+j))^2);
end
end
i=1:nn;
figure(8);
plot(i,array);
xlabel('Period Number');
ylabel('|e_i(t)|_{sup}');
for j=0:TN
er(j+1)=(e1((nn-1)*TN+j))^2+(e2((nn-1)*TN+j))^2;
end
j=0:TN;
figure(9);
plot(j,er);
xlabel('time(s)');
ylabel('final tracking error')
