create extended kalman in matlab

To create an extended Kalman filter in MATLAB, follow these steps:

  1. Define the state-space model of the system you wish to estimate. This will include equations describing the dynamics of the system as well as any measurements that you have available.
main.m
% Define the state-space model
syms x y theta v omega N1 N2
f = [x + (v/omega)*(-sin(theta) + sin(theta + omega*N1));
     y + (v/omega)*(cos(theta) - cos(theta + omega*N1));
     theta + omega*N1 + omega*N2];
g = [x; y; theta];
h = [x; y; theta];
248 chars
8 lines
  1. Initialize the filter by defining the initial state of the system and the covariance matrix for the initial estimate.
main.m
% Initialize the filter
x0 = [0; 0; 0]; % initial state estimate
P0 = diag([1, 1, pi/4]); % initial covariance matrix
118 chars
4 lines
  1. Specify the noise characteristics of the system and the sensors.
main.m
% Specify noise characteristics
Q = diag([0.1, 0.1, 0.01]); % process noise
R = diag([0.1, 0.1, 0.01]); % measurement noise
124 chars
4 lines
  1. Implement the extended Kalman filter.
main.m
% Implement the extended Kalman filter
x = x0;
P = P0;
for i = 1:length(data)
    % Prediction step
    F = jacobian(f, [x; v; omega]);
    G = jacobian(g, [x; v; omega]);
    x = simplify(subs(f, [x; v; omega], [x; data(i,1); data(i,2)]));
    P = simplify(F*P*F.' + G*Q*G.');
    
    % Update step
    H = jacobian(h, [x; v; omega]);
    y = simplify(subs(h, [x; v; omega], [x; data(i,3); data(i,4)])) - y_hat;
    S = H*P*H.' + R;
    K = P*H.'/S;
    x = simplify(x + K*y);
    P = simplify((eye(3) - K*H)*P);
    
    % Store results
    x_est(:,i) = x;
    P_est(:,i) = diag(P);
end
590 chars
23 lines

This code defines a state-space model for a system with position (x,y) and heading angle theta, moving at a constant speed v and turning at a constant rate omega. The measurements available are the position (x,y) and heading angle theta, but they are corrupted by noise. The extended Kalman filter algorithm is used to estimate the state of the system at each timestep based on the available measurements.

Note that this is just an example and the actual implementation may vary depending on the specific system being modeled and estimated.

gistlibby LogSnag