Kalman Filter in MATLAB: Code, EKF Design & State Estimation

% =========================================================================
% STANDALONE DISCRETE KALMAN FILTER IN MATLAB (1D KINEMATIC TRACKING)
% Zero Toolbox Required - Compatible with base MATLAB (R2018a - R2026b)
% Prepared by: MATLABSolutions State Estimation Team
% =========================================================================

clear; clc; close all;

%% 1. Simulation Parameters & Ground Truth Setup
N = 60;                    % Total simulation discrete time steps
dt = 0.5;                  % Sampling time interval (seconds)
true_accel = 0.15;         % Constant acceleration (m/s^2)
t = (0:N-1) * dt;          % Time vector

% True physics-based trajectory (constant acceleration)
true_pos = 0.5 * true_accel * t.^2;
true_vel = true_accel * t;

% Sensor Noise Simulation (Gaussian white noise)
meas_noise_var = 9.0;      % Measurement variance R (sigma^2 = 9 m^2)
measurements = true_pos + sqrt(meas_noise_var) * randn(1, N);

%% 2. Discrete State-Space Model Formulation
% State vector x = [Position; Velocity]
A = [1, dt;                % State transition matrix
     0,  1];
H = [1,  0];               % Measurement observation matrix (position only)

% Noise Covariance Matrices
Q = [0.05, 0.00;           % Process noise covariance matrix (model uncertainty)
     0.00, 0.05];
R = meas_noise_var;        % Measurement noise covariance (scalar)

%% 3. Kalman Filter Initialization
x_est = [0; 0];            % Initial state estimate [pos_0; vel_0]
P_est = [10,  0;           % Initial state error covariance uncertainty
          0, 10];
I = eye(2);                % 2x2 Identity matrix

% Preallocate memory for logging
est_pos = zeros(1, N);
est_vel = zeros(1, N);
sigma_pos = zeros(1, N);

%% 4. Recursive Predict & Correct Loop
for k = 1:N
    % --- STEP 1: TIME UPDATE (PREDICT) ---
    x_pred = A * x_est;
    P_pred = A * P_est * A' + Q;
    
    % --- STEP 2: MEASUREMENT UPDATE (CORRECT) ---
    z_k = measurements(k);                     % Current sensor reading
    y_k = z_k - H * x_pred;                    % Innovation residual
    S_k = H * P_pred * H' + R;                 % Residual covariance
    K_k = P_pred * H' / S_k;                   % Optimal Kalman Gain
    
    x_est = x_pred + K_k * y_k;                % Corrected state estimate
    P_est = (I - K_k * H) * P_pred;            % Corrected error covariance
    
    % Store history
    est_pos(k) = x_est(1);
    est_vel(k) = x_est(2);
    sigma_pos(k) = sqrt(P_est(1, 1));          % Standard deviation bound
end

%% 5. Performance Metrics & Visualization
rmse_pos = sqrt(mean((true_pos - est_pos).^2));
rmse_raw = sqrt(mean((true_pos - measurements).^2));
noise_reduction_pct = (1 - rmse_pos / rmse_raw) * 100;

fprintf('--- Kalman Filter Tracking Results ---
');
fprintf('Raw Sensor Measurement RMSE : %.3f meters
', rmse_raw);
fprintf('Kalman Filter Estimate RMSE : %.3f meters
', rmse_pos);
fprintf('Noise Attenuation Percentage : %.1f%%
', noise_reduction_pct);

% Generate Plots
figure('Color', 'w', 'Position', [100, 100, 800, 500]);
subplot(2, 1, 1);
plot(t, measurements, 'r.', 'MarkerSize', 8, 'DisplayName', 'Noisy Sensor (R)');
hold on;
plot(t, true_pos, 'k-', 'LineWidth', 1.8, 'DisplayName', 'True Trajectory');
plot(t, est_pos, 'b-', 'LineWidth', 2.0, 'DisplayName', 'Kalman Estimate');
fill([t, fliplr(t)], [est_pos + 3*sigma_pos, fliplr(est_pos - 3*sigma_pos)], ...
     'b', 'FaceAlpha', 0.15, 'EdgeColor', 'none', 'DisplayName', '\pm3\sigma Covariance Bound');
grid on; xlabel('Time (s)'); ylabel('Position (m)');
title(sprintf('1D Kalman Filter Tracking (RMSE: %.2f m | %.1f%% Noise Reduction)', ...
      rmse_pos, noise_reduction_pct));
legend('Location', 'NorthWest');

subplot(2, 1, 2);
estimation_error = true_pos - est_pos;
plot(t, estimation_error, 'm-', 'LineWidth', 1.5, 'DisplayName', 'Estimation Error');
hold on;
plot(t, 3*sigma_pos, 'k--', 'LineWidth', 1.2, 'DisplayName', '+3\sigma Bound');
plot(t, -3*sigma_pos, 'k--', 'LineWidth', 1.2, 'DisplayName', '-3\sigma Bound');
grid on; xlabel('Time (s)'); ylabel('Error (m)');
title('Tracking Error within \pm3\sigma Confidence Envelope');
legend('Location', 'NorthWest');