% True system: car moves with velocity 1 m/s dt = 0.1; % time step (seconds) t = 0:dt:10; % time vector true_position = t; % true position (no noise)
% Simulate noisy measurements (e.g., GPS error) measurement_noise = 0.5; measurements = true_position + measurement_noise * randn(size(t));
Imagine you are in a car trying to measure your speed. Your speedometer is good, but it's a bit noisy (it fluctuates when you hit bumps). You also have a GPS that measures your position, but it updates slowly and is sometimes inaccurate.
The Kalman Filter is an algorithm that combines these two imperfect sources of information to estimate the "true" state of your system better than either source could alone.
It works in a two-step loop:
Create a new script called kalman_beginner_example1.m and type the following:
%% Kalman Filter for Beginners - Example 1: Tracking Position % Author: Tutorial for "kalman filter for beginners" % Description: Track a moving object using a noisy position sensor.clear; clc; close all;
%% 1. SIMULATE THE REAL WORLD dt = 0.1; % Time step (seconds) t = 0:dt:10; % Time vector (10 seconds) N = length(t); % Number of time steps
% True state: [Position; Velocity] true_pos = zeros(1, N); true_vel = 1.0; % Constant velocity = 1 m/s
for k = 1:N true_pos(k) = true_vel * t(k); end
% Noisy Measurements (Position only, with noise) measurement_noise_std = 5; % Standard deviation of sensor noise measurements = true_pos + measurement_noise_std * randn(1, N);
%% 2. KALMAN FILTER INITIALIZATION % State vector: [Position; Velocity] x_est = [0; 0]; % Initial guess: position 0, velocity 0 P_est = [100, 0; % High uncertainty in initial position 0, 10]; % Lower uncertainty in initial velocity
% State Transition Matrix F (Position = Pos + Vel*dt, Velocity unchanged) F = [1, dt; 0, 1]; % True system: car moves with velocity 1 m/s dt = 0
% Observation Matrix H (We only measure position, not velocity) H = [1, 0];
% Process Noise Covariance Q (How much our motion model might be wrong) % We assume small random acceleration changes Q = [0.01, 0; 0, 0.01];
% Measurement Noise Covariance R (How noisy is the sensor) R = measurement_noise_std^2; % = 25
% Storage for results stored_x = zeros(2, N); stored_P = zeros(2, 2, N);
%% 3. KALMAN FILTER LOOP for k = 1:N % --- PREDICTION STEP --- x_pred = F * x_est; % Predict state P_pred = F * P_est * F' + Q; % Predict covariance
% --- CORRECTION STEP (Using the measurement) --- z = measurements(k); % Current measurement y = z - H * x_pred; % Innovation (measurement residual) S = H * P_pred * H' + R; % Innovation covariance K = P_pred * H' / S; % Kalman Gain x_est = x_pred + K * y; % Update state estimate P_est = (eye(2) - K * H) * P_pred; % Update covariance estimate % Store results stored_x(:, k) = x_est; stored_P(:, :, k) = P_est;end
%% 4. PLOT RESULTS figure('Position', [100, 100, 800, 600]);
subplot(2,1,1); plot(t, true_pos, 'g-', 'LineWidth', 2); hold on; plot(t, measurements, 'r.', 'MarkerSize', 6); plot(t, stored_x(1,:), 'b-', 'LineWidth', 2); legend('True Position', 'Noisy Measurements', 'Kalman Filter Estimate'); xlabel('Time (s)'); ylabel('Position (m)'); title('Kalman Filter: Tracking Position with Noisy Sensor'); grid on;
subplot(2,1,2); plot(t, stored_x(2,:), 'b-', 'LineWidth', 2); yline(true_vel, 'g--', 'True Velocity'); xlabel('Time (s)'); ylabel('Velocity (m/s)'); title('Estimated Velocity (Kalman Filter)'); legend('Estimated Velocity', 'True Velocity'); grid on;
% Calculate and display error rmse_before = sqrt(mean((measurements - true_pos).^2)); rmse_after = sqrt(mean((stored_x(1,:) - true_pos).^2));
fprintf('RMSE of Raw Measurements: %.2f meters\n', rmse_before); fprintf('RMSE of Kalman Filter: %.2f meters\n', rmse_after);
For absolute beginners:
Let’s build a 1D Kalman Filter in MATLAB to track a car moving at constant velocity. We will generate noisy measurements, run the filter, and plot the beautiful result.