pypose.module.EKF

class pypose.module.EKF(model, Q=None, R=None)

Performs Batched Extended Kalman Filter (EKF).

Parameters:

• model (System) – The system model to be estimated, a subclass of pypose.module.NLS.

• Q (Tensor, optional) – The covariance matrices of system transition noise. Ignored if provided during each iteration. Default: None

• R (Tensor, optional) – The covariance matrices of system observation noise. Ignored if provided during each iteration. Default: None

A non-linear system can be described as

\[\begin{aligned} \mathbf{x}_{k+1} &= \mathbf{f}(\mathbf{x}_k, \mathbf{u}_k, t_k) + \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}) \\ \mathbf{y}_{k} &= \mathbf{h}(\mathbf{x}_k, \mathbf{u}_k, t_k) + \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}) \end{aligned} \]

It will be linearized automatically:

\[\begin{align*} \mathbf{z}_{k+1} = \mathbf{A}_{k}\mathbf{x}_{k} + \mathbf{B}_{k}\mathbf{u}_{k} + \mathbf{c}_{k}^1 + \mathbf{w}_k\\ \mathbf{y}_{k} = \mathbf{C}_{k}\mathbf{x}_{k} + \mathbf{D}_{k}\mathbf{u}_{k} + \mathbf{c}_{k}^2 + \mathbf{v}_k\\ \end{align*} \]

EKF can be described as the following five equations, where the subscript \(\cdot_{k}\) is omited for simplicity.

1. Priori State Estimation.

   \[\mathbf{x}^{-} = \mathbf{f}(\mathbf{x}_k, \mathbf{u}_k, t_k) \]

2. Priori Covariance Propagation.

   \[\mathbf{P}^{-} = \mathbf{A}\mathbf{P}\mathbf{A}^{T} + \mathbf{Q} \]

3. Update Kalman Gain

   \[\mathbf{K} = \mathbf{P}\mathbf{C}^{T} (\mathbf{C}\mathbf{P} \mathbf{C}^{T} + \mathbf{R})^{-1} \]

4. Posteriori State Estimation

   \[\mathbf{x}^{+} = \mathbf{x}^{-} + \mathbf{K} (\mathbf{y} - \mathbf{h}(\mathbf{x}^{-}, \mathbf{u})) \]

5. Posteriori Covariance Estimation

   \[\mathbf{P}^{+} = (\mathbf{I} - \mathbf{K}\mathbf{C}) \mathbf{P}^{-} \]

where superscript \(\cdot^{-}\) and \(\cdot^{+}\) denote the priori and posteriori estimation, respectively.

Example

1. Define a discrete-time non-linear system (NLS) model

>>> import torch, pypose as pp
>>> class NLS(pp.module.NLS):
...     def __init__(self):
...         super().__init__()
...
...     def state_transition(self, state, input, t=None):
...         return state.cos() + input
...
...     def observation(self, state, input, t):
...         return state.sin() + input

2. Create a model and filter

>>> model = NLS()
>>> ekf = pp.module.EKF(model)

3. Prepare data

>>> T, N = 5, 2 # steps, state dim
>>> states = torch.zeros(T, N)
>>> inputs = torch.randn(T, N)
>>> observ = torch.zeros(T, N)
>>> # std of transition, observation, and estimation
>>> q, r, p = 0.1, 0.1, 10
>>> Q = torch.eye(N) * q**2
>>> R = torch.eye(N) * r**2
>>> P = torch.eye(N).repeat(T, 1, 1) * p**2
>>> estim = torch.randn(T, N) * p

4. Perform EKF prediction. Note that estimation error becomes smaller with more steps.

>>> for i in range(T - 1):
...     w = q * torch.randn(N) # transition noise
...     v = r * torch.randn(N) # observation noise
...     states[i+1], observ[i] = model(states[i] + w, inputs[i])
...     estim[i+1], P[i+1] = ekf(estim[i], observ[i] + v, inputs[i], P[i], Q, R)
... print('Est error:', (states - estim).norm(dim=-1))
Est error: tensor([5.7655, 5.3436, 3.5947, 0.3359, 0.0639])

Warning

Don’t introduce noise in System methods state_transition and observation for filter testing, as those methods are used for automatically linearizing the system by the parent class pypose.module.NLS, unless your system model explicitly introduces noise.

Note

Implementation is based on Section 13.2 of this book

• Dan Simon, Optimal State Estimation: Kalman, H∞, and Nonlinear Approaches, Cleveland State University, 2006

property Q

The covariance of system transition noise.

property R

The covariance of system observation noise.

forward(x, y, u, P, Q=None, R=None, t=None)

Performs one step estimation.

Parameters:

• x (Tensor) – estimated system state of previous step.

• y (Tensor) – system observation at current step (measurement).

• u (Tensor) – system input at current step.

• P (Tensor) – state estimation covariance of previous step.

• Q (Tensor, optional) – covariance of system transition model. Default: None

• R (Tensor, optional) – covariance of system observation model. Default: None

• t (Tensor, optional) – timestep of system (only for time variant system). Default: None

Returns:

posteriori state and covariance estimation

Return type:

list of Tensor

set_uncertainty(Q=None, R=None)

Set the covariance matrices of transition noise and observation noise.

Parameters:

• Q (Tensor) – batched square covariance matrices of transition noise.

• R (Tensor) – batched square covariance matrices of observation noise.