드론 내비게이션 - 실험 도서관

드론 내비게이션에서의 확장 칼만 필터 적용

드론의 내비게이션 시스템은 다양한 센서 데이터를 융합하여 위치, 속도, 가속도, 방향 등을 추정하는 복잡한 작업을 포함한다. 이러한 센서 융합의 핵심 알고리즘 중 하나로 확장 칼만 필터(Extended Kalman Filter, EKF)가 널리 사용된다. 드론 내비게이션에서는 주로 GPS, IMU(관성 측정 장치), 그리고 가속도계 및 자이로스코프 데이터가 사용되며, EKF는 이러한 다양한 센서들의 데이터를 통합하여 드론의 정확한 상태를 추정하는 역할을 한다.

시스템 모델

드론의 동적 시스템은 일반적으로 비선형 시스템으로 표현된다. 드론의 상태를 나타내는 벡터를 $\mathbf{x}$ 라고 하고, 센서 측정을 나타내는 벡터를 $\mathbf{z}$ 라고 한다. 드론의 비선형 시스템 모델은 다음과 같이 나타낼 수 있다:

$\mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{w}_k$

$\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k$

여기서,
- $\mathbf{x}_k$ 는 $k$ 번째 상태 벡터,
- $\mathbf{u}_k$ 는 $k$ 번째 입력 벡터 (제어 신호),
- $\mathbf{w}_k$ 는 시스템 노이즈 (시스템 노이즈),
- $\mathbf{z}_k$ 는 $k$ 번째 센서 측정값,
- $\mathbf{v}_k$ 는 측정 잡음이다.

함수 $f(\cdot)$ 는 상태의 비선형 변화를 나타내며, $h(\cdot)$ 는 비선형 측정 모델을 나타낸다.

예측 단계

EKF의 예측 단계에서는 드론의 현재 상태를 기반으로 다음 시간 단계에서의 상태를 예측한다. 예측 단계의 수식은 다음과 같다:

$\hat{\mathbf{x}}_{k+1|k} = f(\hat{\mathbf{x}}_{k|k}, \mathbf{u}_k)$

$\mathbf{P}_{k+1|k} = \mathbf{F}_k \mathbf{P}_{k|k} \mathbf{F}_k^\top + \mathbf{Q}_k$

여기서,
- $\hat{\mathbf{x}}_{k+1|k}$ 는 $k+1$ 번째에서의 예측된 상태,
- $\mathbf{P}_{k+1|k}$ 는 예측된 오차 공분산,
- $\mathbf{F}_k = \frac{\partial f}{\partial \mathbf{x}} \bigg|_{\hat{\mathbf{x}}_{k|k}, \mathbf{u}_k}$ 는 상태 전이 야코비,
- $\mathbf{Q}_k$ 는 시스템 노이즈 공분산이다.

업데이트 단계

센서 측정이 들어오면 EKF는 예측된 상태를 업데이트한다. 업데이트 단계는 다음과 같다:

$\mathbf{y}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k+1|k})$

$\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k+1|k} \mathbf{H}_k^\top + \mathbf{R}_k$

$\mathbf{K}_k = \mathbf{P}_{k+1|k} \mathbf{H}_k^\top \mathbf{S}_k^{-1}$

$\hat{\mathbf{x}}_{k+1|k+1} = \hat{\mathbf{x}}_{k+1|k} + \mathbf{K}_k \mathbf{y}_k$

$\mathbf{P}_{k+1|k+1} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k+1|k}$

여기서,
- $\mathbf{y}_k$ 는 잔차(차이) 벡터,
- $\mathbf{S}_k$ 는 잔차 공분산,
- $\mathbf{H}_k = \frac{\partial h}{\partial \mathbf{x}} \bigg|_{\hat{\mathbf{x}}_{k+1|k}}$ 는 측정 모델의 야코비,
- $\mathbf{K}_k$ 는 칼만 이득(Kalman gain),
- $\mathbf{R}_k$ 는 측정 잡음 공분산이다.

이 단계에서, 잔차 벡터 $\mathbf{y}_k$ 는 실제 측정값과 예측된 측정값의 차이를 나타내며, 이 차이를 통해 드론의 상태를 보정한다.

센서 융합

드론 내비게이션에서 확장 칼만 필터(EKF)는 여러 센서로부터 얻은 데이터를 통합하여 정확한 위치 및 방향 정보를 제공한다. 드론의 주요 센서로는 GPS, IMU(자이로스코프 및 가속도계), 그리고 바로미터가 있다. 이러한 센서들은 서로 다른 주기로 데이터를 제공하며, EKF는 이 센서들로부터 얻은 비선형 데이터를 효과적으로 융합한다.

GPS 센서 모델

GPS는 드론의 위치를 경도, 위도, 고도 형식으로 제공한다. GPS는 대개 낮은 주기로 데이터를 제공하지만 위치 정보는 정확도가 높다. GPS 측정 모델은 다음과 같이 표현된다:

$\mathbf{z}_{\text{GPS}, k} = h_{\text{GPS}}(\mathbf{x}_k) + \mathbf{v}_{\text{GPS}, k}$

여기서,
- $\mathbf{z}_{\text{GPS}, k}$ 는 $k$ 번째에서의 GPS 측정값,
- $h_{\text{GPS}}(\mathbf{x}_k)$ 는 상태 벡터 $\mathbf{x}_k$ 에 따른 GPS 측정 함수,
- $\mathbf{v}_{\text{GPS}, k}$ 는 GPS 측정 잡음이다.

GPS 데이터는 주로 위치 $(x, y, z)$ 와 속도 정보 $(v_x, v_y, v_z)$ 를 제공하며, 이는 EKF 업데이트 단계에서 반영된다.

IMU 센서 모델

IMU는 드론의 가속도와 각속도 정보를 높은 주기로 제공하는 센서로, 드론의 빠른 움직임에 대응하는 데 중요한 역할을 한다. IMU 측정 모델은 다음과 같다:

$\mathbf{z}_{\text{IMU}, k} = h_{\text{IMU}}(\mathbf{x}_k) + \mathbf{v}_{\text{IMU}, k}$

여기서,
- $\mathbf{z}_{\text{IMU}, k}$ 는 $k$ 번째에서의 IMU 측정값,
- $h_{\text{IMU}}(\mathbf{x}_k)$ 는 상태 벡터 $\mathbf{x}_k$ 에 따른 IMU 측정 함수,
- $\mathbf{v}_{\text{IMU}, k}$ 는 IMU 측정 잡음이다.

IMU 데이터는 가속도 $(a_x, a_y, a_z)$ 와 각속도 $(\omega_x, \omega_y, \omega_z)$ 로 표현되며, 이러한 데이터는 드론의 방향 및 자세를 추정하는 데 사용된다.

바로미터 센서 모델

바로미터는 드론의 고도 정보를 제공하는 센서이다. 고도는 기압에 따라 달라지며, GPS보다 높은 주기로 데이터를 제공할 수 있다. 바로미터의 측정 모델은 다음과 같다:

$\mathbf{z}_{\text{Baro}, k} = h_{\text{Baro}}(\mathbf{x}_k) + \mathbf{v}_{\text{Baro}, k}$

여기서,
- $\mathbf{z}_{\text{Baro}, k}$ 는 $k$ 번째에서의 바로미터 측정값,
- $h_{\text{Baro}}(\mathbf{x}_k)$ 는 상태 벡터 $\mathbf{x}_k$ 에 따른 바로미터 측정 함수,
- $\mathbf{v}_{\text{Baro}, k}$ 는 바로미터 측정 잡음이다.

바로미터는 드론의 고도 정보 $z$ 에 대한 보정을 제공하며, GPS 데이터와 융합되어 보다 정확한 고도 추정을 가능하게 한다.

확장 칼만 필터의 드론 내비게이션 적용

상태 벡터 정의

드론의 내비게이션을 위해 EKF에서 사용하는 상태 벡터는 다음과 같이 정의할 수 있다:

$\mathbf{x}_k = \begin{bmatrix} x & y & z & v_x & v_y & v_z & \phi & \theta & \psi \end{bmatrix}^\top$

여기서,
- $x, y, z$ 는 드론의 위치,
- $v_x, v_y, v_z$ 는 드론의 속도,
- $\phi, \theta, \psi$ 는 드론의 자세를 나타내는 오일러 각 (롤, 피치, 요)이다.

예측 단계에서의 드론 동역학

드론의 동역학은 비선형 시스템으로 모델링되며, EKF의 예측 단계에서는 이러한 동역학을 사용하여 드론의 상태를 예측한다. 상태 벡터 $\mathbf{x}_k$ 의 변화는 드론의 입력(제어 신호)과 가속도 및 각속도에 따라 결정된다. 이는 다음과 같은 상태 전이 방정식으로 나타낼 수 있다:

$\mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k)$

여기서, 제어 신호 $\mathbf{u}_k$ 는 드론의 모터 출력 등으로부터 얻어진다.

드론의 예측 모델은 가속도 및 각속도 데이터를 사용하여 위치 및 속도의 변화를 예측하며, 이는 각속도에 대한 비선형 모델에 따라 선형화된다.

드론의 상태 예측

드론의 상태 예측은 IMU로부터 얻은 가속도와 각속도 데이터를 기반으로 위치, 속도, 방향을 추정하는 과정이다. 상태 예측에서 사용되는 주요 방정식은 뉴턴의 운동 법칙과 회전 운동에 대한 비선형 방정식이다. 예측 단계는 다음과 같이 이루어진다:

위치 및 속도 예측

드론의 위치와 속도는 다음의 운동 방정식을 통해 예측된다:

$\mathbf{p}_{k+1} = \mathbf{p}_k + \mathbf{v}_k \Delta t + \frac{1}{2} \mathbf{a}_k \Delta t^2$

$\mathbf{v}_{k+1} = \mathbf{v}_k + \mathbf{a}_k \Delta t$

여기서,
- $\mathbf{p}_k$ 는 현재 시간 단계 $k$ 에서의 위치 벡터 $(x, y, z)$ ,
- $\mathbf{v}_k$ 는 속도 벡터 $(v_x, v_y, v_z)$ ,
- $\mathbf{a}_k$ 는 가속도 벡터 $(a_x, a_y, a_z)$ ,
- $\Delta t$ 는 시간 간격이다.

IMU에서 제공된 가속도 데이터를 사용하여 드론의 위치와 속도를 예측하며, 이 과정에서 드론의 움직임을 고려한 상태 전이가 이루어진다.

방향(자세) 예측

드론의 자세 변화는 각속도 데이터로부터 예측된다. 오일러 각을 사용하는 드론의 자세 예측 방정식은 다음과 같다:

$\mathbf{\Theta}_{k+1} = \mathbf{\Theta}_k + \mathbf{\Omega}_k \Delta t$

여기서,
- $\mathbf{\Theta}_k = \begin{bmatrix} \phi_k & \theta_k & \psi_k \end{bmatrix}^\top$ 는 롤, 피치, 요 각도 벡터,
- $\mathbf{\Omega}_k = \begin{bmatrix} \omega_x & \omega_y & \omega_z \end{bmatrix}^\top$ 는 각속도 벡터,
- $\Delta t$ 는 시간 간격이다.

각속도 벡터 $\mathbf{\Omega}_k$ 는 IMU의 자이로스코프 데이터로부터 얻어지며, 이를 통해 드론의 방향(자세)을 예측할 수 있다.

GPS와 IMU 데이터를 통한 업데이트

드론 내비게이션에서 가장 중요한 업데이트 단계는 GPS 및 IMU 데이터를 EKF의 업데이트 방정식에 적용하는 것이다. 이 과정은 GPS와 IMU가 서로 다른 주기로 데이터를 제공하므로, 주기 차이를 고려한 동기화가 필요하다.

GPS 업데이트

GPS는 보통 저주기로 데이터를 제공하므로, GPS 데이터가 들어오는 시점에 EKF는 예측된 상태와 GPS로부터 얻은 위치 데이터를 비교하여 상태를 보정한다. GPS의 측정 모델은 다음과 같다:

$\mathbf{z}_{\text{GPS}} = \begin{bmatrix} x_{\text{GPS}} \\ y_{\text{GPS}} \\ z_{\text{GPS}} \end{bmatrix}$

업데이트 방정식에서의 잔차(측정과 예측의 차이)는 다음과 같이 계산된다:

$\mathbf{y}_k = \mathbf{z}_{\text{GPS}, k} - h_{\text{GPS}}(\hat{\mathbf{x}}_{k+1|k})$

여기서 $h_{\text{GPS}}(\hat{\mathbf{x}}_{k+1|k})$ 는 예측된 위치 정보를 나타낸다. 이를 통해 EKF는 GPS 데이터로 위치를 보정한다.

IMU 업데이트

IMU는 고주기로 데이터를 제공하며, 각속도 및 가속도 정보를 통해 드론의 상태를 업데이트한다. IMU 데이터는 다음과 같은 잔차를 통해 EKF에서 사용된다:

$\mathbf{y}_k = \mathbf{z}_{\text{IMU}, k} - h_{\text{IMU}}(\hat{\mathbf{x}}_{k+1|k})$

이 단계에서는 가속도 및 각속도 데이터를 사용하여 드론의 자세 및 가속도를 보정한다. IMU의 고주기 데이터는 드론의 빠른 움직임을 정확하게 반영할 수 있도록 한다.

드론의 절대 방향 추정

두 개의 GPS 센서를 사용하는 경우, 드론의 절대 방향(heading)을 추정할 수 있다. 이때 두 GPS 센서의 위치 차이를 이용하여 드론의 절대적인 방향을 계산할 수 있으며, EKF는 이를 이용해 드론의 방향을 보정한다.

두 GPS 센서 간의 위치 차이를 이용한 방향 추정 방정식은 다음과 같다:

$\psi = \arctan\left(\frac{y_2 - y_1}{x_2 - x_1}\right)$

여기서 $x_1, y_1$ 은 첫 번째 GPS 센서의 위치, $x_2, y_2$ 는 두 번째 GPS 센서의 위치이다. 이 계산을 통해 드론의 절대적인 방향을 추정할 수 있으며, 이는 자이로스코프 데이터와 함께 EKF의 상태 벡터에서 자세 정보를 보정하는 데 사용된다.

C++ Eigen을 통한 구현

다음은 위에서 설명한 드론 내비게이션의 확장 칼만 필터를 C++와 Eigen 라이브러리를 사용하여 구현한 코드이다. 이 코드는 GPS, IMU(가속도계, 자이로스코프) 센서 데이터를 융합하여 드론의 위치, 속도, 자세를 추정하는 확장 칼만 필터의 기본 구조를 보여준다.

#include <Eigen/Dense>
#include <iostream>
#include <cmath>

using namespace Eigen;

// 상태 벡터 크기 및 측정 벡터 크기 정의
const int state_size = 9; // x, y, z, vx, vy, vz, roll, pitch, yaw
const int gps_size = 3;   // x, y, z
const int imu_size = 6;   // ax, ay, az, wx, wy, wz

// 시간 간격 (초)
double dt = 0.01;

// 시스템 모델을 나타내는 함수 (예측 단계)
VectorXd predictState(const VectorXd& state, const VectorXd& u) {
    VectorXd next_state(state_size);

    // 위치와 속도 예측 (가속도 계수 적용)
    next_state(0) = state(0) + state(3) * dt + 0.5 * u(0) * dt * dt; // x
    next_state(1) = state(1) + state(4) * dt + 0.5 * u(1) * dt * dt; // y
    next_state(2) = state(2) + state(5) * dt + 0.5 * u(2) * dt * dt; // z

    // 속도 예측
    next_state(3) = state(3) + u(0) * dt; // vx
    next_state(4) = state(4) + u(1) * dt; // vy
    next_state(5) = state(5) + u(2) * dt; // vz

    // 오일러 각 예측 (각속도 계수 적용)
    next_state(6) = state(6) + u(3) * dt; // roll (phi)
    next_state(7) = state(7) + u(4) * dt; // pitch (theta)
    next_state(8) = state(8) + u(5) * dt; // yaw (psi)

    return next_state;
}

// 상태 전이 야코비 행렬 계산
MatrixXd calculateJacobian(const VectorXd& state, const VectorXd& u) {
    MatrixXd F = MatrixXd::Identity(state_size, state_size);

    // 위치와 속도에 대한 부분 미분 (야코비)
    F(0, 3) = dt; // ∂x/∂vx
    F(1, 4) = dt; // ∂y/∂vy
    F(2, 5) = dt; // ∂z/∂vz

    // 각도에 대한 부분 미분 (야코비)
    F(6, 3) = 0; // ∂roll/∂vx
    F(7, 4) = 0; // ∂pitch/∂vy
    F(8, 5) = 0; // ∂yaw/∂vz

    return F;
}

// GPS 측정 모델 (업데이트 단계)
VectorXd gpsMeasurementModel(const VectorXd& state) {
    VectorXd z(gps_size);
    z(0) = state(0); // x
    z(1) = state(1); // y
    z(2) = state(2); // z
    return z;
}

// IMU 측정 모델 (업데이트 단계)
VectorXd imuMeasurementModel(const VectorXd& state) {
    VectorXd z(imu_size);
    // 가속도와 각속도는 상태 벡터에서 직접 얻지 않으므로 측정 모델에 추가 변환 필요
    z(0) = state(3); // vx
    z(1) = state(4); // vy
    z(2) = state(5); // vz
    z(3) = state(6); // roll (phi)
    z(4) = state(7); // pitch (theta)
    z(5) = state(8); // yaw (psi)
    return z;
}

// 확장 칼만 필터 업데이트
void ekfUpdate(VectorXd& state, MatrixXd& P, const VectorXd& z, const MatrixXd& H, const MatrixXd& R) {
    VectorXd y = z - H * state;                         // 잔차 계산
    MatrixXd S = H * P * H.transpose() + R;             // 잔차 공분산
    MatrixXd K = P * H.transpose() * S.inverse();       // 칼만 이득 계산
    state = state + K * y;                              // 상태 보정
    P = (MatrixXd::Identity(state_size, state_size) - K * H) * P; // 공분산 보정
}

int main() {
    // 상태 벡터와 공분산 행렬 초기화
    VectorXd state(state_size);
    state << 0, 0, 0, 0, 0, 0, 0, 0, 0; // 초기 위치, 속도, 오일러 각

    MatrixXd P = MatrixXd::Identity(state_size, state_size) * 0.1; // 초기 공분산

    // 시스템 노이즈 공분산 행렬
    MatrixXd Q = MatrixXd::Identity(state_size, state_size) * 0.01;

    // 측정 잡음 공분산 행렬 (GPS와 IMU)
    MatrixXd R_gps = MatrixXd::Identity(gps_size, gps_size) * 0.05;
    MatrixXd R_imu = MatrixXd::Identity(imu_size, imu_size) * 0.02;

    // 제어 입력 벡터 (가속도 및 각속도)
    VectorXd u(imu_size);
    u << 0.1, 0.1, 0.1, 0.01, 0.01, 0.01; // 임의의 제어 입력

    // 예측 단계
    state = predictState(state, u);                 // 상태 예측
    MatrixXd F = calculateJacobian(state, u);       // 야코비 계산
    P = F * P * F.transpose() + Q;                  // 공분산 예측

    // GPS 측정 값
    VectorXd gps_measurement(gps_size);
    gps_measurement << 10.0, 5.0, 1.0; // 임의의 GPS 측정 값

    // IMU 측정 값
    VectorXd imu_measurement(imu_size);
    imu_measurement << 0.1, 0.1, 0.1, 0.01, 0.01, 0.01; // 임의의 IMU 측정 값

    // 업데이트 단계 - GPS
    MatrixXd H_gps = MatrixXd::Zero(gps_size, state_size);
    H_gps.block<3, 3>(0, 0) = MatrixXd::Identity(3, 3); // 위치에 대한 H 행렬
    ekfUpdate(state, P, gps_measurement, H_gps, R_gps);

    // 업데이트 단계 - IMU
    MatrixXd H_imu = MatrixXd::Zero(imu_size, state_size);
    H_imu.block<3, 3>(0, 3) = MatrixXd::Identity(3, 3); // 속도에 대한 H 행렬
    ekfUpdate(state, P, imu_measurement, H_imu, R_imu);

    // 결과 출력
    std::cout << "Updated State: \n" << state << std::endl;
    std::cout << "Updated Covariance: \n" << P << std::endl;

    return 0;
}

상태 벡터: 9개의 상태 변수로 구성되며, 각각 $x, y, z$ (위치), $v_x, v_y, v_z$ (속도), $\phi$ (롤), $\theta$ (피치), $\psi$ (요)을 나타낸다.
제어 입력 벡터: 가속도 $a_x, a_y, a_z$ 와 각속도 $\omega_x, \omega_y, \omega_z$ 로 구성된 입력 벡터이다.
예측 단계: 드론의 위치와 속도는 운동 방정식을 기반으로 예측되며, 각도는 각속도를 기반으로 예측된다.
야코비 계산: 상태 전이 모델의 선형화를 위해 야코비 행렬이 사용된다.
업데이트 단계: GPS와 IMU로부터 들어온 데이터를 사용하여 EKF의 상태를 업데이트한다. GPS는 위치를 제공하고, IMU는 가속도 및 각속도 정보를 제공한다.

이 코드는 드론 내비게이션에서 센서 데이터를 융합하여 드론의 상태(위치, 속도, 자세)를 추정하는 EKF의 기본 구조를 보여준다.