2.1 시스템 모델
표적차량 운동을 모델링하기 위해 그림 1의 상대기하를 고려하자. 사용된 주요 변수들의 정의는 다음과 같다.
$(X_{I},\:Y_{I})$: 관성좌표계(I-frame)
$(X_{V},\:Y_{V})$: 차량좌표계(V-frame)
$\vec{R}_{h}^{I}=\left[x_{h}^{I},\: y_{h}^{I}\right]^{T}$: 자차위치벡터(I-frame)
$\vec{R}_{t}^{I}=\left[x_{t}^{I},\: y_{t}^{I}\right]^{T}$: 표적차량위치벡터(I-frame)
$\vec{R}_{ht}^{V}=[x,\: y]^{T}$: 표적차량의 상대위치(V-frame)
$v_{h},\: v_{t}$: 자차 및 표적차량 속력
$\omega_{h},\:\omega_{t}$: I-frame에 대한 자차 및 표적 각속도
$\psi$: 자차 헤딩각(I-frame)
$\gamma ,\:\phi$: I-frame 및 V-frame 표적차량 헤딩각
$r,\:\lambda$: 표적 상대거리 및 시선각
상대운동 모델링을 위해 도입된 좌표계는 다음과 같다.
∙ 관성좌표계(I-frame)
차량의 관성운동을 기술하기 위한 기준 좌표계로, 본 논문에서는 편의상 NED(North-East-Down) 좌표계로 설정된다. I-frame의 원점은
자차의 초기위치로 설정된다.
∙ 차량좌표계(V-frame)
원점이 자차의 중심과 일치하고, $X_{V}$ 축은 차량 진행방향, $Y_{V}$ 축은 차량 우측방향인 오른손 좌표계이다. 그림 1에 도시한 바와 같이 V-frame은 I-frame을 자차 헤딩각 $\psi$ 만큼 회전한 후, 자차의 현재 위치로 원점 이동하여 얻어진다. $Z$
축에 대해 각도 $\epsilon$ 만큼을 회전시키는 회전변환행렬을 $R_{z}(\epsilon)$이라 하면, I-frame으로 부터 V-frame으로의
좌표변환행렬 및 그 역변환은 다음과 같이 정의된다.
대부분의 차량들이 Ackerman 조향 방식을 채택하고 있으므로 일반적으로 다음 가정들이 성립한다.
D1. 표적차량의 횡기동은 크지 않으며, 이에 따라 헤딩각 변화는 각속도로 근사된다. $(\dot\gamma\approx\omega_{t})$
D2. 짧은 시구간에서 표적차량 속력 및 각속도 변화는 무시할 만하다. $(\dot v_{t}\approx 0,\:\dot\omega_{t}\approx
0)$
D3. 자차의 속력 및 각속도 변화는 무시할 만하다.
$(\dot v_{h}\approx 0,\:\dot\psi\approx\omega_{h})$
D4. 자차 동특성에 비해 샘플링 주기가 충분히 짧다.
위의 가정 및 로부터 I-frame 상에서의 표적차량의 동특성을 등속 선회운동으로 근사할 수 있다.
연속시간에서 기술된 표적차량 운동모델 식 (2)을 샘플링 주기 $T$ 로 이산화하면 식 (3)을 얻는다.
여기서 $k$는 현재시점을 나타내며, 샘플링주기 동안의 표적차량 위치증분 $\Delta\vec{R}_{t,\:k}^{I}$은 다음과 같이 정의된다.
$\Delta\vec{R}_{t,\:k}^{I}=\begin{bmatrix}\int_{t}^{t+T}v_{t}(\beta)\cos\gamma(\beta)d\beta
&\int_{t}^{t+T}v_{t}(\beta)\sin\gamma(\beta)d\beta\end{bmatrix}^{T}$
표적차량 위치증분 $\Delta\vec{R}_{t,\:k}^{I}$을 계산하기 위해 가정 에 따라 표적차량의 속력 $v_{t,\:k}$와 각속도 $\omega_{t,\:k}$를
상수 취급한 후, 삼각함수의 합차공식을 재차 적용하면 다음과 같다.
LIDAR가 제공하는 상대거리 $r$ 및 시선각 $\lambda$는 표적 상대위치와 관련된 것이므로, 추적필터 설계 시에도 자차에 대한 표적 상대운동
모델을 사용하는 것이 편리하다. 가정 에 따라 샘플링주기 동안 자차 각속도를 상수 취급하면 I-frame에서의 자차의 위치 변화를 기술할 수 있다.
여기서 I-frame 자차 위치증분 $\Delta\vec{R}_{h,\:k}^{I}$은 좌표변환행렬 $C_{V}^{I}$과 가용정보로 산출되는 V-frame
자차 위치증분 $\Delta\vec{R}_{h,\:k}^{V}$을 이용하여 다음과 같이 정의된다.
$\Delta\vec{R}_{h}^{I}=C_{V}^{I}(T\cdot\vec{V}_{h}^{V}+\dfrac{1}{2}T^{2}\vec{A}_{h}^{V})=
C_{V}^{I}\Delta\vec{R}_{h}^{V}$
가정 D3에 따라 V-frame 자차 가속도는 $\vec{A}_{h}^{V}\approx\left[0v_{h}\omega_{h}\right]^{T}$으로
쓸 수 있다. 자차 가속도 $\vec{A}_{h}^{V}$와 속도 $\vec{V}_{h}^{V}=\left[v_{h}^{x}v_{h}^{y}\right]^{T}$는
모두 주행거리계 및 각속도계 정보로 계산된다. 식 (3)~ (5)로부터 I-frame 표적 상대위치 변화를 계산할 수 있다.
불행하게도 대부분의 차량에는 I-frame에서 정의되는 자차의 헤딩각 $\psi$를 제공하는 고가의 항법센서가 탑재되어 있지 않다. 더욱이, LIDAR는
표적차량의 V-frame 상대위치 $\vec{R}_{ht}^{V}$에 관련된 정보를 제공한다. 앞서 언급한 가용정보들만을 사용하여 식 (6)을 다시 쓰면 다음 식을 얻는다.
위의 식에서 자차의 헤딩각 증분 $\Delta\psi_{k}$는 가정 및 에 따라 자차 각속도 $\omega_{h}$와 샘플링주기 $T$의 곱으로
근사 된다.
V-frame 표적 헤딩각을 $\phi_{k}≜\gamma_{k}-\psi_{k}$라 하면, 앞서와 유사한 방법으로 식 (7)의 우변 마지막 항을 계산할 수 있다.
참고로 표적차량의 각속도가 충분히 작은 경우 $(\omega_{t}\ll1)$, 식 (9)을 더욱 간단한 형태로 정리할 수 있다.
$R_{z}(-\psi_{k+1})\Delta\vec{R}_{t,\:k}^{I}\approx Tv_{t,\:k}\begin{bmatrix}\cos\left(\phi_{k}-\omega_{h,\:k}T
+\dfrac{T}{2}\omega_{t,\:k}\right)\\\sin\left(\phi_{k}-\omega_{h,\:k}T +\dfrac{T}{2}\omega_{t,\:k}\right)\end{bmatrix}$
마찬가지로 가정 및 에 따라 표적차량의 I-frame 헤딩각 $\gamma$ 역시 표적 각속도 $\omega_{t}$를 이용해 근사 가능하다.
따라서, 위의 근사식 (8) 및 (10)에 의해 V-frame 표적 헤딩각 $\phi$은 다음 차분 방정식을 만족한다.
확장표적추적 문제에서는 표적의 운동학적 상태변수와 더불어 표적형상을 추가적인 상태변수로 고려해야 한다. 표적차량의 전폭 $w$과 전장 $l$은 시간에
따라 크게 변화하지 않으므로 랜덤워크로 모델링해도 무방하다. 따라서, 식 (7), (9) 및 (11)를 종합하면 가용정보 $v_{h}^{x},\: v_{h}^{y},\: v_{h}$ 및 $\omega_{h}$을 이용하여 표적추적필터 설계를 위한 시스템
모델을 기술할 수 있다.
여기서 추정하고자 하는 표적차량의 상태변수 $x$와 자차 관련 외부입력 $u^{c}$, 비선형 함수 $f(\bullet)$의 정의는 다음과 같다.
$x =\begin{bmatrix}x \\ y \\ v_{t}\\\phi \\\omega_{t}\\ l \\ w\end{bmatrix}$, $u^{c}=\begin{bmatrix}v_{h}^{x}\\
v_{h}^{y}\\ v_{h}\\\omega_{h}\end{bmatrix}$, $f(x ,\: u^{c})=\begin{bmatrix}f_{p}^{m}(x
,\: u^{c})\\ v_{t}\\\phi_{k}+T(\omega_{t}-\omega_{h})\\\omega_{t}\\ l \\ w\end{bmatrix}$,
$f_{p}^{m}(x ,\: u^{c})=\begin{bmatrix}\cos(\omega_{h}T)&\sin(\omega_{h}T)\\ -\sin(\omega_{h}T)&\cos(\omega_{h}T)\end{bmatrix}\begin{bmatrix}x-Tv_{h}^{x}\\
y-\left(Tv_{h}^{y}+\dfrac{1}{2}T^{2}v_{h}\omega_{h}\right)\end{bmatrix}$
$+\dfrac{2v_{t}}{\omega_{t}}\\sin\left(\dfrac{T}{2}\omega_{t}\right)\begin{bmatrix}\cos\left(\phi
-\omega_{h}T+\dfrac{T}{2}\omega_{t}\right)\\\sin\left(\phi -\omega_{h}T+\dfrac{T}{2}\omega_{t}\right)\end{bmatrix},\:$
위의 식에서 공정잡음 $u_{k}$는 편의상 분산이 $Q_{k}$인 영평균 백색 정규잡음으로 가정한다.
$Q_{k}=diag\left(0^{2\times 2},\:T^{2}\sigma_{a}^{2},\: \begin{bmatrix}\dfrac{T^{3}}{3}\sigma_{\alpha}^{2}&\dfrac{T^{2}}{2}\sigma_{\alpha}^{2}\\\dfrac{T^{2}}{2}\sigma_{\alpha}^{2}&
T^{2}\sigma_{\alpha}^{2}\end{bmatrix},\:T^{2}\sigma_{l}^{2},\:T^{2}\sigma_{w}^{2}\right)$
2.2 표적 특징정보 측정치 모델
확장표적추적 필터의 실시간 구현을 위해 Hough 변환을 적용하여 추출된 특징정보에 관한 측정방정식을 유도한다.
2.2.1 LIDAR 포인트 클라우드 특징 추출
포인트 클라우드가 획득되면 클러스터링 등의 과정을 거쳐 관심영역을 설정하여 불필요한 측정치를 사전에 제거한다. 전처리 과정을 거친 LIDAR 확장측정치는
$N$ 개의 원소를 갖는 집합 $Z=\{x(n),\:y(n)\}_{n=1}^{N}$으로 쓸 수 있다. 표적 특징정보 추출에 필요한 기본가정은 다음과
같다.
수평면에 투영된 표적차량의 개략적 형상은 직사각형 형태로 근사된다.
확장측정치 $Z$는 직사각형의 특정 꼭짓점 $C^{i}=(x^{i},\:y^{i})$와 인접한 두개의 모서리 $E_{j}^{i}(j=1,\:2)$부근에서
획득된다(그림 2(a)). 따라서, $Z$ 는 $E_{j}^{i}$로부터 유래된 확장측정치 부분집합 $Z_{j}^{i}$의 합집합 $Z=\bigcup_{j}Z_{j}^{i}$
이다.
이후로는 수식전개 및 알고리듬 구현이 용이하도록 정보 $\chi$가 표적차량의 전장과 관련된 경우에는 $\chi_{1}$, 전폭과 관련된 경우에는
$\chi_{2}$로 구분하여 표기한다.
그림 2 표적특징 추출 개념
Fig. 2 Basic concept of target feature extraction
표적특징 추출 개념은 다음과 같다. 통상 가정 에 따라 그림 3과 같이 LIDAR 빔이 조사되는 표적차량 모서리 $E_{1}^{i}$및 $E_{2}^{i}$ 부근에서 확장측정치 부분집합 $Z_{1}^{i}$ 와
$Z_{2}^{i}$가 획득된다. 이때, 표적 동체에 가려진 반대편 모서리들에 대해서는 포인트 클라우드가 획득되지 않는다. 실제로 어떤 모서리로부터
포인트 클라우드가 획득되는지 여부는 LIDAR의 관측각($L=\lambda -\phi$)에 의해 결정된다.
그림 3 Hough 공간 표적특징정보 측정치
Fig. 3 Target feature measurment in Hough space
잘 알려진 바와 같이, Hough 변환은 확장측정치 집합 $Z$ 를 Hough 공간상의 한 점 $H_{j}^{i}$ 에 대응시키며, 본 논문에서는
이를 표적특징 측정치로 명명한다(11).
여기서 Hough 공간으로 대응된 점 $(\rho_{j}^{i},\: \theta_{j}^{i})$는 그림 2(a)에 도시한 바와 같이 V-frame 원점으로부터 LIDAR 빔이 조사되는 표적차량 모서리 $E_{j}^{i}$를 연장한 직선에 내린 수선의 발 $H_{j}^{i}$까지의
거리 및 각도로 이해할 수 있다.
Hough 공간 표적특징 측정치의 활용은 표적추적 필터 설계 및 구현 측면에서 다양한 장점을 지니고 있다.
∙ $N$개의 원소로 구성된 포인트 클라우드 $Z$는 단 2개의 원소를 갖는 표적특징 측정치 집합 $H^{i}=\left\{H_{1}^{i},\:
H_{2}^{i}\right\}$로 매핑된다. 따라서, 데이터 차원 축소와 LIDAR 확장측정치-표적 상태변수 간 자료연관문제의 복잡도를 크게 완화되는
효과를 누릴 수 있다.
∙ 만일 표적속도 벡터가 표적동체의 $X$축과 거의 일치한다면, 표적 헤딩각 $\phi$는 전폭 유래 특징점 $H_{2}^{i}$의 각도 $\theta_{2}^{i}$와
같아진다(그림 2(a) 참조). 즉, Hough 공간 표적특징 측정치를 활용하는 경우, 표적 헤딩각 정보를 직접 사용할 수 있어 표적추적 필터의 수렴특성 개선이 가능해진다.
∙ Hough 변환은 획득된 포인트 클라우드를 Hough 공간($\rho -\theta$ 평면)으로 매핑하면서 기울기가 $\tan\theta_{j}$인
직선 상에 위치하는 포인트 클라우드 개수 $N_{j}$를 함께 반환한다. $N_{j}$는 표적 전장 혹은 전폭의 길이와 밀집한 관계를 지니므로 표적
형상추정에 큰 도움이 된다.
Remark 1. 일반적인 상황에서는 직사각형 형상을 갖는 표적차량에 대해 2개의 표적특징 측정치가 획득되며 이들 특징 측정치의 각도는 대체로 $\left
|\theta_{1}^{i}-\theta_{2}^{i}\right |=\pi /2$을 만족한다. 하지만 그림 2(b)에 도시된 바와 같이 LIDAR가 표적차량의 한쪽면만 바라보는 특이상황에서는 이론적으로 1개의 표적특징 측정치만 산출된다. 이 경우, 추정 알고리듬의
안정적 운용을 위해 모서리 $E_{1}^{i}$의 양 끝점에서 $E_{1}^{i}$에 직각인 직선을 정의한 후 두 개의 가상 특징 측정치 $H_{2}^{i}$와
$H_{3}^{i}$가 계산되도록 특징 추출 알고리듬을 구성하는 것이 바람직하다.
그림 4 꼭짓점 $C^{i}$과 표적특징 $H_{j}^{i}$ 간의 상관관계
Fig. 4 Relationship between $C^{i}$ and $H_{j}^{i}$
2.2.2 측정방정식 유도
이제 Hough 공간 표적특징 측정치 $H_{j}^{i}$에 대한 측정방정식을 유도해보자. 그림 4(a)와 같이 표적차량의 꼭짓점 $C^{i}$의 번호는 표적 차량 전방 우측으로부터 시계 방향 순으로 정의된다. $C^{i}$의 위치 $(x^{i},\:
y^{i})$는 표적차량의 중심위치 $\vec{R}_{ht}^{V}$, 헤딩각 $\phi$, 전장 $l$ 및 전폭 $w$의 함수로 쓸 수 있다.
여기서 $D^{i}(\phi)$는 signum 함수 $sgn(\bullet)$를 이용하여 정의된다.
$D^{i}(\phi)=\dfrac{1}{2}\begin{bmatrix}\cos\phi & -\sin\phi \\\sin\phi &\cos\phi\end{bmatrix}\begin{bmatrix}sgn(\cos\varphi^{i})&
0 \\ 0 & sgn(\sin\varphi^{i})\end{bmatrix},\:\varphi^{i}≜\dfrac{2i-1}{4}\pi$
특징 측정치 $(\rho_{j}^{i},\:\theta_{j}^{i})$는 원점으로부터 직선 $E_{j}^{i}$에 내린 수선의 발 $H_{j}^{i}$과
밀접한 상관관계를 갖는다 (그림 4(b)). 원점과 $H_{j}^{i}$를 잇는 벡터 $\vec{h}_{j}^{i}=\left[x_{j}^{i}y_{j}^{i}\right]^{T}$는 모서리
$E_{j}^{i}$에 평행한 단위 벡터 $\vec{u}_{j}$에 벡터 $\vec{c}^{i}=\left[x^{i}y^{i}\right]^{T}$를
투영함으로써 계산가능하다.
여기서 단위 벡터 $\vec{u}_{j}$의 정의는 다음과 같다.
$\vec{u}_{j}=\begin{bmatrix}\sin\beta_{j}& -\cos\beta_{j}\end{bmatrix}^{T},\:\beta_{j}=\phi
+(j-1)\pi / 2$
앞서 구한 표적특징 측정치 위치벡터 $\vec{h}_{j}^{i}$를 이용하여 Hough 공간상의 표적특징 측정치 $(\rho_{j}^{i},\:\theta_{j}^{i})$를
식 (12)에 정의된 상태변수 $x$에 관한 비선형 함수로 기술할 수 있다. 이때, 함수 $g_{\rho ,\:j}(\bullet)$ 및 $g_{\theta
,\:j}(\bullet)$는 특징 측정치 획득가설을 결정하는 꼭짓점 $C^{i}$에 따라 다르게 모델링된다.
여기서 $s_{i}≜sgn(\sin\varphi^{i}),\:$ $c_{i}≜sgn(\cos\varphi^{i})$라 하면,
$\begin{aligned} g_{\rho, j}(\bullet)=& \cos \left(\beta_{j}-\phi\right) \cdot \sqrt{\left(x
\sin \beta_{j}-y \cos \beta_{j}-0.5 w s_{i}\right)^{2}} \\ &+\sin \left(\beta_{j}-\phi\right)
\cdot \sqrt{\left(x \sin \beta_{j}-y \cos \beta_{j}+0.5 l c_{i}\right)^{2}} \\ g_{\theta,
j}(\bullet)=&\left\{\begin{array}{ll}(\phi-\pi / 2) \cos \left(\beta_{j}-\phi\right)+\phi
\sin \left(\beta_{j}-\phi\right), & \phi \geqq 0 \\ (\phi+\pi / 2) \cos \left(\beta_{j}-\phi\right)+\phi
\sin \left(\beta_{j}-\phi\right), & \phi<0\end{array}\right.\end{aligned}$
식 (16)로부터 다음 비선형 측정방정식이 유도된다.
여기서
$y_{k}=\begin{bmatrix}\rho_{1}\\\theta_{1}\\\rho_{2}\\\theta_{2}\\l \\ w\end{bmatrix},\:
h(x_{k})\begin{bmatrix}{g}_{\rho ,\:1}(x,\:y,\:\phi ,\:l,\:w,\:C^{i})\\{g}_{\theta
,\:1}(x,\:y,\:\phi ,\:l,\:w,\:C^{i})\\{g}_{\rho ,\:2}(x,\:y,\:\phi ,\:l,\:w,\:C^{i})\\{g}_{\theta
,\:2}(x,\:y,\:\phi ,\:l,\:w,\:C^{i})\\ l \\ w\end{bmatrix},\:$ $R_{k}=diag([\sigma_{\rho}^{2},\:\sigma_{\theta}^{2},\:\sigma_{\rho}^{2},\:\sigma_{\theta}^{2},\:\sigma_{s}^{2},\:\sigma_{s}^{2}])$
2.3 특징측정치 획득가설 및 확률적 추론
2.3.1 가림현상으로 인한 특징정보의 모호성 문제
LIDAR 확장표적추적의 기술적 이슈 중 하나는 표적 동체에 의한 측정치 가림현상이다. 예를 들어 그림 5와 같이 동일한 특징측정치라 할지라도 상대기하에 따라 서로 다른 꼭짓점 $C^{i}$과 관련될 수 있다. 이러한 특징측정치의 모호성은 측정모델의 오차로
작용하여 추정성능을 저하시키는 원인이 된다. 이를 해결하기 위해 본 논문에서는 표적 관측각에 따른 확장측정치 획득가설을 설정하고 확률적 추론을 통해
획득된 확장측정치가 어떤 꼭짓점으로부터 유래되었는지 판별한다. 가정 에 의해 표적 형상이 직사각형으로 근사된다면 확장측정치 획득 양상이 관측각에 따라
달라진다. 이를 확인하기 위해, 표적차량의 시선각 $\lambda$가 $0^{\circ}$인 상황에서 표적차량의 헤딩각 $\phi$를 $-180^{\circ}$부터
$180^{\circ}$까지 변화시켜 포인트 클라우드를 획득해보자. 그림 6과 같이 관측각에 따라 획득되는 확장 측정치는 관측가능한 표적차량의 꼭짓점에 의해 좌우된다. 표적차량의 꼭짓점 $C^{i}$가 LIDAR 가시범위
내에 존재하는 관측각 $L$의 조건은 다음과 같다.
그림 5 상대기하에 따른 특징정보의 모호성
Fig. 5 Ambiguity of feature data
그림 6 관측각에 따른 포인트 클라우드 획득 양상
Fig. 6 Point cloud according to look angle
$C^{i}: | L-m_{i}|<\dfrac{\pi}{4},\:m_{i}=(2(i-1)-3)\dfrac{\pi}{4},\:i=1\sim 4$
표적차량 형상이 직사각형이므로, 각 꼭짓점은 관측 각도 영역의 중심 $m_{i}$부근에서 관찰될 가능성이 가장 커진다.
이상의 관찰 결과로부터 측정치 획득가설은 관측각에 따른 꼭짓점 관찰여부로 구분하고, 각 가설의 신뢰도를 그림 7과 같이 확률밀도함수 형태로 사전 지식화할 수 있다. 측정치 획득가설에 대한 확률모델은 추후 특징 측정치 우도와 함께 가설 별로 설계된 부필터 선택에
유용하게 활용된다.
그림 7 특징 측정치 획득 가설 확률밀도함수
Fig. 7 PDF based on meas. acquisition hypothesis
2.3.2 확장표적추적을 위한 다중모델기반 무향칼만필터
표적 관측각에 따른 측정치 획득가설을 고려하기 위해 상호다중모델 필터 구조를 적용한다. 다중모델 필터의 $i$번째 부필터(sub-filter)는 각
꼭짓점 $C^{i}$에 해당하는 특징 측정치가 획득되었다는 전제 하에 설계된다. 그림 8에 도시된 LIDAR 표적추적 필터링 과정의 상세 내용은 다음과 같다.
1) 상호작용 및 혼합과정
모델천이확률행렬 $\pi$와 이전 시점 모델 확률 $\mu_{k-1}$을 이용하여 혼합 확률(mixing probability)을 구한다.
여기서 $N_{r}$은 모델 개수를 의미한다.
이전 시점 부필터 별 사후추정치 $\hat x_{k-1}^{j}$와 혼합 확률 $\mu_{k-1}^{i|j}$을 이용하여 가우시안 혼합 개념에 따라
혼합 예측치와 오차공분산을 구하면 다음과 같다.
그림 8 LIDAR 표적추적필터의 흐름도
Fig. 8 Diagram of LIDAR target tracking filter
2) 필터링
비선형 시스템 모델 식 (14)에 UKF(Unscented Kalman Filter)를 적용하여 혼합 예측치와 오차공분산 식 (20)에 대해 시스템 전파를 수행한다.
여기서 $\zeta =\sqrt{(L_{ukf}+\lambda_{ukf})}$이다.
측정치 갱신을 수행하기에 앞서, Hough 공간 표적특징 측정치가 표적차량의 전장 모서리 혹은 전폭 모서리로부터 유래되었는지 판별해야하므로 획득된
특징 측정치들에 대해 게이팅을 수행한다. 사용되는 게이트의 정의는 다음과 같다.
여기서
$r_{1,\:k}^{i,\:j}=\begin{bmatrix}\widetilde\rho_{k}^{j}\\\widetilde\theta_{k}^{j}\end{bmatrix}-\begin{bmatrix}\overline{\rho}_{1,\:k}^{i}\\\overline{\theta}_{1,\:k}^{i}\end{bmatrix},\:
r_{2,\:k}^{i,\:j}=\begin{bmatrix}\widetilde\rho_{k}^{j}\\\widetilde\theta_{k}^{j}\end{bmatrix}-\begin{bmatrix}\overline{\rho}_{2,\:k}^{i}\\\overline{\theta}_{2,\:k}^{i}\end{bmatrix},\:$
$i=1,\:\cdots ,\:N_{r},\: j=1,\:\cdots ,\:M$
위의 식에서 $\Omega$는 유효측정치 판별 영역, $(\widetilde\rho_{k}^{j},\:\widetilde\theta_{k}^{j})$는
Hough 공간에서 추출된 $j$ 번째 특징 측정치, $M$은 특징 측정치의 총 개수, $(\overline{\rho}_{1,\:k}^{i},\:\overline{\theta}_{1,\:k}^{i})$,
$(\overline{\rho}_{2,\:k}^{i},\:\overline{\theta}_{2,\:k}^{i})$는 $i$번째 부필터에 의해 산출된
표적차량의 전장/전폭 특징점 사전추정치, $S_{k}$는 잔차 $r_{k}$의 공분산, $D_{\gamma}$는 게이팅 확률 $P_{G}$에 따라
정해지는 값이다.
게이트 내에 유효측정치가 존재하는 경우, 전역 최근린 자료연관기법을 적용하여 표적차량의 전장/전폭 모서리로부터 유래된 측정치를 판별한다. 식 (21)에 의해 유효측정치에 대한 측정치 우도를 산출할 수 있다.
두 게이트 내에 유효측정치가 모두 존재하는 경우 측정치 갱신을 수행한다. 비선형 측정방정식 (17)에 대한 UKF 측정치 갱신식은 다음과 같다.
다중모델 필터 중 어떤 부필터가 현 시점에서의 표적 관측각에 적합한 것인지는 Hough 공간 측정치 우도 $\Lambda_{z,\:k}^{i}$를
통해 추론할 수 있다. 3단계에서 판별된 측정치로 정의되는 잔차 $r_{k}^{i}= y_{k}- h(x_{k})$를 이용하여 측정치 우도를 산출한다.
이때, 그림 7의 측정치 획득가설 확률밀도함수 $\Lambda_{L}^{i}$를 이용하여 모델 별로 획득가설에 대한 확률적 평가를 수행할 수 있다. 획득가설 확률을
모델확률 갱신과정에 추가 반영하면, 표적차량의 동체 가림현상으로 관찰되지 않는 표적차량의 꼭짓점 $C^{i}$에 해당되는 모델확률은 낮아지고 현재
관측되는 표적차량의 꼭짓점 $C^{i}$에 해당되는 모델확률은 높아지게 된다. 관측각에 따른 확장측정치 획득가설 확률밀도함수는 모델에 따라 다음과
같이 근사할 수 있다.
여기서 $\overline{L}_{i}=\tan^{-1}(\overline{y}/\overline{x})-\overline{\phi}$은 $i$ 번째
부필터에 사전추정치로 산출된 관측각 예측치, $m_{i}$는 꼭짓점 $C^{i}$가 관찰되는 관측각 범위의 평균, $P_{L}$은 확률밀도함수에 대한
분산을 의미한다.
3) 모델확률 갱신
각 부필터에서 산출된 측정치 우도 식 (24) 및 확장측정치 획득가설 확률 식 (25)을 이용하여 상호작용 다중모델 필터의 모델확률을 다음과 같이 갱신한다.
여기서 $c_{L}$ 와 $c_{z}$는 정규화 상수를 의미한다.
측정치 우도뿐만 아니라 관측각에 따른 측정치 획득확률을 모델확률 갱신에 반영함으로써, 다중모델 필터를 안정적으로 운용할 수 있다.
4) 상태추정치 결합
갱신된 모델 확률을 이용하여 가우시안 혼합 개념에 따라 상태추정치 결합을 수행한다. 이때, 현 시점에 관측되는 표적차량의 꼭짓점과 관련된 모델에 높은
가중치가 부여된다.