extended kalman filter in matlab

Introduction

The Extended Kalman Filter (EKF) is a state estimation algorithm that enables estimation of nonlinear systems. In this answer, we will cover how to implement the EKF in MATLAB for state estimation of nonlinear systems.

Implementation

The implementation of the EKF can be divided into two parts: the prediction step and the update step.

Prediction Step

In the prediction step, the state of the system is predicted using the system model. The state is predicted using the following equation:

x(k|k-1) = f(x(k-1|k-1), u(k-1))

where x(k|k-1) is the predicted state at time k, x(k-1|k-1) is the estimated state at time k-1, and u(k-1) is the input at time k-1.

The covariance matrix of the state estimate is also predicted using the following equation:

P(k|k-1) = F(k-1)P(k-1|k-1)F(k-1)' + Q(k-1)

where F(k-1) is the Jacobian matrix of the system model at x(k-1|k-1) and Q(k-1) is the process noise covariance matrix.

Update Step

In the update step, the predicted state estimate is corrected using the measurement z(k). The corrected state estimate is computed using the following equation:

x(k|k) = x(k|k-1) + K(k)(z(k) - h(x(k|k-1)))

where x(k|k) is the updated state estimate, z(k) is the measurement, h is the measurement model, and K(k) is the Kalman gain.

The Kalman gain is computed using the following equation:

K(k) = P(k|k-1)H(k)'(H(k)P(k|k-1)H(k)' + R(k))^-1

where H(k) is the Jacobian matrix of the measurement model at x(k|k-1) and R(k) is the measurement noise covariance matrix.

The covariance matrix of the updated state estimate is also computed using the following equation:

P(k|k) = (I - K(k)H(k))P(k|k-1)

MATLAB Code

The following code implements the EKF in MATLAB for state estimation of a nonlinear system:

main.m
% State estimation of a nonlinear system using Extended Kalman Filter (EKF)

% Define system model
f = @(x, u) [x(1) + u(1)*cos(x(3)*pi/180)*0.1; 
             x(2) + u(1)*sin(x(3)*pi/180)*0.1; 
             x(3) + u(2)*0.1];
F = @(x, u) [1, 0, -u(1)*sin(x(3)*pi/180)*0.1; 
             0, 1, u(1)*cos(x(3)*pi/180)*0.1; 
             0, 0, 1];
Q = diag([0.1^2, 0.1^2, 0.01^2]);

% Define measurement model
h = @(x) [sqrt(x(1)^2 + x(2)^2); atan(x(2)/x(1))*180/pi];
H = @(x) [x(1)/sqrt(x(1)^2 + x(2)^2), x(2)/sqrt(x(1)^2 + x(2)^2), 0; 
          -x(2)/(x(1)^2 + x(2)^2), x(1)/(x(1)^2 + x(2)^2), 0];
R = diag([0.1^2, 1^2]);

% Initialize state estimate
x = [0; 0; 0];
P = diag([1^2, 1^2, 10^2]);

% Simulate system
N = 100;
u = [1*ones(N, 1), randn(N, 1)*5];
x_true = zeros(N, 3);
z = zeros(N, 2);
for k = 1:N
    x_true(k, :) = f(x_true(k-1, :)', u(k, :))';
    z(k, :) = h(x_true(k, :)' + mvnrnd([0;0], R))';
    x_pred = f(x, u(k, :));
    F_pred = F(x, u(k, :));
    P_pred = F_pred*P*F_pred' + Q;
    K = P_pred*H(x_pred)'/(H(x_pred)*P_pred*H(x_pred)' + R);
    x = x_pred + K*(z(k, :)' - h(x_pred));
    P = (eye(3) - K*H(x_pred))*P_pred;
end

% Plot results
t = 1:N;
figure
subplot(311)
plot(t, x_true(:, 1), 'b', t, x(:, 1), 'r')
ylabel('x_1')
legend('True', 'Estimated')
subplot(312)
plot(t, x_true(:, 2), 'b', t, x(:, 2), 'r')
ylabel('x_2')
legend('True', 'Estimated')
subplot(313)
plot(t, x_true(:, 3), 'b', t, x(:, 3), 'r')
ylabel('x_3')
legend('True', 'Estimated')
xlabel('Time')
figure
subplot(211)
plot(t, u(:, 1))
ylabel('Input 1')
subplot(212)
plot(t, u(:, 2))
ylabel('Input 2')
xlabel('Time')
figure
subplot(211)
plot(t, z(:, 1), 'b', t, h(x_true')', 'r')
ylabel('Measurement 1')
legend('Measured', 'True')
subplot(212)
plot(t, z(:, 2), 'b', t, h(x_true')', 'r')
ylabel('Measurement 2')
legend('Measured', 'True')
xlabel('Time')
1844 chars
72 lines

This code applies the EKF to estimate the state of a nonlinear system with two inputs and two measurements. The state of the system is estimated with a reasonable accuracy, as shown in the plots.