STM32 쿼터니언 필터: Madgwick, Mahony 구현과 Yaw 드리프트

STM32로 만든 비행 컨트롤러(FC)에서 자이로와 가속도 센서만으로 Roll, Pitch, Yaw를 구하는 쿼터니언 필터를 구현했습니다. ICM-20602 IMU 값을 Madgwick 필터와 Mahony 필터로 처리해 비교한 과정과, Yaw에서 드리프트가 생기는 이유를 Madgwick 보고서와 함께 정리했습니다.

STM32 자작드론 시리즈 · GPS 수신(UART DMA) · SBUS 수신기 · Oneshot125 PWM 모터 출력 · SPI 플래시 · ADC 배터리 전압

한눈에 보기

• 센서: ICM-20602(SPI), 자이로 ±2000dps, 가속도 ±16g 스케일 상수를 사용했습니다.
• 구현한 필터: Madgwick(β 0.6, ζ 0.03), Mahony(Kp 1.0 상당, Ki 0).
• 실행 시간(코드 주석 측정): 6축 읽기 약 39µs, Madgwick 57µs, Mahony 42µs, 오일러각 변환 10µs. Roll, Pitch 오차는 둘 다 크지 않았고 Yaw 드리프트는 둘 다 나타났습니다.
• Yaw 드리프트는 필터의 결함이 아니라 자이로+가속도만으로는 방위 기준이 없기 때문입니다. 자력계나 GPS 같은 방위 기준이 필요합니다.

자이로·가속도 기반 자세 필터의 데이터 흐름 (직접 제작)

자이로와 가속도만으로 어떤 각도를 믿을 수 있나요?

Roll과 Pitch는 믿을 수 있지만 Yaw는 믿을 수 없습니다. Madgwick의 보고서는 "An IMU alone can only measure an attitude relative to the direction of gravity"라고 설명합니다(Madgwick, 2010). 중력 방향은 수직이라서 기울기(Roll, Pitch)는 알려 주지만 어느 쪽을 보고 있는지(Yaw)는 알려 주지 못합니다. 자이로는 각속도를 적분해서 자세를 구할 수 있지만, MEMS 자이로의 측정 오차는 적분하면 계속 쌓입니다. 그래서 자이로 혼자서는 절대 자세를 알 수 없고, 가속도로 중력 방향을 기준 삼아 계속 보정해 주어야 합니다.

이 두 센서를 합치는 일을 센서 퓨전(sensor fusion)이라고 합니다. 칼만 필터가 대표적이지만 계산량이 많아서, 작은 MCU에서는 더 가벼운 Madgwick이나 Mahony 필터를 많이 씁니다. Madgwick 보고서는 IMU용 필터가 갱신당 109번의 스칼라 연산이면 되고 10Hz 같은 낮은 샘플링에서도 쓸 수 있다고 주장합니다.

센서 값 읽기와 단위 변환

ICM-20602에서 SPI로 읽은 16비트 값을 각속도(deg/s)와 가속도(g)로 바꾸는 상수는 헤더에 아래처럼 정의했습니다.

#define GYRO_DEGFSEL3  0.06103515625f   // 4000/65536 deg/s per LSB  -> 풀스케일 ±2000 dps
#define ACC_DEGFSEL3   0.00048828125f   // 32/65536 g per LSB         -> 풀스케일 ±16 g
#define RAD2DEG        57.295779515f
#define DEG2RAD        0.017453289f

풀스케일 범위가 넓을수록 한 LSB가 나타내는 값이 커져 분해능이 낮아집니다. 이 프로젝트에서는 자이로 ±2000dps, 가속도 ±16g로 두었고, 필터에 들어가기 전에 자이로 값은 라디안으로 바꿔 쓰는 방식입니다.

Madgwick 필터

가속도 측정값을 정규화해서 "현재 쿼터니언이 예측하는 중력 방향"과의 차이(목적 함수)를 만들고, 경사 하강법(gradient descent)으로 오차가 줄어드는 방향을 구해 쿼터니언을 보정합니다. 적분한 자이로 값에서 이 보정량을 뺀 뒤 쿼터니언을 다시 정규화합니다(Madgwick 보고서). 파라미터는 두 개입니다.

파라미터	제 설정	의미
β	0.6	자이로 측정 오차의 크기. 클수록 가속도(중력) 보정을 강하게 따라갑니다.
ζ	0.03	자이로 바이어스 드리프트 보정 속도. 시간이 지나며 변하는 자이로 영점 오차를 적분해서 뺍니다.

β가 크면 가속도의 진동 노이즈가 자세에 섞이고, 작으면 자이로 오차가 오래 남습니다. 비행 중에는 모터 진동 때문에 가속도에 노이즈가 많이 실리니 β를 실제 기체에서 다시 조정해야 합니다.

Mahony 필터

같은 중력 방향 비교를 외적(cross product)으로 하고, 그 오차를 PI 제어로 자이로 값에 되먹입니다. 비례 이득 Kp는 가속도를 얼마나 빠르게 따라갈지, 적분 이득 Ki는 자이로 바이어스를 보정하는 속도를 정합니다. Madgwick 보고서도 자이로 바이어스를 오차의 적분 피드백으로 보정하는 방식이 Mahony 등의 연구에서 나왔다고 설명합니다. 제 구현에서는 Kp를 1.0 상당(코드의 twoKp 1), Ki는 0으로 두어 적분 보정을 쓰지 않았습니다.

쿼터니언을 Roll, Pitch, Yaw로 바꾸기

필터가 내놓는 것은 쿼터니언 q[0..3]이고, 사람이 보기 쉬운 오일러각으로 바꾸는 부분은 아래와 같습니다. 좌표축과 부호 규약에 따라 식이 달라지니, 여기서는 이 프로젝트의 센서 장착 방향에 맞춘 식입니다.

void Quaternion_Update(float* q)
{
    float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];

    Yaw   = -atan2f(2.0f * (q2*q3 + q1*q4), q1*q1 + q2*q2 - q3*q3 - q4*q4);
    Pitch = -asinf (2.0f * (q2*q4 - q1*q3));
    Roll  =  atan2f(2.0f * (q1*q2 + q3*q4), q1*q1 - q2*q2 - q3*q3 + q4*q4);

    Pitch *= RAD2DEG;  Roll *= RAD2DEG;  Yaw *= RAD2DEG;

    // Yaw를 0~360도로 맞춤
    if (Yaw >= 0) Yaw = 360.0f - Yaw;
    else          Yaw = -Yaw;
}

필터는 샘플 주기(sampleFreq)로 적분하므로, 이 값은 실제 루프 주기와 맞아야 합니다. 저는 DWT 사이클 카운터로 센서 데이터가 준비될 때마다 직전 시각과의 차이를 재서 sampleFreq를 매번 갱신했습니다. 메인 루프의 처리 순서는 아래와 같습니다.

if (ICM20602_DataReady() == 1)
{
    t2 = DWT->CYCCNT;
    sampleFreq = 1000000.0f / (((float)(t2 - t1)) / CLOCK_PER_USEC);   // 직전 갱신과의 간격으로 주파수 계산
    t1 = t2;

    ICM20602_Get6AxisRawData(&ICM20602.acc_x_raw, &ICM20602.gyro_x_raw);   // 약 39us

    // 영점 보정 후 단위 변환: 자이로는 deg/s를 라디안/s로, 가속도는 g로
    GYROX_RATE = (ICM20602.gyro_x_raw - gx_cal) * 0.06103515625f * 0.017453289f;
    GYROY_RATE = (ICM20602.gyro_y_raw - gy_cal) * 0.06103515625f * 0.017453289f;
    GYROZ_RATE = (ICM20602.gyro_z_raw - gz_cal) * 0.06103515625f * 0.017453289f;
    ACCX_RATE  = (ICM20602.acc_x_raw  - ax_cal) * 0.00048828125f;
    ACCY_RATE  = (ICM20602.acc_y_raw  - ay_cal) * 0.00048828125f;
    ACCZ_RATE  =  ICM20602.acc_z_raw            * 0.00048828125f;

    MahonyAHRSupdateIMU(&GYROX_RATE, &GYROY_RATE, &GYROZ_RATE,
                        &ACCX_RATE,  &ACCY_RATE,  &ACCZ_RATE);   // 42us (Madgwick는 57us)
    Quaternion_Update(q);                                            // 오일러각 변환 10us
    printf("%d %d %d\n", (int)Roll, (int)Pitch, (int)Yaw);           // 시리얼 플로터용 출력
}

시작할 때 자이로 세 축과 가속도 X, Y축의 영점(gx_cal 등)을 측정해서 뺍니다. 가속도 Z는 중력이 걸려 있어서 영점을 빼지 않았습니다.

실행 시간 측정

구간	소요 시간
ICM-20602 6축 SPI 읽기	약 39µs
Madgwick 필터 갱신	57µs
Mahony 필터 갱신	42µs
쿼터니언 → 오일러각	10µs
전체 루프(출력 포함)	약 230µs

출처: 당시 코드의 측정 주석. 시리얼 출력이 들어가면 시간이 더 늘어나므로, 비행 중에는 출력을 꺼야 합니다.

비교한 결과와 한계

• 수행 시간은 Mahony가 짧았습니다(42µs 대 57µs, 위 측정 표 참고). 다만 한 번의 측정 주석일 뿐이고 평균이나 최댓값은 기록하지 않았습니다.
• Roll과 Pitch는 두 알고리즘 모두 오차가 크지 않았습니다. 시리얼 플로터로 값을 보며 확인했고, 정량 오차는 따로 재지 못했습니다.
• Yaw 드리프트는 두 알고리즘에 모두 있었습니다. 위에서 설명한 대로 IMU만 쓰는 필터는 Yaw를 보정할 기준이 없기 때문에 자연스러운 결과입니다.

Madgwick 보고서는 중력으로는 보정할 수 없는 오차가 있고 그것은 헤딩 기준이 따로 있어야 보정된다고 설명합니다. 해결 방법은 세 가지입니다. 자력계를 더해 MARG(AHRS) 필터로 가거나, GPS 속도 방향으로 Yaw를 보정하거나, 짧은 비행에서는 Yaw 드리프트를 감수하고 조종사가 보정하는 것입니다. 같은 시리즈에서 GPS를 받는 부분은 GPS 수신 글에서 다루었습니다.

다음에 확인할 것

• 같은 데이터를 로그로 저장해 두 필터의 수행 시간과 각도 오차를 수치로 비교합니다. 플래시에 로그를 저장하는 방법은 SPI 플래시 글에서 만들었습니다.
• 모터를 돌릴 때(PWM 출력 글)의 진동이 β 설정에 미치는 영향을 봅니다.
• 자력계 또는 GPS 방위로 Yaw를 보정합니다.

소스는 GitHub의 STM32F4_FC_DEV에 있고, 이 글의 시점은 "done implementation for quaternion algorithm(Madgwick/Mahony) & serial plotter" 커밋입니다. 자작 FC에서 자세 추정에 어떤 필터를 쓰고 계신가요? 센서와 필터를 댓글로 알려 주세요.

2026년 10월 기준입니다. 구현은 2023년 2월 직접 한 내용이고, 알고리즘 설명은 2026년 10월에 Madgwick 보고서(2010)로 확인했습니다.