Since I cannot provide a direct file download link, I have provided the complete source code below. You can copy and paste this directly into a MATLAB script file ( .m ) to run it immediately.
% Measurement Noise Covariance R (How noisy is the sensor) R = measurement_noise_std^2; % = 25
% Predict xhat_p = A*xhat; P_p = A*P*A' + Q; % Update K = P_p*H'/(H*P_p*H' + R); xhat = xhat_p + K*(z - H*xhat_p); P = (eye(4) - K*H)*P_p; Since I cannot provide a direct file download
% State: [position; velocity] x_est = [0; 1]; % initial guess P_est = eye(2); % initial uncertainty
% --- Setup Parameters --- dt = 1; % Time step (seconds) A = [1 dt; 0 1]; % State transition matrix [pos; vel] C = [1 0]; % Measurement matrix (we only measure pos) Q = [0.01 0; 0 0.01]; % Process noise covariance R = 1; % Measurement noise covariance P = eye(2); % Initial error covariance x = [0; 0]; % Initial state [pos; vel] % --- Simulated Data --- true_pos = (1:100)'; % Real position (moving at 1 unit/sec) noise = sqrt(R) * randn(100,1); % Sensor noise measurements = true_pos + noise; % --- Kalman Filter Loop --- estimates = zeros(100, 1); for k = 1:100 % 1. Prediction Step x = A * x; P = A * P * A' + Q; % 2. Update Step z = measurements(k); % New measurement K = P * C' / (C * P * C' + R); % Kalman Gain x = x + K * (z - C * x); P = (eye(2) - K * C) * P; estimates(k) = x(1); % Store position estimate end % --- Plot Results --- plot(measurements, 'k.', 'MarkerSize', 8); hold on; plot(true_pos, 'g-', 'LineWidth', 2); plot(estimates, 'r-', 'LineWidth', 2); legend('Measurements', 'True Path', 'Kalman Estimate'); xlabel('Time'); ylabel('Position'); title('Simple Kalman Filter Tracking'); Use code with caution. Copied to clipboard Top Resources & Downloads Prediction Step x = A * x; P = A * P * A' + Q; % 2
end
%% 2. Kalman Filter Setup
% Step 1: Predict x_est = A * x_prev; P = A * P_prev * A' + Q;