구현 읽기 — 1차원 최소 구현을 따라간다

여기까지의 내용을 코드로 옮기면, 1차원 칼만 필터는 매우 짧게 쓸 수 있습니다. 중요한 것은 "줄 수"가 아니라, 각 줄이 어떤 식에 대응하는지 따라갈 수 있다는 점입니다.

Python 최소 구현

상태(x_hat, P)와 파라미터(Q, R)는 클래스로 묶습니다. global 을 사용해 모듈 변수를 다시 쓰는 방식은 여러 필터를 동시에 동작시킬 수 없고 테스트도 어렵기 때문에 피합니다.

class KalmanFilter1D:
    def __init__(self, x_hat=0.0, P=10.0, Q=0.05, R=2.0):
        self.x_hat = x_hat
        self.P = P
        self.Q = Q
        self.R = R

    def step(self, z):
        x_pred = self.x_hat
        P_pred = self.P + self.Q

        K = P_pred / (P_pred + self.R)
        self.x_hat = x_pred + K * (z - x_pred)
        self.P = (1.0 - K) * P_pred
        return self.x_hat, self.P

(1.0 - K) 라고 쓰는 이유는 정수로 계산되지 않게 하기 위해서입니다. Python 에서는 1 - K 라고 써도 K 가 float 라면 결과는 float 가 되므로 동작상으로는 같지만, C 등 다른 언어로 이식했을 때의 동작을 맞추려는 의도로 1.0 으로 명시합니다.

C 최소 구현

C 버전도 상태와 파라미터를 구조체로 묶고, 함수는 그 구조체를 받아 다시 쓰는 형태로 합니다. 전역 변수는 사용하지 않습니다.

typedef struct {
    double x_hat;
    double P;
    double Q;
    double R;
} KalmanFilter1D;

double kf_step(KalmanFilter1D *kf, double z) {
    double x_pred = kf->x_hat;
    double P_pred = kf->P + kf->Q;

    double K = P_pred / (P_pred + kf->R);
    kf->x_hat = x_pred + K * (z - x_pred);
    kf->P = (1.0 - K) * P_pred;
    return kf->x_hat;
}

사용 예: KalmanFilter1D kf = { .x_hat = 0.0, .P = 10.0, .Q = 0.05, .R = 2.0 }; 으로 초기화한 뒤, 관측마다 kf_step(&kf, z) 를 호출합니다.

식과 변수의 대응

코드를 읽을 때의 포인트

1. 먼저 예측의 2 줄과 업데이트의 3 줄을 나눈다
2. K 가 무엇에 의존하는지 확인한다
3. 마지막에 분산 P 가 제대로 업데이트되었는지 확인한다