본문으로 바로가기

예전에 칼만필터의 이해라는 책을 소개했던 적이 있습니다.[바로가기] 너~~~무 쉽게 설명되어 있어서 칼만필터의 문외한이 공부해도 따라하기 수준에서 뭔가 해볼 수 있겠다 싶을 정도로 쉽게 설명된 책이었습니다. 물론 칼만필터의 증명 등등에는 관심없이 그냥 몹시 급하게(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

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

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

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


댓글을 달아 주세요

  1. BlogIcon 핑구야 날자 2015.08.20 07:45 신고

    오늘은 좀 어려운데요 그래도 잘 배우고 갑니다

  2. BlogIcon 스칼렛 오하라 2015.08.20 12:28 신고

    음,, 뭐할까? 열심히 설명하지만 문외한으로 음,,,
    고생하셧어요

  3. BlogIcon 땅이. 2015.08.20 16:03 신고

    잘 보고 갑니다 오늘 도 즐거운 하루 되세요

  4. BlogIcon 귀여운걸 2015.08.23 16:20 신고

    차근차근 쉽게 설명해주셔서 귀에 쏙쏙 들어오네요~
    덕분에 많이 배워갑니다^^

  5. BlogIcon 영준이 2015.09.08 22:51 신고

    오, 재밌게 읽었습니다.

  6. 칼만필터연구 2017.02.14 17:19 신고

    칼만필터를 적용하고 싶은데 노이즈 공분산값인 Q,R 지정하는 법과 초기값x,P를 어떻게 설정해야할지 이해가 잘안되네요 ㅠㅠ

    • BlogIcon PinkWink 2017.02.15 07:30 신고

      이 부분은 거의 튜닝의 영역으로 생각하시고 초반에 접근해야하는것 같습니다. 물론 시스템을 잘 알고 바로 좋은 값을 선정할 수 있으면 좋겠지만 말이죠.

    • 칼만필터연구 2017.02.15 15:03 신고

      가속도센서값을 처음에는 로패스필터로 사용해서 적용해보고 그다음엔 칼만필터로 해보려고하는데 시스템 모델링을 어떻게해야할지 잘 모르겠네요

    • BlogIcon PinkWink 2017.02.15 16:56 신고

      가속도센서나 자이로 센서를 다루실거면... 이 책에 있던데요.. 그 대로 응용해보시면 되지 않을까요

  7. SpeedHong 2017.03.16 10:09 신고

    한가지 질문이 있습니다.! 만약 속도에 노이즈가 들어와 미분을 하여 각속도를 구할때 그 노이즈가 심해져 사용이 어렵다고 한다면...Low pass filter를 치고 난 후에 미분을 하면 안되나요? Kalman Filter를 사용하는게 더 좋은 이유가 있을까요?

    • BlogIcon PinkWink 2017.03.16 10:31 신고

      앗.. 이글은.. 칼만필터로 이렇게 속도를 구해볼 수있다는.. 칼만필터의 사용에 관한 아주 심플한 예제이구요...
      만약 속도를 구하는데 이렇게 사용하면 일반적으로 살짝 LPF 씌우고 차분해서 사용하는 것과 별반 차이가 없습니다.^^
      다시 말씀드리면...
      이 글은 그저 칼만에 대한 예제입니다.^^

    • 2017.03.16 14:36

      비밀댓글입니다

    • BlogIcon PinkWink 2017.03.16 14:44 신고

      예측 초기 변수라는 의미로 bar를 붙였고... 안붙은것과는 다르니까요...
      T는 transpose를 의미합니다.

    • 2017.03.16 17:48

      비밀댓글입니다

    • BlogIcon PinkWink 2017.03.16 18:41 신고

      그저...
      첫번째 데이터가 위치였고.. 그 다음이 속도 성분이라 그저 그렇게 표현했을 뿐입니다.

    • 2017.03.17 11:08

      비밀댓글입니다

    • BlogIcon PinkWink 2017.03.17 11:24 신고

      본문 중간쯤에 보면...

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

      라는 구절이 있습니다. 이 뜻은 뭐냐면...
      상태 x라는 벡터는 회전하는 각도와 각속도로 이루어져있다는 뜻으로... 크기는 2*1짜리 행렬이 됩니다. 그리고, 첫번째 자리는 위치(각도) 두번째는 속도(각속도)가 됩니다.

    • 2017.03.20 10:44

      비밀댓글입니다

    • BlogIcon PinkWink 2017.03.20 10:56 신고

      작지만 도움이 되었다니.. 그나마 다행입니다. 아무튼 화이팅입니다.

    • 2017.03.21 11:08

      비밀댓글입니다

    • BlogIcon PinkWink 2017.03.22 10:49 신고

      음... 측정할 수 있는 상태가 위치값 뿐이다 라는 의미를 가지고 있습니다. (엔코더를 대상으로 한 것이니까요)

    • 2017.03.27 07:41

      비밀댓글입니다

    • BlogIcon PinkWink 2017.03.27 08:50 신고

      네.. 칼만은 결국 최초 설계를 어떻게 접근하는가 문제인듯 합니다.
      아주... 어려운듯 하구요~~
      그걸 잘 해야... 좋은 결과가 나오는 듯 합니다.^^

  8. Kalman 2017.05.23 12:05 신고

    글 잘 봤습니다.즉 칼만 이득을 계속 적용하여 계속 루프를 돌려 값을 추정하는것 같은데 매트랩 예제에서는 그럼 몇번의 루프를 통해서 값을 추정한것인가요? 매트랩 코드내에 그러한 횟수가 적혔있나요?

  9. s당근 2018.02.07 14:55 신고

    대략 SNR 75dB 환경의 성능이네요..

  10. 구우 2018.04.18 13:56 신고

    저 책에서 제공하고 있던 예제 데이터 파일 (특히 13, 14 장의 자이로 센서 데이터) 혹시 가지고 계신가요?? ㅠㅜㅠㅜ 예제에 적용해 보고 싶은데 (EKF 랑 UKF) 지금은 홈페이지가 막혀서 구할 수가 없네요 ㅠㅜㅠㅜ