Coverage for src/gncpy/math/__init__.py: 72%
143 statements
« prev ^ index » next coverage.py v7.6.1, created at 2024-09-13 06:15 +0000
1"""Useful math utility functions."""
2import warnings
4import numpy as np
5from copy import deepcopy
8def get_jacobian(x, fnc, f_args=(), step_size=10**-7):
9 """Calculates the jacobian of a function.
11 Numerically calculates the jacobian using the central difference method.
13 Parameters
14 ----------
15 x : numpy array
16 The point to evaluate about.
17 fnc : callable
18 The function to evaluate, must be of the form `f(x, *f_args)`.
19 f_args : tuple, optional
20 Additional argumets for `fnc`. The default is ().
21 step_size : float, optional
22 The step size to use when calculating the jacobian. The default is
23 10**-7.
25 Returns
26 -------
27 jac : N x 1 numpy array
28 The jacobain of the function
29 """
30 inv_step2 = 1 / (2 * step_size)
31 n_vars = x.size
32 J = np.zeros((n_vars, 1))
33 for ii in range(n_vars):
34 x_r = x.copy().astype(float)
35 x_l = x.copy().astype(float)
36 x_r[ii] += step_size
37 x_l[ii] -= step_size
38 J[ii] = (fnc(x_r, *f_args) - fnc(x_l, *f_args))
39 return J * inv_step2
42def get_hessian(x, fnc, f_args=(), step_size=np.finfo(float).eps**(1 / 4)):
43 """Calculates the hessian of a function.
45 Numerically calculates the hessian using the central difference method.
47 Parameters
48 ----------
49 x : numpy array
50 DESCRIPTION.
51 fnc : callable
52 The function to evaluate, must be of the form `f(x, *f_args)`.
53 f_args : tuple, optional
54 Additional arguments for the function. The default is ().
55 step_size : float, optional
56 Step size for differentiation. The default is np.finfo(float).eps**(1 / 4).
58 Returns
59 -------
60 N x N array
61 Hessian of the function.
63 """
64 den = 1 / (4 * step_size**2)
65 n_vars = x.size
66 H = np.zeros((n_vars, n_vars))
67 for ii in range(n_vars):
68 delta_i = np.zeros(x.shape)
69 delta_i[ii] = step_size
71 x_ip = x.astype(float) + delta_i
72 x_im = x.astype(float) - delta_i
73 for jj in range(n_vars):
74 # only get upper triangle since hessian is symmetric
75 if jj < ii:
76 continue
77 delta_j = np.zeros(x.shape)
78 delta_j[jj] = step_size
80 x_ip_jp = x_ip + delta_j
81 x_ip_jm = x_ip - delta_j
82 x_im_jm = x_im - delta_j
83 x_im_jp = x_im + delta_j
85 H[ii, jj] = (fnc(x_ip_jp, *f_args) - fnc(x_ip_jm, *f_args)
86 - fnc(x_im_jp, *f_args) + fnc(x_im_jm, *f_args)) * den
88 # fill full H matrix from upper triangle
89 for ii in range(n_vars):
90 for jj in range(n_vars):
91 if jj >= ii:
92 break
93 H[ii, jj] = H[jj, ii]
94 return H
97def get_state_jacobian(t, x, fncs, f_args, u=None, **kwargs):
98 r"""Calculates the jacobian matrix for the state of a state space model.
100 Notes
101 -----
102 Numerically calculates the jacobian using the central difference method
103 for the state of the standard statespace model
105 .. math::
106 \dot{x} = f(t, x, u)
109 Parameters
110 ----------
111 t : float
112 timestep to evaluate at.
113 x : N x 1 numpy array
114 state to calculate the jocobain about.
115 fncs : list
116 1 function per state in order. They must have the signature
117 `f(t, x, u, *f_args)` if `u` is given or `f(t, x, *f_args)` if `u` is
118 not given.
119 f_args : tuple
120 Additional arguemnts to pass to each function in `fncs`.
121 u : Nu x 1 numpy array, optional
122 the control signal to calculate the jacobian about. The default is
123 None.
124 \*\*kwargs : dict, optional
125 Additional keyword arguments for :meth:`gncpy.math.get_jacobian`.
127 Returns
128 -------
129 jac : N x N numpy array
130 Jaccobian matrix.
131 """
132 n_states = x.size
133 A = np.zeros((n_states, n_states))
134 for row in range(0, n_states):
135 if u is not None:
136 res = get_jacobian(x.copy(),
137 lambda _x, *_f_args: fncs[row](t, _x, u, *_f_args),
138 f_args=f_args, **kwargs)
139 else:
140 res = get_jacobian(x.copy(),
141 lambda _x, *_f_args: fncs[row](t, _x, *_f_args),
142 f_args=f_args, **kwargs)
144 A[[row], :] = res.T
145 return A
148def get_input_jacobian(t, x, u, fncs, f_args, **kwargs):
149 r"""Calculates the jacobian matrix for the input of a state space model.
151 Notes
152 -----
153 Numerically calculates the jacobian using the central difference method
154 for the input of the standard statespace model
156 .. math::
157 \dot{x} = f(t, x, u)
160 Parameters
161 ----------
162 t : float
163 timestep to evaluate at.
164 x : N x 1 numpy array
165 state to calculate the jocobain about.
166 u : Nu x 1 numpy array
167 control input to calculate the jocobian about.
168 fncs : list
169 1 function per state in order. They must have the signature
170 `f(t, x, u, *f_args)` if `u` is given or `f(t, x, *f_args)` if u is
171 not given.
172 f_args : tuple
173 Additional arguemnts to pass to each function in `fncs`.
174 \*\*kwargs : dict, optional
175 Additional keyword arguments for :meth:`gncpy.math.get_jacobian`.
177 Returns
178 -------
179 jac : N x Nu numpy array
180 jacobian matrix.
182 """
183 n_states = x.size
184 n_inputs = u.size
185 B = np.zeros((n_states, n_inputs))
186 for row in range(0, n_states):
187 res = get_jacobian(u.copy(),
188 lambda _u, *_f_args: fncs[row](t, x, _u, *_f_args),
189 **kwargs)
190 B[[row], :] = res.T
191 return B
194def rk4(f, x, h, **kwargs):
195 """Implements a classic Runge-Kutta integration RK4.
197 Parameters
198 ----------
199 f : callable
200 function to integrate, must take x as the first argument and arbitrary
201 kwargs after
202 x : numpy array, or float
203 state needed by function
204 h : float
205 step size
207 Returns
208 -------
209 state : numpy array, or float
210 Integrated state
211 """
212 k1 = h * f(x, **kwargs)
213 k2 = h * f(x + 0.5 * k1, **kwargs)
214 k3 = h * f(x + 0.5 * k2, **kwargs)
215 k4 = h * f(x + k3, **kwargs)
216 return x + (1 / 6) * (k1 + 2 * k2 + 2 * k3 + k4)
219def rk4_backward(f, x, h, **kwargs):
220 """Implements a backwards classic Runge-Kutta integration RK4.
222 Parameters
223 ----------
224 f : callable
225 function to reverse integrate, must take x as the first argument and
226 arbitrary kwargs after
227 x : numpy array, or float
228 state needed by function
229 h : float
230 step size
232 Returns
233 -------
234 state : numpy array, or float
235 Reverse integrated state
236 """
237 k1 = f(x, **kwargs)
238 k2 = f(x - 0.5 * h * k1, **kwargs)
239 k3 = f(x - 0.5 * h * k2, **kwargs)
240 k4 = f(x - h * k3, **kwargs)
241 return x - (h / 6) * (k1 + 2 * k2 + 2 * k3 + k4)
244def log_sum_exp(lst):
245 """Utility function for a log-sum-exponential trick.
247 Parameters
248 ----------
249 lst : list
250 list of values.
252 Returns
253 -------
254 tot : float
255 result of log-sum-exponential calculation.
256 """
257 if len(lst) == 0:
258 return None
259 m_val = max(lst)
260 tot = 0
261 for x in lst:
262 tot = tot + np.exp(x - m_val)
263 tot = np.log(tot) + m_val
264 return tot
267def gamma_fnc(alpha):
268 r"""Implements a gamma function.
270 Notes
271 -----
272 This implements the gamma function as
274 .. math::
275 \Gamma(\alpha) = (\alpha -1)!
277 Todo
278 ----
279 Add support for complex number input
281 Parameters
282 ----------
283 alpha : int
284 number to evaluate the gamma function at.
286 Returns
287 -------
288 int
289 result of the gamma function.
291 """
292 return np.math.factorial(int(alpha - 1))
295def get_elem_sym_fnc(z):
296 if z.size == 0:
297 esf = np.array([[1]])
298 else:
299 z_loc = deepcopy(z).reshape(z.size)
300 i_n = 1
301 i_nminus = 2
303 n_z = z_loc.size
304 F = np.zeros((2, n_z))
306 for n in range(1, n_z + 1):
307 F[i_n - 1, 0] = F[i_nminus - 1, 0] + z_loc[n - 1]
308 for k in range(2, n + 1):
309 if k == n:
310 with warnings.catch_warnings():
311 warnings.filterwarnings("error", message=".*overflow encountered in double_scalars.*")
312 try:
313 F[i_n - 1, k - 1] = z_loc[n - 1] * F[i_nminus - 1, k - 1 - 1]
314 except RuntimeWarning:
315 F[i_n - 1, k - 1] = np.finfo(float).max
316 else:
317 with warnings.catch_warnings():
318 warnings.filterwarnings("error", message=".*overflow encountered in double_scalars.*")
319 try:
320 F[i_n - 1, k - 1] = F[i_nminus - 1, k - 1] \
321 + z_loc[n - 1] * F[i_nminus - 1, k - 1 - 1]
322 except RuntimeWarning:
323 F[i_n - 1, k - 1] = np.finfo(float).max
324 tmp = i_n
325 i_n = i_nminus
326 i_nminus = tmp
327 esf = np.hstack((np.array([[1]]), F[[i_nminus - 1], :]))
328 esf = esf.reshape((esf.size, 1))
329 return esf
332def weighted_sum_vec(w_lst, x_lst):
333 """Calculates the weighted sum of a list of vectors.
335 Parameters
336 ----------
337 w_lst : list of floats, or N numpy array
338 list of weights.
339 x_lst : list of n x 1 numpy arrays, or N x n x 1 numpy array
340 list of vectors to be weighted and summed.
342 Returns
343 -------
344 w_sum : n x 1 numpy array
345 weighted sum of inputs.
346 """
347 if isinstance(x_lst, list):
348 x = np.stack(x_lst)
349 else:
350 x = x_lst
351 if isinstance(w_lst, list):
352 w = np.array(w_lst)
353 else:
354 w = w_lst
355 return np.sum(w.reshape((-1,) + (1,) * (x.ndim - 1)) * x, axis=0)
358def weighted_sum_mat(w_lst, P_lst):
359 """Calculates the weighted sum of a list of matrices.
361 Parameters
362 ----------
363 w_lst : list of floats or numpy array
364 list of weights.
365 P_lst : list of n x m numpy arrays or N x n x n numpy array
366 list of matrices to be weighted and summed.
368 Returns
369 -------
370 w_sum : n x m numpy array
371 weighted sum of inputs.
372 """
373 if isinstance(P_lst, list):
374 cov = np.stack(P_lst)
375 else:
376 cov = P_lst
377 if isinstance(w_lst, list):
378 w = np.array(w_lst)
379 else:
380 w = w_lst
381 return np.sum(w.reshape((-1,) + (1,)*(cov.ndim - 1)) * cov, axis=0)
384def gaussian_kernel(x, sig):
385 """Implements a Gaussian Kernel.
387 Parameters
388 ----------
389 x : float
390 point to evaluate the kernel at.
391 sig : float
392 kernel parameter.
394 Returns
395 -------
396 float
397 kernel value.
399 """
400 return np.exp(-x**2 / (2 * sig**2))
403def epanechnikov_kernel(x):
404 """Implements the Epanechnikov kernel.
406 Parameters
407 ----------
408 x : numpy array
409 state to evaluate the kernel at
411 Returns
412 -------
413 val : float
414 kernal value
415 """
416 def calc_vn(n):
417 if n == 1:
418 return 2
419 elif n == 2:
420 return np.pi
421 elif n == 3:
422 return 4 * np.pi / 3
423 else:
424 return 2 * calc_vn(n - 2) / n
426 n = x.size
427 mag2 = np.sum(x**2)
428 if mag2 < 1:
429 vn = calc_vn(n)
430 val = (x.size + 2) / (2 * vn) * (1 - mag2)
431 else:
432 val = 0
433 return val