Sympy: How to compute Lie derivative of matrix with respect to a vector field

Question

I have a system(x'=f(x)+g(x)u), such that f(x) is f:R3->R3 and g(x) is g:R3->R(3x2) . My system is

As you can see, it is a MIMO nonlinear control system and I wish to find the controllability matrix for my system. Controllability matrix in this case is formulated by
C=[g [f,g] [f,[f,g]] ..],
where [f,g] denotes the lie bracket operation between f and g. That is the reason why I need to compute Lie derivative of a matrix with respect to a vector field and vice versa. Because [f,g]=f dg/dx-g df/dx

Here in my system, f is 3x1 and g is 3x2 as there are two inputs available. And I wish to calculate the above matrix C in Python. My system is
f=sm.Matrix([[x1**2],[sin(x1)+x3**2],[cos(x3)+x1**2]]) and
g=sm.Matrix([[cos(x1),0],[x1**2,x2],[0,0]]) .

My code is:

from sympy.diffgeom import *
from sympy import sqrt,sin,cos

M     = Manifold("M",3)
P     = Patch("P",M)

coord          = CoordSystem("coord",P,["x1","x2","x3"])
x1,x2,x3       = coord.coord_functions()
e_x1,e_x2,e_x3 = coord.base_vectors()

f      = x1**2*e_x1 + (sin(x1)+x3**2)*e_x2 + (cos(x3) + x1**2)*e_x3
g      = (cos(x1))*e_x1+(x1**2,x2)*e_x2 + 0*e_x3

#h1    = x1
#h2    = x2
#Lfh1  = LieDerivative(f,h1)
#Lfh2  = LieDerivative(f,h2)
#print(Lfh1)
#print(Lfh2)

Lfg    = LieDerivative(f,g)
print(Lfg)

Why isn't my code giving me correct answer?

Answer 1

The only error in your code is due to the tuple used for multiple inputs. For LieDerivative in Sympy.diffgeom to work you need a vector field defined properly.

For single input systems, the exact code that you have works without the tuple, so, in that case, for example if you have g = (cos(x1))*e_x1+x1**2*e_x2 + 0*e_x3

(that is g(x) is 3 x 1 matrix with just the first column). Then, making the above change, you get the correct Lie derivatives.

For multiple input case, (as in your question), you can simply separate the two columns into g1 and g2 and proceed as the above case. This works because for multiple inputs case, See math here

where g_1 and g_2 are the two columns. The final result for Lgh is a 1 x 2 matrix which you can basically get if you have the two results calculated above (Lg1h and Lg2h).

Code -

from sympy.diffgeom import *
from sympy import sqrt,sin,cos
from sympy import *
M = Manifold("M",3)
P = Patch("P",M)

coord          = CoordSystem("coord",P,["x1","x2","x3"])
x1,x2,x3       = coord.coord_functions()
e_x1,e_x2,e_x3 = coord.base_vectors()

f  = x1**2*e_x1 + (sin(x1)+x3**2)*e_x2 + (cos(x3) + x1**2)*e_x3
g1 = (cos(x1))*e_x1+(x1**2)*e_x2 + 0*e_x3
g2 = 0*e_x1+ x2*e_x2 + 0*e_x3
h = x1

Lg1h = LieDerivative(g1,h)
Lg2h = LieDerivative(g2,h)
Lgh = [Lg1h, Lg2h]
print(Lgh)