Your filter trusts the model too much. Increase the value of entries in the process noise matrix Q , or decrease the measurement noise variance R .
(Measurement Noise): Tell the filter that your sensors are highly untrustworthy. The filter response will smooth out beautifully, but it will react very slowly to real changes in direction or speed.
When a new sensor measurement arrives, the filter refines its prediction. Your filter trusts the model too much
Imagine trying to track a speeding train using only a noisy position sensor. One reading might place it at 101 meters, the next at 99 meters, and the next at 102.5 meters. If you trust each reading blindly, your perception of the train's motion will be jittery and inaccurate. Conversely, if you only rely on a physical model that predicts the train should be moving at a constant speed, you'll be ignoring valuable real-world data, and any unexpected change in velocity will throw off your entire estimate.
To learn by doing, you can download pre-made examples from the : The filter response will smooth out beautifully, but
% --- 5. VISUALIZE THE MAGIC --- figure('Position', [100, 100, 1000, 600]);
% 1. Predict State (x_pred = F*x + B*u) x = F * x + B * u; One reading might place it at 101 meters,
% --- True System Model (A hidden "ground truth") --- A = [1 dt; 0 1]; % State transition matrix: x_k+1 = A * x_k H = [1 0]; % Measurement matrix: we only measure the position. Q = [0.01 0; 0 0.01]; % Process noise covariance (uncertainty in our model) R = 1; % Measurement noise covariance (uncertainty in our sensor)
Update: K_k = P_k-1 H^T (H P_k H^T + R)^-1 x̂_k = x̂_k-1 + K_k (z_k - H x̂_k-1) P_k = (I - K_k H) P_k