% Run the Kalman filter x = zeros(2, length(t)); P = zeros(2, 2, length(t)); x(:, 1) = x0; P(:, :, 1) = P0; for i = 2:length(t) % Prediction step x_pred = A * x(:, i-1); P_pred = A * P(:, :, i-1) * A' + Q;
%% 4. PLOT RESULTS figure('Position', [100, 100, 800, 600]);
% Update the state estimate and covariance matrix innovation = z(i) - H * x_pred; S = H * P_pred * H' + R; K = P_pred * H' / S; x_est(i) = x_pred + K * innovation; P_est(i) = P_pred - K * H * P_pred; end % Run the Kalman filter x = zeros(2,
: Forecasts the future state based on past data and a physical model. Update (Correction) Step
% Covariance Matrix: How unsure are we about our initial guess? P = [1 0; 0 1]; P = [1 0; 0 1]; real_velocity =
real_velocity = 5; % Constant velocity (m/s) real_position = 0; % Start at 0 meters
If you want to master Kalman Filters, you must understand these four variables: The Update Phase
The algorithm uses the laws of physics or system dynamics to project the current state forward in time. This creates a "blind" guess of where the system should be. 2. The Update Phase