Kalman filter is a technology to achieve Bayes filter. It’s an optimal linear state estimation method (eqauls to optimal linear filter under the minimum mean square error criterion), which seeks the state vector that best fits the observed data by mathematical methods.

In localization and navigation of robots, kalman filter is the most commonly used method to estimante the robot’s status, as it solve the problem recursively and only the estimation value in the last sampling period and the measurement in the current sampling period are needed, which saves memory and calculating time.

Principle

Prerequisite

Before using kalman filter, 3 prerequisites should be statisfied:

  • State transition probability must be the linear function fo parameters with random Gaussian noise
  • Measurement probabilty also has to contain Gaussian component
  • Initial confidence is normally distributed

Problem Description

Define a status vector with random-Gaussian-noise parameters:

xt=Atxt−1+Btut+ϵt\begin{align*} x_t = A_tx_{t-1} + B_tu_t + \epsilon_t \end{align*}

In this equation, xtx_t and xt−1x_{t-1} represents the status vector (n×1n \times 1) in moment tt and t−1t-1 respectively; AA is the state transition matrix in nxnn x n dimension; utu_t represents the control vector (m×1m \times 1) on moment tt; BtB_t is an optional gain matrix of control vector in n×mn \times m dimension; ϵt\epsilon_t is a random Gaussian vector that represents the uncertainty introduced by status transition.

Note: all vectors are column vectors in the following form:

xt=[x1,tx2,t..xn,t]andut=[u1,tu2,t..um,t]\begin{align*} x_t = \begin{bmatrix} x_{1, t} \\ x_{2, t} \\ . \\ . \\ x_{n, t} \end{bmatrix} \quad and \quad u_t = \begin{bmatrix} u_{1, t} \\ u_{2, t} \\ . \\ . \\ u_{m, t} \end{bmatrix} \end{align*}

Define another observation vector (k∗1k * 1):

zt=Htxt+δt\begin{align*} z_t = H_tx_{t} + \delta_t \end{align*}

In this equation, HtH_t is the gain matrix in k∗nk * n dimension; δt\delta_t is the observation noise.

Example

Give an example to explain the above equations. Suppose that we are tracking a car in uniform motion through GPS and other sensors. In general, we would like to get information about its position and velocity (or orientation) when we observe a vehicle.
Therefore, status vector could be defined as xt=(xt,yt,vx,vy)Tx_t = (x_t, y_t, v^x, v^y)^T. Simultanenously, it’s known that in uniform motion the control vector is ut=(vx,vy)Tu_t = (v^x, v^y)^T.

As a result, the matrix AA and BB are defined as follow:

A=[1000010000100001]andB=[dt00dt0000]\begin{align*} A = \begin{bmatrix} 1 \quad 0 \quad 0 \quad 0 \\ 0 \quad 1 \quad 0 \quad 0 \\ 0 \quad 0 \quad 1 \quad 0 \\ 0 \quad 0 \quad 0 \quad 1 \\ \end{bmatrix} \quad and \quad B = \begin{bmatrix} dt \quad 0 \\ 0 \quad dt \\ 0 \quad 0 \\ 0 \quad 0 \\ \end{bmatrix} \end{align*}

Finally, we can get the status equation:

xt=Atxt−1+Btut+ϵt⇒[xtytvxvy]=[1000010000100001][xt−1yt−1vxvy]+[dt00dt0000][vxvy]+ϵt=[xt−1+vx∗dtyt−1+vy∗dtvxvy]+ϵt\begin{align*} x_t = A_tx_{t-1} + B_tu_t + \epsilon_t \Rightarrow \begin{bmatrix} x_t \\ y_t \\ v^x \\ v^y \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} x_{t-1} \\ y_{t-1} \\ v^x \\ v^y \end{bmatrix} + \begin{bmatrix} dt \quad 0 \\ 0 \quad dt \\ 0 \quad 0 \\ 0 \quad 0 \\ \end{bmatrix} \begin{bmatrix} v^x \\ v^y \end{bmatrix} + \epsilon_t = \begin{bmatrix} x_{t-1} + v^x * dt \\ y_{t-1} + v^y * dt \\ v^x \\ v^y \end{bmatrix} + \epsilon_t \end{align*}

Similarly, HH is also required to be defined. As we can get the information about vehicle’s position from GPS sensor, the observation vector is defined as zt=(xt,yt)Tz_t = (x_t, y_t)^T.
HH is a k∗nk * n dimensional matrix, so that we can get the equation as follow:

zt=Htxt+δt⇒[10000100][xtytvxvy]+δt=[xtyt]+δt\begin{align*} z_t = H_tx_{t} + \delta_t \Rightarrow \begin{bmatrix} 1 \quad 0 \quad 0 \quad 0 \\ 0 \quad 1 \quad 0 \quad 0 \\ \end{bmatrix} \begin{bmatrix} x_t \\ y_t \\ v^x \\ v^y \end{bmatrix} + \delta_t = \begin{bmatrix} x_t \\ y_t \end{bmatrix} + \delta_t \end{align*}

Algorithm

After defining xtx_t, ztz_t, AA and HH, we can use kalman fitler algorithm to estimate xtx_t.

Prediction

x^t−=Axt−1^+ButP^t−=APt−1^AT+Q\begin{align*} \hat{x}^-_t = A\hat{x_{t-1}} + Bu_t \\ \hat{P}^-_t = A\hat{P_{t-1}}A^T + Q \end{align*}

Update

Kt=P^t−HTHTP^t−HT+Rx^t=x^t−+Kt(zt−Hx^t−)P^t=(I−KtH)P^t−\begin{align*} K_t = \frac{\hat{P}^-_tH^T}{H^T\hat{P}^-_tH^T + R} \\ \hat{x}_t = \hat{x}^-_t + K_t(z_t - H\hat{x}^-_t) \\ \hat{P}_t = (I - K_tH)\hat{P}^-_t \end{align*}

Explanaiton of parameters in these equations:

  • x^t−\hat{x}^-_t: the priori estimated value of status vector at moment tt
  • x^t\hat{x}_t: the posterior estimated value of status vector at moment tt, the result of kalman filter
  • x^t−1\hat{x}_{t-1}: the posterior estimated value of status vector at moment t−1t-1
  • AA: status transition matrix
  • BB: an optional gain matrix of control vector
  • utu_{t}: control vector at moment tt
  • P^t−\hat{P}^-_t: the priori estimated covariance at moment tt
  • P^t−1\hat{P}_{t-1}: the posterior estimated covariance at moment tt, a part of result of filter
  • KtK_t: kalman gain coefficient, intermediate variable of equation
  • QQ: covariance of process noise that represents the error between status transition matrix and actual process.It’s difficult to seek the best value of QQ,
    but it follows some priciples: In general, QQ is diagonal matrix and the values are small for prompt convergence.
  • RR: covariance of measured noise that relates to measured instruments, the smaller of this value, the higher of the convergence speed

Expansion

Unfortunately, kalman fitler only can be applied to problems with linear status transition matrix and measurement, which confine it in simple robotic problem. Therefore, Extended Kalman Filter is introduced to break the limitation.

Reality

Essentially, the linear prediction and measurement in kalman filter are replaced by no-linear prediction and measurement in extended kalman filter. As the result, Axt−1+ButAx_{t-1} + Bu_t is replaced by Jacobi Matrix g(xt−1,ut)g(x_{t-1}, u_t) and HxtHx_t is replaced by Jacobi Matrix h(xt)h(x_t) .

KF EKF
Axt−1^+ButA\hat{x_{t-1}} + Bu_t g(xt−1,ut)g(x_{t-1}, u_t)
Hx^t−H\hat{x}^-_t h(x^t−)h(\hat{x}^-_t)

Compute Jacobi Matrix

Take the above vehicle as an example:

JA=[dxdxdxdydxdvxdxdvydydxdydydydvxdydvydvxdxdvxdydvxdvxdvxdvydvydxdvydydvydvxdvydvy]=[10dt0010dt00100001]JH=[dxdxdxdydxdvxdxdvydydxdydydydvxdydvy]=[10000100]\begin{align*} J_{A} = \begin{bmatrix} \frac{dx}{dx} \quad \frac{dx}{dy} \quad \frac{dx}{dv^x} \quad \frac{dx}{dv^y} \\ \frac{dy}{dx} \quad \frac{dy}{dy} \quad \frac{dy}{dv^x} \quad \frac{dy}{dv^y} \\ \frac{dv^x}{dx} \quad \frac{dv^x}{dy} \quad \frac{dv^x}{dv^x} \quad \frac{dv^x}{dv^y} \\ \frac{dv^y}{dx} \quad \frac{dv^y}{dy} \quad \frac{dv^y}{dv^x} \quad \frac{dv^y}{dv^y} \end{bmatrix} = \begin{bmatrix} 1 \quad 0 \quad dt \quad 0 \\ 0 \quad 1 \quad 0 \quad dt \\ 0 \quad 0 \quad 1 \quad 0 \\ 0 \quad 0 \quad 0 \quad 1 \\ \end{bmatrix} \\ \\ J_{H} = \begin{bmatrix} \frac{dx}{dx} \quad \frac{dx}{dy} \quad \frac{dx}{dv^x} \quad \frac{dx}{dv^y} \\ \frac{dy}{dx} \quad \frac{dy}{dy} \quad \frac{dy}{dv^x} \quad \frac{dy}{dv^y} \\ \end{bmatrix} = \begin{bmatrix} 1 \quad 0 \quad 0 \quad 0 \\ 0 \quad 1 \quad 0 \quad 0 \\ \end{bmatrix} \end{align*}

Prediction and Update

x^t−=g(xt−1,ut)P^t−=APt−1^AT+QKt=P^t−HTHTP^t−HT+Rx^t=x^t−+Kt(zt−h(x^t−))P^t=(I−KtH)P^t−\begin{align*} \hat{x}^-_t = g(x_{t-1}, u_t) \\ \hat{P}^-_t = A\hat{P_{t-1}}A^T + Q \\ K_t = \frac{\hat{P}^-_tH^T}{H^T\hat{P}^-_tH^T + R} \\ \hat{x}_t = \hat{x}^-_t + K_t(z_t - h(\hat{x}^-_t)) \\ \hat{P}_t = (I - K_tH)\hat{P}^-_t \end{align*}

Codes

References