Differential Steer Robot Localization Using Extended Kalman Filter

Introduction

Kalman Filter

A mathematical method named after Rudolf E. Kalman, the Kalman Filter’s main goal being to use measurements observed over time that contain noise and other inaccuracies. It then tries to produce values close to the true values of the measurements and their associated calculated values. This particular filter has several applications, which, in a way, makes it an essential area in the development of military and space technologies.
The Kalman filter is a in which the only needed data to make an estimate on the current state are the estimate from the previous observation and a current measurement or input and the covariance matrix. This is the contrast to that of a batch estimation method where history of the observations and/or estimates or required.
An example of the Kalman filter would be a car or truck with a GPS unit that can provide a somewhat precise estimate of the car or truck location within a few meters. Additionally, over time, by evaluating or integrating the vehicle’s velocity and direction, the location of the vehicle can be determined by using a technique known as dead reckoning. The Kalman filter is thought of as the prediction and the update. Moreover, this can be broken down into two more detail phases. For instance the prediction phase would be where the vehicle’s post position is modified to fit the laws of motion plus it includes any changes that are caused by any acceleration or turns the vehicle undergoes. From there a new position will be calculated. Here the covariance should be in proportion to the speed of the vehicle due to the fact that there is a little uncertainty formulated around the accuracy of the dead reckoning estimate. Next would be the update phase where measurements of the vehicle’s position from the GPS unit. As so as the prediction phase there are still some uncertainties with this phase do to the fact that the covariance is relative to the prediction phase.
Additionally, the predict phase is also known as the a priori state estimate because, although it is an estimate of the state at the current timestep, it does not include observation information from the current timestep. While in the update phase or the posteriori state, the current a priori state is combined with the data from the current observation to refine the state estimate.

Multivariate Normal Distribution

In probability theory and statistics, the multivariate normal distribution or multivariate Gaussian distribution, is a generalization of the one-dimensional (univariate) normal distribution to higher dimensions. A random vector is said to be multivariate normally distributed if every linear combination of its components has a univariate normal distribution.

Extended Kalman Filter

The Kalman filter requires a linear model of a system, but for mobile robots, the kinematic equations are nonlinear. Therefore, the Kalman Filter cannot be used for robot localization. On the other hand, the Extended Kalman Filter (EKF) can handle nonlinear equations. The EKF operates on the same principle as the regular Kalman filter, but uses a linearized model of the system to predict the robot’s states. Some of the benefits of using the extended Kalman filter is that it requires little computation power, and it can be easily implemented on a mobile platform.

There are several practical issues that must be addressed when using the extended Kalman filter. First, the nonlinear equations that govern a differential steer robot’s motion are linearized by first order Taylor expansion possibly losing important information. Secondly, the EKF assumes normal distribution of the measured states and model predicted location which may or may not be true. These problems will be demonstrated later.

Pros
1. It is computationally fast
2.The Extended Kalman Filter can handle non-linear models
3.It only needs the estimates from the previous observation and a current
measurement or input and the covariance matrix

Cons
1. The Extended Kalman filter will linearize the model which can cause one to
lose information
2. It Assumes Gaussian position which at times may or may not be true.

Thrun Algorithm

Line1: Extended_Kalman_filter($\mu_{t-1}$,$\Sigma_{t-1}$,$u_t$,$z_t$)
Line2: $\bar{\mu}_t=g(u_t,\mu_{t-1})$
Line3: $\bar{\Sigma}_t=G_t\Sigma_{t-1}G^T_t+R_t$
Line4: $K_t = \bar{\Sigma}_{t}H^T_t(H_t\bar{\Sigma}_tH^T_t+Q_t)^{-1}$
Line5: $\mu_t=\bar{\mu}_t+K_t(z_t-h(\bar{\mu}_t))$
Line6: $\Sigma_t = (I-K_{t}H_{t})\bar{\Sigma}_{t}$
Line7: return $\mu_t$,$\Sigma_t$

$\mu_{t-1}$:Previous mean states
$\Sigma_{t-1}$: Previous covariance matrix
$u_t$: Current input
$z_t$: Current measurement
$\bar{\mu}_t$: Model predicted mean states
$\bar{\Sigma}_t$: Model predicted covariance matrix
$g(u_t,\mu_{t-1})$: Nonlinear system model which is a function of the previous states and input
$h(\bar{\mu}_t)$: Nonlinear model of measurement which is a function of predicted states
$G_t$: Jacobian matrix of model
$H_t$: Jacobian matrix of measurement
$R_t$: Covariance matrix of model
$Q_t$: Covariance matrix of measurement
$K_t$: Kalman gain
$\mu_t$: Updated mean states
$\Sigma_t$: Updated covariance matrix

Derivation for Differential Steer Robot

Robot Model and Covariance

For a complete explanation of the differential steer robot, refer to Direct Kinematics of Wheeled Platforms

The kinematic equations for a differential steer robot:

(1)
\begin{align} g(u_t,\mathcal{X}_{t-1}) = \left \{ \begin{array}{c} x_{t-1} \\ y_{t-1} \\ \theta_{t-1} \end{array} \right \} +\left[ \begin{array}{cc} \frac{r}{2}cos(\theta_{t-1}) & \frac{r}{2}cos(\theta_{t-1}) \\ \frac{r}{2}sin(\theta_{t-1}) & \frac{r}{2}sin(\theta_{t-1}) \\ -\frac{r}{b} & \frac{r}{b} \end{array} \right] \left \{ \begin{array}{c} \dot\theta_{Lt} \\ \dot\theta_{Rt} \end{array} \right \} \Delta t \end{align}

Where,

(2)
\begin{align} \mathcal{X}_{t-1} = \left \{ \begin{array}{c} x_{t-1} \\ y_{t-1} \\ \theta_{t-1} \end{array} \right \} \end{align}
(3)
\begin{align} u_t= \left \{ \begin{array}{c} \dot\theta_{Lt} \\ \dot\theta_{Rt} \end{array} \right \} \end{align}

and $\Delta t$ is the change in time between the previous model update to the new model update.

The Jacobian for the model is defined as follows:

(4)
\begin{align} G_t = \frac{\partial g(u_t,\mathcal{X}_{t-1}) }{\partial \mathcal{X}_{t-1}} = \left[ \begin{array}{ccc} \frac{\partial x_t}{\partial x_{t-1}} & \frac{\partial x_t}{\partial y_{t-1}} & \frac{\partial x_t}{\partial \theta_{t-1}} \\ \frac{\partial y_t}{\partial x_{t-1}} & \frac{\partial y_t}{\partial y_{t-1}} & \frac{\partial y_t}{\partial \theta_{t-1}} \\ \frac{\partial \theta_t}{\partial x_{t-1}} & \frac{\partial \theta_t}{\partial y_{t-1}} & \frac{\partial \theta_t}{\partial \theta_{t-1}} \end{array} \right] \end{align}
(5)
\begin{align} G_t = \left[ \begin{array}{ccc} 1 & 0 & -\dot\theta_{Lt}\frac{r}{2}sin(\theta_{t-1}) -\dot\theta_{Rt}\frac{r}{2}sin(\theta_{t-1}) \\ 0 & 1 & \dot\theta_{Lt}\frac{r}{2}cos(\theta_{t-1}) +\dot\theta_{Lt}\frac{r}{2}cos(\theta_{t-1}) \\ 0 & 0 &1 \end{array} \right] \end{align}

The covariance of the model will be defined according to the model input $u_t$. In order to calculate the covariance matrix $R_t$, the covariance of the inputs must be defined and then mapped to the robot’s states $\mathcal{X}_{t-1}$. Let $M_t$ be the variance of the differential steer robot’s inputs be defined as follows:

(6)
\begin{align} M_t = \left[ \begin{array}{cc} \alpha_1\dot\theta_{Lt}+\alpha_2\dot\theta_{Rt} & 0 \\ 0 & \alpha_1\dot\theta_{Lt}+\alpha_2\dot\theta_{Rt} \end{array} \right] \end{align}

The Jacobian that maps the covariance of the inputs to the covariance of the model is defined as:

(7)
\begin{align} V_t = \frac{\partial g(u_t,\mathcal{X}_{t-1}) }{\partial u_t} = \left[ \begin{array}{cc} \frac{\partial x_t}{\partial \dot\theta_{Lt}} & \frac{\partial x_t}{\partial \dot\theta_{Rt}} \\ \frac{\partial y_t}{\partial \dot\theta_{Lt}} & \frac{\partial y_t}{\partial \dot\theta_{Rt}} \\ \frac{\partial \theta_t}{\partial \dot\theta_{Lt}} & \frac{\partial \theta_t}{\partial \dot\theta_{Rt}} \end{array} \right] = \left[ \begin{array}{cc} \frac{r}{2}cos(\theta_{t-1}) & \frac{r}{2}cos(\theta_{t-1}) \\ \frac{r}{2}sin(\theta_{t-1}) & \frac{r}{2}sin(\theta_{t-1}) \\ -\frac{r}{b} & \frac{r}{b} \end{array} \right]\Delta t \end{align}

Finally, $R_t$ can be calculated.

(8)
\begin{equation} R_t = V_tM_tV^T_t \end{equation}

Measurement and Covariance

For now, it will be assumed that the robot’s states ($x$, $y$, and $\theta$) will be measured directly. Thus, the Jacobian for the measurement system will be:

(9)
\begin{align} H_t = \left[ \begin{array}{ccc} 1&0&0\\0&1&0\\0&0&1 \end{array} \right] \end{align}

The cavariance of the measurement system can be defined as follows:

(10)
\begin{align} Q_t = \left[ \begin{array}{ccc} \hat\sigma^2_x&0&0\\0&\hat\sigma^2_y&0\\0&0&\hat\sigma^2_{\theta} \end{array} \right] \end{align}

Where, $\hat\sigma_x$, $\hat\sigma_y$, and $\hat\sigma_{\theta}$ are the standard deviations of the measured $x$ location, $y$ location, and heading respectively. Note that standard deviation values could depend on several factors such as GPS precision, GPS signal, and velocity.

Add a New Comment
Unless otherwise stated, the content of this page is licensed under Creative Commons Attribution-ShareAlike 3.0 License