Generalized second order chained form (Lie Bracket problem)

• Jul 7th 2011, 06:33 PM
ain
Generalized second order chained form (Lie Bracket problem)
Dear all,

I have a problem to find z31 and z32, and I already got z11,z12,z21,z22, z41 and z42. Here my code using Matlab.

%------------------------
% xddot = u1;
% psiddot = u2;
% yddot = [m1/m2 tan(psi)]u1
% zddot = [-m1/m3 tan(theta) sec(psi)] u1
%-----------------------------------------
syms xdot xddot psidot psiddot ydot yddot zdot zddot x y z phi theta psi m1 m2 m3 u1 u2 u4 v1 v2 v4
f=[xdot;0;psidot;0;ydot;0;zdot;0];
F=[xdot;1;psidot;0;ydot;m1/m2*tan(psi);zdot;-m1/m3*tan(theta)*sec(psi)];
g1=[0;1;0;0;0;m1/m2*tan(psi);0;-m1/m3*tan(theta)*sec(psi)];
g2=[0;0;0;1;0;0;0;0];
vars=[x,xdot, psi, psidot,y, ydot, z, zdot];

fg1=jacobian(g1,vars)*f-jacobian(f,vars)*g1
Fg2=jacobian(g2,vars)*F-jacobian(F,vars)*g2
F2g2=jacobian(Fg2,vars)*F-jacobian(F,vars)*Fg2
F3g2=jacobian(F2g2,vars)*F-jacobian(F,vars)*F2g2

h1=[x;0;0;0;0;0;0;0]; % h1 =x
h2=[0;0;0;0;y;0;0;0]; % h2 =y
u1=v1;
fh1=jacobian(h1,vars)*f-jacobian(f,vars)*h1 %Lfh1 = xdot
fh2=jacobian(h2,vars)*f-jacobian(f,vars)*h2 %Lfh2 = ydot
g1fh2=jacobian(fh2,vars)*g1-jacobian(g1,vars)*fh2 %Lg1Lfh2 = m1/m2*tan(psi)
fg1fh2=jacobian(g1fh2,vars)*f-jacobian(f,vars)*g1fh2 %Lfg1Lfh2 = m1/m2*(1+tan(psi)^2)*psidot
%
f2h2=jacobian(fh2,vars)*f-jacobian(f,vars)*fh2; %Lf2h2
g1f2h2=jacobian(f2h2,vars)*g1-jacobian(g1,vars)*f2h2; %Lf2h2
fg1f2h2=jacobian(g1f2h2,vars)*f-jacobian(f,vars)*g1f2h2; %Lfg1f2h2

%find v2
u1=v1;
f2g1fh2=jacobian(fg1fh2,vars)*f-jacobian(f,vars)*fg1fh2; %f2g1Lfh2
g1fg1fh2=jacobian(fg1fh2,vars)*g1-jacobian(g1,vars)*fg1fh2; %g1fg1Lfh2
g2fg1fh2=jacobian(fg1fh2,vars)*g2-jacobian(g2,vars)*fg1fh2; %g2fg1Lfh2

v2= f2g1fh2 + (g1fg1fh2)*u1 + (g2fg1fh2)*u2

%------------feedback transformation--------------
% z=Phi(X) u = Beta(X)v
% with
%
% z11 = h1 = x
% z12 = Lfh1 = xdot
% z21 = Lg1Lfh2 = m1/m2*tan(psi)
% z22 = LfLg1Lfh2 = m1/m2*(1+tan(psi)^2)*psidot
% z31 = ?
% z32 = ?
% z41 = h2 = y
% z42 = Lfh2 = ydot
%
% to get generalized chained form (second order)
% z11_ddot = v1
% z21_ddot = v2
% z31_ddot = z21 v1
% z41_ddot = z31 v1