navlie.utils.common.jacobian¶

navlie.utils.common.jacobian(fun: Callable, x: ndarray | State, step_size=None, method='forward', *args, **kwargs) → ndarray¶

Compute the Jacobian of a function. Example use:

x = np.array([1, 2]).reshape((-1,1))

A = np.array([[1, 2], [3, 4]])
def fun(x):
    return 1/np.sqrt(x.T @ A.T @ A @ x)

jac_test = jacobian(fun, x, method=method)
jac_true = (- x.T @ A.T @ A)/((x.T @ A.T @ A @ x)**(3/2))

assert np.allclose(jac_test, jac_true, atol=1e-6)

This function is also compatible with State objects, and hence can compute on-manifold derivatives. Example use:

T = SE23State([0.1,0.2,0.3,4,5,6,7,8,9], direction="right")

def fun(T: SE23State):
    # Returns the normalized body-frame velocity vector
    C_ab = T.attitude
    v_zw_a = T.velocity
    v_zw_b = C_ab.T @ v_zw_a
    return v_zw_b/np.linalg.norm(v_zw_b)

jac_fd = jacobian(fun, T)

Parameters:

fun (Callable) – function to compute the Jacobian of
x (Union[np.ndarray, State]) – input to the function
step_size (float, optional) – finite difference step size, by default 1e-6
method (str, optional) – “forward”, “central” or “cs”, by default “forward”. “forward” calculates using a forward finite difference procedure. “central” calculates using a central finite difference procedure. “cs” calculates using the complex-step procedure. If using “cs”, you must be careful to ensure that the function can handle and propagate through complex components.

Returns:

Jacobian of the function, where M is the DOF of the output and N is the DOF of the input.

Return type:

np.ndarray with shape (M, N)

navlie.utils.common.find_nearest_stamp_idx

navlie.utils.common.load_tum_trajectory