You can implement a basic time-varying Kalman filter using a standard for loop in MATLAB:
: The error covariance matrix (how uncertain we are about our estimate).
To run these scripts locally on your machine, you can download them directly via the links below, or save them manually from the code blocks above. Download 2D Trajectory Tracking Script (kalman_2d_demo.m) kalman filter for beginners with matlab examples download
%% Kalman Filter Beginner Example: Tracking a Constant Value clear; clc; close all; % 1. Simulation Parameters duration = 100; % Number of time steps true_value = -0.37727; % The actual true state we want to find noise_sigma = 0.1; % Standard deviation of measurement noise % Generate noisy sensor measurements rng(1); % Seed random number generator for reproducibility measurements = true_value + noise_sigma * randn(duration, 1); % 2. Initialize Kalman Filter Variables x_estimated = zeros(duration, 1); % Array to store filtered results P_estimated = zeros(duration, 1); % Array to store error covariance % Initial guesses x_estimated(1) = 0.0; % Initial state estimate (guess) P_estimated(1) = 1.0; % Initial error covariance (high uncertainty) % Filter Parameters A = 1; % System dynamics (value stays constant) H = 1; % Measurement matrix Q = 1e-5; % Process noise covariance (we trust our physical model) R = noise_sigma^2; % Measurement noise covariance (variance of sensor) % 3. The Kalman Filter Loop for k = 2:duration % --- PREDICT STEP --- x_prior = A * x_estimated(k-1); P_prior = A * P_estimated(k-1) * A + Q; % --- UPDATE STEP --- % Calculate Kalman Gain K = (P_prior * H) / (H * P_prior * H + R); % Update estimate with new measurement x_estimated(k) = x_prior + K * (measurements(k) - H * x_prior); % Update error covariance P_estimated(k) = (1 - K * H) * P_prior; end % 4. Plot the Results figure; plot(1:duration, measurements, 'r.', 'MarkerSize', 10); hold on; plot(1:duration, x_estimated, 'b-', 'LineWidth', 2); yline(true_value, 'g--', 'LineWidth', 2); hold off; title('1D Kalman Filter Simulation'); xlabel('Time Step'); ylabel('Value'); legend('Noisy Measurements', 'Kalman Filter Estimate', 'True Value'); grid on; Use code with caution. Code Explanation and Tweaking
[1Δt01]the 2 by 2 matrix; Row 1: Column 1: 1, Column 2: delta t; Row 2: Column 1: 0, Column 2: 1 end-matrix; Measurement Matrix ( [10]the 1 by 2 row matrix; 1, 0 end-matrix; (since we only measure position) MATLAB Code You can implement a basic time-varying Kalman filter
: A priori error covariance (how uncertain we are about our guess). Phase 2: Update (Measurement Correction) We adjust our prediction using the new sensor measurement.
Next, we incorporate the new measurement z . We compute the Kalman gain K and then use it to correct our predicted state. Simulation Parameters duration = 100; % Number of
xk−=Axk−1+Bukx sub k raised to the negative power equals cap A x sub k minus 1 end-sub plus cap B u sub k (Where is the state, is the state transition matrix, is the control matrix, and is the control input).
The Kalman filter operates in a continuous, two-step loop: and Update (or Correct).
The filter compares the guess and the sensor data, weighting them based on their uncertainty, to produce a final, improved estimate.
Mathematically, the filter is a algorithm, meaning it doesn't need to store all past data. At each time step, it operates in two distinct phases: