pypose.module.PF

class pypose.module.PF(model, Q=None, R=None, particles=1000)

Performs Batched Particle Filter (PF).

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

• particles (Int, optional) – The number of particle. Default: 1000

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{g}(\mathbf{x}_k, \mathbf{u}_k, t_k) + \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}) \end{aligned} \]

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

1. Generate Particles.

   \[\begin{aligned} &\mathbf{x}_{k}=\mathbf{p}(\mathbf{x},n\cdot\mathbf{P}_{k},N)&\quad k=1,...,N \\ &\mathbf{P}_{k}=\mathbf{P} &\quad k=1,...,N \end{aligned} \]

   where \(N\) is the number of Particles and \(\mathbf{p}\) is the probability density function (PDF), \(n\) is the dimension of state.

2. Priori State Estimation.

   \[\mathbf{x}^{-}_{k} = f(\mathbf{x}_{k}, \mathbf{u}_{k}, t) \]

3. Relative Likelihood.

   \[\begin{aligned} & \mathbf{q} = \mathbf{p} (\mathbf{y} |\mathbf{x}^{-}_{k}) \\ & \mathbf{q}_{i} = \frac{\mathbf{q}_{i}}{\sum_{j=1}^{N}\mathbf{q}_{j}} \end{aligned} \]

4. Resample Particles.

   1. Generate random numbers \(r_i \sim U(0,1), i=1,\cdots,N\), where \(U\) is uniform distribution.

   2. Find \(j\) satisfying \(\sum_{m=1}^{j-1}q_m\leq r_i<\sum_{m=1}^{j}q_m\), then new particle \(\mathbf{x}^{r-}_{i}\) is set equal to old particle \(\mathbf{x}^{-}_{j}\).

5. Refine Posteriori And Covariances.

   \[\begin{aligned} & \mathbf{x}^{+} =\frac{1}{N} \sum_{i=1}^{n}\mathbf{x}^{r-}_{i} \\ & P^{+} = \sum_{i=1}^{N} (\mathbf{x}^{+} - \mathbf{x}^{r-}_{i}) (\mathbf{x}^{+} - \mathbf{x}^{r-}_{i})^{T} + \mathbf{Q} \end{aligned} \]

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()
>>> pf = pp.module.PF(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 PF 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] = pf(estim[i], observ[i] + v, inputs[i], P[i], Q, R)
... print('Est error:', (states - estim).norm(dim=-1))
Est error: tensor([10.7083,  1.6012,  0.3339,  0.1723,  0.1107])

Note

Implementation is based on Section 15.2 of this book

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

compute_cov(a, b, Q=0)

Compute covariance of two set of variables.

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 (int, optional) – set system timestamp for estimation. If None, current system time is used. Default: None.

Returns:

posteriori state and covariance estimation

Return type:

list of Tensor

generate_particles(x, P)

Randomly generate particles

Parameters:

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

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

Returns:

particles

Return type:

list of Tensor

relative_likelihood(y, ye, R)

Compute the relative likelihood

resample_particles(q, x)

Resample the set of a posteriori particles