

In [2]:

    
%matplotlib inline
import os, sys
import numpy as np
import matplotlib.pyplot as plt
sys.path.insert(1, os.path.join(sys.path[0], '..'))

from utils.helpers import load_trajectory_by_path
from utils.kalman import SwitchingKalmanState, SwitchingKalmanFilter, KalmanState
from utils.kalman.models import NDCWPA, NDBrownian



In [3]:

    
positions = load_trajectory_by_path('../data/drivers/3331/100.csv')
n = positions.shape[0]

The Continuous Wiener Process Acceleration is a continuous process on the position, velocity and acceleration where the acceleration of the object is perturbated by a white noise. Once discretized, the dynamic equation is parametrized by the matrices

$$A = \left( \begin{array}{cccccc} 1 & 0 & \Delta t & 0 & \Delta t^{2} / 2 & 0 \\ 0 & 1 & 0 & \Delta t & 0 & \Delta t^{2}/2 \\ 0 & 0 & 1 & 0 & \Delta t & 0 \\ 0 & 0 & 0 & 1 & 0 & \Delta t \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{array} \right) \qquad Q = q \cdot \left( \begin{array}{cccccc} \Delta t^{5} / 20 & 0 & \Delta t^{4} / 8 & 0 & \Delta t^{3} / 6 & 0 \\ 0 & \Delta t^{5} / 20 & 0 & \Delta t^{4} / 8 & 0 & \Delta t^{3} / 6 \\ \Delta t^{4} / 8 & 0 & \Delta t^{3} / 6 & 0 & \Delta t^{2} / 2 & 0 \\ 0 & \Delta t^{4} / 8 & 0 & \Delta t^{3} / 6 & 0 & \Delta t^{2} / 2 \\ \Delta t^{3} / 6 & 0 & \Delta t^{2} / 2 & 0 & \Delta t & 0 \\ 0 & \Delta t^{3} / 6 & 0 & \Delta t^{2} / 2 & 0 & \Delta t \end{array} \right)$$

The Brownian Motion is continuous process on the position only where the fixed position is perturbated by a white noise. Once discretized, the dynamic equation is parametrized by the matrices

$$A = \left( \begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array} \right) \qquad Q = q \cdot \left( \begin{array}{cc} \Delta t & 0 \\ 0 & \Delta t \end{array} \right)$$



In [4]:

    
models = [
    NDCWPA(dt=1.0, q=2e-1, r=10.0, n_dim=2),
    NDBrownian(dt=1.0, q=2e-1, r=10.0, n_dim=2)
]
transmat = np.array([
    [0.99, 0.01],
    [0.01, 0.99]
])
masks = [
    np.array([
        np.diag([1, 0, 1, 0, 1, 0]),
        np.diag([0, 1, 0, 1, 0, 1])
    ]),
    np.array([
        np.diag([1, 0]),
        np.diag([0, 1])
    ])
]

T = np.kron(np.array([1, 0, 0]), np.eye(2))
embeds = [
    [np.eye(6), T],
    [T.T, np.eye(2)]
]

kalman = SwitchingKalmanFilter(models=models, log_transmat=np.log(transmat), masks=masks, embeds=embeds)



In [5]:

    
state = SwitchingKalmanState(n_models=2)
state._states[0] = KalmanState(mean=np.zeros(6), covariance=10.0 * np.eye(6))
state._states[1] = KalmanState(mean=np.zeros(2), covariance=10.0 * np.eye(2))
state.M = np.ones(2) / 2.0



In [6]:

    
filtered_states = [state] * n
for i in xrange(n):
    observation = positions[i]
    state = kalman.filter(state, observation)
    filtered_states[i] = state

smoothed_states = [state] * n
for i in xrange(1, n):
    j = n - 1 - i
    state = kalman.smoother(state, filtered_states[j])
    smoothed_states[j] = state



In [7]:

    
collapsed_states = map(lambda state: state.collapse([np.eye(6), T.T]), smoothed_states)
smoothed_positions = np.asarray(map(lambda state: state.m, collapsed_states))
smoothed_stops = np.asarray(map(lambda state: np.exp(state.M[1]), smoothed_states))



In [8]:

    
fig = plt.figure(figsize=(21,7))
subplot_shape = (1,2)

ax1 = plt.subplot2grid(subplot_shape, (0,0))
plt1, = ax1.plot(smoothed_positions[:,0], smoothed_positions[:,1], 'k-', label='Smoothed trajectory')
plt2, = ax1.plot(smoothed_positions[smoothed_stops>0.50,0], \
    smoothed_positions[smoothed_stops>0.50,1], 'ro', label='Stop points')
ax1.legend(handles=[plt1, plt2])
ax1.set_title('GPS trajectory')

ax2 = plt.subplot2grid(subplot_shape, (0,1))
ax2.plot(range(n), smoothed_stops, 'b-')
ax2.plot(range(n), 0.5 * np.ones(n), 'r--')
ax2.axis([0, n - 1, 0.0, 1.0])
ax2.set_title('Probability of being a stop')

plt.show()



In [29]: