# Vehicle Tracking with Noisy Sensors and Missing Data

We will try to pinpoint the location of a moving vehicle with high accuracy using only noisy (and sometimes missing) sensor data. We'll do this with a simple model for the physics of the vehicle, and an assumption that the vehicle's input acceleration is piecewise constant, with only occasional changes. That is, changes to the acceleration are sparse.

## Model

We'll model the vehicle with a discrete-time linear dynamical system with state vector $x_t \in \mathbf{R}^4$ at time $t$, where $(x_{t,0}, x_{t,1})$ is the position of the vehicle in two dimensions, and $(x_{t,2}, x_{t,3})$ is the vehicle velocity. The system has a control input $u_t \in \mathbf{R}^2$, which is the input acceleration in two dimensions. We only observe noisy measurements, $y_t \in \mathbf{R}^2$, of the vehicle's position at each time step.

The system evolves according to the equations

$$
\begin{align}
x_{t+1} &= A x_t + B u_t \\
y_{t} &= C x_t + \nu_t,
\end{align}
$$
with matrices
$$
A = \begin{bmatrix}
1 & 0 & \left(1-\frac{\gamma}{2}\Delta t\right) \Delta t & 0 \\
0 & 1 & 0 & \left(1-\frac{\gamma}{2} \Delta t\right) \Delta t\\
0 & 0 & 1-\gamma \Delta t & 0 \\
0 & 0 & 0 & 1-\gamma \Delta t
\end{bmatrix},
$$

$$
B = \begin{bmatrix}
\frac{1}{2}\Delta t^2 & 0 \\
0 & \frac{1}{2}\Delta t^2 \\
\Delta t & 0 \\
0 & \Delta t \\
\end{bmatrix},
$$

$$
C = \begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0
\end{bmatrix},
$$

where $\nu_t$ is measurement noise for the vehicle position, and $\gamma$ is a damping parameter.

## 1D Model
The recurrence is derived from the following relations in a single dimension. For this subsection, let $x_t, v_t, a_t$ be the vehicle position, velocity, and input acceleration. The true acceleration of the vehicle is $a_t - \gamma v_t$, with $\gamma$ a damping parameter.

The discretized dynamics are then
$$
\begin{align}
x_{t+1} &= x_t + \left(1-\frac{\gamma \Delta t}{2}\right)v_t \Delta t + \frac{1}{2}a_{t} \Delta t^2\\
v_{t+1} &= \left(1-\gamma\right)v_t + a_t \Delta t.
\end{align}
$$

## Contents
- Exploiting Sparsity: $\ell_1$ Regularization
- Handling Outliers: $\ell_1$ Loss for Robust Kalman Filtering
- Missing Data
- Do we need convex optimization?: Comparison with Least-Squares Approaches
    - $\ell_2$ Regularization
    - $\ell_2$ Smoothing: Acceleration
    - $\ell_2$ Smoothing: Velocity
    - $\ell_2$ Smoothing: Position
- (possible) Noisy dynamics

## Helper Functions


```
import matplotlib.pyplot as plt
import numpy as np

def accel_sched(vectors, times, n):
    times = np.array(times, dtype=float)
    times /= times.sum()
    times = np.int_(np.floor(times*n))
    times[-1] += n - times.sum()
    
    count = 0
    u = np.zeros((2,n))
    for v, k in zip(vectors,times):
        v = np.array(v).reshape(2,1)
        u[:,count:count+k] = np.tile(v,(1,k))
        count += k
        
    return u

def plot_state(actual, estimated=None):
    '''
    plot position, speed, and acceleration in the x and y coordinates for
    the actual data, and optionally for the estimated data
    '''
    trajectories = [actual]
    if estimated is not None:
        trajectories.append(estimated)
        
    fig, ax = plt.subplots(3, 2, sharex='col', sharey='row', figsize=(8,8))
    for x, u in trajectories:  
        ax[0,0].plot(t,x[0,:])
        ax[0,1].plot(t,x[1,:])
        ax[1,0].plot(t,x[2,:])
        ax[1,1].plot(t,x[3,:])
        ax[2,0].plot(t,u[0,:])
        ax[2,1].plot(t,u[1,:])
        
    ax[0,0].set_ylabel('x position')
    ax[1,0].set_ylabel('x speed')
    ax[2,0].set_ylabel('x acceleration')
    
    ax[0,1].set_ylabel('y position')
    ax[1,1].set_ylabel('y speed')
    ax[2,1].set_ylabel('y acceleration')
    
    ax[0,1].yaxis.tick_right()
    ax[1,1].yaxis.tick_right()
    ax[2,1].yaxis.tick_right()
    
    ax[0,1].yaxis.set_label_position("right")
    ax[1,1].yaxis.set_label_position("right")
    ax[2,1].yaxis.set_label_position("right")
    
    ax[2,0].set_xlabel('time')
    ax[2,1].set_xlabel('time')

def plot_positions(xtrue, y=None, recovered=None):
    '''
    show point clouds for true, observed, and recovered positions
    '''
    titles = ['True', 'Noisy', 'Recovered']
    trajectories = [xtrue]
    if y is not None:
        trajectories += [y]
    if recovered is not None:
        trajectories += [recovered]

    n = len(trajectories)

    fig, ax = plt.subplots(1, n, sharex=True, sharey=True,figsize=(12, 4))
    if n == 1:
        ax = [ax]
    
    for i,x in enumerate(trajectories):
        ax[i].plot(x[0,:], x[1,:], 'ro', alpha=.1)
        ax[i].set_title(titles[i])
        #ax[i].set_aspect('equal')
        #ax[i].axis('equal')
        
    #x0,x1 = ax[0].get_xlim()
    #y0,y1 = ax[0].get_ylim()
    #ax[0].set_aspect(abs(x1-x0)/abs(y1-y0))
```

## Problem Data
- set problem parameters
- form dynamics matrices
- simulate vehicle movement
- form noisy observations of just the vehicle position


```
n = 1000 # number of timesteps
T = 10 # time will vary from 0 to T with step delt
t, delt = np.linspace(0,T,n,endpoint=True, retstep=True)
gamma = 1 # damping, 0 is no damping
noise_level = .15 # sensing errors

A = np.zeros((4,4))
B = np.zeros((4,2))

A[0,0] = 1
A[1,1] = 1
A[0,2] = (1-gamma*delt/2)*delt
A[1,3] = (1-gamma*delt/2)*delt
A[2,2] = 1 - gamma*delt
A[3,3] = 1 - gamma*delt

B[0,0] = delt**2/2
B[1,1] = delt**2/2
B[2,0] = delt
B[3,1] = delt

x = np.zeros((4,n))
x[:,0] = [0,0,0,0]

# form the piecewise constant acceleration schedule
u = accel_sched([(1,1.2),(-1,.6),(0,-1.3), (-1.2,.6),], [300,400,500,700], n)

# simulate the system forward in time
for i in range(n-1):
    x[:,i+1] = A.dot(x[:,i]) + B.dot(u[:,i])
    
xtrue = x.copy()
utrue = u.copy()

# form noisy observations
y = noise_level*np.random.randn(2,n) + xtrue[0:2,:]

# plot system state
plot_state((xtrue,utrue))

# plot true and noisy position observations
plot_positions(xtrue,y)
```

## Exploiting Sparsity: Total Variation Regularization

The noise in the position measurements makes it difficult to pinpoint the vehicle's location. However, we can exploit the fact that the measurments don't *look* like how a vehicle would actually move. The measurments have the vehicle jumping around erratically. Thus, we'll require our recovered vehicle's movements to follow the known dynamics exactly as

$$
x_{t+1} = A x_t + B u_t,
$$

which will constrain the way in which the model vehicle can move.

This isn't enough, however, beacause no matter what the observed positions $y_t$ are, there is some control schedule $u_t$, which hits those points exactly. But for noisy data like this, that control schedule will probably vary erratically. We can use exploit the fact that we know the changes in the acceleration schedule are sparse. We can try to induce such sparsity by adding total variation regularization to our model:

$$
\begin{array}{ll}
  \mbox{minimize} & \sum_{t=0}^{n} \|y_t - x_{t,0:1}\|_2^2 + \rho\sum_{t=1}^{n} \|u_t - u_{t-1}\|_1\\
  \mbox{subject to} & x_{t+1} = A x_t + B u_t
\end{array}
$$

The first term in the objective penalizes differences between the model and observed positions. The second term penalizes changes in the control schedule. The parameter $\rho$ trades off between the two. For example, setting $\rho = 0$ would match the observed positions exactly, but the acceleration schedule wouln't match our expecation.

## CVXPY Implementation
The code below uses CVXPY to implement and solve the model. The plots compare the true and recovered system state, which end up being very close!


```
import cvxpy as cvx

x = cvx.Variable(4,n)
u = cvx.Variable(2,n)
rho = .1
    
objective = cvx.norm(x[0,:].T - y[0,:])/n + cvx.norm(x[1,:].T - y[1,:])/n
objective += rho*cvx.norm1(u[0,0:n-1] - u[0,1:n])/n + rho*cvx.norm1(u[1,0:n-1] - u[1,1:n])/n
objective = cvx.Minimize(objective)

constraints = []
for i in range(n-1):
    constraints += [x[:,i+1] == A*x[:,i] + B*u[:,i]]

prob = cvx.Problem(objective, constraints)

result = prob.solve(solver=cvx.ECOS, verbose=True)

x = np.array(x.value)
u = np.array(u.value)

plot_state((xtrue,utrue),(x,u))
plot_positions(xtrue,y,x)
```

# Robust Kalman Filtering


```
noise_level = .1
outlier_percentage = .10

outlier_inds = np.random.permutation(n)[:int(np.ceil(outlier_percentage*n))]
# noisy observations
y = noise_level*np.random.randn(2,n) + xtrue[0:2,:]
y[:,outlier_inds] += 10*noise_level*np.random.randn(2,len(outlier_inds)) 

plot_state((xtrue,utrue))
plot_positions(xtrue,y)
```


```
import cvxpy as cvx

x = cvx.Variable(4,n)
u = cvx.Variable(2,n)
rho = 1
    
objective = cvx.norm1(x[0,:].T - y[0,:])/n + cvx.norm1(x[1,:].T - y[1,:])/n
objective += rho*cvx.norm1(u[0,0:n-1] - u[0,1:n])/n + rho*cvx.norm1(u[1,0:n-1] - u[1,1:n])/n
objective = cvx.Minimize(objective)

constraints = []
for i in range(n-1):
    constraints += [x[:,i+1] == A*x[:,i] + B*u[:,i]]

prob = cvx.Problem(objective, constraints)

result = prob.solve(solver=cvx.ECOS, verbose=True)

x = np.array(x.value)
u = np.array(u.value)

plot_state((xtrue,utrue),(x,u))
plot_positions(xtrue,y)
```

# Missing Data

We randomly observe only 40% of the data points. These observations are still noisy.


```
noise_level = .1
present_percentage = .40

inds = np.random.permutation(n)[:int(np.ceil(present_percentage*n))]

# noisy observations
y = noise_level*np.random.randn(2,n) + xtrue[0:2,:]
y = y[:,inds]

plot_state((xtrue,utrue))
plot_positions(xtrue,y)
```


```
import cvxpy as cvx

x = cvx.Variable(4,n)
u = cvx.Variable(2,n)
rho = 1

m = len(inds)
y2 = np.zeros((2,n))
y2[:,inds] = y
    
objective = sum(cvx.square(x[0,ind] - y2[0,ind])/m + cvx.square(x[1,ind] - y2[1,ind])/m for ind in inds )
objective += rho*cvx.norm1(u[0,0:n-1] - u[0,1:n])/n + rho*cvx.norm1(u[1,0:n-1] - u[1,1:n])/n
objective = cvx.Minimize(objective)

constraints = []
for i in range(n-1):
    constraints += [x[:,i+1] == A*x[:,i] + B*u[:,i]]

prob = cvx.Problem(objective, constraints)

result = prob.solve(solver=cvx.ECOS, verbose=True)

x = np.array(x.value)
u = np.array(u.value)

plot_state((xtrue,utrue),(x,u))
plot_positions(xtrue,y,x)
```


```

```

2b957c0cef89804038ffbe294618086fb938cf02
ipynb
Jupyter Notebook
examples/notebooks/WWW/alt_versions/kalman_sparse_input.ipynb
jasondark/cvxpy
56aaa01b0e9d98ae5a91a923708129a7b37a6f18
ECL-2.0 Apache-2.0
2015-01-03T04:02:29.000Z
2021-04-19T14:51:29.000Z
examples/notebooks/WWW/alt_versions/kalman_sparse_input.ipynb
h-vetinari/cvxpy
86307f271819bb78fcdf64a9c3a424773e8269fa
ECL-2.0 Apache-2.0
2015-01-01T19:40:14.000Z
2021-04-18T23:37:31.000Z
examples/notebooks/WWW/alt_versions/kalman_sparse_input.ipynb
h-vetinari/cvxpy
86307f271819bb78fcdf64a9c3a424773e8269fa
ECL-2.0 Apache-2.0
2015-01-02T19:29:39.000Z
2021-04-20T00:50:43.000Z
Qwen/Qwen-72B
1. YES
2. YES