% Update step innovation = z(:, i) - H * x_pred; K = P_pred * H' * (H * P_pred * H' + R)^-1; x_est = x_pred + K * innovation; P_est = (eye(2) - K * H) * P_pred;