1. 개요
IMU(Inertial Measurement Unit)는 가속도계와 자이로스코프를 결합하여 물체의 움직임과 자세를 추정하는 핵심 센서입니다. 그러나 각 센서에는 뚜렷한 한계가 있습니다.
- 가속도계: 중력 방향을 기준으로 각도를 얻을 수 있지만, 진동이나 외란에 취약합니다.
- 자이로스코프: 각속도를 적분해 부드러운 자세 변화를 제공하지만, 시간이 지남에 따라 누적 오차(드리프트)가 발생합니다.
이러한 한계를 극복하기 위해 두 센서의 장점을 결합하는 센서 융합(Sensor Fusion) 기법이 필요합니다. 그 대표적인 방법이 바로 칼만 필터(Kalman Filter)입니다. 본 글에서는 칼만 필터의 기본 원리와 수식, 그리고 실제 코드 구현 과정을 연결해 설명하며, 이를 통해 IMU 기반 자세 추정이 어떻게 이루어지는지 살펴봅니다.
2. 시스템 모델
칼만 필터는 선형 동적 시스템을 모델링하여 상태를 추정합니다.
- 상태 벡터: \(\mathbf{x}_k = [\theta_k, b_k]^T\)
- \(\theta_k\): 각도 (롤 또는 피치).
- \(b_k\): 자이로스코프의 편향(bias).
- 상태 전이 모델:
\[\mathbf{x}_k = \mathbf{F} \mathbf{x}_{k-1} + \mathbf{w}_k, \quad \mathbf{w}_k \sim \mathcal{N}(0, \mathbf{Q})\] \[\mathbf{F} = \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix}, \quad \mathbf{Q} = \begin{bmatrix} Q_{\text{angle}} & 0 \\ 0 & Q_{\text{bias}} \end{bmatrix}\]설명: 자이로 각속도를 적분해 각도를 예측, 편향은 일정하다고 가정.
- 측정 모델:
\[z_k = \mathbf{H} \mathbf{x}_k + v_k, \quad v_k \sim \mathcal{N}(0, R)\] \[\mathbf{H} = [1, 0]\]설명: 가속도계는 각도만 측정, \(R\)은 측정 노이즈 공분산.
3. 칼만 필터 수식과 코드의 연관성
칼만 필터는 예측 단계와 업데이트 단계로 나뉘며, 각 단계는 코드의 특정 라인과 매핑됩니다.
3.1 예측 단계 (Time Update)
1) 상태 예측
\[\hat{\mathbf{x}}_{k|k-1} = \mathbf{F} \hat{\mathbf{x}}_{k-1|k-1}\] \[\hat{\theta}_{k|k-1} = \hat{\theta}_{k-1|k-1} + \Delta t (\omega_k - \hat{b}_{k-1|k-1}), \quad \hat{b}_{k|k-1} = \hat{b}_{k-1|k-1}\]
코드:
float rate = newRate - *bias; // (1) 보정된 각속도
*angle += dt * rate; // (2) 각도 예측
설명:
- (1) 자이로스코프의 각속도(
newRate)에서 추정된 편향(*bias)을 빼서 보정된 각속도(rate)를 계산: \(\omega_{\text{corrected}} = \omega_k - \hat{b}_{k-1|k-1}\). - (2) 보정된 각속도를 시간 적분하여 예측된 각도를 갱신: \(\hat{\theta}_{k|k-1} = \hat{\theta}_{k-1|k-1} + \Delta t \cdot \omega_{\text{corrected}}\).
2) 공분산 예측
\[\mathbf{P}_{k|k-1} = \mathbf{F} \mathbf{P}_{k-1|k-1} \mathbf{F}^T + \mathbf{Q}\] \[\mathbf{P}_{k|k-1} = \begin{bmatrix} P_{00} + \Delta t (-P_{01} - P_{10} + \Delta t P_{11}) + Q_{\text{angle}} & P_{01} - \Delta t P_{11} \\ P_{10} - \Delta t P_{11} & P_{11} + Q_{\text{bias}} \end{bmatrix}\]
코드:
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); // (3)
P[0][1] -= dt * P[1][1]; // (4)
P[1][0] -= dt * P[1][1]; // (5)
P[1][1] += Q_bias * dt; // (6)
설명:
- (3) \(P_{00} \leftarrow P_{00} + \Delta t (-P_{01} - P_{10} + \Delta t P_{11}) + Q_{\text{angle}}\).
- (4), (5) \(P_{01} \leftarrow P_{01} - \Delta t P_{11}\), \(P_{10} \leftarrow P_{10} - \Delta t P_{11}\).
- (6) \(P_{11} \leftarrow P_{11} + Q_{\text{bias}} \Delta t\).
- 공분산 행렬을 업데이트하여 예측 단계의 불확실성을 반영.
3.2 업데이트 단계 (Measurement Update)
1) 칼만 이득 계산
\[\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}^T (\mathbf{H} \mathbf{P}_{k|k-1} \mathbf{H}^T + R)^{-1}\] \[S = P_{00} + R, \quad \mathbf{K} = \begin{bmatrix} P_{00} / S \\ P_{10} / S \end{bmatrix}\]
코드:
float S = P[0][0] + R_measure; // (7)
float K[2] = {P[0][0] / S, P[1][0] / S}; // (8)
설명:
- (7) 혁신 공분산 계산: \(S = P_{00} + R\).
- (8) 칼만 이득 계산: \(K_0 = P_{00} / S\), \(K_1 = P_{10} / S\). 이는 예측과 측정값의 상대적 신뢰도를 결정.
2) 상태 업데이트
\[\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k (z_k - \mathbf{H} \hat{\mathbf{x}}_{k|k-1})\] \[y = z_k - \hat{\theta}_{k|k-1}, \quad \hat{\theta}_{k|k} = \hat{\theta}_{k|k-1} + K_0 y, \quad \hat{b}_{k|k} = \hat{b}_{k|k-1} + K_1 y\]
코드:
float y = newAngle - *angle; // (9)
*angle += K[0] * y; // (10)
*bias += K[1] * y; // (11)
설명:
- (9) 측정 잔차 계산: \(y = z_k - \hat{\theta}_{k|k-1}\), 즉 가속도계 각도(
newAngle)와 예측 각도(*angle)의 차이. - (10) 각도 업데이트: \(\hat{\theta}_{k|k} = \hat{\theta}_{k|k-1} + K_0 y\).
- (11) 편향 업데이트: \(\hat{b}_{k|k} = \hat{b}_{k|k-1} + K_1 y\).
3) 공분산 업데이트
\[\mathbf{P}_{k|k} = (I - \mathbf{K}_k \mathbf{H}) \mathbf{P}_{k|k-1}\] \[\mathbf{P}_{k|k} = \begin{bmatrix} (1 - K_0) P_{00} & (1 - K_0) P_{01} \\ P_{10} - K_1 P_{00} & P_{11} - K_1 P_{01} \end{bmatrix}\]
코드:
P[0][0] -= K[0] * P[0][0]; // (12)
P[0][1] -= K[0] * P[0][1]; // (13)
P[1][0] -= K[1] * P[0][0]; // (14)
P[1][1] -= K[1] * P[0][1]; // (15)
설명:
- (12) \(P_{00} \leftarrow (1 - K_0) P_{00}\).
- (13) \(P_{01} \leftarrow (1 - K_0) P_{01}\).
- (14) \(P_{10} \leftarrow P_{10} - K_1 P_{00}\).
- (15) \(P_{11} \leftarrow P_{11} - K_1 P_{01}\).
- 보정된 상태의 불확실성을 반영.
4. 전체 코드
// I2C 통신과 MPU-6050 센서 라이브러리를 포함
#include <Wire.h>
#include <MPU6050.h>
// MPU6050 객체 생성
MPU6050 mpu;
// 칼만 필터 상태 변수: 각도(롤 또는 피치)와 자이로스코프 편향
float angle = 0, bias = 0;
// 공분산 행렬 (2x2): 상태 추정의 불확실성을 나타냄
float P[2][2] = {{0, 0}, {0, 0}};
// 노이즈 파라미터: 실험적으로 조정된 값
float Q_angle = 0.001; // 프로세스 노이즈 (각도)
float Q_bias = 0.003; // 프로세스 노이즈 (편향)
float R_measure = 0.03; // 측정 노이즈 (가속도계)
// 칼만 필터 함수: 가속도계 각도(newAngle)와 자이로 각속도(newRate)를 입력으로 받아 상태를 추정
void kalmanFilter(float newAngle, float newRate, float dt, float* angle, float* bias) {
// 예측 단계 (Time Update)
// (1) 자이로스코프 각속도에서 추정된 편향을 빼서 보정된 각속도를 계산
// 수식: ω_corrected = ω_k - b̂_{k-1|k-1}
float rate = newRate - *bias;
// (2) 보정된 각속도를 시간 적분하여 예측된 각도를 갱신
// 수식: θ̂_{k|k-1} = θ̂_{k-1|k-1} + Δt * ω_corrected
*angle += dt * rate;
// (3) 공분산 행렬의 P[0][0] 요소를 업데이트
// 수식: P_{00} += Δt * (-P_{01} - P_{10} + Δt * P_{11}) + Q_angle
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
// (4) 공분산 행렬의 P[0][1] 요소를 업데이트
// 수식: P_{01} -= Δt * P_{11}
P[0][1] -= dt * P[1][1];
// (5) 공분산 행렬의 P[1][0] 요소를 업데이트
// 수식: P_{10} -= Δt * P_{11}
P[1][0] -= dt * P[1][1];
// (6) 공분산 행렬의 P[1][1] 요소를 업데이트
// 수식: P_{11} += Q_bias * Δt
P[1][1] += Q_bias * dt;
// 업데이트 단계 (Measurement Update)
// (7) 혁신 공분산 계산
// 수식: S = P_{00} + R
float S = P[0][0] + R_measure;
// (8) 칼만 이득 계산
// 수식: K_0 = P_{00} / S, K_1 = P_{10} / S
float K[2] = {P[0][0] / S, P[1][0] / S};
// (9) 측정 잔차 계산: 가속도계 각도와 예측 각도의 차이
// 수식: y = z_k - θ̂_{k|k-1}
float y = newAngle - *angle;
// (10) 각도 업데이트
// 수식: θ̂_{k|k} = θ̂_{k|k-1} + K_0 * y
*angle += K[0] * y;
// (11) 편향 업데이트
// 수식: b̂_{k|k} = b̂_{k|k-1} + K_1 * y
*bias += K[1] * y;
// (12) 공분산 행렬의 P[0][0] 요소를 업데이트
// 수식: P_{00} = (1 - K_0) * P_{00}
P[0][0] -= K[0] * P[0][0];
// (13) 공분산 행렬의 P[0][1] 요소를 업데이트
// 수식: P_{01} = (1 - K_0) * P_{01}
P[0][1] -= K[0] * P[0][1];
// (14) 공분산 행렬의 P[1][0] 요소를 업데이트
// 수식: P_{10} = P_{10} - K_1 * P_{00}
P[1][0] -= K[1] * P[0][0];
// (15) 공분산 행렬의 P[1][1] 요소를 업데이트
// 수식: P_{11} = P_{11} - K_1 * P_{01}
P[1][1] -= K[1] * P[0][1];
}
// 초기화 함수: I2C 통신과 MPU-6050 센서를 초기화, 시리얼 통신 시작
void setup() {
Wire.begin(); // I2C 통신 초기화
mpu.initialize(); // MPU-6050 센서 초기화
Serial.begin(9600); // 시리얼 통신 시작 (9600 baud rate)
}
// 메인 루프: 센서 데이터를 읽고 칼만 필터를 적용하여 각도를 추정
void loop() {
// 가속도계와 자이로스코프 데이터를 읽기 위한 변수
int16_t ax, ay, az, gx, gy, gz;
// MPU-6050에서 3축 가속도와 3축 각속도 데이터를 읽음
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// 가속도계 데이터를 사용해 롤 각도를 계산 (단위: 도)
// 수식: θ_acc = atan2(ay, az) * 180 / π
float accelAngle = atan2(ay, az) * 180 / PI;
// 자이로스코프 x축 각속도를 변환 (MPU-6050 감도: 131 LSB/(도/초))
// 단위: 도/초
float gyroRate = gx / 131.0;
// 시간 간격 (10ms = 0.01초)
float dt = 0.01;
// 칼만 필터를 호출하여 각도와 편향을 추정
kalmanFilter(accelAngle, gyroRate, dt, &angle, &bias);
// 추정된 각도를 시리얼 모니터에 출력
Serial.println(angle);
// 10ms 대기하여 루프 주기를 유지
delay(10);
}5. 응용 및 구현 팁
응용
- 드론의 자세 제어: 정확한 롤/피치 추정으로 비행 안정성 확보.
- 로봇의 SLAM: 환경 매핑과 위치 추정(Simultaneous Localization and Mapping).
- VR/AR 헤드셋: 부드러운 모션 추적으로 몰입감 향상.
구현 팁
- 노이즈 파라미터 튜닝:
Q_angle,Q_bias,R_measure는 센서 특성과 환경에 따라 조정. 예:Q_angle은 자이로 드리프트,R_measure는 가속도계 노이즈를 반영. 실험적으로 값을 변경하며 최적화. - 센서 보정: MPU-6050은 초기 오프셋 보정이 필요. 자이로 편향은 정지 상태에서 수초간 데이터를 수집해 평균값으로 보정.
- 성능 최적화: Arduino의 제한된 연산 능력을 고려해 부동소수점 연산 최소화. 공분산 행렬의 대칭성(
P[0][1] = P[1][0])을 활용해 연산량 감소. - 라이브러리 활용:
RTIMULib또는TinyEKF를 사용하면 칼만 필터 구현을 간소화할 수 있음. - 루프 주기 관리:
dt는 루프 주기(예: 10ms)에 맞춰 설정. 정확한 타이밍을 위해 타이머 인터럽트 사용 고려.
6. 결론
칼만 필터는 서로 다른 특성을 가진 가속도계와 자이로스코프 데이터를 융합하여 노이즈와 드리프트 문제를 효과적으로 줄여줍니다. 단순히 수학적 개념에 그치지 않고, 코드의 각 줄이 수식과 어떻게 매핑되는지를 이해하면 구현과 튜닝 과정이 훨씬 명확해집니다.
이 글에서 다룬 방법은 드론의 자세 안정화, 로봇 내비게이션, VR/AR 기기 등 다양한 분야에 적용될 수 있습니다. 특히, 노이즈 파라미터 조정과 센서 보정을 적절히 수행하면 소형 마이크로컨트롤러에서도 안정적이고 정밀한 자세 추정을 구현할 수 있습니다.