Kalman Filter를 쓰면 된다! 내부 State를 업데이트하면서 노이즈를 제거해준다. 아래 그림에서 검은 색이 실제 데이터 혹은 내가 원하는 Tracking이라고 하면, 현실에선 노이즈가 껴서 빨간색처럼 관측될 수 있음. 근데 노이즈가 랜덤성이 짙다는 가정 하에 칼만 필터를 이를 잘 제거해줄 수 있다!

시계열 데이터에는 모두 적용가능함 (e.g. 노이즈가 낄 수 있는 log, 온도 센서 같은 것?, 근데 생각해보니 온도 센서 같은 건 이미 내부 소프트웨어에 이런 것 쯤은 들어가 있을 듯)

아래는 예시 코드

def kalman_filter_smoothing_points(points_list):
    """
    Args:
        points_list: (N, 2)
    Returns:
        smoothed_points: (N, 2)

    Example Usage 1:

        print(f"[DEBUG] det_list: {det_list.shape}\n[0]: {det_list[0]}\n[-1]: {det_list[-1]}")
        for point_index in [0, 2, 5, 7, 9, 11, 13]:
            det_list[:,point_index:point_index+2] = kalman_filter_smoothing_points(det_list[:,point_index:point_index+2])
        print(f"[DEBUG] det_list-after: {det_list.shape}\n[0]: {det_list[0]}\n[-1]: {det_list[-1]}")

    Example Usage 2:

        for point_index in range(landmark.shape[1]):
            landmark[:,point_index,:] = kalman_filter_smoothing_points(landmark[:,point_index,:])
    """
    # Define the Kalman filter parameters
    dt = 1  # Time step
    sigma_x = 10  # Process noise standard deviation
    sigma_y = 10
    sigma_measurement = 50  # Measurement noise standard deviation

    # Initialize the Kalman filter
    kf = cv2.KalmanFilter(4, 2)  # 4 states, 2 measurements
    kf.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
    kf.transitionMatrix = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
    kf.processNoiseCov = np.array([[dt**4/4, 0, dt**3/2, 0], [0, dt**4/4, 0, dt**3/2], [dt**3/2, 0, dt**2, 0], [0, dt**3/2, 0, dt**2]], np.float32) * sigma_x**2
    kf.measurementNoiseCov = np.array([[sigma_measurement**2, 0], [0, sigma_measurement**2]], np.float32)

    # Initialize the state
    for i in range(len(points_list) - 1, 0, -1):
        measurement = np.array(points_list[i], np.float32)
        prediction = kf.predict()
        kf.correct(measurement)

    # Smooth the points with the Kalman filter
    smoothed_points = []
    for i in range(len(points_list)):
        measurement = np.array(points_list[i], np.float32)
        prediction = kf.predict()
        kf.correct(measurement)
        smoothed_points.append(prediction[:2, 0])  # Extract the first two elements of the predicted state
    smoothed_points = np.array(smoothed_points)
    return smoothed_points