diff --git a/CFS/iter_lie.py b/CFS/iter_lie.py index b80a9f4..1251efe 100644 --- a/CFS/iter_lie.py +++ b/CFS/iter_lie.py @@ -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)