Skip to content

Refactor iterative Lie derivatives for numerical evaluation #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
ManojVS06 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Refactor iterative Lie derivatives for numerical evaluation #4

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
393 changes: 145 additions & 248 deletions CFS/iter_lie.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,253 +1,150 @@
# Function that provides the iterative Lie derivatives
def iter_lie(h,vector_field,z,Ntrunc):
import numpy as np
import sympy as sp
# iter_lie_numeric.py
import numpy as np
from itertools import product
from typing import Callable, List, Tuple

def numerical_gradient(f: Callable[[np.ndarray], float],
z: np.ndarray,
eps: float = 1e-6) -> np.ndarray:
"""Central-difference gradient of scalar function f at point z."""
z = np.asarray(z, dtype=float)
n = z.size
grad = np.zeros(n, dtype=float)
for i in range(n):
z_plus = z.copy(); z_minus = z.copy()
z_plus[i] += eps
z_minus[i] -= eps
grad[i] = (f(z_plus) - f(z_minus)) / (2 * eps)
return grad

def lie_derivative_at_point(f: Callable[[np.ndarray], float],
g: Callable[[np.ndarray], np.ndarray],
z: np.ndarray,
eps: float = 1e-6) -> float:
"""Compute L_g f at point z numerically: (grad f)(z) . g(z)."""
grad_f = numerical_gradient(f, z, eps=eps)
g_val = np.asarray(g(z), dtype=float)
return float(np.dot(grad_f, g_val))

def compose_lie_function(prev_f: Callable[[np.ndarray], float],
g: Callable[[np.ndarray], np.ndarray],
eps: float = 1e-6) -> Callable[[np.ndarray], float]:
"""Return a function that computes L_g(prev_f)(z) numerically."""
def new_f(z: np.ndarray) -> float:
return lie_derivative_at_point(prev_f, g, z, eps=eps)
return new_f

def iter_lie_numeric(h_func: Callable[[np.ndarray], float],
g_funcs: List[Callable[[np.ndarray], np.ndarray]],
z0: np.ndarray,
Ntrunc: int,
eps: float = 1e-6) -> Tuple[List[Callable[[np.ndarray], float]], np.ndarray]:
"""
Returns the list of all the Lie derivatives indexed by the words of length from 1 to Ntrunc
Given the system
dot{z} = g_0(z) + sum_{i=1}^m g_i(z) u_i(t),
y = h(z)
with g_i: S -> IR^n and h: S -> IR, S is a subset of IR^n for all i in {0, ..., n}
The Lie derivative L_eta h of the output function h(z) indexed by the word
eta = x_{i_1}x_{i_2}...x_{i_k} is defined recursively as
L_eta h = L_(x_{i_2}...x_{i_k}) (partial/partial z h) cdot g_{i_1}



Parameters:
-----------
h: symbolic
The symbolic function that represents the output of the system.

vector_field: symbolic array
The array that contains the vector fields of the system.
vector_field =
sp.transpose(sp.Matrix([[(g_0)_1, ..., (g_0)_n],[(g_1)_1, ..., (g_1)_n], ..., [(g_m)_1, ..., (g_m)_n]]))

z: symbolic array
The domain of the vector fields.
z = sp.Matrix([z1, z2, ...., zn])

Ntrunc: int
The truncation length of the words index of the Lie derivatives



Returns:
--------
list: symbolic array

[
L_{x_0} h
.
.
.
L_{x_m} h
---------------
L_{x_0x_0} h
.
.
.
L_{x_mx_m} h
---------------
.
.
.
---------------
L_{x_0...x_0} h
.
.
.
L_{x_m...x_m} h
]



Examples:
---------

# Consider the system:
# dx1 = -x1*x2 + x1 * u1
# dx2 = x1*x2 - x2* u2

# Define the state of the system:
x1, x2 = sp.symbols('x1 x2')
x = sp.Matrix([x1, x2])

# h is a real value function representing the output.
h = x1

# each column of g = [[g10, g11, g12, ..., g1m], [g20, g21, g22, ..., g2m], ..., [gn1, gn2, gn3, ..., gnm]]
# is a vector field where xdot = g0 + g_1*u_1 + g_2*u_2 +...+ g_m*u_m
# g0 = transpose([g10, g20, ...., gn0])
# g_1 = transpose([g11, g21, ...., gn1])
# g_2 = transpose([g12, g22, ...., gn2])
# .
# .
# .
# g_m = transpose([g0m, g1m, ...., gnm])

# Define the vector field
g = sp.transpose(sp.Matrix([[-x1*x2, x1*x2], [x1, 0], [0, - x2]]))

# Ntrunc is the maximum length of words
Ntrunc = 4

Ceta = np.array(iter_lie(h,g,x,Ntrunc).subs([(x[0], 1/6),(x[1], 1/6)]))
Ceta


Numerical iterative Lie derivatives.

Parameters
----------
h_func : callable
Scalar function h(z): R^n -> R.
g_funcs : list of callables
Each g_funcs[i] is a callable g_i(z) returning a length-n array.
z0 : array-like
Point at which to evaluate the Lie derivatives (returned in `values`).
Ntrunc : int
Maximum word length (>=1).
eps : float
Step for finite differences.

Returns
-------
funcs : list of callables
Functions corresponding to each Lie derivative in the same ordering as `values`.
values : np.ndarray
Numeric values of these functions evaluated at z0, shape (total_lderiv,).
"""

if h is None:
raise ValueError("The output function, %s , cannot be null" %h)

if vector_field is None:
raise ValueError("The vector_field, %s, cannot be null" %vector_field)

if z is None:
raise ValueError("The argument of output function and the vector field, %s, cannot be null" %z)

if Ntrunc < 1:
raise ValueError("The truncation length, %s, must be non-negative." %Ntrunc)

if not isinstance(Ntrunc, int):
raise ValueError("The truncation length, %s, must be an integer." %Ntrunc)


# The number of vector fields is obtained.
# num_vfield = m
num_vfield = np.size(vector_field,1)

# Initializes the total number of Lie derivatives.
total_lderiv = 0
# The total number of Lie derivatives of word length less than or equal to the truncation length is computed.
# total_lderiv = num_input + num_input**2 + ... + num_input**Ntrunc
if num_vfield == 1:
for i in range(Ntrunc):
total_lderiv += pow(num_vfield, i+1)
else:
total_lderiv = num_vfield*(1-pow(num_vfield,Ntrunc))/(1-num_vfield)

total_lderiv = int(total_lderiv)


# The list that will contain all the Lie derivatives is initiated.
Ltemp = sp.Matrix(np.zeros((total_lderiv, 1), dtype='object'))
ctrLtemp = np.zeros((Ntrunc+1,1), dtype = 'int')


# ctrLtemp[k] = num_input + num_input**2 + ... + num_input**k, 1<=k<=Ntrunc
for i in range(Ntrunc):
ctrLtemp[i+1] = ctrLtemp[i] + num_vfield**(i+1)


# The Lie derivative L_eta h(z) of words eta of length 1 are computed
LT = sp.Matrix([h]).jacobian(z)*vector_field

# Transforms the lie derivative from a row vector to a column vector
LT = LT.reshape(LT.shape[0]*LT.shape[1], 1)

# Adds the computed Lie derivatives to a repository
Ltemp[:num_vfield, 0] = LT


# The Lie derivatives of the words of length k => 1, L_{x_{i_1}...x_{i_k}}h(z) for all i_j in {0, ..., m},
# are computed at each iteration.

for i in range(1, Ntrunc):
# start_prev_block = num_input + num_input**2 + ... + num_input**(i-1)
start_prev_block = int(ctrLtemp[i-1])
# end_prev_block = num_input + num_input**2 + ... + num_input**i
end_prev_block = int(ctrLtemp[i])
# end_current_block = num_input + num_input**2 + ... + num_input**(i+1)
end_current_block = int(ctrLtemp[i+1])
# num_prev_block = num_input**i
num_prev_block = end_prev_block - start_prev_block
# num_current_block = num_input**(i+1)
num_current_block = end_current_block - end_prev_block

"""
LT =
[
[L_{x_0...x_0x_0}h(z)],
[L_{x_0...x_1x_0}h(z)],
.
.
.
[L_{x_m...x_m}h(z)],
]
these are the Lie derivatives indexed by words of length i
"""
LT = Ltemp[start_prev_block:end_prev_block,0]

"""
LT =
[
[partial/ partial z L_{x_0...x_0x_0}h(z)],
[partial/ partial z L_{x_0...x_1x_0}h(z)],
.
.
.
[partial/ partial z L_{x_m...x_m}h(z)],
]
*
[g_0, g_1, ..., g_m]

=

[
L_{x_0x_0...x_0}h(z) | L_{x_0x_0..x_0x_1x_0}h(z) | | L_{x_0x_m..x_mx_m}h(z)
L_{x_1x_0...x_0}h(z) | L_{x_1x_0..x_0x_1x_0}h(z) | | L_{x_1x_m..x_mx_m}h(z)
L_{x_2x_0...x_0}h(z) | L_{x_2x_0..x_0x_1x_0}h(z) | | L_{x_2x_m..x_mx_m}h(z)
. | . | ... | .
. | . | ... | .
. | . | ... | .
L_{x_mx_0...x_0}h(z) | L_{x_mx_0..x_0x_1x_0}h(z) | | L_{x_mx_m..x_mx_m}h(z)
]

"""
LT = LT.jacobian(z)*vector_field
# Transforms the lie derivative from a row vector to a column vector

"""
LT =
[
L_{x_0x_0...x_0}h(z)
L_{x_1x_0...x_0}h(z)
L_{x_2x_0...x_0}h(z)
.
.
.
L_{x_mx_0...x_0}h(z)
------------------------
L_{x_0x_0...x_1x_0}h(z)
L_{x_1x_0...x_1x_0}h(z)
L_{x_2x_0...x_1x_0}h(z)
.
.
.
L_{x_mx_0...x_1x_0}h(z)
------------------------
.
.
.
------------------------
L_{x_0x_m...x_m}h(z)
L_{x_1x_m...x_m}h(z)
L_{x_2x_m...x_m}h(z)
.
.
.
L_{x_mx_m...x_m}h(z)
]
these are the Lie derivatives indexed by words of length i+1
"""
LT = LT.reshape(LT.shape[0]*LT.shape[1], 1)
# Adds the computed Lie derivatives to the repository
Ltemp[end_prev_block:end_current_block,:]=LT

return Ltemp
if Ntrunc < 1 or not isinstance(Ntrunc, int):
raise ValueError("Ntrunc must be a positive integer.")

m = len(g_funcs) # number of vector fields
z0 = np.asarray(z0, dtype=float)

# compute total number of Lie derivatives: m + m^2 + ... + m^Ntrunc
total = 0
for k in range(1, Ntrunc + 1):
total += m ** k
funcs = [] # list of callables
values = np.zeros(total, dtype=float)

# order: all words of length 1, then length 2, ... (lexicographic by product order)
idx = 0

# first-order Lie derivatives: L_{g_j} h
first_order_funcs = []
for j in range(m):
g = g_funcs[j]
f_j = compose_lie_function(h_func, g, eps=eps)
first_order_funcs.append(f_j)
funcs.append(f_j)
values[idx] = f_j(z0)
idx += 1

prev_order_funcs = first_order_funcs

# higher orders
for k in range(2, Ntrunc + 1):
current_order_funcs = []
# For each word of length k, we want L_{g_{i1} g_{i2} ... g_{ik}} h.
# We produce them by applying L_{g_{i1}} to the function that corresponds to word (i2,...,ik).
# We'll follow ordering: iterate over product(range(m), repeat=k) in lexicographic order.
# To avoid recomputing from scratch, build functions by composing g on shorter-word functions.
# Approach: generate all words of length k using product, then build by iterative composition.
# However, for simplicity and clarity we build by nested loops using prev_order_funcs:
# prev_order_funcs contains functions for words of length k-1 in the same ordering.

# For each g_index from 0..m-1 (this becomes the first symbol in the word),
# we apply L_{g_index} to each function in prev_order_funcs in order.
for i in range(m):
g_i = g_funcs[i]
for prev_f in prev_order_funcs:
new_f = compose_lie_function(prev_f, g_i, eps=eps)
current_order_funcs.append(new_f)
funcs.append(new_f)
values[idx] = new_f(z0)
idx += 1

prev_order_funcs = current_order_funcs

return funcs, values

# ---------------------------
# Example usage (toy system from your docstring)
# dx1 = -x1*x2 + x1 * u1 -> g0 for drift: [-x1*x2, x1]
# dx2 = x1*x2 - x2 * u2 -> g0??? (the doc had 3 vector fields in earlier example)
# We will replicate the example with g0, g1, g2 as in your doc:
if __name__ == "__main__":
# define state functions and vector fields
def h(z):
# output function h = x1
return float(z[0])

# vector fields as callables: g0, g1, g2 producing arrays of length 2
def g0(z):
x1, x2 = z
return np.array([-x1 * x2, x1], dtype=float)

def g1(z):
x1, x2 = z
return np.array([0.0, 0.0], dtype=float) # example; adapt to your system

def g2(z):
x1, x2 = z
return np.array([0.0, -x2], dtype=float)

g_list = [g0, g1, g2] # m = 3

z0 = np.array([1/6, 1/6], dtype=float)
Ntrunc = 3

funcs, vals = iter_lie_numeric(h, g_list, z0, Ntrunc)
print("Total Lie derivatives:", vals.size)
print("Values at z0:\n", vals)