gistlib
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 chars54 lines
% 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;
gistlibby LogSnag