KALMAN FİLTRESİ VE UYGULAMASI1

Çeviren :Semuel Franko2

1) Giriş

Filtre kullanımı çoğu mühendislik sisteminde tercih edilen bir durumdur. Örneğin radyo
sinyalleri gürültü ile bozulmaya uğrar. İyi bir filtreleme algoritması elektromanyetik
sinyallerden gürültüyü çıkartarak bizim için gerekli bilgiyi ayıklar. Diğer bir örnek ise güç
besleyicilerin (power supply) gerilimidir. Kesintisiz güç kaynakları elektrik hattındaki voltajı
filtreleyerek bilgisayar veya yazıcı gibi elektrikli cihazların ömrünün azalmasına neden
olabilecek istenmeyen gerilim dalgalanmalarını yumuşatırlar.

Kalman filtresi pek çok farklı alandaki sistemlerin değerlerini kestirebilen bir yöntemdir.
Matematiksel olarak ifade etmek gerekirse, Kalman filtresi doğrusal bir sistemin durumlarını
kestirir. Kalman filtresi sadece pratikte yararlı değildir, aynı zamanda teorik olarak da
güçlüdür. Zira mevcut bütün filtreler içinde kestirim hatasını minimize eden tek filtredir.
Kalman filtresi genelde gömülü sistemlere uygulanır, çünkü bir sistemi kontrol edebilmek için
sistemin durumları ile ilgili kesin bilgiye ihtiyaç duyulur.

2) Lineer sistemler

Sinyalden gürültüyü çıkartan Kalman filtresini kullanabilmek için, ölçüm yaptığımız sistem
doğrusal olarak ifade edilebilmelidir. Çoğu sistem, örneğin yolda giden bir araç, Dünya’yı
dolaşan bir uydu, motor mili veya sinüzoidal olarak radyo frekans sinyali yaklaşık olarak
ifade edecek şekilde doğrusallaştırılabilir. Doğrusal bir sistem basitçe aşağıdaki denklemle
ifade edilebilir.

Durum denklemi:
xk +1 = Axk + Buk + wk

Çıkış denklemi:
yk = Cxk + zk

Yukarıdaki denklemlerde A,B ve C matrisleri durum uzay modelini oluşturan durum, giriş ve
çıkış matrisleridir. k zaman göstergesini, x sistemin durumunu, u sistemin bilinen girişi, y
ölçülen çıkış, w ve z ise sırasıyla durum ve ölçüm gürültüsüdür. Bu bileşenler çoğunlukla
birden fazla elemana sahiptir ve vektör/matris olarak ifade edilir. x vektörü sistemin mevcut
durumuyla ilgili tüm bilgiyi taşır ancak x’i doğrudan ölçemeyiz. Bunun yerine, x’in bir
fonksiyonu olan ve z gürültüsü ile bozulmuş olan y’yi doğrudan ölçebiliriz. y’yi kullanarak
x’in kestirimini yapabiliriz, ancak y çıkışı da gürültü etkisindedir. Ölçüm sonuçlarını belirli
bir kapsamda kullanabiliriz ancak tamamen güvenmek mümkün değildir.

Örneğin düz bir hat boyunca ilerleyen aracı modellemek istediğimizi düşünelim. Sistem
durumlarının konum ve hız için sırasıyla p ve v’yi içerdiğini söyleyebiliriz. u girişi emredilen
hız değeri, y çıkışı ise ölçülen konum değeridir. Aracın ivmesini değiştirdiğimizi konumunu

1
Bu çalışma Dan Simon’ın Kalman Filtering adlı makalesinin çevirisidir.
2
skfranko et gmail.com

1/8
 her T anında ölçtüğümüzü düşünelim. Bu durumda fiziğin temel kanunlarından hareketle hız
aşağıdaki formülle ifade edilebilir.

vk +1 = vk + Tuk

Bu durumda şu andan T saniye sonra aracın yeni hızı, önceki hızı ve emredilen ivmesinin T
ile çarpımına eşittir. Ancak önceki denklem vk +1 ’in değeri ile ilgili kesin bir bilgi
vermemektedir. Aracın hızı rüzgâr, yoldaki çukur ve diğer bozucular nedeniyle değişmiştir.
Hızın maruz kaldığı gürültü zamanla değişen rastgele bir değerdir. Bu nedenle v’yi aşağıdaki
formülle daha doğru bir şekilde ifade etmek mümkündür.

vk +1 = vk + Tuk + vk −

Denklemin sondaki terimi hızın maruz kaldığı gürültüyü temsil etmektedir. Benzer bir
denklem konum için de aşağıdaki şekilde ifade edilebilir:
1
pk +1 = pk + Tvk + T 2uk + pk −
2
Benzer bir şekilde sondaki terim konumun gürültüsünü temsil etmektedir. Böylece x durum
vektörü aşağıdaki şekilde ifade edilir.
⎡p ⎤
xk = ⎢ k ⎥
⎣ vk ⎦

Ölçülen çıkışın konum bilgisi olduğunu da belirleyerek aşağıdaki durum uzay modeline
ulaşmak mümkündür.

⎡T 2 ⎤
⎡ 1 T ⎤
xk +1 ⎢ ⎥ xk + ⎢ 2 ⎥ uk + wk
⎣0 1 ⎦ ⎢ ⎥
⎣⎢ T ⎦⎥
yk = [1 0]xk + zk

Durum uzay modelindeki zk terimi ölçüm cihazlarından kaynaklanan ölçüm gürültüsünü

temsil etmektedir. Sistemi bir geri besleme ile kontrol etmek istiyorsak p konum ve v hız
bilgisini doğru bir şekilde kestirebilmemiz gerekmektedir. Diğer bir deyimle x durumlarını
kestirebilmek için bir yola ihtiyacımız var. İşte Kalman filtresi burada devreye girmektedir.

3) Kalman filtresi teorisi ve algoritması

Yukarıda adı geçen durum uzay modelini kullanarak y ölçümlerine dayanarak x durumlarını
kestirmek istediğimizi düşünelim. Durum denklemlerine göre sistemin nasıl davranması
gerektiğini biliyoruz, konumla ilgili ölçümlere sahibiz peki x durumlarını nasıl kestireceğiz?
Doğrudan ölçemesek bile durumları doğru bir şekilde kestirmemizi sağlayacak bir kestiriciye
ihtiyacımız var. Kestirici hangi gerekleri yerine getirmelidir? Şu iki gerekliliği yerine
getirmelidir.

Birinci olarak durum değişkeninin kestirimlerinin ortalamasının, gerçek sistemin değerlerinin

ortalamasına eşit olmasını istiyoruz. Zira bu durumda kestirimlerimiz bir yöne doğru

2/8
meyletmemiş olacak. Matematiksel olarak ifade etmek gerekirse, kestirimimizin beklenen
değeri durumun beklenen değerine eşit olmalıdır.

İkinci olarak ise, durum kestiriminin gerçek durum değerine olabildiği kadar eşit olmasını
istiyoruz. Böylece, sadece ortalamanın doğruluğu değil, değerin gerçek değerden sapması da
azaltılmış olur. Matematiksel olarak ifade etmek gerekirse, hatanın varyansını en küçük yapan
kestiriciye ihtiyacımız var.

Kalman filtresi bu iki kriteri de yerine getirebilir. Ancak Kalman filtresinin uygulanabilmesi
için bazı ön kabuller yapmamız gerekmektedir. Hatırlanacağı gibi durum gürültüsünü w,
ölçüm gürültüsünü ise z ile ifade etmiştik. w’nun ve z’nin ortalama değerlerinin sıfır
olduğunu kabul etmemiz gerekiyor. Bunun yanında w ve x arasında bir bağıntı (correlation)
olmadığı kabulünü yapıyoruz. Yani herhangi bir k anında wk ve zk değerleri bağımsız
rastgele değerlerdir. Bunun sonucunda gürültünün ortak değişinti (covariance) matrisleri
S w ve S z aşağıdaki şekilde ifade edilebilir:

Durum gürültüsü ortak değişintisi:

S w = E ( wk wk T )

Ölçüm gürültüsü ortak değişintisi:

S z = E ( wz wzT )

Yukarıdaki denklemlerde wT ve z T , w ve z rastgele gürültü vektörlerinin devriğini

(transpose), E(·) ise beklenen değeri ifade etmektedir.

Artık Kalman filtresi denklemlerine göz atabiliriz. Kalman filtresi denklemlerini ifade etmek
için pek çok gösterim şekli mevcuttur. Bu denklemler şu şekilde ifade edilebilir:

K k = APk C T (CPk C T + S z ) −1
xˆk +1 = ( Axˆk + Buk ) + K k ( yk +1 − Cxˆk )
Pk +1 = APk AT + S w − APk C T S z −1CPk AT

Kalman filtresinin temelini, yukarıdaki 3 denklem ifade eder. Denklemlerde a-1 altsimgesi
matrisin tersinin alındığını, T üstsimgesi ise matrisinin devriğinin alındığını ifade etmektedir.
K matrisi Kalman kazancı ve P matrisi ise kestirim hatasının ortak değişintisini (covariance)
göstermektedir.

x̂ kestirilen durumu göstermektedir. k+1 anındaki durum kestirimini elde edebilmek için k
anındaki durum kestirimini A ile çarpıp, k anındaki girişin değeriyle B’yi çarparız. Elimizde
ölçüm bilgisi olmadığında durum kestirimi bilgisini elde etmiş oluruz. x̂ denklemindeki ikinci
terim ise düzeltme terimi olarak adlandırılmaktadır. Ölçüm sonuçlarımıza göre durum
kestirim değerlerini düzeltmeye yarar.

K denklemi incelendiğinde, eğer ölçüm gürültüsü yüksek ise S z ’nin de yüksek olacağı bu
durumda K’nın daha küçük bir değer alacağı ve yeni x̂ değerini hesaplarken ölçüm değeri
y’nin güvenilirliği düşürülecektir. Diğer bir yandan ölçüm gürültüsü küçük olduğu

3/8
durumlarda S z daha küçük bir değer alacak, K büyüyecek ve x̂ ’nin hesaplanması sırasında y
ölçüm değerinin güvenilirliği arttırılacaktır.

4) Araç seyrüseferi

Yukarıda denklemleri verilen aracın konumunun 10 feet (bir standart sapma) hata ile
ölçülebildiğini kabul edelim. Emredilen ivme değeri sabit olarak 1 feet/san2, ölçüm gürültüsü
ise 0.2 feet/san2 (bir standart sapma) olsun. Konum saniyede 10 kere ölçülmektedir. (T=0.1
saniye) Hareketli olan bu aracın konumunu en iyi şekilde nasıl kestirebiliriz?

T=0.1 saniye olduğuna göre sistemimizi tanımlayan doğrusal modelimizi aşağıdaki şekilde
ifade edebiliriz:

⎡1 0.1⎤ ⎡0.005⎤
xk +1 ⎢ ⎥ xk + ⎢ ⎥ uk + wk
⎣0 1 ⎦ ⎣ 0.1 ⎦
yk = [1 0]xk + zk

Ölçüm gürültüsünün standart sapması 10 feet olduğu için S z matrisi basit bir şekilde 100’e
eşit olur.
Şimdi ise S w matrisini elde edelim. Konum ile ivmelenme arasında 0.005’lik bir oran
olduğuna ve ivmelenme gürültüsü 0.2 feet/san2 olduğuna göre, konum gürültüsünün varyansı
(0.005) 2 ⋅ (0.2) 2 = 10−6 olarak elde edilir. Benzer bir şekilde hızla ivmelenme arasında 0.1’lik
bir oran bulunduğuna göre hızın gürültüsünün varyansı (0.1) 2 ⋅ (0.2) 2 = 4 ⋅10−4 olarak elde
edilir. Son olarak konum gürültüsü ile hız gürültüsü arasındaki ortak değişinti (covariance)
ise, konum gürültüsünün standart sapması ile hız gürültüsünün standart sapmasının çarpımına
eşittir. (0.005 ⋅ 0.2) ⋅ (0.1 ⋅ 0.2) = 2 ⋅10−5 Bütün bu hesaplamaları bir araya toplarsak S w
matrisinin aşağıdaki şekilde elde ederiz:

⎡ p⎤
S w = E ( xxT ) = E ( ⎢ ⎥ [ p v ])
⎣v⎦
⎡ p 2 pv ⎤ ⎡ 10−6 2 × 10−5 ⎤
= E⎢ 2 ⎥
= ⎢ −5 ⎥
⎣ vp v ⎦ ⎣ 2 ×10 4 × 10−4 ⎦

Şimdi ise xˆo ’ı en iyi birincil konum ve hız tahminimiz olarak, P0 ’ı ise birincil belirsizlik
kestirimimiz olarak alıyoruz.

5) Matlab benzetimi

Kalman filtre denklemlerini her bir adım için Matlab ortamında koşturunca aşağıdaki sonuçlar
elde edilmiştir.

Figure 1 aracın gerçek konumunu, ölçülen konumunu ve kestirilen konumunu göstermektedir.

Düzgün olarak çizilmiş iki eğri, konumun gerçek ve kestirilen değerlerini göstermektedir.

4/8
Birbirlerinden ayırması güç olacak şekilde yakın çıkmıştır bu eğriler. Gürültülü eğri ise
ölçülen konum değerini temsil etmektedir.
Figure 1 - Arac Konumu (Gercek, Olculen ve Kestirilen)
500

400

300
Konum (feet)

200

100

-100
0 5 10 15 20 25 30
Zaman (san)

Figure 2 ise gerçek konum ile ölçülen konum arasındaki hatayı göstermektedir. Bunun
yanında gerçek konum ile Kalman filtresinin kestirdiği konumu da göstermektedir. Ölçüm
hatası 10 feetlik standart sapmaya sahiptir, 3 sigma değerinden dolayı bu hata 30 feete kadar
çıkabilmektedir. Kestirilen konum hatası ise 2 feet aralığında kalmıştır.

Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi

40

30

20
Konum Hatasi (feet)

10

-10

-20

-30
0 5 10 15 20 25 30
Zaman (san)

5/8
Figure 3 ise Kalman filtresinin bonusunu göstermektedir. Araç hızı x durumlarının bir parçası
olduğu için hız kestirimini konum kestirimi ile beraber elde ederiz.

Figure 3 - Hiz (Gercek ve Kestirilen)

35

30

25
Hiz (feet/san)

20

15

10

0
0 5 10 15 20 25 30
Zaman (san)

Figure 4 ise gerçek hız ile Kalman filtresinin kestirdiği değer arasındaki farkı göstermektedir.

Figure 4 - Hiz Kestirim Hatasi

0.4

0.3

0.2
Hiz Hatasi (feet/san)

0.1

-0.1

-0.2

-0.3

-0.4
0 5 10 15 20 25 30
Zaman (san)

6/8
 6) Matlab kodu:

clc
clear
close all

% Kalman filter simulation for a vehicle travelling along a road.

% INPUTS
% duration = length of simulation (seconds)
% dt = step size (seconds)
duration=30;
dt=0.1;

measnoise = 10; % position measurement noise (feet)

accelnoise = 0.2; % acceleration noise (feet/sec^2)
a = [1 dt; 0 1]; % transition matrix
b = [dt^2/2; dt]; % input matrix
c = [1 0]; % measurement matrix
x = [0; 0]; % initial state vector
xhat = x; % initial state estimate
Sz = measnoise^2; % measurement error covariance
Sw = accelnoise^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % process noise cov
P = Sw; % initial estimation covariance
% Initialize arrays for later plotting.
pos = []; % true position array
poshat = []; % estimated position array
posmeas = []; % measured position array
vel = []; % true velocity array
velhat = []; % estimated velocity array

for t = 0 : dt: duration

% Use a constant commanded acceleration of 1 foot/sec^2.
u = 1;
% Simulate the linear system.
ProcessNoise = accelnoise * [(dt^2/2)*randn; dt*randn];
x = a * x + b * u + ProcessNoise;
% Simulate the noisy measurement
MeasNoise = measnoise * randn;
y = c * x + MeasNoise;
% Extrapolate the most recent state estimate to the present time.
xhat = a * xhat + b * u;
% Form the Innovation vector.
Inn = y - c * xhat;
% Compute the covariance of the Innovation.
s = c * P * c' + Sz;
% Form the Kalman Gain matrix.
K = a * P * c' * inv(s);
% Update the state estimate.
xhat = xhat + K * Inn;
% Compute the covariance of the estimation error.
P = a * P * a' - a * P * c' * inv(s) * c * P * a' + Sw;
% Save some parameters for plotting later.
pos = [pos; x(1)];
posmeas = [posmeas; y];
poshat = [poshat; xhat(1)];
vel = [vel; x(2)];
velhat = [velhat; xhat(2)];
end

7/8
% Plot the results
close all;
t = 0 : dt : duration;
figure;
plot(t,pos, t,posmeas, t,poshat);
grid;
xlabel('Zaman (san)');
ylabel('Konum (feet)');
title('Figure 1 - Arac Konumu (Gercek, Olculen ve Kestirilen)')
figure;
plot(t,pos-posmeas, t,pos-poshat);
grid;
xlabel('Zaman (san)');
ylabel('Konum Hatasi (feet)');
title('Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi');
figure;
plot(t,vel, t,velhat);
grid;
xlabel('Zaman (san)');
ylabel('Hiz (feet/san)');
title('Figure 3 - Hiz (Gercek ve Kestirilen)');
figure;
plot(t,vel-velhat);
grid;
xlabel('Zaman (san)');
ylabel('Hiz Hatasi (feet/san)');
title('Figure 4 - Hiz Kestirim Hatasi');

8/8