Source code for filterpy.common.kinematic

# -*- coding: utf-8 -*-
#pylint: disable=invalid-name

"""Copyright 2018 Roger R Labbe Jr.

FilterPy library.
http://github.com/rlabbe/filterpy

Documentation at:
https://filterpy.readthedocs.org

Supporting book at:
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

This is licensed under an MIT license. See the readme.MD file
for more information.
"""
from __future__ import (absolute_import, division, print_function,
                        unicode_literals)

import math
import numpy as np
from scipy.linalg import block_diag


[docs]def kinematic_state_transition(order, dt): """ create a state transition matrix of a given order for a given time step `dt`. """ if not(order >= 0 and int(order) == order): raise ValueError("order must be an int >= 0") # hard code common cases for computational efficiency if order == 0: return np.array([[1.]]) if order == 1: return np.array([[1., dt], [0., 1.]]) if order == 2: return np.array([[1., dt, 0.5*dt*dt], [0., 1., dt], [0., 0., 1.]]) # grind it out computationally.... N = order + 1 F = np.zeros((N, N)) # compute highest order row for n in range(N): F[0, n] = float(dt**n) / math.factorial(n) # copy with a shift to get lower order rows for j in range(1, N): F[j, j:] = F[0, 0:-j] return F
[docs]def kinematic_kf(dim, order, dt=1., dim_z=1, order_by_dim=True): """ Returns a KalmanFilter using newtonian kinematics of arbitrary order for any number of dimensions. For example, a constant velocity filter in 3D space would have order 1 dimension 3. Examples -------- A constant velocity filter in 3D space with delta time = .2 seconds would be created with >>> kf = kinematic_kf(dim=3, order=1, dt=.2) >>> kf.F >>> array([[1. , 0.2, 0. , 0. , 0. , 0. ], [0. , 1. , 0. , 0. , 0. , 0. ], [0. , 0. , 1. , 0.2, 0. , 0. ], [0. , 0. , 0. , 1. , 0. , 0. ], [0. , 0. , 0. , 0. , 1. , 0.2], [0. , 0. , 0. , 0. , 0. , 1. ]]) which will set the state `x` to be interpreted as [x, x', y, y', z, z'].T If you set `order_by_dim` to False, then `x` is ordered as [x y z x' y' z'].T As another example, a 2D constant jerk is created with >> kinematic_kf(2, 3) Assumes that the measurement z is position in each dimension. If this is not true you will have to alter the H matrix by hand. P, Q, R are all set to the Identity matrix. H is assigned assuming the measurement is position, one per dimension `dim`. >>> kf = kinematic_kf(2, 1, dt=3.0) >>> kf.F array([[1., 3., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 3.], [0., 0., 0., 1.]]) Parameters ---------- dim : int, >= 1 number of dimensions (2D space would be dim=2) order : int, >= 0 order of the filter. 2 would be a const acceleration model with a stat dim_z : int, default 1 size of z vector *per* dimension `dim`. Normally should be 1 dt : float, default 1.0 Time step. Used to create the state transition matrix order_by_dim : bool, default=True Defines ordering of variables in the state vector. `True` orders by keeping all derivatives of each dimensions) [x x' x'' y y' y''] whereas `False` interleaves the dimensions [x y z x' y' z' x'' y'' z''] """ from filterpy.kalman import KalmanFilter if dim < 1: raise ValueError("dim must be >= 1") if order < 0: raise ValueError("order must be >= 0") if dim_z < 1: raise ValueError("dim_z must be >= 1") dim_x = order + 1 kf = KalmanFilter(dim_x=dim * dim_x, dim_z=dim) F = kinematic_state_transition(order, dt) if order_by_dim: diag = [F] * dim kf.F = block_diag(*diag) else: kf.F.fill(0.0) for i, x in enumerate(F.ravel()): f = np.eye(dim) * x ix, iy = (i // dim_x) * dim, (i % dim_x) * dim kf.F[ix:ix+dim, iy:iy+dim] = f if order_by_dim: for i in range(dim): kf.H[i, i * dim_x] = 1. else: for i in range(dim): kf.H[i, i] = 1. return kf
if __name__ == "__main__": _kf = kinematic_kf(2, 1, dt=3., order_by_dim=False) print(_kf.F) print('\n\n') _kf = kinematic_kf(3, 1, dt=3., order_by_dim=False) print(_kf.F)