18

KALMAN FİLTRESİ VE UYGULAMASI1 Ccedileviren Semuel Franko2

1) Giriş

Filtre kullanımı ccediloğu muumlhendislik sisteminde tercih edilen bir durumdur Oumlrneğin radyo sinyalleri guumlruumlltuuml ile bozulmaya uğrar İyi bir filtreleme algoritması elektromanyetik sinyallerden guumlruumlltuumlyuuml ccedilıkartarak bizim iccedilin gerekli bilgiyi ayıklar Diğer bir oumlrnek ise guumlccedil besleyicilerin (power supply) gerilimidir Kesintisiz guumlccedil kaynakları elektrik hattındaki voltajı filtreleyerek bilgisayar veya yazıcı gibi elektrikli cihazların oumlmruumlnuumln azalmasına neden olabilecek istenmeyen gerilim dalgalanmalarını yumuşatırlar Kalman filtresi pek ccedilok farklı alandaki sistemlerin değerlerini kestirebilen bir youmlntemdir 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 guumlccedilluumlduumlr Zira mevcut buumltuumln filtreler iccedilinde kestirim hatasını minimize eden tek filtredir Kalman filtresi genelde goumlmuumlluuml sistemlere uygulanır ccediluumlnkuuml bir sistemi kontrol edebilmek iccedilin sistemin durumları ile ilgili kesin bilgiye ihtiyaccedil duyulur

2) Lineer sistemler Sinyalden guumlruumlltuumlyuuml ccedilıkartan Kalman filtresini kullanabilmek iccedilin oumllccediluumlm yaptığımız sistem doğrusal olarak ifade edilebilmelidir Ccediloğu sistem oumlrneğin yolda giden bir araccedil Duumlnyarsquoyı dolaşan bir uydu motor mili veya sinuumlzoidal olarak radyo frekans sinyali yaklaşık olarak ifade edecek şekilde doğrusallaştırılabilir Doğrusal bir sistem basitccedile aşağıdaki denklemle ifade edilebilir Durum denklemi

1k k k kx Ax Bu w+ = + + Ccedilıkış denklemi

k k ky Cx z= + Yukarıdaki denklemlerde AB ve C matrisleri durum uzay modelini oluşturan durum giriş ve ccedilıkış matrisleridir k zaman goumlstergesini x sistemin durumunu u sistemin bilinen girişi y oumllccediluumllen ccedilıkış w ve z ise sırasıyla durum ve oumllccediluumlm guumlruumlltuumlsuumlduumlr Bu bileşenler ccediloğunlukla birden fazla elemana sahiptir ve vektoumlrmatris olarak ifade edilir x vektoumlruuml sistemin mevcut durumuyla ilgili tuumlm bilgiyi taşır ancak xrsquoi doğrudan oumllccedilemeyiz Bunun yerine xrsquoin bir fonksiyonu olan ve z guumlruumlltuumlsuuml ile bozulmuş olan yrsquoyi doğrudan oumllccedilebiliriz yrsquoyi kullanarak xrsquoin kestirimini yapabiliriz ancak y ccedilıkışı da guumlruumlltuuml etkisindedir Oumllccediluumlm sonuccedillarını belirli bir kapsamda kullanabiliriz ancak tamamen guumlvenmek muumlmkuumln değildir Oumlrneğin duumlz bir hat boyunca ilerleyen aracı modellemek istediğimizi duumlşuumlnelim Sistem durumlarının konum ve hız iccedilin sırasıyla p ve vrsquoyi iccedilerdiğini soumlyleyebiliriz u girişi emredilen hız değeri y ccedilıkışı ise oumllccediluumllen konum değeridir Aracın ivmesini değiştirdiğimizi konumunu

1 Bu ccedilalışma Dan Simonrsquoın Kalman Filtering adlı makalesinin ccedilevirisidir 2 skfranko et gmailcom

28

her T anında oumllccediltuumlğuumlmuumlzuuml duumlşuumlnelim Bu durumda fiziğin temel kanunlarından hareketle hız aşağıdaki formuumllle ifade edilebilir

1k k kv v Tu+ = + Bu durumda şu andan T saniye sonra aracın yeni hızı oumlnceki hızı ve emredilen ivmesinin T ile ccedilarpımına eşittir Ancak oumlnceki denklem 1kv + rsquoin değeri ile ilgili kesin bir bilgi vermemektedir Aracın hızı ruumlzgacircr yoldaki ccedilukur ve diğer bozucular nedeniyle değişmiştir Hızın maruz kaldığı guumlruumlltuuml zamanla değişen rastgele bir değerdir Bu nedenle vrsquoyi aşağıdaki formuumllle daha doğru bir şekilde ifade etmek muumlmkuumlnduumlr

1k k k kv v Tu v minus+ = + +

Denklemin sondaki terimi hızın maruz kaldığı guumlruumlltuumlyuuml temsil etmektedir Benzer bir denklem konum iccedilin de aşağıdaki şekilde ifade edilebilir

21

12k k k k kp p Tv T u p minus

+ = + + +

Benzer bir şekilde sondaki terim konumun guumlruumlltuumlsuumlnuuml temsil etmektedir Boumlylece x durum vektoumlruuml aşağıdaki şekilde ifade edilir

kk

k

px

v⎡ ⎤

= ⎢ ⎥⎣ ⎦

Oumllccediluumllen ccedilıkışın konum bilgisi olduğunu da belirleyerek aşağıdaki durum uzay modeline ulaşmak muumlmkuumlnduumlr

2

1

120 1

[1 0]

k k k k

k k k

TTx x u w

Ty x z

+

⎡ ⎤⎡ ⎤ ⎢ ⎥+ +⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎢ ⎥⎣ ⎦= +

Durum uzay modelindeki kz terimi oumllccediluumlm cihazlarından kaynaklanan oumllccediluumlm guumlruumlltuumlsuumlnuuml 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 iccedilin bir yola ihtiyacımız var İşte Kalman filtresi burada devreye girmektedir

3) Kalman filtresi teorisi ve algoritması Yukarıda adı geccedilen durum uzay modelini kullanarak y oumllccediluumlmlerine dayanarak x durumlarını kestirmek istediğimizi duumlşuumlnelim Durum denklemlerine goumlre sistemin nasıl davranması gerektiğini biliyoruz konumla ilgili oumllccediluumlmlere sahibiz peki x durumlarını nasıl kestireceğiz Doğrudan oumllccedilemesek 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 gerccedilek sistemin değerlerinin ortalamasına eşit olmasını istiyoruz Zira bu durumda kestirimlerimiz bir youmlne doğru

38

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 gerccedilek durum değerine olabildiği kadar eşit olmasını istiyoruz Boumlylece sadece ortalamanın doğruluğu değil değerin gerccedilek değerden sapması da azaltılmış olur Matematiksel olarak ifade etmek gerekirse hatanın varyansını en kuumlccediluumlk yapan kestiriciye ihtiyacımız var Kalman filtresi bu iki kriteri de yerine getirebilir Ancak Kalman filtresinin uygulanabilmesi iccedilin bazı oumln kabuller yapmamız gerekmektedir Hatırlanacağı gibi durum guumlruumlltuumlsuumlnuuml w oumllccediluumlm guumlruumlltuumlsuumlnuuml ise z ile ifade etmiştik wrsquonun ve zrsquonin ortalama değerlerinin sıfır olduğunu kabul etmemiz gerekiyor Bunun yanında w ve x arasında bir bağıntı (correlation) olmadığı kabuluumlnuuml yapıyoruz Yani herhangi bir k anında kw ve kz değerleri bağımsız rastgele değerlerdir Bunun sonucunda guumlruumlltuumlnuumln ortak değişinti (covariance) matrisleri

wS ve zS aşağıdaki şekilde ifade edilebilir Durum guumlruumlltuumlsuuml ortak değişintisi

( )Tw k kS E w w=

Oumllccediluumlm guumlruumlltuumlsuuml ortak değişintisi

( )Tz z zS E w w=

Yukarıdaki denklemlerde Tw ve Tz w ve z rastgele guumlruumlltuuml vektoumlrlerinin devriğini (transpose) E() ise beklenen değeri ifade etmektedir Artık Kalman filtresi denklemlerine goumlz atabiliriz Kalman filtresi denklemlerini ifade etmek iccedilin pek ccedilok goumlsterim şekli mevcuttur Bu denklemler şu şekilde ifade edilebilir

1

1 1

11

( )ˆ ˆ ˆ( ) ( )

T Tk k k z

k k k k k k

T T Tk k w k z k

K AP C CP C Sx Ax Bu K y Cx

P AP A S AP C S CP A

minus

+ +

minus+

= +

= + + minus

= + minus

Kalman filtresinin temelini yukarıdaki 3 denklem ifade eder Denklemlerde a-1 altsimgesi matrisin tersinin alındığını T uumlstsimgesi 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) goumlstermektedir x kestirilen durumu goumlstermektedir k+1 anındaki durum kestirimini elde edebilmek iccedilin k anındaki durum kestirimini A ile ccedilarpıp k anındaki girişin değeriyle Brsquoyi ccedilarparız Elimizde oumllccediluumlm bilgisi olmadığında durum kestirimi bilgisini elde etmiş oluruz x denklemindeki ikinci terim ise duumlzeltme terimi olarak adlandırılmaktadır Oumllccediluumlm sonuccedillarımıza goumlre durum kestirim değerlerini duumlzeltmeye yarar K denklemi incelendiğinde eğer oumllccediluumlm guumlruumlltuumlsuuml yuumlksek ise zS rsquonin de yuumlksek olacağı bu durumda Krsquonın daha kuumlccediluumlk bir değer alacağı ve yeni x değerini hesaplarken oumllccediluumlm değeri yrsquonin guumlvenilirliği duumlşuumlruumllecektir Diğer bir yandan oumllccediluumlm guumlruumlltuumlsuuml kuumlccediluumlk olduğu

48

durumlarda zS daha kuumlccediluumlk bir değer alacak K buumlyuumlyecek ve x rsquonin hesaplanması sırasında y oumllccediluumlm değerinin guumlvenilirliği arttırılacaktır

4) Araccedil seyruumlseferi Yukarıda denklemleri verilen aracın konumunun 10 feet (bir standart sapma) hata ile oumllccediluumllebildiğini kabul edelim Emredilen ivme değeri sabit olarak 1 feetsan2 oumllccediluumlm guumlruumlltuumlsuuml ise 02 feetsan2 (bir standart sapma) olsun Konum saniyede 10 kere oumllccediluumllmektedir (T=01 saniye) Hareketli olan bu aracın konumunu en iyi şekilde nasıl kestirebiliriz T=01 saniye olduğuna goumlre sistemimizi tanımlayan doğrusal modelimizi aşağıdaki şekilde ifade edebiliriz

1

1 01 00050 1 01[1 0]

k k k k

k k k

x x u w

y x z

+

⎡ ⎤ ⎡ ⎤+ +⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦= +

Oumllccediluumlm guumlruumlltuumlsuumlnuumln standart sapması 10 feet olduğu iccedilin zS matrisi basit bir şekilde 100rsquoe eşit olur Şimdi ise wS matrisini elde edelim Konum ile ivmelenme arasında 0005rsquolik bir oran olduğuna ve ivmelenme guumlruumlltuumlsuuml 02 feetsan2 olduğuna goumlre konum guumlruumlltuumlsuumlnuumln varyansı

2 2 6(0005) (02) 10minussdot = olarak elde edilir Benzer bir şekilde hızla ivmelenme arasında 01rsquolik bir oran bulunduğuna goumlre hızın guumlruumlltuumlsuumlnuumln varyansı 2 2 4(01) (02) 4 10minussdot = sdot olarak elde edilir Son olarak konum guumlruumlltuumlsuuml ile hız guumlruumlltuumlsuuml arasındaki ortak değişinti (covariance) ise konum guumlruumlltuumlsuumlnuumln standart sapması ile hız guumlruumlltuumlsuumlnuumln standart sapmasının ccedilarpımına eşittir 5(0005 02) (01 02) 2 10minussdot sdot sdot = sdot Buumltuumln bu hesaplamaları bir araya toplarsak wS matrisinin aşağıdaki şekilde elde ederiz

[ ]2 6 5

2 5 4

( ) ( )

10 2 102 10 4 10

Tw

pS E xx E p v

v

p pvE

vp v

minus minus

minus minus

⎡ ⎤= = ⎢ ⎥

⎣ ⎦⎡ ⎤ ⎡ ⎤times

= =⎢ ⎥ ⎢ ⎥times times⎣ ⎦ ⎣ ⎦

Şimdi ise ˆox rsquoı en iyi birincil konum ve hız tahminimiz olarak 0P rsquoı ise birincil belirsizlik kestirimimiz olarak alıyoruz

5) Matlab benzetimi

Kalman filtre denklemlerini her bir adım iccedilin Matlab ortamında koşturunca aşağıdaki sonuccedillar elde edilmiştir Figure 1 aracın gerccedilek konumunu oumllccediluumllen konumunu ve kestirilen konumunu goumlstermektedir Duumlzguumln olarak ccedilizilmiş iki eğri konumun gerccedilek ve kestirilen değerlerini goumlstermektedir

58

Birbirlerinden ayırması guumlccedil olacak şekilde yakın ccedilıkmıştır bu eğriler Guumlruumlltuumlluuml eğri ise oumllccediluumllen konum değerini temsil etmektedir

0 5 10 15 20 25 30-100

0

100

200

300

400

500

Zaman (san)

Kon

um (f

eet)

Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)

Figure 2 ise gerccedilek konum ile oumllccediluumllen konum arasındaki hatayı goumlstermektedir Bunun yanında gerccedilek konum ile Kalman filtresinin kestirdiği konumu da goumlstermektedir Oumllccediluumlm hatası 10 feetlik standart sapmaya sahiptir 3 sigma değerinden dolayı bu hata 30 feete kadar ccedilıkabilmektedir Kestirilen konum hatası ise 2 feet aralığında kalmıştır

0 5 10 15 20 25 30-30

-20

-10

0

10

20

30

40

Zaman (san)

Kon

um H

atas

i (fe

et)

Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi

68

Figure 3 ise Kalman filtresinin bonusunu goumlstermektedir Araccedil hızı x durumlarının bir parccedilası olduğu iccedilin hız kestirimini konum kestirimi ile beraber elde ederiz

0 5 10 15 20 25 300

5

10

15

20

25

30

35

Zaman (san)

Hiz

(fee

tsan

)Figure 3 - Hiz (Gercek ve Kestirilen)

Figure 4 ise gerccedilek hız ile Kalman filtresinin kestirdiği değer arasındaki farkı goumlstermektedir

0 5 10 15 20 25 30-04

-03

-02

-01

0

01

02

03

04

Zaman (san)

Hiz

Hat

asi (

feet

san

)

Figure 4 - Hiz Kestirim Hatasi

78

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=01 measnoise = 10 position measurement noise (feet) accelnoise = 02 acceleration noise (feetsec^2) a = [1 dt 0 1] transition matrix b = [dt^22 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^44 dt^32 dt^32 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 footsec^2 u = 1 Simulate the linear system ProcessNoise = accelnoise [(dt^22)randn dtrandn] 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

88

Plot the results close all t = 0 dt duration figure plot(tpos tposmeas tposhat) grid xlabel(Zaman (san)) ylabel(Konum (feet)) title(Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)) figure plot(tpos-posmeas tpos-poshat) grid xlabel(Zaman (san)) ylabel(Konum Hatasi (feet)) title(Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi) figure plot(tvel tvelhat) grid xlabel(Zaman (san)) ylabel(Hiz (feetsan)) title(Figure 3 - Hiz (Gercek ve Kestirilen)) figure plot(tvel-velhat) grid xlabel(Zaman (san)) ylabel(Hiz Hatasi (feetsan)) title(Figure 4 - Hiz Kestirim Hatasi)

28

her T anında oumllccediltuumlğuumlmuumlzuuml duumlşuumlnelim Bu durumda fiziğin temel kanunlarından hareketle hız aşağıdaki formuumllle ifade edilebilir

1k k kv v Tu+ = + Bu durumda şu andan T saniye sonra aracın yeni hızı oumlnceki hızı ve emredilen ivmesinin T ile ccedilarpımına eşittir Ancak oumlnceki denklem 1kv + rsquoin değeri ile ilgili kesin bir bilgi vermemektedir Aracın hızı ruumlzgacircr yoldaki ccedilukur ve diğer bozucular nedeniyle değişmiştir Hızın maruz kaldığı guumlruumlltuuml zamanla değişen rastgele bir değerdir Bu nedenle vrsquoyi aşağıdaki formuumllle daha doğru bir şekilde ifade etmek muumlmkuumlnduumlr

1k k k kv v Tu v minus+ = + +

Denklemin sondaki terimi hızın maruz kaldığı guumlruumlltuumlyuuml temsil etmektedir Benzer bir denklem konum iccedilin de aşağıdaki şekilde ifade edilebilir

21

12k k k k kp p Tv T u p minus

+ = + + +

Benzer bir şekilde sondaki terim konumun guumlruumlltuumlsuumlnuuml temsil etmektedir Boumlylece x durum vektoumlruuml aşağıdaki şekilde ifade edilir

kk

k

px

v⎡ ⎤

= ⎢ ⎥⎣ ⎦

Oumllccediluumllen ccedilıkışın konum bilgisi olduğunu da belirleyerek aşağıdaki durum uzay modeline ulaşmak muumlmkuumlnduumlr

2

1

120 1

[1 0]

k k k k

k k k

TTx x u w

Ty x z

+

⎡ ⎤⎡ ⎤ ⎢ ⎥+ +⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎢ ⎥⎣ ⎦= +

Durum uzay modelindeki kz terimi oumllccediluumlm cihazlarından kaynaklanan oumllccediluumlm guumlruumlltuumlsuumlnuuml 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 iccedilin bir yola ihtiyacımız var İşte Kalman filtresi burada devreye girmektedir

3) Kalman filtresi teorisi ve algoritması Yukarıda adı geccedilen durum uzay modelini kullanarak y oumllccediluumlmlerine dayanarak x durumlarını kestirmek istediğimizi duumlşuumlnelim Durum denklemlerine goumlre sistemin nasıl davranması gerektiğini biliyoruz konumla ilgili oumllccediluumlmlere sahibiz peki x durumlarını nasıl kestireceğiz Doğrudan oumllccedilemesek 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 gerccedilek sistemin değerlerinin ortalamasına eşit olmasını istiyoruz Zira bu durumda kestirimlerimiz bir youmlne doğru

38

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 gerccedilek durum değerine olabildiği kadar eşit olmasını istiyoruz Boumlylece sadece ortalamanın doğruluğu değil değerin gerccedilek değerden sapması da azaltılmış olur Matematiksel olarak ifade etmek gerekirse hatanın varyansını en kuumlccediluumlk yapan kestiriciye ihtiyacımız var Kalman filtresi bu iki kriteri de yerine getirebilir Ancak Kalman filtresinin uygulanabilmesi iccedilin bazı oumln kabuller yapmamız gerekmektedir Hatırlanacağı gibi durum guumlruumlltuumlsuumlnuuml w oumllccediluumlm guumlruumlltuumlsuumlnuuml ise z ile ifade etmiştik wrsquonun ve zrsquonin ortalama değerlerinin sıfır olduğunu kabul etmemiz gerekiyor Bunun yanında w ve x arasında bir bağıntı (correlation) olmadığı kabuluumlnuuml yapıyoruz Yani herhangi bir k anında kw ve kz değerleri bağımsız rastgele değerlerdir Bunun sonucunda guumlruumlltuumlnuumln ortak değişinti (covariance) matrisleri

wS ve zS aşağıdaki şekilde ifade edilebilir Durum guumlruumlltuumlsuuml ortak değişintisi

( )Tw k kS E w w=

Oumllccediluumlm guumlruumlltuumlsuuml ortak değişintisi

( )Tz z zS E w w=

Yukarıdaki denklemlerde Tw ve Tz w ve z rastgele guumlruumlltuuml vektoumlrlerinin devriğini (transpose) E() ise beklenen değeri ifade etmektedir Artık Kalman filtresi denklemlerine goumlz atabiliriz Kalman filtresi denklemlerini ifade etmek iccedilin pek ccedilok goumlsterim şekli mevcuttur Bu denklemler şu şekilde ifade edilebilir

1

1 1

11

( )ˆ ˆ ˆ( ) ( )

T Tk k k z

k k k k k k

T T Tk k w k z k

K AP C CP C Sx Ax Bu K y Cx

P AP A S AP C S CP A

minus

+ +

minus+

= +

= + + minus

= + minus

Kalman filtresinin temelini yukarıdaki 3 denklem ifade eder Denklemlerde a-1 altsimgesi matrisin tersinin alındığını T uumlstsimgesi 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) goumlstermektedir x kestirilen durumu goumlstermektedir k+1 anındaki durum kestirimini elde edebilmek iccedilin k anındaki durum kestirimini A ile ccedilarpıp k anındaki girişin değeriyle Brsquoyi ccedilarparız Elimizde oumllccediluumlm bilgisi olmadığında durum kestirimi bilgisini elde etmiş oluruz x denklemindeki ikinci terim ise duumlzeltme terimi olarak adlandırılmaktadır Oumllccediluumlm sonuccedillarımıza goumlre durum kestirim değerlerini duumlzeltmeye yarar K denklemi incelendiğinde eğer oumllccediluumlm guumlruumlltuumlsuuml yuumlksek ise zS rsquonin de yuumlksek olacağı bu durumda Krsquonın daha kuumlccediluumlk bir değer alacağı ve yeni x değerini hesaplarken oumllccediluumlm değeri yrsquonin guumlvenilirliği duumlşuumlruumllecektir Diğer bir yandan oumllccediluumlm guumlruumlltuumlsuuml kuumlccediluumlk olduğu

48

durumlarda zS daha kuumlccediluumlk bir değer alacak K buumlyuumlyecek ve x rsquonin hesaplanması sırasında y oumllccediluumlm değerinin guumlvenilirliği arttırılacaktır

4) Araccedil seyruumlseferi Yukarıda denklemleri verilen aracın konumunun 10 feet (bir standart sapma) hata ile oumllccediluumllebildiğini kabul edelim Emredilen ivme değeri sabit olarak 1 feetsan2 oumllccediluumlm guumlruumlltuumlsuuml ise 02 feetsan2 (bir standart sapma) olsun Konum saniyede 10 kere oumllccediluumllmektedir (T=01 saniye) Hareketli olan bu aracın konumunu en iyi şekilde nasıl kestirebiliriz T=01 saniye olduğuna goumlre sistemimizi tanımlayan doğrusal modelimizi aşağıdaki şekilde ifade edebiliriz

1

1 01 00050 1 01[1 0]

k k k k

k k k

x x u w

y x z

+

⎡ ⎤ ⎡ ⎤+ +⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦= +

Oumllccediluumlm guumlruumlltuumlsuumlnuumln standart sapması 10 feet olduğu iccedilin zS matrisi basit bir şekilde 100rsquoe eşit olur Şimdi ise wS matrisini elde edelim Konum ile ivmelenme arasında 0005rsquolik bir oran olduğuna ve ivmelenme guumlruumlltuumlsuuml 02 feetsan2 olduğuna goumlre konum guumlruumlltuumlsuumlnuumln varyansı

2 2 6(0005) (02) 10minussdot = olarak elde edilir Benzer bir şekilde hızla ivmelenme arasında 01rsquolik bir oran bulunduğuna goumlre hızın guumlruumlltuumlsuumlnuumln varyansı 2 2 4(01) (02) 4 10minussdot = sdot olarak elde edilir Son olarak konum guumlruumlltuumlsuuml ile hız guumlruumlltuumlsuuml arasındaki ortak değişinti (covariance) ise konum guumlruumlltuumlsuumlnuumln standart sapması ile hız guumlruumlltuumlsuumlnuumln standart sapmasının ccedilarpımına eşittir 5(0005 02) (01 02) 2 10minussdot sdot sdot = sdot Buumltuumln bu hesaplamaları bir araya toplarsak wS matrisinin aşağıdaki şekilde elde ederiz

[ ]2 6 5

2 5 4

( ) ( )

10 2 102 10 4 10

Tw

pS E xx E p v

v

p pvE

vp v

minus minus

minus minus

⎡ ⎤= = ⎢ ⎥

⎣ ⎦⎡ ⎤ ⎡ ⎤times

= =⎢ ⎥ ⎢ ⎥times times⎣ ⎦ ⎣ ⎦

Şimdi ise ˆox rsquoı en iyi birincil konum ve hız tahminimiz olarak 0P rsquoı ise birincil belirsizlik kestirimimiz olarak alıyoruz

5) Matlab benzetimi

Kalman filtre denklemlerini her bir adım iccedilin Matlab ortamında koşturunca aşağıdaki sonuccedillar elde edilmiştir Figure 1 aracın gerccedilek konumunu oumllccediluumllen konumunu ve kestirilen konumunu goumlstermektedir Duumlzguumln olarak ccedilizilmiş iki eğri konumun gerccedilek ve kestirilen değerlerini goumlstermektedir

58

Birbirlerinden ayırması guumlccedil olacak şekilde yakın ccedilıkmıştır bu eğriler Guumlruumlltuumlluuml eğri ise oumllccediluumllen konum değerini temsil etmektedir

0 5 10 15 20 25 30-100

0

100

200

300

400

500

Zaman (san)

Kon

um (f

eet)

Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)

Figure 2 ise gerccedilek konum ile oumllccediluumllen konum arasındaki hatayı goumlstermektedir Bunun yanında gerccedilek konum ile Kalman filtresinin kestirdiği konumu da goumlstermektedir Oumllccediluumlm hatası 10 feetlik standart sapmaya sahiptir 3 sigma değerinden dolayı bu hata 30 feete kadar ccedilıkabilmektedir Kestirilen konum hatası ise 2 feet aralığında kalmıştır

0 5 10 15 20 25 30-30

-20

-10

0

10

20

30

40

Zaman (san)

Kon

um H

atas

i (fe

et)

Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi

68

Figure 3 ise Kalman filtresinin bonusunu goumlstermektedir Araccedil hızı x durumlarının bir parccedilası olduğu iccedilin hız kestirimini konum kestirimi ile beraber elde ederiz

0 5 10 15 20 25 300

5

10

15

20

25

30

35

Zaman (san)

Hiz

(fee

tsan

)Figure 3 - Hiz (Gercek ve Kestirilen)

Figure 4 ise gerccedilek hız ile Kalman filtresinin kestirdiği değer arasındaki farkı goumlstermektedir

0 5 10 15 20 25 30-04

-03

-02

-01

0

01

02

03

04

Zaman (san)

Hiz

Hat

asi (

feet

san

)

Figure 4 - Hiz Kestirim Hatasi

78

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=01 measnoise = 10 position measurement noise (feet) accelnoise = 02 acceleration noise (feetsec^2) a = [1 dt 0 1] transition matrix b = [dt^22 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^44 dt^32 dt^32 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 footsec^2 u = 1 Simulate the linear system ProcessNoise = accelnoise [(dt^22)randn dtrandn] 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

88

Plot the results close all t = 0 dt duration figure plot(tpos tposmeas tposhat) grid xlabel(Zaman (san)) ylabel(Konum (feet)) title(Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)) figure plot(tpos-posmeas tpos-poshat) grid xlabel(Zaman (san)) ylabel(Konum Hatasi (feet)) title(Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi) figure plot(tvel tvelhat) grid xlabel(Zaman (san)) ylabel(Hiz (feetsan)) title(Figure 3 - Hiz (Gercek ve Kestirilen)) figure plot(tvel-velhat) grid xlabel(Zaman (san)) ylabel(Hiz Hatasi (feetsan)) title(Figure 4 - Hiz Kestirim Hatasi)

38

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 gerccedilek durum değerine olabildiği kadar eşit olmasını istiyoruz Boumlylece sadece ortalamanın doğruluğu değil değerin gerccedilek değerden sapması da azaltılmış olur Matematiksel olarak ifade etmek gerekirse hatanın varyansını en kuumlccediluumlk yapan kestiriciye ihtiyacımız var Kalman filtresi bu iki kriteri de yerine getirebilir Ancak Kalman filtresinin uygulanabilmesi iccedilin bazı oumln kabuller yapmamız gerekmektedir Hatırlanacağı gibi durum guumlruumlltuumlsuumlnuuml w oumllccediluumlm guumlruumlltuumlsuumlnuuml ise z ile ifade etmiştik wrsquonun ve zrsquonin ortalama değerlerinin sıfır olduğunu kabul etmemiz gerekiyor Bunun yanında w ve x arasında bir bağıntı (correlation) olmadığı kabuluumlnuuml yapıyoruz Yani herhangi bir k anında kw ve kz değerleri bağımsız rastgele değerlerdir Bunun sonucunda guumlruumlltuumlnuumln ortak değişinti (covariance) matrisleri

wS ve zS aşağıdaki şekilde ifade edilebilir Durum guumlruumlltuumlsuuml ortak değişintisi

( )Tw k kS E w w=

Oumllccediluumlm guumlruumlltuumlsuuml ortak değişintisi

( )Tz z zS E w w=

Yukarıdaki denklemlerde Tw ve Tz w ve z rastgele guumlruumlltuuml vektoumlrlerinin devriğini (transpose) E() ise beklenen değeri ifade etmektedir Artık Kalman filtresi denklemlerine goumlz atabiliriz Kalman filtresi denklemlerini ifade etmek iccedilin pek ccedilok goumlsterim şekli mevcuttur Bu denklemler şu şekilde ifade edilebilir

1

1 1

11

( )ˆ ˆ ˆ( ) ( )

T Tk k k z

k k k k k k

T T Tk k w k z k

K AP C CP C Sx Ax Bu K y Cx

P AP A S AP C S CP A

minus

+ +

minus+

= +

= + + minus

= + minus

Kalman filtresinin temelini yukarıdaki 3 denklem ifade eder Denklemlerde a-1 altsimgesi matrisin tersinin alındığını T uumlstsimgesi 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) goumlstermektedir x kestirilen durumu goumlstermektedir k+1 anındaki durum kestirimini elde edebilmek iccedilin k anındaki durum kestirimini A ile ccedilarpıp k anındaki girişin değeriyle Brsquoyi ccedilarparız Elimizde oumllccediluumlm bilgisi olmadığında durum kestirimi bilgisini elde etmiş oluruz x denklemindeki ikinci terim ise duumlzeltme terimi olarak adlandırılmaktadır Oumllccediluumlm sonuccedillarımıza goumlre durum kestirim değerlerini duumlzeltmeye yarar K denklemi incelendiğinde eğer oumllccediluumlm guumlruumlltuumlsuuml yuumlksek ise zS rsquonin de yuumlksek olacağı bu durumda Krsquonın daha kuumlccediluumlk bir değer alacağı ve yeni x değerini hesaplarken oumllccediluumlm değeri yrsquonin guumlvenilirliği duumlşuumlruumllecektir Diğer bir yandan oumllccediluumlm guumlruumlltuumlsuuml kuumlccediluumlk olduğu

48

durumlarda zS daha kuumlccediluumlk bir değer alacak K buumlyuumlyecek ve x rsquonin hesaplanması sırasında y oumllccediluumlm değerinin guumlvenilirliği arttırılacaktır

4) Araccedil seyruumlseferi Yukarıda denklemleri verilen aracın konumunun 10 feet (bir standart sapma) hata ile oumllccediluumllebildiğini kabul edelim Emredilen ivme değeri sabit olarak 1 feetsan2 oumllccediluumlm guumlruumlltuumlsuuml ise 02 feetsan2 (bir standart sapma) olsun Konum saniyede 10 kere oumllccediluumllmektedir (T=01 saniye) Hareketli olan bu aracın konumunu en iyi şekilde nasıl kestirebiliriz T=01 saniye olduğuna goumlre sistemimizi tanımlayan doğrusal modelimizi aşağıdaki şekilde ifade edebiliriz

1

1 01 00050 1 01[1 0]

k k k k

k k k

x x u w

y x z

+

⎡ ⎤ ⎡ ⎤+ +⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦= +

Oumllccediluumlm guumlruumlltuumlsuumlnuumln standart sapması 10 feet olduğu iccedilin zS matrisi basit bir şekilde 100rsquoe eşit olur Şimdi ise wS matrisini elde edelim Konum ile ivmelenme arasında 0005rsquolik bir oran olduğuna ve ivmelenme guumlruumlltuumlsuuml 02 feetsan2 olduğuna goumlre konum guumlruumlltuumlsuumlnuumln varyansı

2 2 6(0005) (02) 10minussdot = olarak elde edilir Benzer bir şekilde hızla ivmelenme arasında 01rsquolik bir oran bulunduğuna goumlre hızın guumlruumlltuumlsuumlnuumln varyansı 2 2 4(01) (02) 4 10minussdot = sdot olarak elde edilir Son olarak konum guumlruumlltuumlsuuml ile hız guumlruumlltuumlsuuml arasındaki ortak değişinti (covariance) ise konum guumlruumlltuumlsuumlnuumln standart sapması ile hız guumlruumlltuumlsuumlnuumln standart sapmasının ccedilarpımına eşittir 5(0005 02) (01 02) 2 10minussdot sdot sdot = sdot Buumltuumln bu hesaplamaları bir araya toplarsak wS matrisinin aşağıdaki şekilde elde ederiz

[ ]2 6 5

2 5 4

( ) ( )

10 2 102 10 4 10

Tw

pS E xx E p v

v

p pvE

vp v

minus minus

minus minus

⎡ ⎤= = ⎢ ⎥

⎣ ⎦⎡ ⎤ ⎡ ⎤times

= =⎢ ⎥ ⎢ ⎥times times⎣ ⎦ ⎣ ⎦

Şimdi ise ˆox rsquoı en iyi birincil konum ve hız tahminimiz olarak 0P rsquoı ise birincil belirsizlik kestirimimiz olarak alıyoruz

5) Matlab benzetimi

Kalman filtre denklemlerini her bir adım iccedilin Matlab ortamında koşturunca aşağıdaki sonuccedillar elde edilmiştir Figure 1 aracın gerccedilek konumunu oumllccediluumllen konumunu ve kestirilen konumunu goumlstermektedir Duumlzguumln olarak ccedilizilmiş iki eğri konumun gerccedilek ve kestirilen değerlerini goumlstermektedir

58

Birbirlerinden ayırması guumlccedil olacak şekilde yakın ccedilıkmıştır bu eğriler Guumlruumlltuumlluuml eğri ise oumllccediluumllen konum değerini temsil etmektedir

0 5 10 15 20 25 30-100

0

100

200

300

400

500

Zaman (san)

Kon

um (f

eet)

Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)

Figure 2 ise gerccedilek konum ile oumllccediluumllen konum arasındaki hatayı goumlstermektedir Bunun yanında gerccedilek konum ile Kalman filtresinin kestirdiği konumu da goumlstermektedir Oumllccediluumlm hatası 10 feetlik standart sapmaya sahiptir 3 sigma değerinden dolayı bu hata 30 feete kadar ccedilıkabilmektedir Kestirilen konum hatası ise 2 feet aralığında kalmıştır

0 5 10 15 20 25 30-30

-20

-10

0

10

20

30

40

Zaman (san)

Kon

um H

atas

i (fe

et)

Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi

68

Figure 3 ise Kalman filtresinin bonusunu goumlstermektedir Araccedil hızı x durumlarının bir parccedilası olduğu iccedilin hız kestirimini konum kestirimi ile beraber elde ederiz

0 5 10 15 20 25 300

5

10

15

20

25

30

35

Zaman (san)

Hiz

(fee

tsan

)Figure 3 - Hiz (Gercek ve Kestirilen)

Figure 4 ise gerccedilek hız ile Kalman filtresinin kestirdiği değer arasındaki farkı goumlstermektedir

0 5 10 15 20 25 30-04

-03

-02

-01

0

01

02

03

04

Zaman (san)

Hiz

Hat

asi (

feet

san

)

Figure 4 - Hiz Kestirim Hatasi

78

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=01 measnoise = 10 position measurement noise (feet) accelnoise = 02 acceleration noise (feetsec^2) a = [1 dt 0 1] transition matrix b = [dt^22 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^44 dt^32 dt^32 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 footsec^2 u = 1 Simulate the linear system ProcessNoise = accelnoise [(dt^22)randn dtrandn] 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

88

Plot the results close all t = 0 dt duration figure plot(tpos tposmeas tposhat) grid xlabel(Zaman (san)) ylabel(Konum (feet)) title(Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)) figure plot(tpos-posmeas tpos-poshat) grid xlabel(Zaman (san)) ylabel(Konum Hatasi (feet)) title(Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi) figure plot(tvel tvelhat) grid xlabel(Zaman (san)) ylabel(Hiz (feetsan)) title(Figure 3 - Hiz (Gercek ve Kestirilen)) figure plot(tvel-velhat) grid xlabel(Zaman (san)) ylabel(Hiz Hatasi (feetsan)) title(Figure 4 - Hiz Kestirim Hatasi)

48

durumlarda zS daha kuumlccediluumlk bir değer alacak K buumlyuumlyecek ve x rsquonin hesaplanması sırasında y oumllccediluumlm değerinin guumlvenilirliği arttırılacaktır

4) Araccedil seyruumlseferi Yukarıda denklemleri verilen aracın konumunun 10 feet (bir standart sapma) hata ile oumllccediluumllebildiğini kabul edelim Emredilen ivme değeri sabit olarak 1 feetsan2 oumllccediluumlm guumlruumlltuumlsuuml ise 02 feetsan2 (bir standart sapma) olsun Konum saniyede 10 kere oumllccediluumllmektedir (T=01 saniye) Hareketli olan bu aracın konumunu en iyi şekilde nasıl kestirebiliriz T=01 saniye olduğuna goumlre sistemimizi tanımlayan doğrusal modelimizi aşağıdaki şekilde ifade edebiliriz

1

1 01 00050 1 01[1 0]

k k k k

k k k

x x u w

y x z

+

⎡ ⎤ ⎡ ⎤+ +⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦= +

Oumllccediluumlm guumlruumlltuumlsuumlnuumln standart sapması 10 feet olduğu iccedilin zS matrisi basit bir şekilde 100rsquoe eşit olur Şimdi ise wS matrisini elde edelim Konum ile ivmelenme arasında 0005rsquolik bir oran olduğuna ve ivmelenme guumlruumlltuumlsuuml 02 feetsan2 olduğuna goumlre konum guumlruumlltuumlsuumlnuumln varyansı

2 2 6(0005) (02) 10minussdot = olarak elde edilir Benzer bir şekilde hızla ivmelenme arasında 01rsquolik bir oran bulunduğuna goumlre hızın guumlruumlltuumlsuumlnuumln varyansı 2 2 4(01) (02) 4 10minussdot = sdot olarak elde edilir Son olarak konum guumlruumlltuumlsuuml ile hız guumlruumlltuumlsuuml arasındaki ortak değişinti (covariance) ise konum guumlruumlltuumlsuumlnuumln standart sapması ile hız guumlruumlltuumlsuumlnuumln standart sapmasının ccedilarpımına eşittir 5(0005 02) (01 02) 2 10minussdot sdot sdot = sdot Buumltuumln bu hesaplamaları bir araya toplarsak wS matrisinin aşağıdaki şekilde elde ederiz

[ ]2 6 5

2 5 4

( ) ( )

10 2 102 10 4 10

Tw

pS E xx E p v

v

p pvE

vp v

minus minus

minus minus

⎡ ⎤= = ⎢ ⎥

⎣ ⎦⎡ ⎤ ⎡ ⎤times

= =⎢ ⎥ ⎢ ⎥times times⎣ ⎦ ⎣ ⎦

Şimdi ise ˆox rsquoı en iyi birincil konum ve hız tahminimiz olarak 0P rsquoı ise birincil belirsizlik kestirimimiz olarak alıyoruz

5) Matlab benzetimi

Kalman filtre denklemlerini her bir adım iccedilin Matlab ortamında koşturunca aşağıdaki sonuccedillar elde edilmiştir Figure 1 aracın gerccedilek konumunu oumllccediluumllen konumunu ve kestirilen konumunu goumlstermektedir Duumlzguumln olarak ccedilizilmiş iki eğri konumun gerccedilek ve kestirilen değerlerini goumlstermektedir

58

Birbirlerinden ayırması guumlccedil olacak şekilde yakın ccedilıkmıştır bu eğriler Guumlruumlltuumlluuml eğri ise oumllccediluumllen konum değerini temsil etmektedir

0 5 10 15 20 25 30-100

0

100

200

300

400

500

Zaman (san)

Kon

um (f

eet)

Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)

Figure 2 ise gerccedilek konum ile oumllccediluumllen konum arasındaki hatayı goumlstermektedir Bunun yanında gerccedilek konum ile Kalman filtresinin kestirdiği konumu da goumlstermektedir Oumllccediluumlm hatası 10 feetlik standart sapmaya sahiptir 3 sigma değerinden dolayı bu hata 30 feete kadar ccedilıkabilmektedir Kestirilen konum hatası ise 2 feet aralığında kalmıştır

0 5 10 15 20 25 30-30

-20

-10

0

10

20

30

40

Zaman (san)

Kon

um H

atas

i (fe

et)

Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi

68

Figure 3 ise Kalman filtresinin bonusunu goumlstermektedir Araccedil hızı x durumlarının bir parccedilası olduğu iccedilin hız kestirimini konum kestirimi ile beraber elde ederiz

0 5 10 15 20 25 300

5

10

15

20

25

30

35

Zaman (san)

Hiz

(fee

tsan

)Figure 3 - Hiz (Gercek ve Kestirilen)

Figure 4 ise gerccedilek hız ile Kalman filtresinin kestirdiği değer arasındaki farkı goumlstermektedir

0 5 10 15 20 25 30-04

-03

-02

-01

0

01

02

03

04

Zaman (san)

Hiz

Hat

asi (

feet

san

)

Figure 4 - Hiz Kestirim Hatasi

78

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=01 measnoise = 10 position measurement noise (feet) accelnoise = 02 acceleration noise (feetsec^2) a = [1 dt 0 1] transition matrix b = [dt^22 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^44 dt^32 dt^32 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 footsec^2 u = 1 Simulate the linear system ProcessNoise = accelnoise [(dt^22)randn dtrandn] 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

88

Plot the results close all t = 0 dt duration figure plot(tpos tposmeas tposhat) grid xlabel(Zaman (san)) ylabel(Konum (feet)) title(Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)) figure plot(tpos-posmeas tpos-poshat) grid xlabel(Zaman (san)) ylabel(Konum Hatasi (feet)) title(Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi) figure plot(tvel tvelhat) grid xlabel(Zaman (san)) ylabel(Hiz (feetsan)) title(Figure 3 - Hiz (Gercek ve Kestirilen)) figure plot(tvel-velhat) grid xlabel(Zaman (san)) ylabel(Hiz Hatasi (feetsan)) title(Figure 4 - Hiz Kestirim Hatasi)

58

Birbirlerinden ayırması guumlccedil olacak şekilde yakın ccedilıkmıştır bu eğriler Guumlruumlltuumlluuml eğri ise oumllccediluumllen konum değerini temsil etmektedir

0 5 10 15 20 25 30-100

0

100

200

300

400

500

Zaman (san)

Kon

um (f

eet)

Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)

Figure 2 ise gerccedilek konum ile oumllccediluumllen konum arasındaki hatayı goumlstermektedir Bunun yanında gerccedilek konum ile Kalman filtresinin kestirdiği konumu da goumlstermektedir Oumllccediluumlm hatası 10 feetlik standart sapmaya sahiptir 3 sigma değerinden dolayı bu hata 30 feete kadar ccedilıkabilmektedir Kestirilen konum hatası ise 2 feet aralığında kalmıştır

0 5 10 15 20 25 30-30

-20

-10

0

10

20

30

40

Zaman (san)

Kon

um H

atas

i (fe

et)

Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi

68

Figure 3 ise Kalman filtresinin bonusunu goumlstermektedir Araccedil hızı x durumlarının bir parccedilası olduğu iccedilin hız kestirimini konum kestirimi ile beraber elde ederiz

0 5 10 15 20 25 300

5

10

15

20

25

30

35

Zaman (san)

Hiz

(fee

tsan

)Figure 3 - Hiz (Gercek ve Kestirilen)

Figure 4 ise gerccedilek hız ile Kalman filtresinin kestirdiği değer arasındaki farkı goumlstermektedir

0 5 10 15 20 25 30-04

-03

-02

-01

0

01

02

03

04

Zaman (san)

Hiz

Hat

asi (

feet

san

)

Figure 4 - Hiz Kestirim Hatasi

78

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=01 measnoise = 10 position measurement noise (feet) accelnoise = 02 acceleration noise (feetsec^2) a = [1 dt 0 1] transition matrix b = [dt^22 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^44 dt^32 dt^32 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 footsec^2 u = 1 Simulate the linear system ProcessNoise = accelnoise [(dt^22)randn dtrandn] 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

88

Plot the results close all t = 0 dt duration figure plot(tpos tposmeas tposhat) grid xlabel(Zaman (san)) ylabel(Konum (feet)) title(Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)) figure plot(tpos-posmeas tpos-poshat) grid xlabel(Zaman (san)) ylabel(Konum Hatasi (feet)) title(Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi) figure plot(tvel tvelhat) grid xlabel(Zaman (san)) ylabel(Hiz (feetsan)) title(Figure 3 - Hiz (Gercek ve Kestirilen)) figure plot(tvel-velhat) grid xlabel(Zaman (san)) ylabel(Hiz Hatasi (feetsan)) title(Figure 4 - Hiz Kestirim Hatasi)

68

Figure 3 ise Kalman filtresinin bonusunu goumlstermektedir Araccedil hızı x durumlarının bir parccedilası olduğu iccedilin hız kestirimini konum kestirimi ile beraber elde ederiz

0 5 10 15 20 25 300

5

10

15

20

25

30

35

Zaman (san)

Hiz

(fee

tsan

)Figure 3 - Hiz (Gercek ve Kestirilen)

Figure 4 ise gerccedilek hız ile Kalman filtresinin kestirdiği değer arasındaki farkı goumlstermektedir

0 5 10 15 20 25 30-04

-03

-02

-01

0

01

02

03

04

Zaman (san)

Hiz

Hat

asi (

feet

san

)

Figure 4 - Hiz Kestirim Hatasi

78

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=01 measnoise = 10 position measurement noise (feet) accelnoise = 02 acceleration noise (feetsec^2) a = [1 dt 0 1] transition matrix b = [dt^22 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^44 dt^32 dt^32 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 footsec^2 u = 1 Simulate the linear system ProcessNoise = accelnoise [(dt^22)randn dtrandn] 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

88

Plot the results close all t = 0 dt duration figure plot(tpos tposmeas tposhat) grid xlabel(Zaman (san)) ylabel(Konum (feet)) title(Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)) figure plot(tpos-posmeas tpos-poshat) grid xlabel(Zaman (san)) ylabel(Konum Hatasi (feet)) title(Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi) figure plot(tvel tvelhat) grid xlabel(Zaman (san)) ylabel(Hiz (feetsan)) title(Figure 3 - Hiz (Gercek ve Kestirilen)) figure plot(tvel-velhat) grid xlabel(Zaman (san)) ylabel(Hiz Hatasi (feetsan)) title(Figure 4 - Hiz Kestirim Hatasi)

78

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=01 measnoise = 10 position measurement noise (feet) accelnoise = 02 acceleration noise (feetsec^2) a = [1 dt 0 1] transition matrix b = [dt^22 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^44 dt^32 dt^32 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 footsec^2 u = 1 Simulate the linear system ProcessNoise = accelnoise [(dt^22)randn dtrandn] 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

88

Plot the results close all t = 0 dt duration figure plot(tpos tposmeas tposhat) grid xlabel(Zaman (san)) ylabel(Konum (feet)) title(Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)) figure plot(tpos-posmeas tpos-poshat) grid xlabel(Zaman (san)) ylabel(Konum Hatasi (feet)) title(Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi) figure plot(tvel tvelhat) grid xlabel(Zaman (san)) ylabel(Hiz (feetsan)) title(Figure 3 - Hiz (Gercek ve Kestirilen)) figure plot(tvel-velhat) grid xlabel(Zaman (san)) ylabel(Hiz Hatasi (feetsan)) title(Figure 4 - Hiz Kestirim Hatasi)

88

Plot the results close all t = 0 dt duration figure plot(tpos tposmeas tposhat) grid xlabel(Zaman (san)) ylabel(Konum (feet)) title(Figure 1 - Arac Konumu (Gercek Olculen ve Kestirilen)) figure plot(tpos-posmeas tpos-poshat) grid xlabel(Zaman (san)) ylabel(Konum Hatasi (feet)) title(Figure 2 - Konum Olcum Hatasi ve Konum Kestirim Hatasi) figure plot(tvel tvelhat) grid xlabel(Zaman (san)) ylabel(Hiz (feetsan)) title(Figure 3 - Hiz (Gercek ve Kestirilen)) figure plot(tvel-velhat) grid xlabel(Zaman (san)) ylabel(Hiz Hatasi (feetsan)) title(Figure 4 - Hiz Kestirim Hatasi)