Kalman Filter For Beginners With Matlab Examples Download Top |verified| Site

% 2D Position and Velocity Tracking Kalman Filter clear; clc; close all; % 1. Setup Simulation Data dt = 1; % Time step (1 second) steps = 30; % Total duration t = 1:steps; % True motion: Constant acceleration equations true_pos = 0.5 * 0.2 * t.^2; true_vel = 0.2 * t; % Generate noisy position measurements noise_sigma = 5; rng(123); z = true_pos + noise_sigma * randn(1, steps); % 2. Matrices Setup % State Vector: [Position; Velocity] X = [0; 0]; % Initial state guess F = [1 dt; 0 1]; % State transition matrix (Physics model) H = [1 0]; % Measurement matrix (We only measure position) Q = [0.1 0; 0 0.1]; % Process noise matrix R = noise_sigma^2; % Measurement noise variance P = eye(2) * 100; % Initial uncertainty matrix % Arrays to store outputs stored_pos = zeros(1, steps); stored_vel = zeros(1, steps); % 3. Filter Execution Loop for k = 1:steps % --- PREDICT --- X = F * X; P = F * P * F' + Q; % --- UPDATE --- K = (P * H') / (H * P * H' + R); % Kalman Gain X = X + K * (z(k) - H * X); % Correct state P = (eye(2) - K * H) * P; % Correct uncertainty % Save data stored_pos(k) = X(1); stored_vel(k) = X(2); end % 4. Visualization figure('Position', [100, 100, 900, 400]); % Position Plot subplot(1,2,1); plot(t, true_pos, 'g-', 'LineWidth', 2); hold on; plot(t, z, 'rx', 'MarkerSize', 6); plot(t, stored_pos, 'b-^', 'LineWidth', 1.5); title('Position Tracking'); xlabel('Time (s)'); ylabel('Distance (m)'); legend('True', 'Measured', 'Kalman'); grid on; % Velocity Plot (Hidden State Estimation) subplot(1,2,2); plot(t, true_vel, 'g-', 'LineWidth', 2); hold on; plot(t, stored_vel, 'b-^', 'LineWidth', 1.5); title('Velocity Estimation (Unmeasured)'); xlabel('Time (s)'); ylabel('Speed (m/s)'); legend('True', 'Kalman Estimated'); grid on; Use code with caution. Best Practices for Tuning Your Filter