Extended Kalman Filter

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:

State Update:

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 and velocity in its state

We can send any combination of linear velocity and angular velocity to our robot.

Our robot makes GPS-like observatoins to its current location.

Motion Model

Our robot will be a differential drive robot, so it's equations of motion are described as follows:

For a small time step we can update the state with the following eqation:

We can express this in matrix form

The jacobian of F is:

Observation Model

The matrix form of our observation model is:

The jacobian of our observation model is:

Doing the Filtering

We propagate our state using the extendedKalmanFilter filter function which is basically identical to the equations given above.


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;
}