27.3.2.1. 상태 예측 함수(predictState) 내부의 쿼터니언 적분식 코드 흐름
드론의 상태를 추정하는 거대한 물리 엔진인 EKF의 첫 번째 핵심 톱니바퀴는 ’기체의 기울어짐(자세)’을 계산하는 것이다. 이 연산은 src/lib/ecl/EKF/ekf.cpp (혹은 관련 모듈)의 상태 예측(State Prediction) 블록 내 최상단에서 이루어지며, 오일러 각도(Euler Angles)의 특이점(Gimbal Lock) 문제를 피하기 위해 4차원 복소수 체계인 쿼터니언(Quaternion) 을 이용해 수행된다.
본 절에서는 250Hz 이상의 고속으로 호명되는 predictState() 함수 내부에서 센서 원시 데이터가 어떻게 쿼터니언 상태 변수(인덱스 0~3번)로 적분되는지, 그 C++ 코드의 생리적 흐름을 3단계로 분해하여 해부한다.
1. 단계 1. 센서 델타 앵글(Delta Angle) 추출과 바이어스 보상
IMU 드라이버는 일정 시간(\Delta t) 동안 기체가 회전한 총량인 ’델타 앵글(\Delta \theta)’을 3차원 벡터 형태로 EKF 코어에 밀어 넣는다 (imu_sample.delta_ang). 하지만 앞서 언급했듯, 이 원시 데이터에는 센서 고유의 열팽창/물리적 오차인 바이어스(Bias)가 섞여 있다.
가장 먼저 수행되는 연산은 이 더러워진 측정치에서 EKF 내부적으로 지난 스텝까지 끈질기게 추정해 온 자이로스코프 바이어스(delta_ang_bias, 인덱스 10~12번) 를 빼내어 순수한 물리적 회전량만을 걸러내는 작업이다.
// 1. 센서가 측정한 회전량에서 추정된 오차(바이어스) 상쇄 (단위: rad)
// 예상되는 순수 회전량 = 원시 데이터 - 바이어스
const matrix::Vector3f delta_angle = imu_sample.delta_ang - _state.delta_ang_bias;
이렇게 정제된 delta_angle 벡터(x, y, z 방향의 회전 라디안)야말로 다음 쿼터니언 회전 연산의 핵심 연료가 된다.
2. 단계 2. 델타 앵글의 쿼터니언 변환 및 회전 누적(적분)
3차원 벡터로 표현된 요/피치/롤 회전량을 4차원 쿼터니언 공간에 적용하려면, 회전축과 회전각의 성질을 이용해 해당 \Delta \theta 를 쿼터니언 객체로 캐스팅(Casting)해야 한다.
ECL(Estimation and Control Library)에 내장된 matrix::Quaternion 클래스는 3차원 축-각(Axis-Angle) 벡터를 입력받으면 내부적으로 매클로린 급수 전개(Taylor series)와 삼각함수를 이용하여 미소 회전 쿼터니언 \Delta q 로 자동 변환해 준다.
그 후, 직전 타임스텝의 자세 상태 변수(_state.quat_nominal, 인덱스 0~3번)에 이 미소 회전 쿼터니언을 행렬 곱셈(*) 처리한다. 쿼터니언의 곱셈은 곧 3차원 공간에서의 ‘회전의 연속(적분)’ 물리 현상과 완벽히 동치이다.
// 2-1. 순수 회전량 벡터(delta_angle)를 미소 회전 쿼터니언 행렬(dq)로 변환
matrix::Quaternionf dq(delta_angle);
// 2-2. 기존의 자세 쿼터니언에 미소 회전 쿼터니언을 행렬 곱셈하여 상태 업데이트(적분)
// 수식: q(t) = q(t-1) * dq
_state.quat_nominal = _state.quat_nominal * dq;
3. 단계 3. 쿼터니언 정규화(Normalization)를 통한 수치 오차 제거
수학적으로 회전을 나타내는 쿼터니언은 4개의 성분(q_0, q_1, q_2, q_3)의 제곱의 합이 항상 1.0 (Unit Quaternion) 이 되어야 한다는 절대적인 제약 조건을 갖는다.
q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1
하지만 32비트 부동소수점(float) 곱셈이 수천, 수만 번 반복되다 보면 미세한 반올림 오차(Truncation Error)가 누적되어 쿼터니언 벡터의 길이(Norm)가 서서히 1.0에서 벗어나게 된다. 이 현상을 방치하면 기체의 형태가 기하학적으로 찌그러지거나 팽창하여 결국 EKF의 방향 코사인 행렬(DCM) 변환 전체가 붕괴한다.
따라서 쿼터니언 적분의 마지막 줄에는 항상 자기 자신의 크기로 자신을 나누어 강제로 길이를 1.0으로 복구시키는 정규화(Normalize) 방어 코드가 뒤따른다.
// 3. 부동소수점 연산 누적 오차로 인해 길어진 쿼터니언 벡터를 크기 1로 재정규화
_state.quat_nominal.normalize();
// 내부 구현 로직: q = q / sqrt(q0^2 + q1^2 + q2^2 + q3^2)
이 세 줄의 짧고 강렬한 코드 블록이 무사히 통과되면, 앞선 틱(Tick) 대비 \Delta t 시간만큼 미래 좌표로 기체의 머리를 돌려놓은 최신 쿼터니언이 인덱스 0~3번 방에 안착하게 된다. EKF는 이제 이 최신 자세(Heading) 정보를 바탕으로 가속도계 데이터를 중력 축과 분리해 내는 다음 관문에 돌입한다.