깜지로 쓰려는 블로그

내가 왜 파이썬 로보틱스를 다루려 했더라

전에 못했던 스캔 매칭이나 SLAM 구현을

이젠 진짜 해볼수 있을 만큼 되었다 생각이 들었지만

정작 하면서 영 의욕이 생기질 않더라

지금은 이전에 로보틱스를 공부했을 때 만큼

절박함도 잃었고, 영 힘이 나지 않는다.

한동안 다른거 하다

나중에 다시 해보고 싶을 때가 오겠지

-ㅁ너ㅐㅔ채ㅔㄷㅂ퍼ㅐㅔㅐㅔㅓㄷ패ㅔㅍㄷ재ㅔㅏㅜㅇ류라ㅡㄷㅍ자ㅡ

이번에 정리하고자 하는 내용은 확장 칼만필터

확장 칼만 필터 EKF

확장 칼만 필터는 기존의 칼만필터의 상태 전이와 측정이 선형 함수 형태로 이루어져 있었는데

대부분 비선형적인 동작을 하는 현실 문제들에는 부적합함

칼만 필터에서 선형함수를 사용하여 다음 상태, 관측치를 표현한 것 대신

비선형 함수 g와 h를 사용해서 계산하는데

잠깐 가우시안 분포가 선형 변환을 하면 어떻게 되나 보자

(칼만 필터에서는 다음 상태를 구할때 하던 그거)

선형 변환때는 문제 없이 가우시안 분포를 따르는 다음 상태 p(y)를 구할수 있다.

하지만 문제는 비선형 변환을 할때이다.

가우시안 분포 p(x)를 비선형 함수 g(x)에 넣었더니

지금까지 계속 사용했던 가우시안 분포의 형태가 나오지 않아서 사용할수 없다.

* 파란 가우시안 분포는 p(y)의 평균과 분산을 계산해 만든 가우시안. 

대신 비선형 함수의 해당 지점을 1차 테일러 전개, 선형 근사를 하여 p(x)를 대입시키면

(비선형 함수의 1차 근사식으로 가우시안 분포를 선형 변환)

파란 가우시안 분포에 가까운 (근사화 오차를 가진, 빨간) 가우시안 분포가 나온다.

위에선 비선형 함수를 선형 근사하여 어떻게 가우시안 분포를 적용시켰는지 봤는데

비선형 함수를 이제 1차 테일러 전개, 선형화하면 아래와 같이 표현 

여기서 G_t는 비선형 함수를 상태와 제어에 대해 편미분하여 구한 자코비안 행렬

비선형 함수에대한 선형 근사식으로

상태 전이 확률을 정리하면 다음과 같음

관측 비선형 함수도 선형 근사를 통해 표현할수 있으며, 관측 확률도 가우시안으로 다음과 같음

확장 칼만 필터 알고리즘

기존의 칼만 필터랑 거의 비슷한데 

상태 전이행렬 A, 제어 행렬 B, 관측 행렬 C대신

비선형 함수 f, u와 자코비안 G가 들어간 형태

구현 코드랑 같이 봐야 이게 뭔가 이해되겠지만

아직 그전에 짚고 넘어갈게 좀남아서 바로 구현하질 못하겟네 ㅜㅜ

재ㅓㄹㅂ제ㅐ러뱆ㅇ재ㅏㅇㅈ배ㅔ

가우시안 필터

지금까지 앞에서 정규분포로 자꾸 센서 데이터나 상태를 표현했는데

지난 글에서 작성한 믿음도도 가우시안 분포로 표현해서 다루었었다.

베이즈 필터를 가우시안 분포를 이용해서 정리한게 가우시안 필터라고 할수 있음

칼만 필터는 노이즈를 가우시안 노이즈로 구현한 가우시안 필터를 구현한 것중 하나

매개변수 표현(파라미터화)

가우시안 분포 정규 분포는 보통 평균과 분산 두 수치로 표현하는데 이를 모멘트 파라미터라고 하고,

정보 행렬과 정보 벡터로 표현하는 방법을 캐노니컬 파라미터화라고 함.

정보 필터 쓸때가 아니면 보통 모멘트 표현법을 주로 사용함

다변량 가우시안분포는 다음 형태로 나타내는데

평균 mu와 공분산 sigma로 모멘트를 이용하여 표현함

칼만 필터 개요

이전 파이썬 필터 글에서 봤다시피 칼만 필터는 상태를 모멘트 매개변수화하여 표현했었음 

칼만 필터는 아래의 조건들을 따르면 믿음도, 사후확률을 가우시안으로 표현가능함

예측 과정에 사용되는 상태 전이 확률과 관측 확률이 가우시안 노이즈를 가지고(시스템/관측 행렬 + 노이즈 했던거),

* 상태 전이 함수는 시스템 행렬 A와 제어 행렬 B, 가우시안 노이즈로 구성된 선형 함수

* 관측 함수는 관측 행렬 C와 가우시안 노이즈로 구성되었음

초기 믿음도, 초기 추정치(칼만 필터할때 맨처음에 초기 평균, 공분산 준거)가 정규 분포를 따름

칼만 필터와 베이즈 필터 비교

칼만 필터는 가우시안 분포로 베이즈 필터를 구현한 것들중 하나라고 했는데

한번 비교해서 보자

베이즈 필터에서는 x t-1에서 상태 전이 확률을 통해 t에서 예측 믿음도를 구했었다.

칼만 필터의 경우 (모멘트 파라미터화로 표현해서 평균과 공분산 사용) 예측 평균을 이전 상태와 제어 신호의 선형 함수로

예측 공분산을 이전 공분산에 앞뒤로 시스템 행렬을 곱해서 구해냄

칼만 필터에는 베이즈 필터에는 없는 칼만이득 계산하는 과정이 있는데

칼만 이득은 (다음 갱신과정에서 계산할) 측정 잔차(실제 측정값과 예측 측정값)를 얼마나 반영시킬지 조정하는 역활을 함

이제 관측 갱신 단계로 넘어가서보면

베이즈 필터에서는 예측 믿음도에다가 관측 확률을 곱하여 믿음도를 계산하는데

칼만 필터에서 평균은 예측 평균 + 칼만이득(실 관측값 - 예측관측값), 공분산은 칼만 이득과 예측 공분산으로 다음과 같이 계산한다.

이거 처음 봤을때 모멘트 파라미터화나 믿음도 자체를 이해못해서

베이즈 필터에서는 확률을 곱해서 계산하다가

칼만필터에서는 평균, 공분산으로 계산만하니 갑자기 왜 이래된건가 전혀 이해를 못했었는데

베이즈 필터는 로봇 상태 믿음도를 계산하기 위한 기본 컨샙이고

칼만 필터는 가우시안 분포와 선형 함수로 베이즈 필터를 실제 동작할수 있게 표현한 것 정도로 이해함.

칼만 필터를 이용한 1차원 공간에서의 위치 추정

아래의 그림은 칼만 필터로 믿음도를 예측하고 갱신하는 과정을 개념적으로 보여줌

먼저 그림 a에서 가우시안 분포로 초기 믿음도를 표현해냄 

초기 추정치라 뒤에 계산결과에 비해 약간 분산이 퍼진 형태로 되어있음

다음 그림에는 제어하지는 않았지만 먼저 관측을 해서 관측 확률을 구하였음

관측확률과 초기 확률 평균은 대충 비슷하지만, 관측 확률의 경우 분산이 작음

* 칼만 필터에서는 초기 추정치가 매우 중요함. 초기 추정치와 실 관측과 다르면 계산 잘안됨.

베이즈 필터에서 예측 확률(초기엔 움직이지 않았지만)과 관측 확률을 곱했던것 처럼

칼만 필터에서도 예측 믿음도에 칼만이득 x 관측잔차, 공분산 계산해서 반영시킨 결과

현재 상태, 믿음도의 불확실성이 줄어들어 확률이 더 올라감

로봇이 우측으로 이동하면서 시스템 노이즈가 누적되

불확실성이 증가하였음

제어 후 상태, 예측 믿음도의 분산이 커진걸 볼수있다.

이동한 위치(예측 믿음도)는 대충 평균이 21쯤인데

실 관측치로 봤을땐 로봇의 위치가 25쯤 있는걸로 보고 있음

이 관측치를 상태에 반영시켜 다음과 같은 사후확률, 믿음도를 구해내었음

ㅇㅈ배ㅓ햊ㅂ푸재ㅔㅓ브ㅓㅔㅐ배ㅔ재ㅡ재ㅔ

보통 놀면 계속 노는데 하려고 하니 참 손에 안잡힌다

잠깐 놀다 지난번에 하던거 다시 시작함

지난글 마지막에는 상태, 제어, 관측을 어떻게 확률로 표현하는지와

이산 시간 흐름에따라 이들의 변화 과정을 동적 베이지안 네트워크로 표현하면서 마쳤음

믿음도 belief

다음으로 볼게 belief 이걸 나는 믿음도라고 적는게 적당할것 같은 생각이 드는데

belief를 믿음도라 작성해서 하겠다.

베이지안 확률론에서는 빈도주의 확률론처럼 확실히 나온다기 보다는

확률을 주관적인, 믿음의 정도로 표현한다고 하니 믿음도라고 쓰려고함.

지난 글에서 초음파 데이터가 저런 식으로 있으면

N(10, 1)인 확률 밀도 함수로 표현하자라고 하자, 치자

라고 정의한게 믿음 같은거라고 해야될까

정규분포 그러니까 가우시안으로 근사 시켜 표현했다고 할수 있을듯하다.

그러면 여기서 사용하는 믿음도가 무엇을 믿는거냐면

믿음도는 로봇의 상태가 어떻게 되어있을까?에 대한 확률을 의미한다고 하면되겠다.

뒤에 나올 내용이지만.

2차원 공간에서 이동로봇의 상태가 좌표 x, y와 해딩 방향 theta가 있다고 치자

이 상태들을 위 초음파 데이터를 정규분포로 표현한것 처럼 평균 mu와 공분산 cov로 표현한게 믿음도라고 할수 있을것같다.

일단 믿음도는 사후확률로 표현하면 다음과 같으며

베이즈 필터로 믿음도를 계산하는 과정은 예측 단계와 관측 단계로 나누어져 있다.

예측 단계는 상태에 제어 신호를 줬을때 바뀐 상태에 대한 예측을 나타낸다.

처음 위 식을 봤을때 너무 막연한 표현으로 느껴졌었는데 로봇이 한 지점에 있는데 이를 정규분포로 표현하고 있다가. 10m 전진후에는 어디쯤에 있을까?를 정규분포로 표현하면 평균치는 10m 전진했을지는 몰라도 분산은 이동하면서 생긴 노이즈 때문에 더 퍼져있을것이다.

이 퍼진 분산을 고쳐주는게 베이즈 필터의 관측 단계로 갱신된게 로봇 상태에 대한 사후확률, 믿음도가 되겠다.

예측 단계에서는 z 1~t-1, u 1~t-1 까지 진행했으면 x_t-1을 얻었을것이다.

x_t-1에서 u_t만큼 더 가면 시간 t에서의 예측 상태 bar bel(x_t)를 얻는것이고

이 예측상태를 관측 갱신으로 보정하여 믿음도 bel(x_t)를 얻어낸다.

베이즈 필터

아래의 사진은 베이즈 필터의 기본 그림을 표현한다.

이게 왜 필터라고 하냐. 이전에 노이즈가 낀 초음파 거리계의 정량 데이터를 확률 모델로 표현해서 센서의 평균치와 노이즈 퍼진 정도로 표현했는데, 정량적 수치가 아닌 확률로 표현해서 그런듯 하다. 아래 알고리즘은 센서 데이터가 아닌 로봇의 상태를 추정하는 내용이긴 하지만

베이즈인것은 베이즈 정리에따라 예측 믿음도와 관측 역확률을 가지고 구하고하자던 사후확률을 계산해냈기 때문인듯

 근데 아래 베이즈 필터 알고리즘은 로봇의 상태, 제어, 관측, 예측, 갱신 전체 개념을 확률로써 표현해서 저런 흐름이지 구현 내용을 보기전에 너무 억지로 계산하려 하는건 골치아프다. 뒤에 로봇 문여는 예제와 복도 예제가 있긴한데

로봇 문여는 예제는 계산이 너무 번거로우니 복도 예제를 대충 보고 짚어넘자.

로봇 복도 예제(마르코브 위치추정)

로봇 복도 예제가 뒤에 있는줄 알았는데 앞에 있었네 --

이 예제는 1차원 공간에서 로봇의 위치가 어디에 있는지를 찾아내는 과정을 설명해주는데

우선 가장 먼저 초기 위치를 로봇이 어디있는지 모르니 로봇이 위치 가능성들이 균일하게 퍼져있다고 하자

대충 위치가 이산적으로 10개가 있다고 치고,

균일분포로 초기화 한다는 것은 로봇이 존재할 확률을 균일하게 준다가 되겠다.

이는 초기 믿음도 bel x_t가 된다.

베이즈 필터에서는 예측, 관측을 반복한다 했는데 지금은 관측부터 해보자

아래는 관측 모델, p(z | x)를 대충 표로 나타내어봤다.

관측 모델은 문이 여러개라 가우시안 혼합 모델 형태를 띄는데,

내가 문의 위치를 알고있고, 지금 바로 문앞에 있어서 저런 식으로 나오나 이런건 개념적으로 넘어가자 

그러면 베이즈 필터 관측 단계대로 관측 모델과 초기 믿음도를 곱해주면

아래의 믿음도가 계산된다.

문 3개있는걸 알고, 내가 문앞에 있으니 문 세개 쯤 어딘가에 있구나 정도로 생각하면 될듯. 

다음은 제어후 예측 단계가 되겠다.

믿음도 봉우리들이 이동하면서 노이즈가 첨가되 퍼진 형태를 보인다.

그러다 로봇이 또 문을 찾아내었다.

로봇은 문의 위치를 알고 있으니 지금의 (예측) 믿음도와 관측 모델을 곱해 갱신시켜줘보자.

그러면 아래의 믿음도 같이 가장 봉우리가 큰 지점이 나오며 믿음도가 실제 로봇의 위치에 가까워졋다!

이 상태로 로봇이 계속 전진하면

여전히 이동 노이즈로 믿음도 분산이 점점 커지긴 하지만 실 로봇의 위치를 잘 쫓아간다.

일단 여기까지 확률 이론으로 로봇 상태 추정을 어떻게 하는가 기본적인 느낌 정리

음 파이썬 로보틱스에서 제공하는 예제들은 probabilisitc robotics 내용들을 파이썬으로 구현한 것들인데

이론 내용을 짚고 넘어갈까,

아니면 바로 구현들어가면서 내용정리할까 고민하다가 

사용되는 기초 이론들을 먼저 정리해야할것 같아서

내가 이해한 것이 정확하지는 않더라도 대강 느낌이라도 정리를 먼저하고 시작하려고함

확률로 다루는 로보틱스

- 보통 센서 데이터들을 특정한 정확도의 값(정량 데이터)을 가짐

 초음파 거리계 : 거리

 가속도, 자이로계 : 각속도, 선가속도

 gps : 경도, 위도 위치 좌표 등

- 하지만 이런 센서들 데이터들은 노이즈로 잘못되어있을 수 있으며, 로봇의 상태를 특정한 정확한 값으로 표현은 곤란

 => 센서 데이터를 그대로 쓰기보단 로봇의 상태를 확률 모델로 표현

상태를 확률 변수로 모델링 한다는 것은?

- 초음파 데이터가 대충 10cm 거리에 있는 물체를 감지해서 아래와 같은 값을 받앗다고 치자

- 이를 확률 변수 p(x)는 평균 10, 분산 1인 정규분포 X ~ N(10, 1) 를 따른다른다 라는 식으로 데이터 분포를 확률 밀도 함수로 표현한다.(대충 값이 맞든 틀리든)

 - 로봇과 관련된 상태 값들이 노이즈가 포함되어도 확률 분포로 나타내어 베이즈 필터에 활용된다.

확률 개념 정리

- 확률 변수 X가 사건 x일 확률을 다음과 같이 표현

- 확률 변수 X에서 발생할 모든 사건에 대한 확률들의 합은 1

- 앞으로 사용할 확률 분포들은 가우시안 분포를 따른다고 가정함. x가 스칼라인 경우 1차 정규분포는 다음과 같음

- x가 스칼라가 아닌 벡터인 경우 정규 분포는 다음과 같이 표현

- 두 확률 변수 X, Y가 주어지고, X=x, Y=y인 사건이 동시에 일어날 확률/결합확률분포는 다음과 같음

- 만약 두 확률 변수 X, Y가 독립인 경우 두 사건이 동시에 일어날때 결합확률분포는 각 두 확률의 곱

- Y=y 임을 알고있을때 X=x가 발생할 확률(조건부 확률), 맨 우측은 X,Y가 독립인 경우 성립

- 조건부 확률 정의에 따라 다음의 전체 확률 정리가 성립함 

 -> p(x|y)는 y가 발생했을때 p(x, y)가 발생할 확률

 -> p(x|y)에서 전체 y 사건들이 발생한 경우(이산합, 적분)를 곱하여 합하면 p(x)에 대한 확률이 나온다

- 조건부 확률은 역확률과 관련 있음

- 확률 변수 X의 기댓값은 다음과 같이 계산

- 기댓값을 선형 함수로 표현시 다음과 같이 정리됨

- 확률 변수 X의 공분산은 다음과 같음

- 엔트로피는 x가 갖는 기대 정보량으로 -log2 p(x)는 x 인코딩에 가장 적당한 비트 수를 의미함

지금 왜 조건부 확률과 역확률을 다루는가

- 역확률 p(y | x)를 이용해 p(x | y)를 계산하기 위함

 * y는 센서 데이터를 의미, p(y | x)는 특정 상태 x에서 y를 얻었을때의 확률 분포, p(x | y)는 센서데이터로 얻은 상태 분포

 * 지금 로봇의 상태값들을 표현하기 위해 확률 분포를 사용하고 있는데, 실제 코드로 구현한걸 봐야 좀더 와닿음. 지금은 어쩔수가 없다.

- p(x | y)는 y라는 데이터가 주어졌을때 로봇의 상태 x에 대한 확률 분포를 나타낸다.

 센서 관측 데이터 y를 얻은 후 상태 x를 구한것이므로 이를 사후 확률 분포라 한다.

 뒤에 구현할 예시랑 비슷하게 설명하자면 랜드마크로부터 거리 y가 주어졌을때 로봇의 위치 x가 어디인가를 의미한다

좀더 1차원 공간에서의 (대충) 예시를든다면 남산타워가 4인 지점에 있다는걸 알고 있다.

그리고 나는 거리 측정 센서로 남산타워(랜드마크)까지 거리를 재어보니 2.5, 2.3, 2.7, 2.6, 2.4 같은 식으로 값이 나온다.

나는 아직 내 위치가 어딘지 모른다.

하지만 지금 위치 기준으로 측정 데이터 p(y | x) = X ~ N(2.5, 0.1) 쯤 나온다고 칠수 있을것이고

4 - 2.5를 하면 1.5가 되니 나의 위치는 1.5쯤에 있다를 알수 있는데, 이게 p(x|y)가 된다.

=> p(x | y)를 바로 구할수는 없지만 확률 p(y | x)로부터 구할수 있으므로 p(y | x)를 생성모델이라 부른다.

- 내가 방금 작성한 설명이 p(x|y)에 대한 식을 계산하는것과 정확하게 일치한것 같지는 않은대 역확률 p(y | x)로 사후확률 p(x | y)를 구한다는것은 이런 느낌인데, 1차원 복도 예제를 봐야 왜 이런식으로 계산되는지 조금 더 와닿을것 같다. 

상태, 제어, 관측

- x는 상태, u는 제어 데이터, z는 관측 데이터를 의미

- 시간 t1~t2까지 모든 제어, 관측 데이터를 다음과 같이 표현

- 시간 t에서의 상태 x_t는 시간 t-1에서의 상태 x_t-1에 제어 데이터u_t로부터 얻을 수 있음

 x_t-1는 0~t-1까지의 상태와 제어를 합한 결과로 얻을수 있으며, x_t는 우항과 같이 정리하여 구할수 있다.

 이게 무슨소리냐 대충이라도 계산해보자

 1차원 공간에서 t-1 시점에 로봇은 X ~ N(10, 1) 그러니까 10에 있다고 치자

 +5 만큼 이동하는 제어 신호를 줬다고 하자 제어 신호에 대한 상태 전이 확률 분포를 N(5, 2)라 한다면

t 시점에 로봇은 15에 있을 것이다.

하지만 상태 전이 분포의 분산이 2이므로 이동 노이즈가 가미되어 t 시점의 로봇은 X ~ N(15, 3)으로 표현된다.

만약 제어 신호의 노이즈가 작아서 상태 전이 확률 분포가 N(5,0.001)이라 치면

이동 후 t 시점에서의 로봇은 X ~ N(15, 1.001)에 위치할 것이다.

로봇이 정확하게 이동했으므로 로봇의 상태에 대한 확률 분포의 분산이 크지 않다.

- 시간 t에서 상태 x_t가 주어질때 관측치, 역확률 모델 p(y|x)은 다음과 같이 표현한다.

- 상태 전이 확률 p(x' | x, u) : 상태 x에서 제어 신호 u를 줬을때 얻은 새로운 상태 x'에 대한 확률 분포

- 관측 확률 p(z | x) : 상태 x에서 관측치 z를 얻은 것이 대한 확률 분포

- 상태 x, 제어 u, 관측 z가 이산 시간 흐름에 따라 결정되어 동적 베이즈 네트워크라 부름 

자 이제 진짜 오랜만에 파이썬 로보틱스를 다시 정리해보려고 한다.

예전에는 문서페이지없이 깃헙 페이지만 보고 했었는데

지금은 스핑크스로 문서도 만들어져 있다.

내용이야 probabilistic robotics에 나오는 내용 기반으로 구현되어있고

크게 의존하는 라이브러리 같은게 없어서

기본 파이썬 개발환경에서 대부분 다 돌려볼수 있는 내용들로 구성되어있다.

참 전에 공부할때 많이 도움되었었는데

이번에 다시 정리하면 더 할수 있겠지

아무튼 이 책이랑 구현된 코드랑 같이 한번 다시 정리해볼 생각