参考文献:《一类匹配不确定非线性系统的动态逆全程滑模变结构控制》
clear
clc
H=[5 2 5;
8 1 7;
1 1 2];
invH=inv(H);
a11=invH(1,1);
a12=invH(1,2);
a13=invH(1,3);
a21=invH(2,1);
a22=invH(2,2);
a23=invH(2,3);
a31=invH(3,1);
a32=invH(3,2);
a33=invH(3,3);
u=zeros(3,1);
x=zeros(6,1);
Dx=zeros(6,1);
S1=0;
S2=0;
S3=0;
k1=50;
k2=50;
k3=50;
fea1=10;
fea2=10;
fea3=10;
Tf=0.5;
t=0;
Dt=0.001;
n=1;
for i=1:14000
p1=x(3)*sin(x(6))*cos(x(5))-x(3)*sin(x(6))*sin(x(5))*sin(x(4))-x(3)*cos(x(6))*cos(x(4))+x(2)*sin(x(6))+x(2)*cos(x(6))*sin(x(5));
p2=-x(1)*sin(x(6))-x(1)*cos(x(6))*sin(x(5))-x(3)*cos(x(6))*cos(x(5))+x(3)*cos(x(6))*sin(x(5))*sin(x(4))-x(3)*sin(x(6))*cos(x(4));
p3=x(2)*cos(x(6))*cos(x(5))-x(2)*cos(x(6))*sin(x(5))*sin(x(4))+x(2)*sin(x(6))*cos(x(4))-x(1)*sin(x(6))*cos(x(5))+x(1)*sin(x(6))*sin(x(5))*sin(x(4))+x(1)*cos(x(6))*cos(x(4));
f1=a11*p1+a12*p2+a13*p3;
f2=a21*p1+a22*p2+a23*p3;
f3=a31*p1+a32*p2+a33*p3;
f4=x(1)+x(2)*sin(x(4))*tan(x(5))+x(3)*cos(x(4))*tan(x(5));
f5=x(2)*cos(x(4))-x(3)*sin(x(4));
f6=x(2)*sin(x(4))*sec(x(5))+x(3)*cos(x(4))*sec(x(5));
f=[f1;f2;f3;f4;f5;f6];
g=[a11 a12 a13;a21 a22 a23; a