create extended kalman in matlab

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

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

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

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