# how to code an extended kalman filter? in matlab

To implement an Extended Kalman Filter in Matlab, we need to define the state-space model of the system and the measurement model. The following code shows an example of a non-linear system (a pendulum) and its EKF implementation 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)');

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 chars76 lines

In this example, pendulum_f is the state transition function, pendulum_h is the measurement function, pendulum_A is the state transition Jacobian, and pendulum_H is the measurement Jacobian. The EKF is initialized with the initial state and covariance (x0 and P0), as well as the process and measurement noise covariance matrices (Q and R). The predict and correct functions are used to update the EKF at each time step.

The code simulates the pendulum dynamics and measurement, and updates the EKF at each time step. The results are plotted, showing the true and measured angle over time.

### related categories

gistlibby LogSnag