BeginPackage["Miscellaneous`NONACODE`"] NONACODE::usage=" ---------------------------------------------------------\n ---- NONACODE (NONlinear (Adaptive) COntrol DEsign) -----\n ---------------------------------------------------------\n NONACODE is a Software Package for Analysis and Design of Nonlinear (Adaptive) Control Systems with the Differential-Geometric Approach.\n We consider Nonlinear Systems belonging to the linear analytic class, of the form:\n x' = f(x) + g1(x) u1 + g2(x) u2 + ... + gm(x) um \n y1 = h1(x) ; y2 = h2(x) ; ... ; ym = hm(x)\n Both Lyapunov and Hyperstability based-on techniques for parameter adaptation can be used to solve the Adaptive Output Tracking Problem, when the system is SISO, has parametric uncertainties, and relative degree one. Other control problems such as Output Tracking, Exact Linearization via State Feedback, Normal Form, and Nonlinear Observers, can also be solved for both SISO and MIMO systems, with the aid of NONACODE."; (* !Cls *) Print[" ---------------------------------------------------------\n ---------------- Welcome to NONACODE!! ------------------\n ---------------------------------------------------------\n This Package has been developed by: \n ------ Juan Carlos Gomez --------------------------------------\n ------ Fac. de Ingenieria - Universidad Nacional de Rosario----\n ------ Riobamba 245 Bis - (2000) Rosario - Argentina-----------\n ------ Present Address: ---------------------------------------\n ------ Dept. of Elec. & Comp. Engineering ---------------------\n ------ University of Newcastle - NSW 2308 - Australia----------\n ------ E-MAIL: juan@ee.newcastle.edu.au -----------------------\n ------ July 1994 ----------------------------------------------\n Press ?NONACODE for information about this program"] JACOB::usage="JACOB[f,x] computes the jacobian matrix of the vector field f(x)."; LIEDEV::usage="LIEDEV[f,h,x] computes the Lie derivative of the real valued function h(x) along the direction defined by the vector field f(x)."; KLIEDEV::usage="KLIEDEV[f,h,k,x] computes the k-th Lie derivative of the real valued function h(x) along the direction defined by the vector field f(x), for k >= 1"; LIE::usage="LIE[f,g,x] computes the Lie brackets of the vector fields f(x) and g(x)."; ADJ::usage="ADJ[f,g,k,x] computes the k-th adjoint of f(x) and g(x), for k >= 1."; RELDEG::usage="RELDEG[f,g,h,x] computes the relative degree for the single input single output system:\n x' = f(x) + g(x) u \n y = h(x)"; NORMAL::usage="NORMAL[f,g,h,x] computes a coordinate transformation that takes the system:\n x' = f(x) + g(x) u \n y = h(x) \n to normal form"; EXACTLIN::usage="EXACTLIN[f,g,h,x] computes a state feedback control law that takes the nonlinear system:\n x'= f(x) + g(x) u \n y = h(x) \n to an input-output linear one, of the form:\n D[y,{t,r}] = v \n where v is the new input, and D[y,{t,r}] stands for the r-th time derivative of y(t), provided that the system has relative degree r<= n"; OUTRACK::usage="Given the Nonlinear Minimum Phase SISO System:\n x' = f(x) + g(x) u \n y = h(x) \n with relative degree r<=n, and a desired trajectory 'yr(t)', OUTRACK[f,g,h,x,yr,lambda] gives a state feedback control law that makes the output 'y' to asymptotically track the desired trajectory 'yr(t)'. The output error dynamics has eigenvalues given by the first 'r' components of the vector 'lambda'." LYADAPTRACK::usage="Let the Nonlinear Minimum Phase Relative Degree One SISO System be defined by: \n x' = F(x) P1 + G(x) P2 u \n y = h(x) \n with: \n F(x) = [f1(x), f2(x), ..., fn1(x)] \n G(x) = [g1(x), g2(x), ..., gn2(x)] \n where f1(x), ..., fn1(x), g1(x), ..., gn2(x) are known smooth vector fields, and: \n P1 = [p11, p12, ..., p1n1] \n P2 = [p21, p22, ..., p2n2] \n where p11, ..., p1n1, p21, ...,p2n2 are (n1+n2) unknown parameters. \n LYADAPTRACK[F,G,P1,P2,P1es,P2es,h,x,yr,alf,gam] computes an adaptive control law 'u' that makes the output of the system to asymptotically track the desired trajectory 'yr', using a Lyapunov-based technique for the parameter adaptation. The function also gives the parameter update law.\n The adaptive control law is given by:\n u = (-LIEDEV^[f,h,x] + v)/LIEDEV^[g,h,x]\n where LIEDEV^[f,h,x] and LIEDEV^[g,h,x] stand for the estimates of the Lie derivatives of 'h' along the directions of f(x) and g(x) respectively, and:\n v = D[yr,t] + alf (yr - y)"; HYPADAPTRACK::usage="Let the Nonlinear Minimum Phase Relative Degree One SISO System be defined by:\n x' = F(x) P1 + G(x) P2 u\n y = h(x)\n with:\n F(x) = [f1(x), f2(x), ..., fn1(x)]\n G(x) = [g1(x), g2(x), ..., gn2(x)]\n where f1(x), f2(x), ..., fn1(x),g1(x), g2(x), ..., gn2(x) are known smooth vector fields, and:\n P1 = [p11, p12, ..., p1n1]\n P2 = [p21, p22, ..., p2n2]\n where p11, p12, ..., p1n1,p21, p22, ...,p2n2 are (n1+n2) unknown parameters.\n HYPADAPTRACK[F,G,P1,P2,P1es,P2es,h,x,yr,alf,gam1,gam2] computes an adaptive control law 'u' that makes the output of the system to asymptotically track the desired trajectory 'yr', using a Hyperstability- based technique for the parameter adaptation. The function also gives the parameter update law.\n The adaptive control law is given by:\n u = (-LIEDEV^[f,h,x] + v)/LIEDEV^[g,h,x]\n where LIEDEV^[f,h,x] and LIEDEV^[g,h,x] stand for the Lie derivatives of 'h' along the directions of f(x) and g(x) respectively, and:\n v = D[yr,t] + alf (yr - y)"; OBSMATRIX1::usage="Let the Nonlinear Minimum Phase SISO System be defined by:\n x' = f(x) + g(x) u(t)\n y = h(x)\n with f(x) and g(x) smooth vector fields, and h(x) a smooth function.\n OBSMATRIX1[f,g,h,x] computes the 'Observability Matrix', defined by:\n O(x) =JACOB[{h(x), Lfh(x), Lf2h(x), ..., Lf(n-1)h(x)}',x]. "; OBSERVER1::usage="Given the Nonlinear Minimum Phase SISO System:\n x' = f(x) + g(x) u(t)\n y = h(x)\n with f(x) and g(x) smooth vector fields, and h(x) a smooth function.\n OBSERVER1[f,g,h,x,xes,u,lambda] computes a nonlinear observer for the states x(t), in such a way that the estimation error:\n e(t):= xes(t) - x(t)\n tends exponentially to zero as t tends to infinity, with the eigenvalues given by the vector lambda."; OBSERDYN1::usage="Given the desired eigenvalues 'lambda' for the observer estimation error of the system:\n x' = f(x) + g(x) u(t)\n y = h(x)\n OBSERDYN1[lambda] returns vector K in the observer equation:\n xes' = f(xes) + g(xes) u(t) + Oin(xes) K (y - h(xes))"; VECRELDEG::usage="This function computes the vector relative degree for a MIMO system"; OBSERDYNN::usage="This function computes the matrix K that gives the estimation error dynamics."; OBSMATRIXN::usage="This function computes the observability matrix for the MIMO system having relative degree that equals the state space dimension"; OBSERVERN::usage="This function computes a nonlinear asymptotic observer for the MIMO system having relative degree that equals the state space dimension"; Begin["`Private`"] JACOB[f_, x_] := Block[{n, i, j, t}, n := Length[x]; t := Table[D[f[[i]], x[[j]]], {i, n}, {j, n}]; t]; LIEDEV[f_,h_,x_] := Block[{n,i,t,a}, n := Length[x]; t := Table[D[h,x[[i]]], {i, n}]; a=t . f; a]; KLIEDEV[f_,h_,k_,x_]:= Block[{t},t := h; If[k==0,t=h,Do[t=LIEDEV[f,t,x],{k}]];t]; LIE[f_,g_,x_]:= Block[{t},t := JACOB[g,x] . f - JACOB[f,x] . g;t]; ADJ[f_,g_,k_,x_]:= Block[{t},t := g; Do[t=LIE[f,t,x], {k}];t]; RELDEG[f_,g_,h_,x_]:= Block[{n,i,t0,t,r}, n :=Length[x]; t:= Table[LIEDEV[g,KLIEDEV[f,h,i,x],x],{i,n-1}]; t0:= LIEDEV[g,h,x]; t=Insert[t,t0,1]; i=1; While[t[[i]]==0,i=i+1;i];r=i; r]; NORMAL[f_,g_,h_,x_]:= Block[{n,i,j,k,l,m,r,t0,t1,t2,t3,dtr,trr,tr,tn}, n:=Length[x]; r=RELDEG[f,g,h,x];tn:=Table[KLIEDEV[f,h,i,x],{i,n-1}]; tn=Insert[tn,h,1];trr:=Table[KLIEDEV[f,h,j,x],{j,r-1}]; trr=Insert[trr,h,1]; t1:=Table[x[[k+r]],{k,n-r}]; t2:=Table[(x[[l+r]])^2,{l,n-r}]; t3:=Table[(x[[m+r]])^3,{m,n-r}]; t0:={t1,t2,t3};tr=Join[trr,t1]; Do[If[Det[JACOB[tr,x]]==0,tr=Join[trr,t0[[i]]];tr,tr],{i,2,3}]; dtr=Det[JACOB[tr,x]]; If[r==n,tn];If[(dtr!=0)&&(r!=n),tr]; If[(r!=n)&&(dtr==0),Print["I could not find a non singular coordinate transformation that takes the\nsystem to normal form.\nThe following coordinate transformation is singular"]];tn;tr]; OUTRACK[f_,g_,h_,x_,yr_,lambda_]:= Block[{n,r,rr,u,i,v,vv,e,z,j,p,q,pol,pol0,cc,c,m,K,jj}, n:=Length[x]; r=RELDEG[f,g,h,x]; Print[StringForm["Relative Degree r = ``",r]]; Print[StringForm["The first `` components of lambda will be taken as the error eigenvalues for the subsequent computations",r]]; pol=Expand[Product[(z - lambda[[j]]),{j,r}]]; cc=Table[Coefficient[pol,z^p],{p,r}]; pol0=Expand[Product[-lambda[[q]],{q,r}]]; c=Insert[cc,pol0,1]; c=Table[c[[m]],{m,r}]; K=Table[c[[-jj+r+1]],{jj,r}]; e=yr-h;vv=Sum[K[[i]]*Derivative[r-i][e],{i,r-1}]; v=vv+Derivative[r][yr]+K[[r]]*e; u:=(-KLIEDEV[f,h,r,x]+v)/LIEDEV[g,KLIEDEV[f,h,r-1,x],x];u]; EXACTLIN[f_,g_,h_,x_]:= Block[{n,r,u,v},n:=Length[x];r=RELDEG[f,g,h,x]; If[r>1,u=(-KLIEDEV[f,h,r,x]+v)/LIEDEV[g,KLIEDEV[f,h,r-1,x],x], u=(-LIEDEV[f,h,x]+v)/LIEDEV[g,h,x]];u]; LYADAPTRACK[F_,G_,P1_,P2_,P1es_,P2es_,h_,x_,yr_,alf_,gam_]:= Block[{n,r,n1,n2,f,g,fe,ge,Lfhe,Lghe,v,i,j,W1,WW2,W2,W,e, phi1,phi2,k,m,l,o,FP,GP,ii,jj,z,zz,FPe,Gpe,W1Pe,WW2Pe}, n:=Length[x]; FP= F*P1; GP= G*P2; n1= Length[F]; n2= Length[G]; f= Sum[FP[[l]],{l,n1}]; g= Sum[GP[[o]],{o,n2}]; r:= RELDEG[f,g,h,x]; W1 = Table[LIEDEV[F[[i]],h,x],{i, n1}]; WW2 = Table[LIEDEV[G[[j]],h,x],{j, n2}]; FPe = F*P1es; GPe = G*P2es; fe=Sum[FPe[[z]],{z,n1}]; ge=Sum[GPe[[zz]],{zz,n2}]; W1Pe = W1*P1es; WW2Pe = WW2*P2es; Lfhe= Sum[W1Pe[[ii]],{ii,n1}]; Lghe= Sum[WW2Pe[[jj]],{jj,n2}]; v = D[yr,t] + alf*(yr - h); W2 = WW2*((-Lfhe + v)/Lghe); W = Join[W1,W2]; e = h - yr; phidev = - gam*e*W; u = (-Lfhe + v)/Lghe; Print[StringForm["phidev = ``",phidev]]; Print[StringForm["u = ``",u]]]; HYPADAPTRACK[F_,G_,P1_,P2_,P1es_,P2es_,h_,x_,yr_,alf_,gam1_,gam2_]:= Block[{n,r,n1,n2,f,g,fe,ge,Lfhe,Lghe,v,i,j,W1,WW2,W2,W,e, phi1,phi2,k,m,ii,jj,FP1,GP2,FPe,GPe,W1Pe,WW2Pe,kk,mm}, n:= Length[x]; FP1= F*P1; GP2= G*P2; n1= Length[F]; n2= Length[G]; f= Sum[FP1[[ii]],{ii,n1}]; g= Sum[GP2[[jj]],{jj,n2}] r= RELDEG[f,g,h,x]; W1= Table[LIEDEV[F[[i]],h,x], {i, n1}]; WW2= Table[LIEDEV[G[[j]],h,x], {j, n2}]; FPe= F*P1es; GPe= G*P2es; W1Pe= W1*P1es; WW2Pe= WW2*P2es; Lfhe= Sum[W1Pe[[kk]],{kk,n1}]; Lghe= Sum[WW2Pe[[mm]],{mm,n2}]; v= D[yr,t] + alf*(yr - h); W2= WW2*((-Lfhe + v)/Lghe); W= Join[W1,W2]; e= h - yr; u= (-Lfhe + v)/Lghe; phi= -gam1*e*W - gam2*Integrate[(e[t]*W),t]; Print[StringForm["phi = ``",phi]]; Print[StringForm["u = ``",u]]]; VECRELDEG[f_,G_,H_,x_]:= Block[{n,m,t,i,j,k,R,vrd,a,p,q,deta},n:=Length[x];m:=Length[G]; t:=Table[RELDEG[f,G[[i]],H[[j]],x],{i,m},{j,m}]; R:=Transpose[t];vrd=Table[Min[R[[k]]],{k,m}]; a=Table[LIEDEV[G[[q]],KLIEDEV[f,H[[p]],(vrd[[p]] - 1),x],x],{q,m},{p,m}]; deta=Det[a];If[deta==0,Print[" Matrix A(x) is singular, then the system has no vector relative degree"]];vrd]; OBSMATRIX1[f_,g_,h_,x_]:= Block[{n,phi,phi1,obs,i}, n=Length[x]; phi1=Table[KLIEDEV[f,h,i,x],{i,n-1}]; phi=Insert[phi1,h,1]; obs=JACOB[phi,x];obs]; OBSERVER1[f_,g_,h_,x_,xes_,u_,lambda_]:= Block[{n,j,p,q,fes,ges,hes,Obs,Obsinv,K}, n=Length[x]; K=OBSERDYN1[lambda]; fes= f /. Table[x[[j]] -> xes[[j]],{j,n}]; ges= g /. Table[x[[p]] -> xes[[p]],{p,n}]; hes= h /. Table[x[[q]] -> xes[[q]],{q,n}]; Obs=OBSMATRIX1[fes,ges,hes,xes]; Obsinv=Inverse[Obs]; xesdot=fes + ges*u +(Obsinv . K)*(h - hes); Print[StringForm["xesdot = ``", xesdot]]]; OBSERDYN1[lambda_]:= Block[{nl,pol,x,i,j,pol0,cc,c,m,p,K,l}, nl:=Length[lambda]; pol=Expand[Product[(x - lambda[[i]]),{i,nl}]]; cc=Table[Coefficient[pol,x^j],{j,nl}]; pol0=Expand[Product[-lambda[[l]],{l,nl}]]; c=Insert[cc,pol0,1]; c=Table[c[[m]],{m,nl}]; K=Table[c[[-p+nl+1]],{p,nl}];K]; OBSERDYNN[lambda_,vrd_,H_]:= Block[{nr,nl,m,i,j,k,lam,kk,l,nlam,ii,jj,ka,nj,ni,nm}, nr:=Length[vrd];nl:=Length[lambda]; m:=Length[H]; lam:=Table[Take[lambda,{(Sum[vrd[[j]],{j,1,i-1}]+1), (Sum[vrd[[k]],{k,1,i-1}] + vrd[[i]])}],{i,nr}];nlam:=Length[lam]; kk:=Table[OBSERDYN1[lam[[l]]],{l,nlam}]; ka=Table[ii*jj*0,{ii,nl},{jj,nr}]; Do[ka[[(ni+Sum[vrd[[nm]],{nm,nj-1}]),nj]]=kk[[nj,ni]],{nj,1,nr}, {ni,1,vrd[[nj]]}];ka]; OBSMATRIXN[f_,G_,H_,x_]:= Block[{n,vrd,nr,nh,i,j,k,phi1,phi2,p,q,obs}, n:=Length[x]; vrd:= VECRELDEG[f,G,H,x]; nr:=Length[vrd]; nh:=Length[H]; r:=Sum[vrd[[i]],{i,nr}]; If[r!=n, Print["The relative degree is not equal to the state dimension.\n The observability matriz cannot be computed."]]; phi1:=Table[KLIEDEV[f,H[[k]],j,x],{k,nh},{j,vrd[[k]]-1}]; phi2=Flatten[phi1]; Do[phi2=Insert[phi2,H[[p]], Sum[vrd[[q]],{q,p-1}]+1],{p,nh}]; obs=JACOB[phi2,x];obs]; OBSERVERN[f_,G_,H_,x_,xes_,U_,lambda_]:= Block[{vrd,n,nr,r,nn,i,j,p,q,fes,Ges,Hes,nu}, vrd:=VECRELDEG[f,G,H,x]; n:=Length[x]; nr:=Length[vrd]; r:=Sum[vrd[[i]],{i,nr}];nu:=Length[U]; If[r!=n,Print["The relative degree is not equal to the state space dimension,\n A full observer cannot be computed."]]; fes= f /. Table[x[[j]] -> xes[[j]],{j,n}]; Ges= G /. Table[x[[p]] -> xes[[p]],{p,n}]; Hes= H /. Table[x[[q]] -> xes[[q]],{q,n}]; xesdot = fes + Transpose[Ges].U + Inverse[OBSMATRIXN[fes, Ges,Hes,xes]].OBSERDYNN[lambda,vrd,H].(H - Hes) ; Print[StringForm["xesdot = ``", xesdot]]]; End[] EndPackage[]