%状态方程
function dx=f(t,x)
%利用RTAC系统给定参数进行仿真
psi=0.2;w=3;
Kx=[-16.52 -83.52 -15.4 -20.7];
L1=[3.4152 -3.0473;1.9628 5.5501;-3.4819 11.6188;-4.5875 1.6509];
L2=[-3.3591 -1.0914;-1.0312 -1.7223];
A1=[0 w;-w 0];
%状态方程描述
dx=zeros(12,1);%产生一个12*1的零矩阵
x1=x(1);
x2=x(2);
x3=x(3);
x4=x(4);
v1=x(5);
v2=x(6);
z1=x(7:10);
z2=x(11:12);
x3z2=asin(-x(11)/(psi*w^2));
x4z2=-x(6)/(psi*w)*1/(1-(x(5)/(psi*w^2))^2)^0.5;
xz2=[0,0,x3z2,x4z2]';
%控制器
uz2=(x4z2)^2*tan(x3z2)+x(5)/(psi*cos(x3z2));
u=uz2+Kx*(x(7:10)-xz2);
hz=1-psi^2*cos(x(3))^2;
dx(1)=x(2);
dx(2)=(-x1+psi*x(4)^2*sin(x(3))+x(5)-psi*(cos(x(3)))*u)/hz;
dx(3)=x(4);
dx(4)=(psi*cos(x3)*(x1-psi*x4*x4*sin(x3))-psi*cos(x3)*v1+u)/hz;
dx(5)=w*x(6);
dx(6)=-w*x(5);
ym=[x(1),x(3)];
RT=1-psi*psi*cos(x(9))*cos(x(9));
fz1=[x(8);(-x(7)+psi*x(10)^2*sinx(9))/RT;x(10);psi*cos(x(9))*(x(7)-psi*x(10)*x(10)*sin(x(9)))/RT];
g1z1=[0;(-psi*cos(x(9)))/RT;0;1/RT];
g2z1=[0;1/RT;0;(-psi*cos(x(9)))/RT];
%观测器
dx(7:10)=fz1+g1z1*u+g2z1*[1,0]*x(11:12)+L1*(ym-[x(7);x(10)]);
dx(11:12)=A1*x(11:12)+L2*(ym-[x(7),x(10)]);
%主程序
clear
clc
%设置仿真时间
Tspan=[0,60];
%设置初值
IC=[0.1;0;0;0;0;0.5;0;0;0;0;0;0];
%求解状态方程
%[Tx Yx] =ode45(@RTAC,Tspan,IC);
[t,x]=ode45(@RTAC,Tspan,IC);
y=x(:,1);
figure;
plot(t,y,'r','LineWidth',2);
set(gca,'fontsize',18,'FontName','Times New Roman');
xlabel('Time (sec)','fontsize',20,'FontName','Times New Roman');
ylabel(' Tracking Performance','fontsize',20,'FontName','Times New Roman');
grid on;