Part 3) 칼만 필터 응용 - Ch8. 초간단 칼만 필터 예제

Speedwell🍀·2022년 11월 16일

Kalman matlab

칼만 필터는 어렵지 않아 with MATLAB Examples

목록 보기

9/13

Ch8. 초간단 칼만 필터 예제에서는 칼만 필터로 측정 잡음을 줄이는 예제를 살펴본다. 저주파 통과 필터처럼 칼만 필터도 잡음을 제거하는 기능이 있다.
Ch9. 위치로 속도 추정하기에서는 칼만 필터로 측정하지 않은 값도 추정 가능함을 확인한다. 위치 측정값으로 속도를 추정하거나, 속도 측정값만으로 위치의 추정이 가능하다. 이러한 능력은 시스템 모델 덕분!
Ch10. 영상 속의 물체 추적하기에서는 영상 인식 기법과 칼만 필터를 결합해 사용하는 사례를 소개한다. 10장의 시스템 모델은 9장의 모델을 2차원 평면으로 확장한 형태! 이처럼 시스템 모델을 적절히 구성하여 칼만 필터를 다양한 형태의 문제에 적용 가능!
Ch11. 기울기 자세 측정하기에서는 서로 다른 센서의 측정값을 칼만 필터로 융합해 더 좋은 센서로 개선하는 예제를 살펴본다. 센서 융합 문제는 칼만 필터가 활용되는 주요 분야 중 하나!

예제 8-1

배터리의 전압을 측정하는데, 잡음이 심해서 잴 때마다 전압값이 달랐다. 그래서 칼만 필터로 측정 데이터의 잡음을 제거해보기로 했다. 전압은 0.2초 간격으로 측정한다.

1. 시스템 모델

배터리의 전압을 상태 변수(x_k)로 하는 시스템 모델 (8.1)

시스템 모델(8.1)이 실제 시스템을 제대로 반영하고 있는지 확인해보자!

첫 번째 식은 배터리의 전압이 어떻게 변하기는지를 나타내는 식
- 배터리의 전압이 일정하게 유지되고 있음 (x_k+1 = x_k)
  - 그 값은 14볼트 (x_0 = 14)
➡ 짧은 시간에 전압이 급격하게 변하는 경우는 별로 없으므로 나름 타당한 모델

두 번째 식은 측정값인 전압에 잡음(v_k)이 섞여 있다는 뜻
- 측정 잡음은 평균이 0이고 표준편차가 2인 정규분포를 따르는 신호
➡ 측정값이 전압이라는 사실은 잘 반영되었지만, 잡음의 특성이 정확한지는 신호와 센서를 분석해봐야 한다!
➡ 시스템 모델의 R을 설계 변수로 보고 값을 바꿔가면서 적절한 값을 선정하는 것도 하나의 방법

칼만 필터를 구현해보자!

먼저, 시스템 모델을 구성하는 A, H, Q, R을 구해야 한다!
➡ 시스템 모델식(7.1, 7.2)과 식 (8.1)을 비교해보면 쉽게 구할 수 있다.

A = 1
H = 1
Q = 0
- w_k=0이므로 분산 또한 0
R = 4

초기 예측값을 정하자!

초깃값에 대한 정보가 전혀 없다면 오차 공분산을 크게 잡는 게 좋다.

2. 칼만 필터 함수

SimpleKalman(z)

매개변수: z (전압 측정값)

반환값: 추정한 전압

위의 그림의 과정을 그대로 구현하면 된다!
예측값을 나타내는 변수는 위첨자 대신 이름 뒤에 'p'를 붙였다.

function volt = SimpleKalman(z)
%
%
persistent A H Q R
persistent x P
persistent firstRun

if isempty(firstRun)
    % 시스템 모델 변수 초기화
    A = 1;
    H = 1;
    Q = 0;
    R = 4;
    
    % 초기 예측값 지정
    x = 14;
    P = 6;

    firstRun = 1;
end


xp = A*x;                       % Ⅰ. 추정값 예측
Pp = A*P*A' + Q;                %     오차 공분산 예측

K = Pp*H' / (H*Pp*H' + R);      % Ⅱ. 칼만 이득 계산

x = xp + K*(z - H*xp);          % Ⅲ. 추정값 계산
P = Pp - K*H*Pp;                % Ⅳ. 오차 공분산 계산


volt = x;                       % 추정값 반환

end

MATLAB에선 함수의 첫 번째 줄에서 반환값으로 지정한 변수에 값을 대입해 두면, 함수 실행이 끝날 때 이 값이 자동으로 반환된다.

3. 테스트 프로그램

Ch1의 TestAvgFilter.m과 거의 동일하게 구현하면 된다.
Ch1에서 썼던 GetVolt.m 함수도 동일!

GetVolt.m

function z = GetVolt()
% 전압값을 읽어오는 함수
% GetVolt 함수가 측정하는 전압의 평균은 14.4볼트이고, 여기에는 잡음이 섞여있다.
% 잡음의 평균은 0, 표준편차는 4라고 가정했다.
w = 0 + 4 * randn(1,1);     % 잡음 생성
z = 14.4 + w;
end

TestAvgFilter.m

clear all

dt = 0.2;
t = 0:dt:10;

Nsamples = length(t);

Xsaved = zeros(Nsamples, 1);
Zsaved = zeros(Nsamples, 1);

for k=1:Nsamples
    z = GetVolt();             % 전압값 읽어오기
    volt = SimpleKalman(z);    % 칼만 필터 함수 호출

    Xsaved(k) = volt;
    Zsaved(k) = z;
end

figure
plot(t, Xsaved, 'o-')
hold on
plot(t, Zsaved, 'r:*')
legend('Kalman Filter', 'Measurements')

실선: 칼만 필터로 추정한 전압
- 칼만 필터 출력은 참값(14.4V)에 모여 있다.
  ➡ 칼만 필터가 측정 잡음을 제거해 추정값이 참값에 근접했기 때문
점선: 측정값
- 측정 잡음 때문에 굉장히 넓게 퍼져있다.

4. 오차 공분산과 칼만 이득

SimpleKalman.m 함수의 반환값에 오차 공분산과 칼만 이득을 추가해 SimpleKalman2.m 함수를 만들자!

function [volt, Px, K] = SimpleKalman2(z)

TestSimpleKalman.m에서 각 루프마다 측정값과 추정값 외에도 칼만 이득과 오차 공분산을 함께 저장하도록 수정한 TestSimpleKalman2.m을 만들자!

clear all


dt = 0.2;
t  = 0:dt:10;

Nsamples = length(t);

Xsaved = zeros(Nsamples, 3);
Zsaved = zeros(Nsamples, 1);

for k=1:Nsamples
  z = GetVolt();  
  [volt, Cov, Kg] = SimpleKalman2(z);
  
  Xsaved(k,:) = [ volt Cov Kg ];
  Zsaved(k) = z;
end


figure
plot(t, Xsaved(:,1), 'o-')
hold on
plot(t, Zsaved, 'r:*') 
xlabel('Time [sec]')
ylabel('Voltage [V]')
legend('Kalman Filter', 'Measurements')

figure
plot(t, Xsaved(:,2), 'o-')
xlabel('Time [sec]')
ylabel('P')

figure
plot(t, Xsaved(:,3), 'o-')
xlabel('Time [sec]')
ylabel('K')