27.1.1.1. 비선형 동역학 함수 및 관측 함수의 정의와 물리적 의미
앞선 절에서 확장 칼만 필터(EKF)의 뼈대가 되는 이산 시간 5대 방정식을 살펴보았다. 이 방정식들이 실제로 드론의 비행 역학(Flight Dynamics)을 계산해 내기 위해서는, 그 수식 안의 핵심 기호인 동역학 함수 f 와 관측 함수 h 가 무엇을 의미하는지 공학적으로 정확히 정의되어야 한다.
PX4의 ECL(Estimation and Control Library) 모듈에서 이 두 함수는 단순한 수학 기호를 넘어, 뉴턴의 제2법칙과 오일러(Euler) 회전 운동학을 수백 줄의 C++ 코드로 직조해 놓은 거대한 물리 엔진 그 자체이다.
1. 비선형 상태 동역학 함수: x_{k} = f(x_{k-1}, u_{k-1}) + w_{k-1}
동역학 함수 f()는 드론이 공간상에서 ’어떻게 움직이는가’를 정의하는 규칙이다. PX4 EKF2는 고주파(통상 250Hz 이상)로 쏟아지는 IMU(관성 측정 장치) 데이터를 시스템의 제어 입력(u)으로 간주하여 이 함수를 구동한다. 이를 스트랩다운 관성 항법(Strapdown Inertial Navigation) 체계라고 부른다.
1.1 입력 벡터 (u)와 상태 벡터 (x)
- 제어 입력 벡터 (u): 기압계나 GPS가 아닌, 오직 IMU에서 읽어 들인 각속도(자이로스코프) \omega_m 와 직선 가속도(가속도계) a_m 의 측정값이다.
- 상태 벡터 (x): ECL이 추정하고자 하는 24개의 핵심 변수(자세 쿼터니언, 3차원 속도, 3차원 위치, 센서 바이어스 등)이다.
1.2 함수 f()의 물리적 적분 과정 (The Physics of f)
함수 f() 내부에서는 다음과 같은 세 단계의 연쇄적인 적분(Integration)이 일어난다. 이것이 바로 상태 전이(State Transition)의 본질이다.
-
자세 동역학 (Attitude Dynamics):
센서가 측정해 낸 각속도(\omega_m)에서 EKF가 스스로 추정한 자이로 바이어스(오차) 추정치를 빼서 순수 각속도(\omega)를 구한다. 이 순수 각속도를 이전 타임스텝의 자세 쿼터니언(q_{k-1})에 운동학적으로 곱하고 적분(\Delta t)하여 현재의 기체 회전 상태(q_k)를 도출해 낸다.
물리적 의미: “기체가 찰나의 시간(\Delta t) 동안 얼마나 비틀어졌는가?” -
속도 동역학 (Velocity Dynamics):
센서가 측정해 낸 가속도(a_m)에서 가속도 바이어스 추정치를 뺀 뒤, 방금 1단계에서 구한 최신 쿼터니언(q_k)을 이용해 기체 좌표계(Body Frame)의 가속도를 국소 수평 좌표계(Earth/Nav Frame)로 회전(Rotation) 변환한다. 여기에 지구 중력(Gravity, g) 벡터를 빼서 순수한 이동 가속도를 얻고, 이를 적분하여 3차원 속도(v_k)를 산출한다.
물리적 의미: “기체가 찰나의 시간 동안 중력을 이겨내고 어떤 방향으로 얼마나 가속되었는가?” -
위치 동역학 (Position Dynamics):
2단계에서 구한 현재의 절대 속도(v_k)를 다시 한 번 시간(\Delta t)에 대해 적분하여 3차원 위치(p_k)를 산출한다.
물리적 의미: “기체가 방금 구한 속도로 찰나의 시간 동안 이동했을 때 도달한 지구상의 좌표는 어디인가?”
이 세 가지 운동학적 사슬방정식이 결합하여 형성된 거대한 다변수 함수가 바로 f(x,u) 이다. 3차원 회전 변환(DCM, 쿼터니언 곱셈)이 포함되어 있기 때문에 이 함수는 필연적으로 강력한 비선형성(Non-linear) 을 띨 수밖에 없다.
2. 비선형 센서 관측 함수: z_{k} = h(x_{k}) + v_{k}
동역학 함수 f()가 IMU 데이터를 바탕으로 ’상상 속의 비행’을 하는 것이라면, 관측 함수 h()는 그 상상을 현실과 비교하기 위한 ‘번역기’ 역할을 한다. EKF 내부 모델(x_k)과 외부 센서(GPS, 지자기 등)가 뱉어내는 포맷은 완전히 다르기 때문에 이를 맞춰주는 함수가 필요하다.
관측 함수 h(x)의 근본적인 질문은 이것이다.
“만약 나의 현재 내부 추정 상태(x_k)가 100% 진실이라고 가정할 때, 지금 내 기체에 달린 센서(GPS, 기압계 등)들은 어떤 수치(z_k)를 출력해야만 정상인가?”
2.1 각 센서별 관측 함수 h(x)의 매핑 로직
-
GPS 위치 관측 (GPS Position Measurement):
가장 단순한 형태의 관측 함수다. EKF가 내부적으로 추정하고 있는 3차원 위치 상태 벡터(North, East, Down 좌표)를 그대로 뽑아내어 GPS 센서가 뱉어내는 NED 좌표 데이터 포맷과 1:1로 비교한다. 이 경우 h(x)는 위치 인덱스만 뽑아내는 거의 선형적인(Linear) 매핑에 가깝다. -
기압계 고도 관측 (Barometer Altitude Measurement):
EKF 상태 벡터 내의 P_D (Position Down, 고도) 성분을 뽑아내어, 초기 이륙 시 기록했던 기압 기준점(Reference) 오프셋을 반영한 관측 기대치 높이로 변환한다. -
지자기 센서 관측 (Magnetometer Measurement):
비선형성이 극대화되는 대표적인 관측 함수다. 지자기 센서는 기체 프레임(Body Frame)에 고정되어 있으므로 동체와 함께 이리저리 회전한다. 따라서 EKF는 내부적으로 추정 중인 ’지구 자기장 3차원 벡터 상태’를 EKF의 ’현재 자세 쿼터니언 상태’를 이용해 역(Inverse)으로 회전시켜 기체 프레임으로 억지로 찌그러트린다.
이 변환을 거쳐야만 센서 하드웨어(Magnetometer)가 측정한 X, Y, Z축의 가우스(Gauss) 데이터와 직접적으로 혁신(Innovation) 오차를 뺄셈 연산할 수 있다. 즉, h(x) 내부에는 반드시 복잡한 쿼터니언 수식이 동반된다.
3. 요약: 물리 모델과 데이터의 가교
결론적으로 비선형 동역학 함수 f(x, u)는 기계공학과 뉴턴 물리학이 녹아든 상태 적분 엔진(State Integration Engine) 이며, 비선형 관측 함수 h(x)는 EKF의 상상 속 수치와 현실 센서의 날 것(Raw Data) 사이의 포맷 매칭을 담당하는 투영 렌즈(Projection Lens) 이다.
PX4 ECL 펌웨어에서는 이 두 함수가 수학적 아름다움을 넘어, PredictStates() 라는 IMU 폴링 함수와, 수많은 센서별 분기문을 가진 Fuse() 함수들의 거대한 집합체로써 구현되어 있다. 다음 절에서는 이 복잡한 비선형 함수들이 상태 오차를 예측해 내기 위해 C++ 코드 단에서 어떠한 파이프라인(Pipeline)을 거쳐 연산의 흐름을 만들어내는지 해부해 보도록 하겠다.