Jacobians in navlie

As you may know, many state estimation algorithm require access to process model and measurement model Jacobians, with respect to the state and sometimes other inputs. For states belonging to Lie groups, algorithms will require Lie Jacobians, which differ from traditional derivatives as they conform to the constraints of the group. The abstraction provided by the \(\oplus\) and \(\ominus\) operators (implemented with State.plus and State.minus respectively) allow for a generic definition of a derivative:

\[\left.\frac{D f(\mathcal{X})}{D \mathcal{X}}\right|_{\bar{\mathcal{X}} }\triangleq \left.\frac{\partial f(\bar{\mathcal{X}} \oplus \delta \mathbf{x}) \ominus f(\bar{\mathcal{X}})}{\partial \delta \mathbf{x}}\right|_{\delta \mathbf{x} = \mathbf{0}},\]

which can be shown to fall back to a traditional derivatives when \(\oplus\) and \(\ominus\) are defined to be regular addition/subtraction. This derivative definition is used universally throughout navlie, and roughly follows what is done in the Micro Lie Theory paper. In that reference, seperate definitions are given for “left” and “right” derivatives, whereas we have aggregated them into a single definition, with left and right derivatives naturally arising from the choice of \(\oplus\) and \(\ominus\) operators.

If you dont want to worry about this, the good news is that navlie computes Lie Jacobians for you by default using finite difference. However, finite difference can have some drawbacks, such as being computationally expensive and less accurate than analytic derivatives. In this notebook, we will show you how to use analytic derivatives in navlie, which offer the maximum accuracy and speed.

Jacobians - Traditional Approach

Recall the traditional approach to the previous example. We had defined the state to be \(\mathbf{x} = [\theta, x, y]^T\) and the process model to be

\[\begin{split}\begin{aligned} \theta_{k+1} &= \theta_k + \omega_k \Delta t \\ x_{k+1} &= x_k + v_k \cos(\theta_k) \Delta t \\ y_{k+1} &= y_k + v_k \sin(\theta_k) \Delta t \end{aligned}\end{split}\]

where \(\omega_k\) is the angular velocity and \(v_k\) is the linear velocity. Since the state is just a regular vector, Lie Jacobians fall back to regular Jacobians, and standard derivative techniques lead to the following expressions for the process model Jacobian with respect to the state

\[\begin{split}\mathbf{F} := \frac{\partial f(\mathbf{x}_k, \mathbf{u}_k)}{\partial \mathbf{x}_k} = \begin{bmatrix} 1 & 0 & 0 \\ -v_k \sin(\theta_k) \Delta t & 1 & 0 \\ v_k \cos(\theta_k) \Delta t & 0 & 1 \end{bmatrix}\end{split}\]

To implement this Jacobian in navlie, all we need to do is override the jacobian() method in our process model, and it will get used automatically by the estimation algorithms. Adding this to our process model from before:

[2]:

import navlie as nav
import numpy as np

from navlie.lib import VectorInput, VectorState

Q = np.eye(2) * 0.1**2 # Input noise covariance with 0.1 m/s of standard deviation

class WheeledRobot(nav.ProcessModel):
    def __init__(self, input_covariance):
        self.Q = input_covariance

    def evaluate(self, x: VectorState, u: VectorInput, dt: float) -> VectorState:
        x_next = x.copy()
        x_next.value[0] += u.value[0] * dt
        x_next.value[1] += u.value[1] * dt * np.cos(x.value[0])
        x_next.value[2] += u.value[1] * dt * np.sin(x.value[0])
        return x_next

    def input_covariance(self, x: VectorState, u: VectorInput, dt: float) -> np.ndarray:
        return self.Q

    def jacobian(self, x:VectorState, u: VectorInput, dt: float) -> np.ndarray:
        F = np.eye(3)
        F[1, 0] = -u.value[1] * dt * np.sin(x.value[0])
        F[2, 0] = u.value[1] * dt * np.cos(x.value[0])
        return F

process_model = WheeledRobot(Q) # instantiate it

Now, lets just double check that we did everything correctly by comparing with finite difference. All process models inherit the jacobian_fd() method, which computes the Jacobian using finite difference. We can use this to compare with our analytic Jacobian.

[3]:

x = VectorState([1,2,3])
u = VectorInput([0.1, 0.2])
dt = 0.1

print("Analyical:")
print(process_model.jacobian(x, u, dt))

print("\nFinite difference:")
print(process_model.jacobian_fd(x, u, dt))

Analyical:
[[ 1.          0.          0.        ]
 [-0.01682942  1.          0.        ]
 [ 0.01080605  0.          1.        ]]

Finite difference:
[[ 1.          0.          0.        ]
 [-0.01682943  1.          0.        ]
 [ 0.01080604  0.          1.        ]]

The Jacobians match almost perfectly, but differ slightly due to errors in finite difference. This is expected, as finite difference is only an approximation. Nevertheless, finite difference is generally sufficiently accurate for most applications.

Note: The jacobian methods must always return a 2D array.

Moving on to the measurement model, which was previously defined to be

\[\mathbf{y}_k = || \mathbf{r}_k - \boldsymbol{\ell}^{(i)} ||\]

where \(\mathbf{r}_k\) is the robot’s position and \(\boldsymbol{\ell}^{(i)}\) is the \(i\)th landmark. The measurement model Jacobian with respect to the state is

\[\mathbf{G} := \frac{\partial g(\mathbf{x}_k)}{\partial \mathbf{x}_k} = \begin{bmatrix} 0 & \frac{(\mathbf{r}_k - \boldsymbol{\ell}^{(i)})^T}{||(\mathbf{r}_k - \boldsymbol{\ell}^{(i)})||} \end{bmatrix},\]

and we can implement this in navlie by again overriding the jacobian() method in our measurement model. Adding this to our measurement model from before:

[4]:

class RangeToLandmark(nav.MeasurementModel):
    def __init__(self, landmark_position: np.ndarray):
        self.landmark_position = landmark_position

    def evaluate(self, x: VectorState) -> np.ndarray:
        return np.linalg.norm(x.value[1:] - self.landmark_position)

    def covariance(self, x: VectorState) -> np.ndarray:
        return 0.1**2

    def jacobian(self, x: VectorState) -> np.ndarray:
        G = np.zeros((1, 3))
        r = x.value[1:]
        G[0,1:] = (r - self.landmark_position) / np.linalg.norm(r - self.landmark_position)
        return G

meas_model = RangeToLandmark(np.array([1, 2]))

print("\nAnalyical:")
print(meas_model.jacobian(x))

print("\nFinite difference:")
print(meas_model.jacobian_fd(x))


Analyical:
[[0.         0.70710678 0.70710678]]

Finite difference:
[[0.         0.70710696 0.70710696]]

We can again see that the results match nicely.

Jacobians - Lie Group Approach

Now, lets see how to implement analytical Jacobians when states belong to Lie groups. In the previous example the state was \(\mathbf{T} \in SE(2)\) and the process model was

\[\mathbf{T}_{k+1} = \mathbf{T}_k \exp(\boldsymbol{\varpi}^\wedge_k \Delta t).\]

To derive the Jacobian, we can “perturb” both sides of the equation and manipulate. This is a common technique for deriving Lie Jacobians, and for computing matrix-vector derivatives in general. For more details, we recommend reading State Estimation for Robotics by Tim Barfoot.

\[\begin{split}\begin{aligned} \mathbf{T}_{k+1} &= \mathbf{T}_k \exp(\boldsymbol{\varpi}^\wedge_k \Delta t) \\ \bar{\mathbf{T}}_{k+1} \exp(\delta \boldsymbol{\xi}_{k+1}^\wedge) &= \bar{\mathbf{T}}_{k} \exp(\delta \boldsymbol{\xi}_{k}^\wedge) \exp(\boldsymbol{\varpi}^\wedge_k \Delta t) \\ \exp(\delta \boldsymbol{\xi}_{k+1}^\wedge) &= \underbrace{\bar{\mathbf{T}}_{k+1}^{-1} \bar{\mathbf{T}}_{k}}_{\exp(\boldsymbol{\varpi}^\wedge \Delta t)^{-1}} \exp(\delta \boldsymbol{\xi}_{k}^\wedge) \exp(\boldsymbol{\varpi}^\wedge_k \Delta t) \\ \exp(\delta \boldsymbol{\xi}_{k+1}^\wedge) &= \exp((\mathbf{Ad}(\exp(\boldsymbol{\varpi}^\wedge \Delta t)^{-1}) \delta \boldsymbol{\xi}_{k})^\wedge) \\ \delta \boldsymbol{\xi}_{k+1} &= \underbrace{\mathbf{Ad}(\exp(\boldsymbol{\varpi}^\wedge \Delta t)^{-1})}_{\mathbf{F}} \delta \boldsymbol{\xi}_{k} \end{aligned}\end{split}\]

There, we used the adjoint matrix \(\mathbf{Ad}(\cdot)\), to invoke the identity \(\mathbf{X}^{-1} \exp(\boldsymbol{\xi}^\wedge) \mathbf{X} = \exp(\mathbf{Ad}(\mathbf{X}) \boldsymbol{\xi}^\wedge)\), which is true for any \(\mathbf{X} \in SE(2)\). The adjoint matrix for \(SE(2)\) is given by

\[\begin{split}\mathbf{Ad}(\mathbf{T}) = \begin{bmatrix} 1 & \mathbf{0} \\ - \boldsymbol{\Omega} \mathbf{r} & \mathbf{C} \end{bmatrix}\end{split}\]

where \(\boldsymbol{\Omega} = \begin{bmatrix} 0 & -1 \\ 1 &0 \end{bmatrix}\).

Note: in this Jacobian derivation, we perturbed the state \(\mathbf{T} = \bar{\mathbf{T}} \exp(\delta \boldsymbol{\xi}^\wedge)\) “on the right” because that corresponds to what was implemented in the plus() method of our SE2State class. It is important to be consistent here for everything to work.

Now, we can implement this Jacobian in navlie by overriding the jacobian() method in our process model. Adding this to our process model from before:

[6]:

from navlie.lib import SE2State, VectorInput
from scipy.linalg import expm


def wedge_se2(x:np):
    return np.array([[   0, -x[0], x[1]],
                     [x[0],     0, x[2]],
                     [   0,     0,    0]])

def adjoint_se2(T:np.ndarray):
    C = T[:2, :2]
    r = T[:2, 2]
    Omega = np.array([[0, -1], [1, 0]])
    Ad = np.zeros((3,3))
    Ad[0,0] = 1
    Ad[1:,1:] = C
    Ad[1:,0] = - Omega @ r
    return Ad


class WheeledRobotSE2(nav.ProcessModel):
    def __init__(self, input_covariance_matrix):
        self.Q = input_covariance_matrix

    def evaluate(self, x:SE2State, u:VectorInput, dt:float):
        u = np.array([u.value[0], u.value[1], 0])
        x_next = x.copy()
        x_next.value = x.value @ expm(wedge_se2(u * dt))
        return x_next
    def input_covariance(self, x:SE2State, u:VectorInput, dt:float):
        return self.Q

    def jacobian(self, x:SE2State, u:VectorInput, dt:float):
        u = np.array([u.value[0], u.value[1], 0])
        return adjoint_se2(expm(-wedge_se2(u * dt)))

Q = np.eye(2) * 0.1**2
process_model = WheeledRobotSE2(Q)
x = SE2State(expm(wedge_se2(np.array([1,2,3]))))
u = VectorInput([4, 2])
dt = 0.1

print("\nAnalyical:")
print(process_model.jacobian(x, u, dt))

print("\nFinite difference:")
print(process_model.jacobian_fd(x, u, dt))


Analyical:
[[ 1.          0.          0.        ]
 [ 0.0394695   0.92106099  0.38941834]
 [ 0.19470917 -0.38941834  0.92106099]]

Finite difference:
[[ 1.00000000e+00 -1.10747782e-11 -1.10747782e-11]
 [ 3.94695038e-02  9.21060995e-01  3.89418343e-01]
 [ 1.94709171e-01 -3.89418342e-01  9.21060994e-01]]

Note that when using Lie groups, our Jacobian no longer has dependence on the state itself. This can be a tangible advantage when the state estimate has high uncertainty, where using a traditional approach can result in excessive linearization errors when the state estimate is far from the true value.

previous

Toy Problem - Lie group approach

next

Composite States