Using the laws of physics or a known system model, the filter projects the current state forward in time to estimate the next state. Because time has passed and we are introducing imperfect physical assumptions, our uncertainty (variance) grows during this step. Step 2: Update (Measurement Update)
% State Transition Matrix (The Physics Model) % x_new = x_old + v_old * dt % v_new = v_old F = [1 dt; 0 1];
: Every chapter is balanced with a theoretical background followed immediately by a MATLAB example , allowing you to see the filter in action on problems like position and velocity estimation.
The Kalman filter is a recursive algorithm that uses a combination of prediction and measurement updates to estimate the state of a system. It is based on the state-space model, which describes the system dynamics and the measurement process. The algorithm was first introduced by Rudolf Kalman in the 1960s and has since become a widely used tool in many fields.
Model: state x = [position; velocity] A = [1 dt; 0 1], B = [0;0], H = [1 0] (measure position) Using the laws of physics or a known
When a new sensor measurement arrives, the filter refines its prediction.
The Kalman filter is an optimal recursive estimator for linear dynamical systems with Gaussian noise. It fuses prior estimates and noisy measurements to produce minimum‑variance state estimates. Applications: navigation, tracking, control, sensor fusion, and time‑series forecasting.
Have you ever wondered how GPS navigation knows exactly where you are, even when you drive through a tunnel? Or how a drone stays stable in a gusty wind? The secret ingredient in many of these technologies is the .
You know how fast the car was going, so you can predict where it should be in one second. The Kalman filter is a recursive algorithm that
This example tracks a 1D position with constant velocity. You can copy this directly into your MATLAB Command Window.
In this article, we introduced the Kalman filter and provided MATLAB examples to help beginners understand and implement the algorithm. We also discussed the working principle of the Kalman filter and provided top resources for downloading MATLAB examples. With this article, you should be able to implement a simple Kalman filter in MATLAB and understand the basics of the algorithm.
% --- The "Truth" (Simulation of reality) --- true_position = 100 - 0.5 * g * t.^2; % Falling from 100m true_velocity = -g * t;
% Kalman Filter for Beginners: Constant Voltage Tracking clear; clc; % 1. Parameters true_voltage = 1.2; n_iterations = 50; process_noise = 1e-5; % How much the actual value changes sensor_noise = 0.1; % How "jittery" the voltmeter is % 2. Initial Guesses estimate = 0; % Initial guess of voltage error_est = 1; % Initial error in our guess % Data storage for plotting results = zeros(n_iterations, 1); measurements = zeros(n_iterations, 1); % 3. The Kalman Loop for k = 1:n_iterations % Simulate a noisy measurement measurement = true_voltage + randn * sensor_noise; measurements(k) = measurement; % --- KALMAN STEPS --- % A. Prediction (In this simple case, we assume voltage stays the same) % estimate = estimate; error_est = error_est + process_noise; % B. Update (The "Correction") kalman_gain = error_est / (error_est + sensor_noise); estimate = estimate + kalman_gain * (measurement - estimate); error_est = (1 - kalman_gain) * error_est; results(k) = estimate; end % 4. Visualization plot(1:n_iterations, measurements, 'r.', 'DisplayName', 'Noisy Measurement'); hold on; plot(1:n_iterations, repmat(true_voltage, n_iterations, 1), 'g', 'LineWidth', 2, 'DisplayName', 'True Value'); plot(1:n_iterations, results, 'b', 'LineWidth', 2, 'DisplayName', 'Kalman Estimate'); legend; title('Simple Kalman Filter: Voltage Tracking'); xlabel('Time Step'); ylabel('Voltage'); grid on; Use code with caution. How to "Download" and Run This Copy the code above. Open MATLAB or (the free alternative). Paste into a new script and hit Run . Top Resources to Learn More Model: state x = [position; velocity] A =
% State Vector: x = [position; velocity] x = [0; 0]; % Initial guess (we assume it starts at 0,0 - this is wrong on purpose to test the filter)
). However, our voltmeter introduces high-frequency random noise.
Imagine you are driving a car through a long, dark tunnel. You lose your GPS signal, so you must rely on your speedometer to estimate your position. However, your speedometer has a slight error, and over time, that tiny error accumulates, making your estimated position drift.
% ========================================================================= % 2D KALMAN FILTER: POSITION AND VELOCITY TRACKING % ========================================================================= clear; clc; close all; dt = 0.5; % Time step (seconds) num_steps = 60; % Total tracking duration true_acceleration = 0.1; % Small random acceleration fluctuations % State Vectors layout: [Position; Velocity] A = [1 dt; 0 1]; % State transition matrix (Physics: x = x0 + v*dt) H = [1 0]; % Measurement matrix (We only measure position) % Noise Covariances Q = [0.05 0.01; 0.01 0.05]; % Process noise matrix R = 4; % Measurement noise variance (meters squared) % Generate Ground Truth and Noisy Measurements true_x = zeros(2, num_steps); true_x(:,1) = [0; 5]; % Start at position 0m, velocity 5 m/s z = zeros(1, num_steps); % Preallocate measurements rng(123); for t = 2:num_steps % Physical movement + random system disturbance true_x(:,t) = A * true_x(:,t-1) + [0.5*dt^2; dt] * true_acceleration * randn; % Noisy position sensor reading z(t) = H * true_x(:,t) + sqrt(R) * randn; end z(1) = z(2) + sqrt(R)*randn; % Initial raw point % Initialization of Filter State x_est = [0; 0]; % Initial guess (Assume stationary at zero) P = [10 0; 0 10]; % High initial uncertainty covariance matrix % Storage for visualization history_pos = zeros(1, num_steps); history_vel = zeros(1, num_steps); % Filter Loop for t = 1:num_steps % PREDICT x_pred = A * x_est; P_pred = A * P * A' + Q; % UPDATE K = P_pred * H' / (H * P_pred * H' + R); x_est = x_pred + K * (z(t) - H * x_pred); P = (eye(2) - K * H) * P_pred; % Save history_pos(t) = x_est(1); history_vel(t) = x_est(2); end % Plotting results figure('Position', [100, 100, 900, 400]); % Position Plot subplot(1,2,1); plot(true_x(1,:), 'g-', 'LineWidth', 2); hold on; plot(z, 'r.', 'MarkerSize', 10); plot(history_pos, 'b--', 'LineWidth', 2); title('Position Tracking'); xlabel('Steps'); ylabel('Distance (m)'); legend('True Position', 'Noisy Sensor', 'Kalman Estimate', 'Location', 'NorthWest'); grid on; % Velocity Plot subplot(1,2,2); plot(true_x(2,:), 'g-', 'LineWidth', 2); hold on; plot(history_vel, 'b--', 'LineWidth', 2); title('Velocity Estimation'); xlabel('Steps'); ylabel('Speed (m/s)'); legend('True Velocity', 'Kalman Estimate', 'Location', 'SouthWest'); grid on; Use code with caution. 6. Downloading Code and Advanced Resources
% --- 4. RUN THE FILTER LOOP --- for k = 1:n % ----- PREDICT STEP ----- x_pred = F * x_est; P_pred = F * P_est * F' + Q;