how to code an extended kalman filter? in matlab

main.m
% define state-space model
% x(k+1) = f(x(k)) + w(k)
% y(k) = h(x(k)) + v(k)

% state vector: [angle; angular rate]
% input: torque
% output: angle measurement
% continuous time system equations:
% dx(1) = x(2)
% dx(2) = -sin(x(1)) - u + v
% where u is the torque input and v is process noise

% define system matrices
A = [0 1; -cos(x(1)) 0];
B = [0; 1];
C = [1 0];
D = 0;

% define initial conditions
x0 = [0;0];
P0 = eye(2)*0.1;

% define process and measurement noise covariance matrices
Q = eye(2)*0.001;
R = 0.1;

% define EKF
ekf = extendedKalmanFilter(@pendulum_f, @pendulum_h, x0, P0, 'StateTransitionJacobianFcn', @pendulum_A, 'MeasurementJacobianFcn', @pendulum_H, 'ProcessNoise', Q, 'MeasurementNoise', R);

% simulate system
t = 0:0.01:5;
u = sin(t);
angle = zeros(size(t));
measurement = zeros(size(t));
x = x0;
for i = 1:length(t)
    % simulate pendulum dynamics
    x = pendulum_f(x,u(i),Q);
    angle(i) = x(1);
    % simulate measurement
    measurement(i) = pendulum_h(x,R);
    % update EKF
    [x,P] = predict(ekf,u(i));
    [x,P] = correct(ekf,measurement(i));
end

% plot results
figure;
plot(t,angle);
hold on;
plot(t,measurement);
legend('True angle','Measured angle');
xlabel('Time (s)');
ylabel('Angle (rad)');

function x = pendulum_f(x,u,Q)
    % state transition function
    x(1) = x(1) + x(2);
    x(2) = x(2) - sin(x(1)) - u + Q*randn(2,1);
end

function y = pendulum_h(x,R)
    % measurement function
    y = x(1) + R*randn(1);
end

function A = pendulum_A(x,u)
    % state transition jacobian
    A = [1 1; cos(x(1)) 0];
end

function H = pendulum_H(x)
    % measurement jacobian
    H = [1 0];
end
1635 chars
76 lines