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[]
