If you want to dive deeper into the matrix math (the "Linear Algebra" side), here are the best places to go:

fprintf('RMS Error of Raw Measurements: %.2f meters\n', error_measurements); fprintf('RMS Error of Kalman Filter: %.2f meters\n', error_kalman);

% --- STEP 2: UPDATE (MEASUREMENT) --- % Compute the Kalman Gain % This determines how much we trust the measurement vs the prediction K = P * H' / (H * P * H' + R);

x(:, i) = x_upd; P(:, :, i) = P_upd; end

The result? A smooth, precise, and real-time estimate.

% Plot Noisy Measurements plot(measurements, 'r.', 'MarkerSize', 10, 'DisplayName', 'Measurements (Noisy)');