If you have the , you can alternatively explore native functions like kalman() for continuous and discrete-time systems. However, writing the loop explicitly (as shown above) provides the best conceptual foundation for beginners.
He never forgot that night of frantic searching. And every semester, a student would email him: “The blue line is so smooth! Where did you find this?”
% ========================================================================= % 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
Real-world tracking problems are rarely one-dimensional. Usually, we need to track an object moving through space. This requires matrix algebra. Below is a full, self-contained 2D Kalman filter tracking an object moving along a single axis with constant velocity (estimating both its and its speed simultaneously). If you have the , you can alternatively
. This instructs the algorithm to rely more heavily on physics predictions.
It only needs to remember the previous state estimate. It does not require a massive history of data, making it incredibly fast and lightweight for embedded chips. How It Works: The 2-Step Cycle
It only needs the previous state to calculate the current state. You don't need a massive database of past readings. And every semester, a student would email him:
Example command to clone (if you have Git):
If you can tell me a bit more about what you're trying to track (a vehicle, a robot, a signal?), I can provide a more tailored MATLAB example. Or, if you're working on a project,
% Calculate and display error rmse_before = sqrt(mean((measurements - true_pos).^2)); rmse_after = sqrt(mean((stored_x(1,:) - true_pos).^2)); if you're working on a project
The Kalman Filter does this mathematically, balancing how much it trusts its "guess" versus how much it trusts the "sensor." The 2-Step Cycle
For beginners, the most effective way to learn is by observing the filter in action using pre-built simulations.