초기화 방법 - 소프트웨어 융합

초기 상태 벡터의 설정

확장 칼만 필터의 초기화에서 가장 중요한 단계 중 하나는 초기 상태 벡터 $\mathbf{x}_0$ 의 설정이다. 초기 상태는 시스템에 대한 첫 번째 추정치를 의미하며, 이를 올바르게 설정하는 것이 필터의 성능에 큰 영향을 미친다. 시스템에 대한 사전 정보나 초기 측정값을 사용하여 $\mathbf{x}_0$ 을 설정한다. 상태 벡터는 시스템의 상태 변수를 포함하며, 일반적으로 다음과 같이 표현된다:

$\mathbf{x}_0 = \begin{bmatrix} x_1 & x_2 & \dots & x_n \end{bmatrix}^\top$

여기서 $x_1, x_2, \dots, x_n$ 은 시스템의 각 상태 변수이다.

초기 오차 공분산 행렬의 설정

오차 공분산 행렬 $\mathbf{P}_0$ 은 초기 상태 추정치의 불확실성을 나타낸다. $\mathbf{P}_0$ 는 대각선 성분에 각 상태 변수의 분산을 포함하는 대칭 행렬로 정의된다. 초기 상태

$\mathbf{x}_0$ 에 대한 신뢰도가 낮을 경우, 대각선 성분에 큰 값을 설정하여 불확실성을 반영할 수 있다. 반대로, 초기 상태에 대한 신뢰도가 높다면 작은 값을 사용한다. 초기 오차 공분산 행렬은 다음과 같이 표현된다:

$\mathbf{P}_0 = \begin{bmatrix} \sigma_{x_1}^2 & 0 & \dots & 0 \\ 0 & \sigma_{x_2}^2 & \dots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \dots & \sigma_{x_n}^2 \end{bmatrix}$

여기서 $\sigma_{x_i}^2$ 는 상태 변수 $x_i$ 의 초기 분산을 나타낸다. 비대각선 성분은 초기 상태 변수들 간의 상관관계를 나타내며, 만약 상태 변수들 간에 상관관계가 없다면 비대각선 성분은 0으로 설정된다.

시스템 노이즈 공분산 행렬의 초기화

시스템 노이즈 공분산 행렬 $\mathbf{Q}$ 는 시스템 모델의 불확실성을 반영한다. 이는 시스템의 외부에서 발생하는 노이즈나 모델링 오류를 고려하는 것으로, 시스템이 실제 환경에서 예측할 수 없는 변화를 겪을 때 필요한 행렬이다. $\mathbf{Q}$ 는 일반적으로 대각 성분에 각 변수의 노이즈 분산을 포함하며, 다음과 같이 정의된다:

$\mathbf{Q} = \begin{bmatrix} q_1 & 0 & \dots & 0 \\ 0 & q_2 & \dots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \dots & q_n \end{bmatrix}$

여기서 $q_i$ 는 상태 변수 $x_i$ 에 대한 시스템 노이즈 분산을 나타낸다. 이는 시스템의 동작이 얼마나 노이즈에 민감한지를 결정하는 중요한 파라미터이다.

측정 노이즈 공분산 행렬의 초기화

측정 노이즈 공분산 행렬 $\mathbf{R}$ 는 측정 장치의 정확도를 나타낸다. 측정 시스템에 따라 노이즈의 크기와 형태가 다를 수 있으며, 이를 행렬로 정의하여 필터에 반영한다. 측정 노이즈 공분산 행렬은 다음과 같다:

$\mathbf{R} = \begin{bmatrix} r_1 & 0 & \dots & 0 \\ 0 & r_2 & \dots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \dots & r_m \end{bmatrix}$

여기서 $r_i$ 는 각 측정 값에 대한 노이즈 분산을 나타낸다. 측정 장비가 불안정하거나 노이즈가 큰 경우, $\mathbf{R}$ 의 값은 커지며, 이를 통해 필터는 측정 값보다 예측 값을 더 신뢰하게 된다.

칼만 이득의 초기화

확장 칼만 필터에서 중요한 변수 중 하나는 칼만 이득 $\mathbf{K}$ 이다. 칼만 이득은 예측 단계에서의 오차 공분산과 측정 노이즈 공분산 간의 관계를 이용해 계산되며, 필터가 예측 값과 측정 값 중 어느 쪽을 더 신뢰할지 결정하는 역할을 한다.

초기화 단계에서는 칼만 이득을 명시적으로 설정하지 않으며, 첫 예측과 업데이트 단계에서 계산된다. 초기 오차 공분산 행렬 $\mathbf{P}_0$ 과 측정 노이즈 공분산 행렬 $\mathbf{R}$ 이 설정된 후, 이 두 행렬을 기반으로 칼만 이득이 자동으로 계산된다.

칼만 이득은 다음과 같이 계산된다:

$\mathbf{K} = \mathbf{P} \mathbf{H}^\top \left(\mathbf{H} \mathbf{P} \mathbf{H}^\top + \mathbf{R}\right)^{-1}$

여기서 $\mathbf{H}$ 는 측정 모델의 야코비 행렬을 나타내며, 이 행렬은 상태 벡터를 측정 값으로 매핑하는 비선형 함수의 선형 근사이다.

야코비 행렬의 초기화

확장 칼만 필터는 비선형 시스템을 다루기 때문에, 시스템의 상태 변수를 측정 값으로 매핑하는 모델도 비선형일 수 있다. 이때, 측정 모델의 야코비 행렬 $\mathbf{H}$ 는 상태 변수의 작은 변화에 따른 측정 값의 변화를 나타내는 선형 근사 행렬이다.

야코비 행렬은 상태 벡터 $\mathbf{x}$ 의 각 성분에 대한 측정 모델 함수 $h(\mathbf{x})$ 의 편미분으로 정의되며, 다음과 같은 형태를 갖는다:

$\mathbf{H} = \frac{\partial h(\mathbf{x})}{\partial \mathbf{x}} = \begin{bmatrix} \frac{\partial h_1}{\partial x_1} & \frac{\partial h_1}{\partial x_2} & \dots & \frac{\partial h_1}{\partial x_n} \\ \frac{\partial h_2}{\partial x_1} & \frac{\partial h_2}{\partial x_2} & \dots & \frac{\partial h_2}{\partial x_n} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial h_m}{\partial x_1} & \frac{\partial h_m}{\partial x_2} & \dots & \frac{\partial h_m}{\partial x_n} \end{bmatrix}$

초기화 단계에서 야코비 행렬 $\mathbf{H}$ 는 시스템의 상태와 측정 모델에 따라 적절하게 설정되어야 한다. 이 행렬은 측정 단계에서 필수적으로 사용되며, 시스템의 비선형성을 적절히 반영하여 필터가 정확하게 동작하도록 돕는다.

상태 전이 행렬의 초기화

상태 전이 행렬 $\mathbf{F}$ 는 시스템의 상태가 시간에 따라 어떻게 변화하는지를 나타내는 행렬이다. 이는 시스템의 동적 모델을 기반으로 정의되며, 다음과 같이 상태 벡터에 대한 상태 전이 함수 $f(\mathbf{x}, \mathbf{u})$ 의 편미분으로 구성된다:

$\mathbf{F} = \frac{\partial f(\mathbf{x}, \mathbf{u})}{\partial \mathbf{x}} = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} & \dots & \frac{\partial f_1}{\partial x_n} \\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} & \dots & \frac{\partial f_2}{\partial x_n} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_n}{\partial x_1} & \frac{\partial f_n}{\partial x_2} & \dots & \frac{\partial f_n}{\partial x_n} \end{bmatrix}$

$\mathbf{F}$ 는 예측 단계에서 상태 벡터 $\mathbf{x}$ 를 시간에 따라 전파하는 데 사용되며, 비선형 시스템에서는 매번 새로운 상태에 대해 재계산된다.

시스템 노이즈 행렬의 설정

시스템 노이즈 행렬 $\mathbf{Q}$ 는 시스템 모델의 불확실성을 나타내는 행렬로, 시스템의 실제 동작에서 발생할 수 있는 예측 불확실성을 반영한다. 이는 시스템 모델이 비선형이거나 외부 환경에서 예상치 못한 변화가 발생할 수 있는 경우에 특히 중요하다.

$\mathbf{Q}$ 는 시스템의 상태 전이 과정에서 발생할 수 있는 노이즈의 크기와 상관관계를 나타내며, 일반적으로 다음과 같은 형태를 갖는다:

$\mathbf{Q} = \mathbf{G} \mathbf{Q}_d \mathbf{G}^\top$

여기서 $\mathbf{G}$ 는 시스템 노이즈의 상태에 대한 영향을 나타내는 행렬이며, $\mathbf{Q}_d$ 는 시스템 노이즈의 공분산 행렬이다. $\mathbf{G}$ 와 $\mathbf{Q}_d$ 의 초기값은 시스템의 동적 특성에 따라 결정된다.

측정 노이즈 행렬의 설정

측정 노이즈 행렬 $\mathbf{R}$ 는 측정 장비에서 발생할 수 있는 노이즈를 반영하는 행렬로, 필터가 측정 데이터를 어떻게 신뢰할지를 결정한다. 측정 시스템의 특성에 따라 각 측정 변수에 대한 노이즈 분산이 달라지며, 이 정보를 기반으로 $\mathbf{R}$ 을 설정한다.

$\mathbf{R}$ 의 대각선 성분에는 각 측정 값에 대한 노이즈 분산이 들어가며, 비대각선 성분은 측정 값들 간의 상관관계를 나타낸다. 예를 들어, 만약 특정 측정 값들이 서로 상관관계가 없다고 가정되면 비대각선 성분은 0으로 설정된다.

다음과 같이 $\mathbf{R}$ 행렬을 정의할 수 있다:

$\mathbf{R} = \begin{bmatrix} \sigma_{z_1}^2 & 0 & \dots & 0 \\ 0 & \sigma_{z_2}^2 & \dots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \dots & \sigma_{z_m}^2 \end{bmatrix}$

여기서 $\sigma_{z_i}^2$ 는 측정 변수 $z_i$ 에 대한 노이즈 분산을 나타낸다. 측정 장치의 특성에 따라 $\mathbf{R}$ 은 고정된 값이거나, 시간이 지남에 따라 변경될 수 있다.

초기 시간 스텝 설정

확장 칼만 필터의 초기화에서 시간 스텝 $\Delta t$ 의 설정 또한 중요하다. 시간 스텝은 예측 단계에서 상태 변화를 계산하는 데 사용되며, 시스템의 동적 특성을 반영해야 한다. $\Delta t$ 가 너무 크면 필터가 상태 변화를 과도하게 예측할 수 있고, 너무 작으면 필터가 시스템의 변화에 충분히 반응하지 못할 수 있다.

초기화 단계에서 시간 스텝은 시스템의 주기적인 업데이트 속도에 맞춰 적절하게 설정되어야 한다. 시스템이 초당 여러 번 상태를 업데이트하는 경우 $\Delta t$ 는 작은 값으로 설정해야 하며, 업데이트 주기가 긴 시스템의 경우 더 큰 $\Delta t$ 를 사용할 수 있다.