센서 융합 — 결함이 정확히 반대인 두 센서

기울기 하나를 알고 싶다고 해볼게요. 관성 센서 안의 두 소자가 각자 다른 방식으로 답을 주는데, 둘 다 혼자서는 못 써요.

자이로는 각속도를 재요. 각도를 얻으려면 적분해요. 이 적분값은 짧은 시간에는 아주 정확하고 매끈하고 진동에도 강해요. 문제는 정지 상태에서도 0이 아닌 출력, 즉 바이어스예요. 그게 0.05도/s만 있어도 적분이 매초 누적해서 10분 뒤에는 30도가 기울어져 있다고 믿게 돼요. 자이로 적분은 단기는 옳고 장기는 반드시 틀리는 신호예요.

가속도계는 반대예요. 정지 상태에서 중력이 항상 측정되니 중력 벡터가 몸체 축에 어떻게 나뉘는지로 절대 기울기를 얻어요.

double accel_angle = std::atan2(ax, az);   // 중력 성분비로 기울기

이 각도는 아무리 시간이 지나도 흐르지 않아요. 중력이라는 절대 기준을 매번 새로 재니까요. 대신 순간값이 지저분해요. 모터 진동이 그대로 들어오고, 로봇이 가감속하면 운동 가속도가 중력에 섞여서 기울지 않았는데 기울었다고 읽혀요.

	짧은 시간	긴 시간
자이로 적분	정확·매끈	드리프트로 발산
가속도 각도	노이즈에 오염	평균이 참값에 수렴

결함이 반대 대역에 있다는 것, 이게 융합의 근거예요. 그리고 이 구조는 관성 센서만의 것이 아니에요. 바퀴 적분과 절대 기준 센서의 관계도 똑같아서, 아래 내용이 그대로 이식돼요.

한 줄로 두 결함을 지워요

상보 필터는 이 융합을 한 줄로 해요.

angle = alpha * (angle + omega * dt) + (1.0 - alpha) * accel_angle;

기본적으로는 자이로 적분을 따라가되 결과를 조금씩 가속도 각도 쪽으로 끌어당겨요. 계수가 0.95에서 0.99처럼 1에 가까워서 한 스텝의 교정은 아주 작지만, 매 주기 반복되니 느린 드리프트는 결국 끌려와 지워져요. 반대로 진동 같은 빠른 흔들림은 계수가 작아 거의 안 들어오고요.

주파수로 말하면 자이로 경로에는 빠른 변화만, 가속도 경로에는 느린 성분만 통과시킨 거예요. 두 필터가 전 대역을 빈틈없이 나눠 덮어서 상보라고 불러요.

계수의 감각은 시정수로 잡는 게 좋아요. 계수 0.98에 주기 10ms면 시정수가 약 0.49초인데, 0.5초보다 느린 성분은 가속도계를 믿고 그보다 빠른 성분은 자이로를 믿는다는 경계선이에요. 루프 주기가 바뀌면 같은 계수라도 시정수가 달라진다는 것도 기억해 두면, “주기를 바꿨더니 필터가 이상해졌다”를 바로 설명할 수 있어요.

드리프트가 실제로 어떻게 잡히는지 수치로 보면 감이 확실해져요. 참 각도 0도에 자이로 바이어스 0.5도/s, 계수 0.98, 주기 10ms입니다.

경과	자이로 단독	상보 필터
0.1초	0.05도	0.045도
1초	0.5도	0.213도
3초	1.5도	0.244도
이후	계속 증가	0.245도에서 멈춤

자이로 단독은 끝없이 벌어지는데 상보 필터는 0.245도에서 안 커져요. 이 값이 정확히 바이어스 × 시정수예요. 상보 필터가 드리프트를 지우는 게 아니라 유한한 오차로 묶어 둔다는 것이 핵심이고, 그래서 정확도가 아쉬우면 시정수를 줄이거나(진동을 더 받는 대가) 정지 상태에서 자이로 평균을 재서 바이어스 자체를 빼요.

믿음의 정도를 숫자로 계산해요

상보 필터의 계수는 고정값이에요. 그런데 “지금 내 추정을 얼마나 믿을 수 있나”는 상황마다 달라요. 방금 켠 필터의 추정은 못 믿을 것이고, 오래 수렴한 추정은 믿을 만하죠. 칼만 필터는 그 믿음을 분산이라는 숫자로 매 스텝 계산해 섞는 비율을 스스로 조절해요.

등장인물은 넷이에요. 상태 x(추정 각도), 그 추정의 분산 P(클수록 못 믿는 추정), 예측 한 스텝이 불러들이는 불확실성 Q, 측정값의 분산 R이요.

필터는 두 단계를 영원히 반복해요. 먼저 예측입니다.

x = x + omega * dt;   // 자이로 적분으로 예측
P = P + Q;            // 시간이 흐르면 불확실성은 늘기만 한다

P += Q가 예측 단계의 본질이에요. 측정 없이 시간이 흐르는 동안 확신은 줄어들 수만 있다는 것을 식으로 적은 거예요.

다음이 갱신이에요. 측정이 도착하면 섞어요.

double K = P / (P + R);   // 칼만 이득
x = x + K * (z - x);      // 예측과 측정의 차이를 K만큼 반영
P = (1.0 - K) * P;        // 측정을 반영했으니 불확실성 감소

이득이 이 필터의 심장이에요. 내 예측의 불확실성이 예측과 측정의 불확실성 합에서 차지하는 비율이거든요. 그래서 이득은 예측과 측정 중 누구를 믿을지의 답 그 자체예요. 추정이 엉망이면 1로 가서 측정을 거의 그대로 받아들이고, 추정이 이미 정확하면 0으로 가서 측정을 무시해요. 상보 필터의 고정 계수 자리를 매 스텝 새로 계산하는 셈이에요.

수치로 따라가면 보여요. 참값 10도, 초기 추정 0, P=1, R=4, Q=0.01입니다.

스텝	예측 후 P	이득	갱신 후 x	갱신 후 P
1	1.01	0.202	2.02	0.81
2	0.82	0.169	3.37	0.68
3	0.69	0.147	4.34	0.59

추정이 참값을 향해 다가가고, 분산이 줄고, 분산이 주니 이득도 줄어요. 처음에는 측정을 크게 받아들이고 확신이 생길수록 조금씩만 반영한다가 표에 그대로 보여요.

두 손잡이의 뜻

R을 키우면 “측정이 지저분하다”는 선언이에요. 이득이 작아져 출력이 매끈해지고 진동에 둔감해지지만, 참값 변화를 따라가는 속도가 느려져요.

Q를 키우면 “예측을 못 믿겠다”는 선언이에요. 매 스텝 분산이 크게 불어 이득이 커지고, 측정 추종이 빨라지는 대신 출력이 노이즈를 따라 들썩여요.

Q를 0으로 두는 게 함정이에요. 예측이 분산을 안 키우면 갱신이 줄이기만 해서 분산이 0으로, 이득도 0으로 수렴해요. 필터가 잠들어요. 초반에는 완벽해 보이다가, 온도로 바이어스가 변하거나 외란이 오면 측정이 아무리 소리쳐도 반영을 안 해요.

이 함정이 고약한 이유는 증상이 늦게 오기 때문이에요. 벤치 테스트에서는 Q가 0일 때 가장 매끈하고 좋아 보여요. 배포하고 30분 뒤에야 드러납니다.

Q를 0보다 크게 두면 예측이 매번 키우고 갱신이 매번 줄여서 두 힘이 균형을 이루는 지점에서 분산이 멈추고, 분산이 멈추면 이득도 상수가 돼요. 그러니까 잘 튜닝된 칼만은 초반 수렴 구간을 지나면 계수가 고정된 상보 필터와 사실상 같은 동작을 해요.

측정이 끊겨도 분산이 알아서 해요

전체를 합치면 이 형태예요.

for (;;) {
    double dt = stamp_now - stamp_prev;         // 상수로 박지 않는다
    kf_predict(kf, gyro_omega, dt);             // 자이로로 밀고 분산 키우기
    if (accel_ready) {
        kf_update(kf, std::atan2(ax, az));      // 도착한 만큼만 섞기
    }
}

측정이 없으면 예측만 돌면서 분산이 계속 커지고, 나중에 측정이 도착하면 그만큼 커진 이득으로 크게 반영돼요. 센서가 끊겼다 돌아오는 상황을 따로 처리하지 않아도 분산이 알아서 해결하는 것, 이게 칼만이 상보 필터보다 나은 지점의 실물이에요.

선택 기준도 여기서 나와요. 기울기 하나를 재고 파라미터를 정밀하게 잴 계획이 없다면 상보 필터로 충분해요. 곱셈 몇 번이라 마이크로컨트롤러의 1kHz 루프에도 부담이 없고 손잡이가 하나라 실패 양상도 단순해요. 칼만이 필요한 건 불확실성 자체가 산출물일 때예요. 이 추정을 얼마나 믿고 다음 판단에 쓸지가 필요하거나, 주기가 다른 센서 여럿을 묶거나, 측정이 한동안 끊기는 구간이 있을 때요.

두 필터에 공통으로 남는 함정

각도의 경계예요. 위 식들은 각도를 그냥 빼고 더하는데, 방향각처럼 −180도와 +180도가 같은 방향인 값에서는 179도와 −179도의 차이가 실제로는 2도인데 358도로 계산돼 추정이 한 바퀴 튀어요.

기울기는 통상 범위에서 문제가 안 드러나지만 아크탄젠트의 치역 자체는 ±180도라 크게 기울면 같은 일이 생겨요. 방향각을 융합할 때는 차이를 계산한 뒤 접어 주는 처리가 반드시 들어가야 하고, 자세한 건 회전 표현 — 오일러는 UI, 쿼터니언은 데이터에 있어요.

실전 융합의 첫 버그는 dt예요

식들이 전부 시간 간격을 쓰는데, 상수로 박으면 거기서부터 틀어져요. 센서마다 주기가 다르고, 같은 센서도 스케줄링 지터와 버퍼링 때문에 도착 간격이 명목 주기와 달라요. 10ms라고 믿고 적분했는데 실제가 12ms였다면 매 스텝 약 17%씩 덜 적분하는 거예요.

그래서 간격은 수신 시각이 아니라 메시지에 실린 측정 시각의 차이로 계산해요.

double dt = stamp_now - stamp_prev;   // 스탬프 차이
if (dt > 0.0) angle += omega * dt;    // 역전·중복 스탬프는 건너뜀
stamp_prev = stamp_now;

첫 샘플은 기준만 잡고 적분하지 않는 것, 그리고 0 이하 가드가 실무 포인트예요. 기록을 반복 재생하다 시간이 뒤로 점프하거나 늦게 도착한 메시지가 순서를 역전시키면 음수가 들어오는데, 가드가 없으면 각도가 거꾸로 적분돼요.

상태가 각도 하나가 아니라 위치·방향·속도 묶음이 되면 등장인물이 전부 행렬이 되는데, 예측으로 불확실성을 키우고 갱신으로 측정과 섞는 반복 구조는 동일해요. 로봇이 회전하는 순간 상태 전이가 비선형이 되어 매 스텝 현재 추정점 근처에서 선형화해 쓰는 확장 버전이 표준이고, 바퀴 오도메트리와 관성 센서를 묶는 표준 패키지가 내부에서 하는 일이 정확히 이 반복이에요.

융합은 결국 단기가 옳은 적분 센서와 장기가 옳은 절대 센서를 섞는 일이고, 두 손잡이의 뜻만 잡히면 튜닝과 선택이 전부 설명돼요.

hello, robot