create an example of extended kalman filter in matlab

main.m
% Example of Extended Kalman Filter for a simple vehicle tracking

% Define initial conditions
x0 = [0; 0; 0; 0];  % initial state: [position_x; velocity_x; position_y; velocity_y]
P0 = diag([1 1 1 1]);  % initial covariance matrix

% Define system matrices
dt = 0.1;  % time step of 0.1 seconds
A = [1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1];  % state transition matrix
B = [0; 0; 0; 0];  % no control input
H = [1 0 0 0; 0 0 1 0];  % measurement matrix
Q = diag([0.1 0.1 0.1 0.1]);  % process noise covariance matrix
R = diag([1 1]);  % measurement noise covariance matrix

% Generate true position data (with noise)
t = 0:dt:10;
x_true = [sin(t) + 0.5*randn(1,length(t)); cos(t) + 0.5*randn(1,length(t))];

% Generate noisy measurements
y = H * x_true + sqrt(R) * randn(2,length(t));

% Initialize filter
x_hat = x0;
P_hat = P0;

% Execute filter
for k = 1:length(t)
    
    % Predict
    x_pred = A * x_hat + B * 0;
    P_pred = A * P_hat * A' + Q;
    
    % Update
    K = P_pred * H' * inv(H * P_pred * H' + R);
    x_hat = x_pred + K * (y(:,k) - H * x_pred);
    P_hat = (eye(4) - K * H) * P_pred;
    
    % Save estimated position
    x_est(:,k) = x_hat;
    
end

% Plot results
figure;
plot(x_true(1,:), x_true(2,:), 'k:', 'LineWidth', 2);  % true trajectory
hold on;
plot(y(1,:), y(2,:), 'r*');  % noisy measurements
plot(x_est(1,:), x_est(3,:), 'b-', 'LineWidth', 2);  % estimated trajectory
xlabel('X position');
ylabel('Y position');
title('Extended Kalman Filter for Vehicle Tracking');
legend('True', 'Measurements', 'Estimated');
grid on;
1555 chars
54 lines