% Generate true motion and noisy measurements true_position = 0:dt:50; measurements = true_position + sqrt(R)*randn(size(true_position));
est_pos(k) = x(1); end
% Simulate t = 0:dt:5; true_pos = 100 + 0 t + 0.5 (-9.8)*t.^2; measurements = true_pos + sqrt(R)*randn(size(t));
dt = 0.1; A = [1 dt dt^2/2; 0 1 dt; 0 0 1]; H = [1 0 0]; % measure only position Q = 0.01 * eye(3); R = 5; % measurement noise variance x = [100; 0; -9.8]; % start at 100m, 0 velocity, gravity down P = eye(3);
% Filter est_pos = zeros(size(t)); for k = 1:length(t) % Predict x = A * x; P = A * P * A' + Q;
estimated_positions(k) = x(1); end
% Initial state guess x = [0; 10]; % start at 0 m, velocity 10 m/s P = eye(2); % initial uncertainty
State = [position; velocity; acceleration]