칼만 필터의 개요와 아폴로 시스템에서의 역할

칼만 필터(Kalman Filter)는 1960년대 개발된 이후 아폴로 프로젝트의 궤도 계산에 사용되며 그 성능을 입증한 필터링 기술입니다. 현대의 자율주행 시스템에서도 칼만 필터는 제어, 인지, 센서 퓨전 모듈의 핵심 요소로 자리 잡고 있습니다. 바이두의 오픈 소스 자율주행 플랫폼인 Apollo(9.x_alpha 버전 기준) 내 multi_sensor_fusion 컴포넌트에서는 객체의 상태 추정과 센서 데이터 통합을 위해 이 필터를 고도화하여 사용하고 있습니다.

Apollo 칼만 필터 클래스의 구조

Apollo의 칼만 필터 구현체는 modules/perception/multi_sensor_fusion/common 디렉토리에 위치합니다. 이 클래스는 전형적인 '예측(Predict)'과 '보정(Correct)' 단계를 수행하며, 시스템 상태 변수, 상태 협분산 행렬, 상태 전이 행렬 등을 관리합니다.

핵심 메서드 설계 분석

1. 초기화 과정 (Init)

초기화 단계에서는 행렬의 차원을 검증하고 기본값을 설정합니다. 특히 상태 협분산 행렬이 정방 행렬(Square Matrix)인지, 상태 벡터와 크기가 일치하는지 엄격하게 체크하여 런타임 오류를 방지합니다.

bool KalmanFilter::Initialize(const Eigen::VectorXd &initial_state, const Eigen::MatrixXd &initial_cov) {
    if (initial_cov.rows() != initial_cov.cols()) {
        // 협분산 행렬은 반드시 정방 행렬이어야 함
        return false;
    }
    
    int dim = static_cast<int>(initial_cov.rows());
    if (dim <= 0 || dim != initial_state.rows()) {
        return false;
    }

    state_dim_ = dim;
    current_state_ = initial_state;
    uncertainty_cov_ = initial_cov;
    prior_state_storage_ = current_state_;

    // 전이 행렬 및 관측 행렬 초기화
    predict_trans_mat_.setIdentity(dim, dim);
    measure_matrix_.setIdentity(dim, dim);
    process_noise_cov_.setZero(dim, dim);
    
    is_initialized_ = true;
    return true;
}

2. 예측 단계 (Predict)

예측 단계에서는 시스템의 물리적 모델에 기반하여 다음 시점의 상태와 불확실성을 계산합니다. Apollo의 설계는 호출 시점에 전이 행렬(F)과 프로세스 노이즈(Q)를 직접 입력받아 상황에 맞는 유연한 예측을 지원합니다.

bool KalmanFilter::Predict(const Eigen::MatrixXd &f_mat, const Eigen::MatrixXd &q_mat) {
    if (!is_initialized_) return false;

    // 차원 일치 여부 확인
    if (f_mat.rows() != state_dim_ || q_mat.rows() != state_dim_) {
        return false;
    }

    predict_trans_mat_ = f_mat;
    process_noise_cov_ = q_mat;

    // 상태 예측: x = F * x
    current_state_ = predict_trans_mat_ * current_state_;
    // 불확실성 예측: P = F * P * F^T + Q
    uncertainty_cov_ = predict_trans_mat_ * uncertainty_cov_ * predict_trans_mat_.transpose() + process_noise_cov_;

    return true;
}

3. 보정 단계 (Correct)

실제 관측값(Measurement)을 반영하여 예측된 상태를 수정합니다. 칼만 이득(K)을 계산하고 이를 이용해 상태와 협분산을 갱신합니다.

bool KalmanFilter::Correct(const Eigen::VectorXd &z_obs, const Eigen::MatrixXd &r_mat) {
    if (!is_initialized_) return false;

    // 칼만 이득 계산: K = P * H^T * (H * P * H^T + R)^-1
    Eigen::MatrixXd s_mat = measure_matrix_ * uncertainty_cov_ * measure_matrix_.transpose() + r_mat;
    kalman_gain_mat_ = uncertainty_cov_ * measure_matrix_.transpose() * s_mat.inverse();

    // 상태 업데이트: x = x + K * (z - H * x)
    current_state_ = current_state_ + kalman_gain_mat_ * (z_obs - measure_matrix_ * current_state_);

    // 협분산 업데이트: P = (I - K * H) * P (Joseph form 등 변형 가능)
    Eigen::MatrixXd identity_mat = Eigen::MatrixXd::Identity(state_dim_, state_dim_);
    uncertainty_cov_ = (identity_mat - kalman_gain_mat_ * measure_matrix_) * uncertainty_cov_;

    return true;
}

이상치 대응을 위한 Breakdown 설계

현실 세계의 센서 데이터에는 노이즈뿐만 아니라 급격한 튀는 값(Outlier)이 포함될 수 있습니다. Apollo는 이를 처리하기 위해 Breakdown 메커니즘을 도입했습니다. 차량이 물리적으로 불가능한 속도로 가속하거나 위치가 급격히 변할 때, 계산된 칼만 이득이나 상태 변화량이 임계값을 넘으면 이를 강제로 제한하여 필터의 안정성을 유지합니다.

• Gain Breakdown: 특정 차원의 상태 변화가 너무 크면 해당 변화량을 정규화하여 억제합니다.
• Value Breakdown: 결과값이 신뢰 범위를 벗어날 경우 해당 상태 성분을 보정합니다.

Apollo 칼만 필터 구현의 특징 요약

1. 견고한 차원 검사: 행렬 연산 전 차원 불일치로 인한 크래시를 방지하기 위한 엄격한 유효성 검사를 수행합니다.
2. 동적 행렬 입력: 고정된 모델이 아니라 예측 시마다 상태 전이 행렬과 노이즈 행렬을 주입할 수 있어 다양한 센서 모델에 대응 가능합니다.
3. 실용적 안정성 장치: 이론적인 칼만 필터 공식을 넘어 실제 환경에서 발생할 수 있는 데이터 급변 상황을 고려한 Breakdown 로직이 포함되어 있습니다.
4. 일관된 인터페이스: 관측 행렬(H)을 관리하는 별도의 메서드(SetControlMatrix)를 제공하여 다중 센서 퓨전 시 필터 재사용성을 높였습니다.