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.
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.
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} $$
In [56]:
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))
In [61]:
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)
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.
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!
In [60]:
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)
In [4]:
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)
In [5]:
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)
In [47]:
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)
In [52]:
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)
In [ ]: