Optimal Smoothing and Its Application to Self-driving Cars

To quantify the uncertainty associated with an unobserved variable, \(X\), given some information pertaining to that variable, \(Y\). In a sequential context, this relates to the calculation of the posterior density, \(p(x_{1:T}| y_{1:T})\), where \(x_{1:T} := {x_1, \ldots, x_T }\), \(y_{1:T} := {y_1, \ldots, y_T}\), are the state to estimate and the observation processes, and the discrete time index \(t\in \mathbb{N}^+\).

To facilitate sequential estimation, a first order Markovian state space model is often assumed and defined by its initial probability \(X_1 \sim \mu(\cdot)\) and for \(t > 0\):

\[ X_t| (X_{t-1} = x_{t-1}) \sim f(\cdot| x_{t-1}); \]

Furthermore, the observations are conditional upon the state and are assumed to be statistically independent and distributed according to:

\[ Y_t | (X_t=x_t) \sim g(\cdot| x_t), \]

where we assume that \(\mu(\cdot)\), \(f(\cdot| x_{t-1})\) and \(g(\cdot| x_t)\) are probability densities with respect to some dominating measure.

The joint posterior density \(p(x_{1:T}| y_{1:T})\) is simply given by: \[ p(x_{1:t}| y_{1:t}) \propto \mu(x_1)\prod_{k=2}^{t}f(x_k | x_{k-1}) \prod_{k=1}^{t} g(y_k | x_k) \]

Given the empirical observations \(y_{1:T}\), the optimal fix-interval smoother tries to find a sequence of the state variables that gives the maximum a posterior probability (MAP):

\begin{equation} \label{eq:1} \max_{x_{1:T}} \mu(x_1)\prod_{k=2}^{t}f(x_k | x_{k-1}) \prod_{k=1}^{t} g(y_k | x_k) \end{equation}

subjective to \[ X_t| (X_{t-1} = x_{t-1}) \sim f(\cdot| x_{t-1}); \] \[ Y_t | (X_t=x_t) \sim g(\cdot| x_t), \] \[ X_1 \sim \mu(\cdot) \]

The process model: \[ x_{k} = A_k x_{k-1} + w_{k-1} \] The observation model: \[ y_k = H_k x_k + v_k\]

where \(w_k\) is the process noise which is assumed to be drawn from a zero mean multivariate normal distribution, \(w_k \sim \mathcal{N}(0, Q_k)\), and \(v_k\) is the observation noise which is assumed to be zero mean Gaussian white noise with covariance \(R_k\), \(v_k \sim \mathcal{N}(0, R_k)\). The covariances of the two noise models are assumed stationary over time and are given by: \[ Q = E(w_k, w_k) \] \[ R = E(v_k, v_k) \]

Recall Eq. \ref{eq:1}, for the linear process model and observation model, we have: \[ \mu(x_1) = |2 \pi P|^{-2} \exp(-(x_1 - \hat{x}_1)^{\top} P^{-1} (x_1 - \hat{x}_1)) \] \[ f(x_k| x_{k-1}) = |2 \pi Q|^{-2} \exp(-(x_k - A_{k-1} x_{k-1})^{\top} Q^{-1} (x_k - A_{k-1} x_{k-1}) ) \] \[ g(y_k| x_k) = |2 \pi R|^{-2} \exp(-(y_k - H_k x_k)^{\top} R^{-1} (y_k - H_k x_k)) \] where \(\hat{x}_1\) and \(P\) are the initial state's prior estimation and covariance.

Then, the fix-interval optimal smoother problem can be reformulated as:

\begin{equation} \label{eq:fix-interval-smoothing} \min_{x_{1:T}} \frac{1}{2} \sum_{k=1}^{N} v_k^{\top} R^{-1} v_k + \frac{1}{2} \sum_{k=2}^{T} w_k^{\top} Q^{-1} w_k + \frac{1}{2} (x_1 -\hat{x}_1)^{\top} P^{-1} (x_1 - \hat{x}_1) \end{equation}

where \[ v_k = y_k - H_k x_k \] \[w_k = x_k - A_{k-1} x_{k-1} \]

In practice, Eq. \ref{eq:fix-interval-smoothing} is solved by batch least square (BLS) estimation methods (for example, trajectory optimization) or sequential recursive estimation methods, such as Fraser and Potter (1969), Rauch, Tung, and Striebel (1965), and etc.

The state we are interested in is \[ x := [ p_x, p_y, \theta, \delta, u, s, \dot{\delta}, \ddot{\delta}, \dot{u}, \ddot{u}]^{\top} \in \mathbb{R}^{10} \] and the observation is \[ y := [p_x, p_y, \theta, \delta, u ]^{\top} \in \mathbb{R}^5, \]

where \(p_x\) and \(p_y\) is the position, \(\theta\) is the heading of car, \(s\), \(u\), \(\dot{u}\), and \(\ddot{u}\) are the odometry, the speed, the acceleration and the jerk, \(\delta\), \(\dot{\delta}\), and \(\ddot{\delta}\) are the steering wheel angle, swirl and juke.

The problem is to get a MAP estimate of the full state given noise measurements from manual drive data.

We can use optimal linear quadratic smoother to solve this problem.

The process model:

\begin{equation} x_i = \begin{bmatrix} A_{i-1} & B_{i-1}\\ 0 & 1 \end{bmatrix}_{10\times 10} x_{i-1} + w \end{equation}

where \(A_i\) is the discrete-time state jacobian and \(B_i\) is the discrete-time control jacobain at time step \(i\), \(w\) is the process noise and \(w \sim \mathcal{N}(0, Q)\).

The observation model:

\begin{equation} y_i = \begin{bmatrix} I_{5 \times 5} & 0 \end{bmatrix}_{5 \times 10} x_i + v \end{equation}

where \(v\) is the observation noise and \(v \sim \mathcal{N}(0, R)\).

We also need to notice that the posterior covariance is relatively higher towards the beginning and the end than the middle.