This posts outlines an example of localization using an EKF filter. The model is based on a Python implementation by AtsushiSakai on Github. Implementation.
Extended Kalman Filter
The kalman filter is linear algorithm that combines a series of noisy measurements and produces an estimate of unknown variables that tend to be more accurate than if you were to use one estimate. The extended kalman filter is a nonlinear version of the kalman filter in which we linearize the motion model and observation model to propogate our states.
The equations that govern the extended kalman filter are as follows:
State Predict:
\[X_{Pred} = F X_t + B u_t\] \[P_{Pred} = J_F P_t J_F^T + Q\]State Update:
\[z_{Pred} = H X_{Pred}\] \[Y = z - z_{Pred}\] \[S = J_H P_{Pred} J_H^T + R\] \[K = P_{Pred} J_H^T S^{-1}\] \[X_{t+1} = X_{Pred} + KY\] \[P_{t+1} = (I - K J_H) P_{Pred}\]In-depth Example
State, Control and Observation
Let us model the state, control and observation of a 2D robot as follows:
We track the (x, y) position of the robot, heading \(\phi\) and velocity \(v\) in its state
\[X_t = \begin{bmatrix} x_t \\ y_t \\ \phi_t \\ v_t \\ \end{bmatrix}\]We can send any combination of linear velocity and angular velocity to our robot.
\[u_t = \begin{bmatrix} v_t \\ \omega_t \\ \end{bmatrix}\]Our robot makes GPS-like observatoins to its current location.
\[z_t = \begin{bmatrix} x_t \\ y_t \\ \end{bmatrix}\]Motion Model
Our robot will be a differential drive robot, so it’s equations of motion are described as follows:
\[\dot{x} = v cos(\phi)\] \[\dot{y} = v sin(\phi)\] \[\dot{\phi} = \omega\]For a small time step we can update the state with the following eqation:
\[X_{t+1} = \begin{bmatrix} x_t + v cos \phi_t \Delta_t \\ y_t + v sin \phi_t \Delta_t \\ \phi_t + \omega_t \Delta_t \\ v_t \end{bmatrix}\]We can express this in matrix form
\[X_{t+1} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} x_t \\ y_t \\ \phi_t \\ v_t \\ \end{bmatrix} + \begin{bmatrix} cos \phi_t \Delta_t & 0 \\ sin \phi_t \Delta_t & 0 \\ 0 & \Delta_t \\ 1 & 0 \\ \end{bmatrix} \begin{bmatrix} v_t \\ \omega_t \\ \end{bmatrix}\] \[X_{t+1} = F X_{t} + B u_t\]The jacobian of F is:
\[J_F = \begin{bmatrix} \frac{\partial x}{\partial x} & \frac{\partial x}{\partial y} & \frac{\partial x}{\partial \phi} & \frac{\partial x}{\partial v} \\ \frac{\partial y}{\partial x} & \frac{\partial y}{\partial y} & \frac{\partial y}{\partial \phi} & \frac{\partial y}{\partial v} \\ \frac{\partial \phi}{\partial x} & \frac{\partial \phi}{\partial y} & \frac{\partial \phi}{\partial \phi} & \frac{\partial \phi}{\partial v} \\ \frac{\partial v}{\partial x} & \frac{\partial v}{\partial y} & \frac{\partial v}{\partial \phi} & \frac{\partial v}{\partial v} \\ \end{bmatrix} = \begin{bmatrix} 1 & 0 & -v sin \phi \Delta_t & cos \phi \Delta_t \\ 0 & 1 & v cos \phi \Delta_t & sin \phi \Delta_t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}\]Observation Model
The matrix form of our observation model is:
\[z_t = H x_t\] \[z_t = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} x_t \\ y_t \\ \phi_t \\ v_t \end{bmatrix} = \begin{bmatrix} x_t \\ y_t \end{bmatrix}\]The jacobian of our observation model is:
\[J_H = \begin{bmatrix} \frac{\partial x}{\partial x} & \frac{\partial x}{\partial y} & \frac{\partial x}{\partial \phi} & \frac{\partial x}{\partial v} \\ \frac{\partial y}{\partial x} & \frac{\partial y}{\partial y} & \frac{\partial y}{\partial \phi} & \frac{\partial y}{\partial v} \\ \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix}\]Doing the Filtering
We propogate our state using the extendedKalmanFilter filter function which is basically identical to the equations given above.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
void extendedKalmanFilter(Eigen::Vector4f& x_est, Eigen::Matrix4f& P_est,
const Eigen::Vector2f& u, const Eigen::Vector2f& z,
const Eigen::Matrix4f& Q, const Eigen::Matrix2f& R,
const float dt ) {
// state predict
Eigen::Vector4f x_pred = motionModel(x_est, u, dt); // 4x1
Eigen::Matrix4f J_F = jacobianF(x_est, u, dt); //4x4
Eigen::Matrix4f P_pred = J_F * P_est * J_F.transpose() + Q; // 4x4
// state update
Eigen::Vector2f z_pred = observationModel(x_pred);
Eigen::Vector2f y = z - z_pred;
Eigen::Matrix<float, 2, 4> J_H = jacobianH();
Eigen::Matrix2f S = J_H * P_pred * J_H.transpose() + R;
Eigen::Matrix<float, 4, 2> K = P_pred * J_H.transpose() * S.inverse();
x_est = x_pred + K * y;
P_est = (Eigen::Matrix4f::Identity() - K * J_H) * P_pred;
}