% Generate true positions true_pos = real_position + real_velocity * t;
% --- 4. RUN THE FILTER LOOP --- for k = 1:n % ----- PREDICT STEP ----- x_pred = F * x_est; P_pred = F * P_est * F' + Q; % Generate true positions true_pos = real_position +
% --- 3. INITIALIZE THE KALMAN FILTER --- % State vector: [position; velocity] x_est = [0; 5]; % Initial guess (position 0, velocity 5 m/s) P_est = [10, 0; 0, 5]; % Initial uncertainty (high uncertainty) velocity] x_est = [0
% --- 2. SIMULATE NOISY MEASUREMENTS (Our sensor) --- measurement_noise_std = 2; % Sensor noise standard deviation (meters) measurements = true_pos + measurement_noise_std * randn(size(t)); % Initial guess (position 0
x_est = x_pred + K * y; % New state estimate P_est = (eye(2) - K * H) * P_pred; % New uncertainty
% --- 6. COMPUTE ERRORS --- error_measurements = sqrt(mean((measurements - true_pos).^2)); error_kalman = sqrt(mean((estimated_positions - true_pos).^2));