# 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:

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

"""
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)