Knowing these details will allow me to suggest the specific MATLAB scripts from Kim's curriculum that fit your needs.
% Update step K = P_pred * H' / (H * P_pred * H' + R); x_est(:,i) = x_pred + K * (y(i) - H * x_pred); P_est(:,i) = (eye(2) - K * H) * P_pred; end Knowing these details will allow me to suggest