Drone's DIYer

루프만 칼만(Rudolf E. Kalman)이 1960년대 초 개발한 필터(filter)로 과거의 정보와 새로운 측정값을 사용하여 측정값에 포함된 잡음(noise)를 제거시켜 추정(estimate)(최적의 값을 추적)하는데 사용하는 알고리즘(algorithm)입니다. 선형적(linear) 움직임을 갖는 대상에 재귀적(reculsive)으로 동작시킵니다. 이는 누적된 과거 데이터와 현재 얻을 수 있는 최선의 측정치로 현상태를 추정하고자 함으로 결국, 자연계의 움직임은 어느 정도 예측가능하고 일반적 움직임 물성을 갖기 때문입니다.

여기서 선형시스템은 행렬연산을 가능하게 만들기 때문이며, 잡음은 시간에 영향이 없는 백색잡음(white noise)이고 Gaussian 분포(정규분포; normal distribution)를 따라야 하는데 이는 평균과 공분산으로 정확히 모델링이 가능하기 때문입니다. 여기서 LPF(low pass filter)나 HPF(high pass filter)처럼 물리적 필터가 아닌데도 불구하고 필터로 불리는 것은 컴퓨터로 이산신호처리시 시간영역에서 필터링과 같기 때문입니다. 참고로 선형시스템이란 특정 시점에서의 상태는 이전 시점의 상태와 선형적인 관계를 가지고 있는 경우를 말합니다.

만일 대부분의 자연계의 속성처럼 시스템이 비선형이고 잡음도 Gaussian 분포를 따르지 않는 경우가 많으므로 칼만필터의 변형이 요구되는데 이때 확장 칼만필터(Extended Kalman Filter; EKF)가 오류는 있지만 가장 많이 사용되고 있으며, EKF는 선형화 칼만필터(Linearized KF)와 유사하나 선형화하는 기준점을 계속 갱신해 나간다는 특징이 있습니다. UKF 참고

재귀적 자료 처리(recursive) 방식은 과거 데이터의 평균을 알기 위해서 엄청난 양의 데이터를 저장하고 갱신할 때마다 모두 계산하는 방법이 아닌 과거값과 현재의 값만 기억하여 전체 평균을 알고자 함입니다. 예를 들어, k개의 값이 입력되었다면 현재까지의 평균 Y(k)=Y(k-1)*((k-1)/k) + (1/k)*Vk로 표현할 수 있습니다. 여기서 Vk는 k번째 입력이고 k-1번째의 평균값 Y(k-1)만 알면 전체 평균을 알 수 있다는 것입니다.

만일 입력된 2개의 데이터, V1, V2의 평균은 (V1+V2)/2인데 이 경우 두 데이터의 중요도는 같습니다. 그러나 한 데이터가 잡음에 가까워 튀었다면 평균을 계산하는데 가중치가 달라져야 할 것입니다. 평균에서 얼마나 벗어났는가를 나타내는 지표는 표준편차이고 각 데이터의 중요도는 표준편차의 제곱에 반비례합니다. 그러므로 표준편차가 크면 클수록 그 값의 중요도는 떨어지고 평균에 대한 기여도도 적어져야 할 것입니다.

이와 같이 연속적인 이전의 출력값과 새로운 입력값을 중요도를 감안하여 과거와 현재의 2개 데이터로 평균을 구할 수 있고 새로운 최적값(optimized state variables)을 계산하는 측정갱신 알고리즘(measurement update algorithm)의 형태가 만들어지는데 이것이 칼만필터이고 기본식은 일반적으로 행렬(Matrix) 혹은 벡터(Vector) 형태로 나타냅니다.

선형시스템 기술할 때 일반적으로 아래의 2개의 간단한 수식으로 표현됩니다. 첫번째는 상태방정식(state equation)으로 시스템의 신호 파형을 표현하며 'Process Model' 혹은 'Plant Model'라고도 부릅니다. 두번째는 출력방정식으로(output equation) 시스템 신호 중 측정 가능한 값들을 표현하며, 'Measurement Model', 혹은 'Sensor Model'라고 부릅니다.

A는 이전 상태에 기반한 상태 전이 행렬이고 H는 해당 시간에 측정에 관계된 행렬이며, k는 시간, x는 시스템의 상태를 나타내는 벡터(Vector)입니다. 는 센서 등을 이용해서 측정된 값을 의미하며, w, v는 noise 혹은 error입니다. 특히 w는 프로세스 혹은 시스템 잡음(Process Noise), v를 측정 잡음(Measurement Noise)라고 부르며 평균이 '0'인 정규분포입니다. x, z, w, v는 시스템을 설명하는 대부분 다변수이기 때문에 벡터입니다. 여기서 초기 상태와 각 잡음은 상호 독립이어야 합니다.

시스템의 상태벡터 x는 시스템의 현재 상태에 대한 모든 정보, 예를 들어 비행체의 위치 정보와 각도, 가속도 등을 나타냅니다. 하지만 중요한 것은 이들을 직접적으로 측정할 수가 없어서 대신에 측정 잡음 v를 포함하는 센서의 출력을 이용하는 것입니다. 다음은 시스템의 현재 상태 대를 구하는 위한 칼만 필터의 알고리즘 입니다.

위 그림에서 'Time Update'는 상태방정식으로 이전 데이터를 근거고 예측하는 부분이고, 'Measurement Update'는 측정 모델(Measurement model)로써 새로운 측정값으로 교정하는 부분입니다.

x와 P는 초기예측이 필요하고 K는 칼만 이득, P는 오차 공분산 행렬(error covariance matrix), Q는 시스템 잡음(, 위 그림에서 )의 공분산 행렬이고 상태 의 크기가 n*1이면 n*n 크기의 대각행렬이고, R은 상수값으로 측정 잡음()의 공분산 행렬로 센서 제조사가 제공하기도 합니다. x위의 ^(hat)의 의미는 추정된 'estimated(priori state)'이라는 의미이며, 첨자 '-'는 아직 보정되지 않음을 뜻합니다. 

, 의 초기치를 결정합니다.

Time Update:

(1) 으로부터 현재 상태변수를 예측합니다.

(2) 오차 공분산을 예측합니다.

추정 오차(estimation error)는 다음과 같이 유도할 수 있습니다.

그러므로 공분산 행렬은 다음과 같습니다. 여기서 , 입니다.

위 식에서 두번째와 세번째 항은 시스템 잡음()과 추정 오차의 공분산인데 시스템 잡음은 백색잡음으로 추정 오차와 상관성이 없으므로(uncorrelated) '0'이 됩니다.

Measurement Update:

(1) 칼만 이득을 구합니다.

시스템에 대한 추정으로부터 오차 공분산을 구했으면 추정에 대한 불확실성을 표현할 수 있는 가장 간단하면서 강력한 방법 중에 하나는 Gaussian 분포로 표현하는 것입니다. 는 의 추정에 대한 평균과 분산을 가지는 Gaussian 확률 밀도 함수(Probability Density Function, PDF)로 정의할 수 있습니다. 만약 Gaussian PDF로 추정을 한다면, 가 r 에 위치할 확률은 다음과 같습니다.

Gaussian PDF를 이용하여 프로세스 오차 및 측정 오차를 표현하며, 이 Gaussian PDF는 매 state 마다 를 추정할 때와 측정할 때 각각 따로 존재하고, 최종 추정 값은 이 두 Gaussian PDF의 합성(fusion)을 이용해야 한다는 것입니다. 여기서 Gaussian PDF의 중요한 속성을 이용하는데, 서로 다른 두 개의 Gaussian PDF를 합성하면 또 다른 형태의 Gaussian PDF가 만들어진다는 것이다. 우리는 이러한 특성을 이용해 복잡도를 증가시키지 않고 매 state에 대한 Gaussian PDF를 계산할 수 있다는 것입니다.

위 그림에서와 같이 추정에 대한 Gaussian PDF의 식을 다음과 같이 정리하고,

또한 측정에 대한 Gaussian PDF의 식을 다음과 같이 정리할 수 있습니다.

위 두 개의 Gaussian PDF의 곱을 통해 PDF가 합성됨으로 인해 새로운 Gaussian PDF를 다음과 같이 얻을 수 있습니다.

  

                       

위 함수를 다시 정리하면,

여기서 , 는 다음과 같습니다. 

위의 두 식은 시스템 잡음 및 측정 잡음이 같은 도메인 혹은 단위에 있다고 가정하였지만 실제로는 다르기 때문에 한쪽의 Gaussian PDF를 다른 한 쪽의 Gaussian PDF에 곱하기 위해서는 도메인을 맞추어 주어야 합니다. 이를 위해서 시스템 잡음 쪽을 c로 나누어 스케일링을 통해 측정 잡음 도메인으로 맞추었습니다.

 

위 식을 이용하여 합성 평균과 분산을 다시 구하면 다음과 같습니다.

위의 식에서  그리고 으로 치환하면 아래와 같은 식으로 다시 쓸 수 있습니다.

한편 분산은 다음과 같습니다.

위 식을 칼만 필터 알고리즘에 적용을 위해 정리하면 다음과 같습니다.

이와 같이 칼만 필터 알고리즘의 모든 식을 유도할 수 있습니다. 한편 칼만 이득은 오차 공분산의 기대값 을 최소화하는 값을 찾는 것으로도 유도할 수 있는데, 이는 최소 자승법(Least mean square; LMS)으로 오차 공분산의 도함수가 '0'일 때가 최소이며 이 경우에 칼만 이득이 결정되게 됩니다.

위 칼만이득의 식에서 측정잡음(Measurement noise)의 공분산 R이 매우 크면 칼만이득이 작아지고 다음 단계의 추정치를 계산할 때 현재의 측정값이 무시되는데, 이는 측정잡음의 공분산이 크다는 것은 신뢰성이 떨어짐을 의미하므로 현재의 측정치를 무시하게 된다는 것입니다. 또한 예측오차 공분산(Process noise) P가 매우 크면 칼만이득은 '1'에 근접하고 측정치를 그대로 사용하게 되는데 이는 오차가 커져 오직 측정치에 의존함을 보입니다.

(2) 보정된 상태변수를 갱신합니다.

만일 H=I(Identity matrix), 위 식은 다음 식과 같이 변형이 가능합니다.

칼만 이득인 K가 '1'에 가깝다는 것은 센서로부터 측정된 값을 전적으로 신뢰할 수 있슴을 의미합니다. 그러므로 상태변수로부터 예측된 를 무시하고 100% 전적으로 신뢰할 수 있는 측정된 값 를 상태변수로 간주합니다. 하지만 K가 '0'에 가깝다는 말은 센서로부터 측정된 상태변수 값을 전혀 믿지 못하고 이전 k-1 스텝의 상태변수 값을 사용하여 예측된 상태변수를 사용하겠다는 것입니다.

(3) 보정된 오차 공분산을 갱신합니다.

평균(mean, average), 표준편차(standard deviation)와 분산(variance)은 각 데이터가 평균을 중심으로 얼마나 퍼져있는가를 나타내는 척도입니다. 평균은 , 분산과 표준편차 각각 , 로 나타내면 다음과 같습니다. 또한 공분산(covariance)은 2개의 변수간에 상관관계를 나타내는 것으로, Cov로 표시합니다. 단, 는 데이터의 갯수입니다.

공분산이 '0'보다 크면 x가 증가할때 y도 증가하고 '0'보다 작으면 x가 증가할때 y는 감소하는 경향을 가집니다. 만일 두변수가 아무 상관없이 흩어져 있으면 즉, 독립적이라면 '0'에 근접하게 된다는 것입니다. 열벡터를 갖는 공분산 행렬로 나타내면 다음과 같습니다.

공분산의 a,b가 상수라면 다음의 법칙이 성립합니다.

전술한 바와 같이 보정된 오차 공분산은 다음과 같이 구할 수 있습니다.

그리고 'Time Update'의 (1)단계로 되돌아 갑니다.

결국 칼만 필터는 센서를 동반한 선형시스템에서 비행역학(Flight Dynamics)을 이용해 예측오차(P)와 측정잡음(R)의 신뢰도를 공분산을 이용하여 다음 상태의 최적의 통계적 값을 결정하는 알고리즘으로 반복할수록 오차는 점점 수렴하게 됩니다. 게다가 이전의 측정값 등의 데이터를 저장하는 것이 아닌 현 상태의 평균과 같은 통계량만을 저장함으로서 계산량과 저장 장소를 최소화 한 방법이라 하겠습니다.

참고로 잘 정리된 칼만필터에 대한 개념입니다.

Understanding of the Kalman Filter.pdf