칼만 필터를 이용하여 위치에서 속도 구하는 예제 - 김성필 저, 칼만필터의 이해 -

예전에 칼만필터의 이해라는 책을 소개했던 적이 있습니다.[바로가기] 너~~~무 쉽게 설명되어 있어서 칼만필터의 문외한이 공부해도 따라하기 수준에서 뭔가 해볼 수 있겠다 싶을 정도로 쉽게 설명된 책이었습니다. 물론 칼만필터의 증명 등등에는 관심없이 그냥 몹시 급하게(rapidly) 뭔가를 할려는 경우 아주 좋은 교재였지요... 제가 그러하듯이~~^^ 아무튼... 최근 칼만 필터를 좀 쓸일이 있어서 살짝 꺼내 들고 예제 하나 학습했답니다.~~.

이 글은 김성필 님의 "칼만필터의 이해"라는 책에 나오는 간단한 예제를 발췌해서 정리한 아~주 초보적인 글입니다. 특히 저처럼 칼만필터의 증명이나 이론적 이해에는 큰 관심없이... 예제를 빠르게 한 번 익혀볼려는 얍삽(^^)한 마인드에서 시작할려는 분들께... 뭐~~ 이렇게 예제에 접근해보시죠.. 라는 내용이니.. 어떠한 깊이도 없습니다.^^. 아무튼.. 미리미리.. 김성필님께 감사드립니다.^^

그렇습니다.~~. 이렇게 예제를 하나가지고 이야기해보죠~^^ 일단은 칼만필터의 절차부터 이야기 하죠~~^^

칼만필터에서 가장 중요한 것은 위에 나타난대로 시스템을 모델링하는 것입니다.

특히... 상태를 정의하고.. 상태 행렬과 잡음들을 잘 정의해야만 합니다. 칼만필터를 공식처럼 편하게 쓴다고 해도.. 이 모델링 만큼은 공을 들여서 관심분야에 맞게 설정해야만 하더라구요... 뭐 저처럼.. 초보스럽게 접근할때는 논문들을 살짝꿍 뒤져보면 아마 왠만한 것들은 대부분 모델링이 되어 있어서.. 사실.. 뭐 편합니다.^^

김성필님의 책에서는 Q와 R을 정의할 때 방향을 위와 같이 설명하고 있습니다.

그 다음은 초기값을 설정하구요.

그리고... 위 식을 적용하여 추정값과 오차 공분산을 계산합니다.

그리고.. 칼만 이득을 계산하구요^^

그리고.. 칼만 이득으로 추정값을 계산하는 거죠~~~

그리고 또 오차 공분산도 계산하구요~~ 그리고.. 다시 Step 3)으로 돌아가면 됩니다.^^ 이제 김성필님의 책에서 제시한 예제를 한 번 보죠~~~. 엔코더 등등으로 측정된 위치를 이용해서 속도를 추정하는 예제입니다. 아마... 응??? 그냥 차분해서 사용하면 되자나...라고 하실 수 있는데요. 실제로 위치 측정 주기가 아~주 빠른 시스템이거나 위치 측정의 분해능이 떨어지는 경우.. 그 데이터를 차분하면... 꽤 심한 노이즈를 경험하게 됩니다. 뭐~~ 그걸.. 살짝 저역통과필터를 넣어서 보기도 하는데요. 그걸 칼만 필터를 이용해서 추정하는 것이 이 번 예제인거죠.

먼저 시스템 행렬 A와 H를 위와 같이 설정합니다. 상태는 회전 각도와 각속도로 두고...

대입해서 다시 전개해 보면... 당연하지만.. 다음 각도는 현재 각도에서 추가 이동거리가 될거구요...

다음 속도는 현재 속도에 잡음이 섞여있게 될겁니다. 응?? 현재값과 다음값의 미분형태 아니야?? 라고 생각하실 수 있지만.. 속도는 일정하다.. 혹은 큰 변화가 없다는 가정도 예제는 가지고 있지만.. 실제로 그렇지 않다고 두고... 속도의 식을 대입해서 전개해도 저 결과가 나옵니다.~~^^

예제에서는 Q와 R을 위와 같이 잡고 있습니다. 

이제.. 위 내용을 가지고 MATLAB을 이용해서 구현해야지요~~~^^

sampleTime = 0.0001;

t = 0:sampleTime:4;

posOri = sin(2*pi*0.5*t);
posNoise =  posOri + 0.0001*(rand(1, length(t))-0.5);

figure
plot(t, posOri, t, posNoise, 'r'); grid on
title('Original Position and added noise')
legend('original','added noise')
xlabel('time (s)')
ylabel('rad')
set(gcf, 'Color', [1,1,1])

위 코드를 보면... 위치 측정 샘플레잇을 10kHz로 보고 있습니다. 그리고... posOri에서 sin 파 하나를 원 신호로 두구요. 아주 작은 크기(0.0001)를 가지는 노이즈를 실었습니다. 칼만필터의 대상이 되는 신호를 만들어 내는 거죠..

그렇게 만든 원신호(파랑)와 노이즈가 실린 신호(빨강)입니다.. 노이즈를 너~무 작게 잡았으니 저기서는 티가 안나네요~~^^

확대해서 봤더니 저렇게 노이즈가 모입니다. y축을 확인하세요.. 노이즈 정말 작죠~~~^^ 그런데 저걸로 속도를 측정하면 난리가 납니다. 이제 보죠^^

velOri = [0, diff(posOri)]/sampleTime;
velNoise = [0, diff(posNoise)]/sampleTime;

figure
plot(t, velNoise, 'r', t, velOri); grid on
title('Velocity from original and added noise position')
legend('added noise', 'original')
xlabel('time (s)')
ylabel('rad/sec')
set(gcf, 'Color', [1,1,1])

간단히 원신호와 노이즈가 실린 위치 신호를 모두 단순한 차분을 수행하도록 하겠습니다. 그리고난 속도 결과는 

위와 같습니다.ㅠㅠ. 노이즈가 실린 위치신호를 차분해서 얻은 속도(빨간색)는 참담할 정도의 노이즈가 실려있습니다.

뭐~~ 확대해서 보면 엄청난 노이즈라는 것을 알 수 있죠. 이걸 가지고.. 뭐 제어든... 관촬이든 할려면.. 힘좀 들수도 있습니다. 뭐 아니면 말구요~~^^

dt = sampleTime;
A = [1 dt; 0 1];
H = [1 0];
Q = [1 0; 0 1000];
R = 0.01;
x = [0 3]';
P = 3*eye(2);
    
for i = 1:length(t)    
    x_hat = A*x;
    P_hat = A*P*A' + Q;
    
    K = P_hat*H'*inv(H*P_hat*H' + R);
    
    x = x_hat + K*(posNoise(i) - H*x_hat);
    P = P_hat - K*H*P_hat;
    
    tmpPos(i) = x(1);
    tmpVel(i) = x(2);
end

이제 Step으로 정의했던 부분을 MATLAB 코드로 구현했습니다. 이것은 그냥 김성필님의 교재에서 나온 코드를 살짝만 변경한 것입니다. 뭐 아무튼.. x_hat, P_hat을 계산하고.. 칼만 이득 K를 구하고.. 다시 상태(x)와 오차 공분산(P)을 update하는 과정을 거쳐서 속도를 추정하는 것입니다. 

figure
plot(t, tmpVel, t, velOri, 'r');grid on
legend('Kalman', 'original')
xlabel('time (s)')
ylabel('rad/sec')
set(gcf, 'Color', [1,1,1])

먼저.. 위 코드를 이용해서 노이즈가 없는 속도 신호와 노이즈가 있던 신호에 칼만필터를 적용해서 속도를 추정한 결과를 보면

위와 같습니다. 당연한 이야기겠지만.. time delay가 보이긴 하네요.. 그러나.. 그 심하던 노이즈는 사라졌습니다.

확대해서 봤더니.. 저정도의 시 지연을 가지고 있네요.

figure
hold on
plot(t, velNoise, 'c', 'LineWidth', 0.5)
plot(t, tmpVel, 'r', 'LineWidth', 1.5)
grid on
legend('noisy vel', 'Kalman')
xlabel('time (s)')
ylabel('rad/sec')
set(gcf, 'Color', [1,1,1])
hold off

이제.. 노이즈가 심했던 신호와 비교해보죠.

이렇게 보면 참~~ 필터링이 잘되어 보입니다.

네.. 깨끗하구요~~^^ 뭐 물론... 예제로 푸는 것이 목적이니.. 그리 어려운 환경이 아니기 때문이겠죠~^^. 아무튼... 누군가 아름답게 책으로 쓴 내용중 한 예제를 가지고.. 있는 척 떠들어 보았습니다.^^