Quadratic-in-the-State Representation of 
Affine Nonlinear Systems 
by Toshiyuki Ohtsuka,  Sept. '99

Definition of Functions

Differentiation by a Vector:  Dv

Dv[f_, x_] := 
      Module[{j, ans}, 
        ans = Table[
            (∂_(x[[j]]))f, {
              j, Length[x]}]; 
        If[Length[Dimensions[ans]] ≠ 1, ans = Transpose[ans]]; ans]

Union of Set Conserving Order:  UnionOrdered

UnionOrdered[a_, b_] := Module[{}, Join[a, Complement[b, a]]]

Picking Up Added Terms:  Terms

Terms[f_] := Module[{}, If[Head[f] == Plus, Level[f, 1], {f}, {f}]]

Picking Up Multiplied Terms:  TermsMul

TermsMul[f_] := 
      Module[{}, If[Head[f] == Times, Level[f, 1], {f}, {f}]]

Picking Up Multiplied Functions of x:  TermsMulFunc

TermsMulFunc[f_, x_] := 
      Module[{i, j, n = Length[x], tmp = TermsMul[f], test, ans = {}}, 
        Do[test = 
            Table[FreeQ[tmp[[i]], 
                x[[j]]], {j, n}]; 
          If[MemberQ[test, False], 
            AppendTo[ans, tmp[[i]]]], {
            i, Length[tmp]}]; ans]

Picking Up Coefficient and Multiplied Functions of x:  TermsMulCoefFunc

TermsMulCoefFunc[f_, x_] := 
      Module[{i, j, n = Length[x], tmp = TermsMul[f], test, ans1 = {}, 
          ans2 = {}}, 
        Do[test = 
            Table[FreeQ[tmp[[i]], 
                x[[j]]], {j, n}]; 
          If[MemberQ[test, False], 
            AppendTo[ans2, tmp[[i]]], 
            AppendTo[ans1, tmp[[i]]], 
            AppendTo[ans1, tmp[[i]]]], {
            i, Length[tmp]}]; {ans1, ans2}]

Picking Up Coefficient and Monomials of Functions of x:  TermsMulCoefMono

TermsMulCoefMono[f_, x_] := 
      Module[{i, j, n = Length[x], tmp = TermsMul[f], test, ans1 = {}, 
          ans2 = {}}, 
        Do[test = 
            Table[FreeQ[tmp[[i]], 
                x[[j]]], {j, n}]; 
          If[MemberQ[test, False], 
            AppendTo[ans2, tmp[[i]]], 
            AppendTo[ans1, tmp[[i]]], 
            AppendTo[ans1, tmp[[i]]]], {
            i, Length[tmp]}]; {Times@@ans1, Times@@ans2}]

Picking Up Monomials of Functions of x:  Monomials

Monomials[f_, x_] := 
      Module[{i, tmp = Terms[f], mono, ans = {}}, 
        Do[If[(
                mono = TermsMulFunc[
                    tmp[[i]], x]) 
              ≠ {}, AppendTo[ans, Times@@mono]], {i, Length[tmp]}]; 
        ans]

Picking Up Monomials of Functions of x and Coefficients:  MonomialsCoef

MonomialsCoef[f_, x_] := 
      Module[{i, tmp = Terms[f], ans = {}}, 
        Do[AppendTo[ans, 
            TermsMulCoefMono[tmp[[i]], 
              x]], {i, Length[tmp]}]; ans]

Picking Up Monomials of Functions of x in a List:  MonomialsList

MonomialsList[L_, x_] := 
      Module[{i, FL = Flatten[L], ans = {}}, 
        Do[ans = 
            UnionOrdered[ans, 
              Monomials[FL[[i]], x]], {
            i, Length[FL]}]; ans]

Repacing Monomials in Polynomials of Function of x:  MonomialsReplace

MonomialsReplace[f_, x_, alpha_, z_] := 
      Module[{i, j, n = Length[alpha], tmp = MonomialsCoef[f, x]}, 
        Do[If[tmp[[i, 2]] == 
              alpha[[j]], 
            tmp[[i, 2]] = 
              z[[j]]], {i, 
            Length[tmp]}, {j, n}]; Plus@@MapThread[Times, Transpose[tmp]]]

Replacing Monomials in Polynomials of Function of x in a List:  MonomialsReplaceList

MonomialsReplaceList[L_, x_, alpha_, z_] := 
      Module[{ftmp}, ftmp[f_] := MonomialsReplace[f, x, alpha, z]; 
        Map[ftmp, L, {Length[Dimensions[L]]}]]

Determination of Power

SetPower[a_] := 
      Module[{b, c, d, ans}, 
        If[NumberQ[a], 
          If[IntegerQ[a], ans = {Sign[a]}, 
            If[Head[a] == Rational, 
              ans = {Sign[Numerator[a]]/Denominator[a]}, ans = {a}]], 
          b = Terms[a]; c = Select[b, NumberQ]; d = Flatten[SetPower/@c]; 
          ans = Join[+Complement[b, c], d], ans = a]]

Picking Up Variables in a Polynomial of Functions of x:  PolyVarFunc

PolyVarFunc[f_, x_] := 
      Module[{i, j, tmp = Terms[f], mono, ans = {}}, 
        Do[mono = 
            TermsMulFunc[tmp[[i]], x]; 
          Do[If[Head[mono[[j]]] == 
                Power, ans = 
                UnionOrdered[ans, 
                  mono[[j
                        ]][[1
                      ]]^SetPower[
                      mono[[j
                          ]][[2
                        ]]]], 
              ans = UnionOrdered[
                  ans, {mono[[j]]}], 
              ans = UnionOrdered[
                  ans, {mono[[j]]}]], {
              j, Length[mono]}], {i, Length[tmp]}]; ans]

Picking Up Variables in Polynomials of Functions of x in a List:  PolyVarFuncList

PolyVarFuncList[L_, x_] := 
      Module[{i, FL = Flatten[L], ans = {}}, 
        Do[ans = 
            UnionOrdered[ans, 
              PolyVarFunc[FL[[i]], x]], {
            i, Length[FL]}]; ans]

Replacing Variables in Polynomials of Function of x in a List:  PolyVarReplace

PolyVarReplace[L_, x_, z_] := 
      Module[{i, j, xar = {}, zar = {}, ans}, 
        Do[exi = 
            Max[Exponent[L, x[[i]]]]; 
          If[NumberQ[exi] == False, exi = 0]; 
          xar = Join[xar, 
              Table[x[[i]]^j, {j, 
                  exi}]]; 
          zar = Join[zar, 
              Table[z[[i]]^j, {j, 
                  exi}]], {i, Length[x]}]; 
        ans = L //. 
            Table[xar[[i]] -> 
                zar[[i]], {i, 
                Length[xar]}]]

Definition of State Equation and Output Equation
   dx/dt = f(x) + B(x)u
   y = h(x) + D(x)u

xv = Array[x, 4]; uv = Array[u, 1]; \(fx = {x[3], x[4], 
        \(-\(\(2\ x[3]\ x[4]\)\/x[2]\)\) - 3\ Sin[x[1]]\ Cos[x[1]] - 
          \(2\ x[4]\)\/x[2], 
        x[2]\ x[3]\^2 + 2\ x[2]\ x[3] + 3\ x[2]\ Cos[x[1]]\^2}; 
    \  (*\ Vector\ f \((x)\)\ *) 
TrBx = {{0, 0, 0, \(-1\)}}; 
    \ \  (*\ Transpose\ of\ the\ Matrix\ B \((x)\)\ *) \ 
hx = {Log[x[2]]}; 
    \  (*\ Vector\ h \((x)\)\ *) 
TrDx = {{0}}; 
    \ \ \  (*\ Transpose\ of\ the\ Matrix\ D \((x)\)\ *) \)

Immersion into a Polynomial-in-the-State Representation

fx = Expand[fx]; TrBx = Expand[TrBx]; hx = Expand[hx]; TrDx = Expand[TrDx]; fBx = Transpose[Join[{fx}, TrBx]]; hDx = Transpose[Join[{hx}, TrDx]]; alpha = {}; thetalist = {}; finalmat = {}; i = 1; theta = PolyVarFuncList[hDx, xv]; While[theta ≠ {}, Print["theta", i, " =  ", theta]; 
      alpha = Join[alpha, theta]; AppendTo[thetalist, theta]; 
      mat = Dv[theta, xv] . fBx; finalmat = Join[finalmat, mat]; 
      theta = Complement[PolyVarFuncList[mat, xv], alpha]; i = i + 1]dimz = Length[alpha]; zv = Array[z, dimz]; fBz = PolyVarReplace[finalmat, alpha, zv]; TrfBz = Transpose[fBz]; fz = TrfBz[[1]]; TrBz = Rest[TrfBz]; hDz = PolyVarReplace[hDx, alpha, zv]; TrhDz = Transpose[hDz]; hz = TrhDz[[1]]; \(TrDz = Rest[TrhDz]; 

Print["dim z =  ", dimz]; 

    \(Print["z = alpha(x) =  ", alpha]; \)\)Print["f(z) =  ", fz]; Print["Tr[B(z)] =  ", TrBz]; Print["h(z) =  ", hz]; Print["Tr[D(z)] =  ", TrDz];

Immersion into a Quadratic-in-the-State Representation

bet = {}; etalist = {}; finalmat = {}; i = 1; eta = MonomialsList[Join[hDz, fBz], zv]; While[eta ≠ {}, Print["eta", i, " =  ", eta]; 
      bet = Join[bet, eta]; AppendTo[etalist, eta]; mat = Dv[eta, zv]; 
      finalmat = Join[finalmat, mat]; 
      eta = Complement[MonomialsList[mat, zv], bet]; i = i + 1]dimw = Length[bet]; wv = Array[w, dimw]; tempmat1 = MonomialsReplaceList[finalmat, zv, bet, wv]; tempmat2 = MonomialsReplaceList[fBz, zv, bet, wv]; fBw = Expand[tempmat1 . tempmat2]; TrfBw = Transpose[fBw]; fw = TrfBw[[1]]; TrBw = Rest[TrfBw]; hDw = MonomialsReplaceList[hDz, zv, bet, wv]; TrhDw = Transpose[hDw]; hw = TrhDw[[1]]; TrDw = Rest[TrhDw]; \(gam = 
      bet /. Table[
          z[i] -> alpha[[i]], {i, 
            dimz}]; 

Print["dim w =  ", dimw]; 

    \(Print["w = beta(z) =  ", bet]; \)\)Print["w = gamma(x) =  ", gam]; Print["f(w) =  ", fw]; Print["Tr[B(w)] =  ", TrBw]; Print["h(w) =  ", hw]; Print["Tr[D(w)] =  ", TrDw];