21.8.3.1. 고도 데이터의 로우 패스 필터(Low Pass Filter, LPF) 적용

센서의 고주파 노이즈(Spike)를 깎아내는 가장 직관적이고 고전적인 수학적 도구는 바로 로우 패스 필터(LPF) 다. 이름 그대로 “느리게 변하는 저주파(리얼 고도 변화)는 통과시키고, 빠르게 튀는 고주파(바람이나 진동 노이즈)는 차단하겠다“는 의미다.

PX4 커널 소스코드 내부에서 가장 많이 쓰이는 LPF 구현체는 부동소수점 연산량이 매우 적으면서도 강력한 성능을 내는 1차 IIR (Infinite Impulse Response) 필터이다. 우리는 이 필터를 FSM 진입 조건문(if) 직전에 달아줄 것이다.

1. 차 IIR 필터의 공식

수학적으로 1차 IIR 필터를 코드로 옮기면 단 한 줄의 수식으로 압축된다.

Filtered_Value = (Alpha * Raw_Value) + ((1 - Alpha) * Previous_Filtered_Value)

Raw_Value: 방금 uORB에서 읽어온 새롭고 불안정한 날것의 고도 데이터
Previous_Filtered_Value: FSM 루프가 직전 타임에 계산해 둔 안정적인 과거 속 데이터
Alpha (알파 방사율, 0.0 ~ 1.0): 필터의 강도를 결정하는 핵심 튜닝 값

알파(Alpha) 값을 1.0으로 주면 과거 데이터는 완전히 무시하고 현재 데이터만 100% 믿겠다는 뜻으로, 아예 필터링을 안 한 것과 똑같다.
반대로 알파 값을 0.01 같은 아주 작은 소수로 주면, 방금 들어온 센서 값은 1%만 반영하고 99%는 과거의 묵직한 값을 따르겠다는 뜻이 되어 데이터의 출렁임(물리적 충격)을 끈적끈적하게 무시해 버린다.

2. LPF의 C++ 클래스 이식

우선 묵직한 과거의 값을 보존해 줄 상태 변수(State Variable) 하나를 헤더 파일에 선언해야 한다.

// 헤더 파일 추가 변수
float _filtered_altitude_m{0.0f}; // 필터링이 완료된 매끄러운 고도 
bool  _lpf_initialized{false};    // 최초 필터 초기화 여부 플래그

이제 Run() 루프의 최상단에서 센서 데이터를 읽어온 직후, FSM 스위치 구문으로 진입하기 직전에 이 마법의 수식을 돌려주면 된다.

void PayloadAutoDrop::Run() {
    // 1. 센서 수집 (Raw Data)
    vehicle_local_position_s local_pos{};
    float raw_altitude_m = 0.0f;
    if (_local_pos_sub.update(&local_pos) && local_pos.z_valid) {
        raw_altitude_m = -local_pos.z; 
    }

    // 2. [핵심] 1차 IIR LPF 적용
    const float ALPHA = 0.1f; // 필터 강도 조절 (0.1이면 꽤 끈적끈적한 필터)

    if (!_lpf_initialized) {
        // 모듈이 처음 켜질 때는 과거 데이터가 없으므로 첫 데이터로 무식하게 덮어쓴다.
        _filtered_altitude_m = raw_altitude_m;
        _lpf_initialized = true;
    } else {
        // 공식을 그대로 C++ 연산으로 구현!
        _filtered_altitude_m = (ALPHA * raw_altitude_m) + ((1.0f - ALPHA) * _filtered_altitude_m);
    }

    // 3. FSM 코어 전개...
    // 이제 _filtered_altitude_m 변수를 신의 계시처럼 믿고 조건을 판단한다!
    switch (_current_state) {
        // ... (FSM 로직) ...
    }
}

이제 우리의 FSM 조건식에는 뾰족한 가시가 돋친 raw_altitude_m 대신, 둥글둥글하고 매끄러운 _filtered_altitude_m이 삽입된다.
실제로 드론이 48m 부근을 날다가 기압계 돌풍(Gust)에 맞아 1 프레임 동안 55m로 고도가 치솟더라도, 필터 값이 (0.1*55) + (0.9*48) = 48.7m 로 억눌리기 때문에 투하 목표인 50m 문턱을 무사히 방어해 내는 기적을 보게 될 것이다.

하지만 이 ALPHA 값은 소프트웨어 엔지니어가 임의로 정할 수 있는 상수가 아니다.
필터링 딜레이(지연)와 주파수 차단(Cutoff Frequency) 사이의 아슬아슬한 줄타기를 통해 이 알파 값을 어떻게 수학적으로 튜닝해 내는지, 그 설계 과정을 다음 장(21.8.3.1.1)에서 디버깅해 보자.