무향칼만필터 (2)
in Ukalman on UKalman
무향칼만필터의 시퀀스는 선형칼만과 같이 두개의 시퀀스를 가지는 필터이다. 하지만, 시그마 포인트 친구들까지 부르는 바람에 조금 계산이 복잡할 뿐이다.
구체적으로 적으려면 다음과같다.
선형칼만필터에서 추가된 부분만 하이라이트해서 적어본다.
- 시그마 포인트 계산
- 대충 이전의 데이터를 가지고 다음에 들어올 입력값을 예상해놓는다 (예측단계)
1.1 시그마포인트로부터 공분산계산- 입력이 들어왔다. (관측)
- 입력데이터와 예측데이터를 비교해서 최적의 출력값을 추정 (업데이트 단계)
공간벡터 x_k, 즉 거리와 속도의 벡터는, 다음과같은 식을 따른다고 가정하자.
x_k \sim \mathcal{N}(\hat{x}_k(+),P_k)저번에 정의한대로, P_k는 공분산행렬이다.
공분산 행렬의 제곱근 (Square root of covariance matrix) 과
먼저, 우리들은 표준편차 \sigma를 일반적으로 분산의 제곱근으로 정의한다.
어떤 스칼라 숫자의 제급근을 구하기는 쉽다.
하지만, 어떤 공분산 행렬에 대해서, 즉, 벡터값에 대해서 표준편차를 구한다는것은, 먼저 회전을 취하고, 그 회전된 축에 대해서 표준편차를 구할 필요가 있다.
주축에 대해서 이것을 변환하려고 한다면, 특이치 분해를 통해서 하는방법과, 촐레스키 분해 (Cholesky factor) 를 하는 방법이 있다.
무향 칼만필터에서는, 촐레스키 분해를 이용한다.
- 촐레스키 분해
P_k = L_kL_k^\text{T}
저번의 무향변환에서 우리는 시그마포인트를 정한다는 것을 배웠다.
여기서, 촐레스키 분해를 통해서, 시그마값에 이 촐레스키 분해된 항을 대입한다.
x_k^{(0)} = \mu_x = \mathbb{E}(x)x_k^{(1)} = \mu_x +c \sigma = \mu_x +c L_kx_k^{(2)} = \mu_x -c \sigma = \mu_x -c L_k
c는 무엇인가요?
c 는 어떤 양의 정수이며, 가우시안에서 말하는 시그마 포인트들의 실제 확률밀도분포의 우도(Likelihood) 를 나타낸다.
즉, 위의 식을 그대로 사용할 경우에는, 확률밀도의 개념이 들어가지 않으므로, 시그마 포인트들의 가우시안 가중치가 들어가 있지않다.
따라서, 그들의 가중치를 c 로써 부여하고 있다.
무향 변환과 다변량 가우스분포
먼저, 분포가 진화모델과 관측무델 두개인것에 주목하자.
p(x_k) = \mathcal{N}(\mu_x,P_k)p(y_k) = \mathcal{N}(\mu_y,\Phi P_k\Phi^\text{T}+R_k)단,
p(v_k) = \mathcal{N}(0,R_k)
여기서, 가우스분포가 두개 있을때, 예컨데, 진화모델과 (Evolution model), 관측모델 (Observation model) 혹은 프로세스 가 있을때, 두개의 조인트 모멘트는 다음과 같이 쓸 수있다.
p(x_k,y_k) = \mathcal{N}()E\begin{bmatrix}x_k\\h(x_k)+v_k\end{bmatrix}