# Kalman filter with control inputs in python? [closed]

i am trying to fit a simple kalman filter with input controls (in this case step input) in python. i am using filterpy (http://filterpy.readthedocs.org/). my code is:

import filterpy
import numpy as np
from filterpy.kalman import KalmanFilter

my_filter = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
numsteps = 80
f = my_filter
init_state = 1.
f.x = np.array([[init_state]])
f.F = np.array([[1]])
f.H = np.array([[1]])
# covariance matrix
state_noise = 0.02
f.P = state_noise
# measurement noise
measure_noise = 0.8
f.R = np.array([[measure_noise]])
# state uncertainty
f.Q = np.array([[state_noise]])
# control inputs
controls = np.array([0]*1 + [0]*19 + [3]*40 + [0]*20)
# get true states
true_states = np.zeros(numsteps)
true_states[0] = init_state
true_states += controls
# state noise
true_states += np.random.normal(0, state_noise, numsteps)
# measurements
measurements = [(s + np.random.normal(0, measure_noise)) for s in true_states]
all_obs = []
estimates = []
num_obs = numsteps
covs = []
for n in range(num_obs):
my_filter.predict(u=controls[n])
my_filter.update(measurements[n])
x = my_filter.x
res = my_filter.y
estimates.append(x[0])
covs.append(my_filter.P[0])

measurements = np.array(measurements)
estimates = np.array(estimates)
# plot results
plt.figure()
plt.subplot(2,1,1)
plt.plot(range(num_obs), measurements, 'b')
plt.plot(range(num_obs), true_states, 'r')
plt.plot(range(num_obs), estimates, 'g')
plt.legend(('measured', 'true', 'estimates'))
plt.subplot(2,1,2)
plt.plot(range(num_obs), covs, 'b')
plt.legend(('covariance',))
plt.show()


the result is:

the filtering does not look right. i would have expected the covariance to go down with time, as filtering estimates should improve with time. what is wrong with this?

are there better methods for fitting kalman filters with controls in python?

• The question should explain the model that is used rather than just give a Python code dump without explanation (would be impossible to determine e.g. whether there are misunderstandings of filterpy syntax or conceptual issues) Jun 29, 2017 at 6:01

the filtering does not look right.

I agree. It's not clear to me what is going on with the filterpy filtering, but here is some information:

I'm not familiar with filterpy, and their documentation was not immediately helpful for me to see how they define the system and filter. But suppose that the generic linear system in question is:

\begin{align} y_t & = Z x_t + \varepsilon_t \\ x_t & = B u_t + T x_{t-1} + R \eta_t \end{align}

with $x_t \sim N(a_t, P_t)$, and where $y_t$ is the measurement, $x_t$ is the state, $\varepsilon_t$ is the measurement noise, $u_t$ is the control affecting the state, and $\eta_t$ is the state noise.

The first iteration of the Kalman filter takes as inputs $a_0$ and $P_0$ (these have to be specified by the user) and provides as output the optimal estimates of $a_1$ and $P_1$.

You appear to be looking at the following case:

\begin{align} y_t & = x_t + \varepsilon_t \\ x_t & = u_t + \eta_t \end{align}

Here, the states are independent (because the transition matrix $T$ is set to zeros); their mean is just the control and they all have the same variance: \begin{align} a_t & \equiv E(x_t) = u_t \\ P_t & \equiv Var(x_t) = Var(\eta_t) = 0.02 \end{align}

Since the noise components are so small, the filter should generally do a very good job estimating $a_t$ and $P_t$.

i would have expected the covariance to go down with time, as filtering estimates should improve with time. what is wrong with this?

This is not generally true. The covariance here is $P_t$, and in this case we would expect it to converge to the true value $0.02$. The problem with the picture you show is that it converges to just about $0.12$, not that it increases.

Whether or not it increases or decreases depends on what values are used to initialize the filter: $a_0$ and $P_0$. Here, you initialized the filter at the true values $(a_0=1, P_0=0.02)$, so if the filter is doing a good job, it shouldn't increase or decrease but should stay right around $0.02$.

Usually, however, these parameters are unknown, and one option is to initialize the filter with a large initial variance (i.e $P_0 = 10^9$), in which case the covariance would decrease to the true value. If you initialized it with a low variance (e.g. $P_0 = 10^{-9}$), the covariance would increase to the true value.

are there better methods for fitting kalman filters with controls in python?

One alternative (not necessarily better) is the Kalman filter that will be included in the next version (0.7) of Statsmodels (the code is in Github master right now). The emphasis in Statsmodels is parameter estimation (so that filtering is typically performed across an entire dataset rather than one observation at a time) and the Kalman filter is defined slightly differently (it uses an alternate timing of the transition equation: $x_{t+1} = u_t + T x_t + \eta_t$ - you can see the effect of this timing difference in the way I defined the state_intercept, below).

That being said, here is some code to perform the filtering:

import numpy as np
import statsmodels.api as sm
import matplotlib.pyplot as plt

# --- True values --- #
init_state = 1.
# covariance matrix
state_noise = 0.02
# measurement noise
measure_noise = 0.8

# --- Generate Data --- #
np.random.seed(1234)
numsteps = 80

# control inputs
controls = np.array([0]*1 + [0]*19 + [3]*40 + [0]*20)
# get true states
true_states = np.zeros(numsteps)
true_states[0] = init_state
true_states += controls
# state noise
true_states += np.random.normal(0, state_noise, numsteps)

# measurements
measurements = np.array([(s + np.random.normal(0, measure_noise)) for s in true_states])

# --- Create the model --- #
mod = sm.tsa.statespace.MLEModel(measurements, k_states=1, k_posdef=1)
mod['design'] = [1.]
mod['obs_cov'] = [measure_noise]
mod['state_intercept'] = np.r_[controls[1:], np.nan][None,:]
mod['selection'] = [1.]
mod['state_cov'] = [state_noise]
mod.initialize_known(1+controls[0:1], [[0.02]])
res = mod.filter()

# --- Plot the results --- #
fig, axes = plt.subplots(2, 1, figsize=(10,6))
time = range(numsteps)
axes[0].plot(time, measurements, 'b', label='measured')
axes[0].plot(time, true_states, 'r', label='true')
axes[0].plot(time, res.filtered_state[0], 'g', label='estimates')
#axes[0].plot(time, res.forecasts[0], 'g', label='estimates')
axes[0].legend()

axes[1].plot(time, res.filtered_state_cov[0,0,:], 'b', label='covariance')
axes[1].legend()


Resulting in the following figures:

Getting back to the discussion of the variance increasing or decreasing:

# --- Alternate initializations --- #
fig, axes = plt.subplots(2,1,figsize=(10,6))

# Decreasing covariance
mod.initialize_known([0], [[1e9]])
res = mod.filter()
axes[0].plot(time, res.filtered_state_cov[0,0,:], 'b', label='decreasing')
axes[0].legend()

# Increasing covariance
mod.initialize_known([0], [[1e-9]])
res = mod.filter()
axes[1].plot(time, res.filtered_state_cov[0,0,:], 'b', label='increasing')
axes[1].legend()


state transistion matrix should be zero, since there is no state in your system, control transition matrix should be one, since you output control signal directly.

 f.F = np.array([[0]]) f.B = np.array([[1]])