Doğrusal Olmayan Ölçümlü Durum Uzay Modelleri için Kalman Filtresi Kestirimi Yaklaşımlarının Karşılaştırılması 

 

Fredrik Orderud                                                                                                          Sem Sælands vei 7‐9, NO‐7491 Trondheim 

 

Türkçeye çeviren: M.Mehmet NEFES, Ankara Üniversitesi

 

Özet 

Genişletilmiş  Kalman  Filtresi  (GKF)  ,  esasen basit, sağlam ve gerçek  zamanlı uygulamalara uygun  olmasından  dolayı  doğrusal  olmayan durum uzay modelleri kestirimi  için fiili olarak uygulanan  bir    standart  haline  gelmiştir  [11]. Bununla  birlikte,  son  yıllarda  Unscented Kalman  Filtresi  (UKF)  olarak  adlandırılan alternatif bir  yaklaşım da ortaya  çıkmıştır. Bu filtre,  doğrusal  olmayan  modeller  için  daha yüksek  doğruluk  ve  sağlamlılık  sağlamayı amaçlamaktadır.  UKF’nin  doğruluğunu doğrusal    olmayan  süreç  modelleri  için araştıran  bir  kaç  makale  olmasına  karşın bunların  hiç  biri  özellikle  doğrusal  olmayan ölçüm  modellerinin  doğruluğuna  hitap etmemektedir.  Bu makale,  doğrusal  olmayan ölçümlere  sahip  iki  takip modeli  için  GKF  ve UKF  performanslarını  karşılaştırarak  bu boşluğu doldurmayı amaçlamaktadır. 

 

1 Giriş 

Durum  kestirimi  problemi,    sadece  gürültülü ve/veya  doğru  olmayan  ölçümlerine erişilebilen  bir  sürecin  durum  kestirimi  ile ilgilenir.  Bu  problem  ortamına  fen  ve mühendislik  dallarındaki  hemen  hemen  her disiplinde çoğu zaman rastlanır. 

En  yaygın  kullanılan  durum  kestiricisi  Kalman filtresidir.  Kalman  filtresi  doğrusal  sistemler için  optimal  bir  kestiricidir,  ancak  gerçek dünyada çok az sayıda doğrusal sistem vardır. Bu problemin üstesinden gelmenin yaygın bir yaklaşımı , Kalman filtresini kullanmadan önce sistemi  doğrusal  hale  getirmektir  ve  bu 

yaklaşım genişletilmiş Kalman filtresini niteler. Bununla  birlikte,  doğrusallaştırma  kararsız kestirimler  gibi  bazı  problemler  ortaya çıkarabilir [8]. Doğrusal olmayan sistemler için daha  iyi  kestirici  algoritmaların  geliştirilmesi çabaları,  ortaya  çıkacak  yenilikliklerin mühendislik  alanlarında  geniş  bir  yelpazede büyük  yankı  bulmasının  kaçınılmaz olacağından  bilim  dünyasınca  büyük  ilgi görmektedir.  

Notasyon 

Cebirsel  gösterimler  görünüşlerine  göre farklılık  göstermektedir.  Normal  gösterimler (a)  skalar  büyüklükleri,  kalın  gösterimler  (a) vektörleri   ve büyük harfli gösterimler  (A)    ise matrisleri  simgelemektedir.  Alt  simgeler  (Xa) kesikli  zamanı  simgeler.  Koşullu  alt  simgeler (Xa|b  )  , verilmiş olan b anına kadar kalan olan ölçümler ile a anındaki x durumunu simgeler. ^ 

üst  simgesi    ( )  kestirilen  (tahmini)  değeri simgeler. 

a

2 Genel Bilgi 

Durum uzay modeli , durumunun (x) sayısal bir vektör  ile  ifade  edildiği  bir  sürecin matematiksel  modelidir.  Durum  uzay modelleri  iki  ayrık  model  içerirler.  Bu modeller; girdi ve gürültü gibi dış etkenler  ile durumun  zaman  içerisinde  nasıl  ilerlediğini anlatan  süreç modeli  ve  süreçten  ölçümlerin nasıl  alınacağını  ifade  eden  genellikle  de gürültülü  ve/veya  doğru  olmayan  ölçümleri kullanan ölçüm modelidir. 

 

 

2.1 Genel Durum Uzay Modeli 

Durum  uzay  modellerinin  en  genel  biçimi doğrusal  olmayan  modeldir.  Bu  model  tipik olarak iki fonksiyonu (f ve h) içerir. 

 

f  ve  h  fonksiyonları  sırasıyla  durumu  ve ölçümleri  (gözlemleri)  belirleyen fonksiyonlardır.  u  süreç  girdisini,  w  ve  v sırasıyla  durum  ve  ölçüm  (gözlem)  gürültü vektörlerini ve k ise kesikli zamanı ifade eder. 

Durum uzay modelleri hemen hemen her çesit sürecin  modellenmesinde  oldukça  kullanılır.    f  ve  h  fonksiyonları  sürecin  dinamiğini  ve gözlemlerini  belirten  kesikli  hale  getirilmiş diferansiyel  denklemler  üzerine  kurulmuş fonksiyonlardır. 

 

Şekil 1: Genel Durum Uzay Modeli. Z‐1  :  Birim gecikme fonksiyonu (Sayısal sinyal işlemede kullanılan Z‐dönüşümü)   

 

2.2 Doğrusal Durum Uzay Modeli 

Doğrusal  durum  uzay  modeli,  f  ve  h fonksiyonlarının  durum  ve  girdide  doğrusal olduğu  modeldir.  Doğrusal  olan  bu fonksiyonlar,  hesaplamaları  doğrusal  cebire indirgeyen  F,  B  ve  H  matrisleri  ile  ifade edilebilir. Bu  şekildeki  durum  uzay modeli  şu şekilde gösterilir: 

 

Şekil 2: Doğrusal Durum Uzay Modeli 

Bu  doğrusal  modelde  hesaplama  ve  analiz kolaydır.  Kullanıcıya  denetlenebilirlik, gözlenebilirlik,  frekans  yanıtı  gibi  özellikleri irdeleme imkânı sunar. 

Doğrusal durum modelleri ya gerçek doğrusal süreçler,  ya  da  doğrusal  olmayan  süreçlerin basit bir şekilde birinci derece Taylor yaklaşımı aracılığı  ile  doğrusal  hale  getirilmiş  biçimleri üzerine kurulmuş modellerdir. 

 

3 Durum Kestirimi 

Durum  kestirimi,  durumu  doğrudan gözlenemeyen  bir  sürecin  olasılık  yoğunluk fonksiyonunu  (oyf)  tahmin  etme  (kestirme) problemi  ile  ilgilenir.  Bu  olgu,  hem  günceli kullanarak bir  sonraki durumu  tahmin etmeyi hem  de  yapılan  bu  tahmini,  alınan  gürültülü ölçümleri  kullanarak  güncellemeyi/düzeltmeyi kapsar. 

3.1 İndirgemeli Bayes Kestirimi 1 

Durum  kestirimi  için  bilinen  en  genel  biçim indirgemeli  Bayes  kestirimidir  [1].  Bu  biçim, sistemi  ve  ölçüm  modeli  verilen  bir  sürecin durumunun  olasılık  yoğunluk  fonksiyonunu tahmin etmede en uygun yoldur. Bu bölümde, indirgemeli olarak her bir zaman adımında bir önceki  zaman  adımında  elde  edilen  kestirimi ve  yeni ölçümleri baz  alarak  yeni bir  kestirim yapan bu kestiriciyi irdeleyeceğiz. 

                                                            1 ing.:  Recursive Bayesian Estimation 

İndirgemeli  Bayes  kestirimi,  süreci  simule etme ve elde edilen bu süreci gerçek süreçten elde  edilen  yeni  ölçümleri  (z)  hesaba  katarak simulasyon  süreci  ile  aynı  anda  düzeltme prensibi ile çalışır. Hesaplamalar iki aşamalı bir işlem  ile  indirgemeli olarak  yapılır.    İlk olarak bir  sonraki  durum,  f  fonksiyonundan                    elde  edilen  durum  ilerleme  kanısı

                                      2  p  (xk|xk‐1) 

kullanılarak  mevcut  durumun  bir  sonraki durum  üzerine  dış  değerlemesi  ile  tahmin edilir.  İkinci  aşamada,  yeni  ölçümler  göz önünde  bulundurularak  h  fonksiyonundan elde  edilen  ölçüm  olabilirliği3  p  (zk|xk) kullanılarak  yapılan  bu  tahmin düzeltilir/güncellenir. 

İlk  olarak,  xk    durumunun  bir  önceki  zaman dilimine ait olasılık yoğunluk fonksiyonunu k‐1 anına  kadar  olan  ölçümler  baz  alınarak hesaplamak  için Chapman‐Kolmogorov eşitliği kullanılır:  

 

Daha  sonra,  k  anına  kadar  olan  ölçümler alındıktan  sonra bu ölçümler dikkate  alınarak xk   durumunun güncellenmiş olasılık yoğunluk fonksiyonunu  elde  etmek  için  Bayes  kuralı kullanılır: 

 

Şekil 3: İndirgemeli Bayes Kestiricisi döngüsü 

 

                                                            2 ing.:  State Propagation Belief 3 ing.:  Measurement Likelihood 

Bu metotun pratik uygulaması çoğunlukla çok boyutlu  durum  vektörleri    için  büyük  durum uzayları  sebebiyle oldukça  güçtür. Bu  tarz bir durum  uzayında  her  bir  noktada  bir  önceki olasılığın  hesaplanması,    durum  uzayı büyüdükçe  çözümü  gittikçe  zorlaşan  çok boyutlu  bir  integral  içerir.  Bilgisayarlar  da durum  uzayının  kesikli  zamanda  olasılık yoğunluk  fonksiyonu  hesaplamada  sınırlı kalmaktadır.  Ayrıca  bu  hesaplamanın yapılabilmesi için durum uzayınının kesikli hale getirilmesi gerekmektedir. Bu yüzden genelde bu  yöntem  durum  kestirimin  teorik  temeli olarak kabul edilir. Bilgisayar vasıtasıyla Bayes kestirimi  ya  durum  uzayının  kesikli  hale getirilebildiği ya da modele belli kısıtlamaların uygulandığı durumlarda mümkün olmaktadır. 

3.2 Kalman Filtresi 

Durum  kestirim  problemi,  süreç  modeli üzerinde  belirli  zorlama  ve  kısıtlamalar  uygulandığında  kolay  işlenir  hale  gelir.  Bu zorlama ve kısıtlamalar; f ve h fonksiyonlarının doğrusal    olması,  ve    gürültü  terimleri w  ve v’nin birbirleriyle  ilişkisiz, Gauss dağılımlı, sıfır ortalamalı beyaz gürültü olmalarıdır. Yukarıda ifade edilenleri matematiksel olarak şu şekilde gösterebiliriz: 

 

Burada  Q  ve  R  sırasıyla  durumun  ve  ölçüm gürültüsünün  ikinci  derece  özelliklerini belirten  kovaryans  matrisleridir.  Yukarıda tanımlananlar  durum  modelini  aşağıdaki biçime indirger:  

 

Burada F, B ve H zamana bağlı matrislerdir. 

Doğrusal  bir modele  Gauss  dağılımlı  bir  girdi verildiğinde modelin durum ve çıktısı da Gauss dağılımlı olur  [12]. Bu yüzden, durum ve  çıktı olasılık  yoğunluk  fonksiyonu  daima  ortalama 

ve  kovaryansın  yeterli  istatistikte  olduğu normal dağılıma sahiptir. Bu da gösterir ki, bu durumda  bütün  durum  olasılık  yoğunluk fonksiyonun  hesaplamaya  gerek  yoktur. ortalama vektörünü ve durum için P kovaryans matrisini hesaplamak yeterli olacaktır. 

x

 

Şekil 4: Kalman filtresi döngüsü 

 

İndirgemeli Bayes kestirimi yöntemi F, B ve H matrislerinin  f  ve  h  fonksiyonlarının  yerini aldığı  Kalman  filtresine  dönüşür.  Kalman filtresi iki aşamadan oluşan Bayes kestiricisidir. Bu  iki  aşama,  tahmin  etme  ve  güncelleme aşamalarıdır.  Gerekli  hesaplamalar  aşağıda sırasıyla gösterilmiştir:  Ölçümler alınmadan önce bir  sonraki durumu tahmin etme:  

  Ölçümler  alındıktan  sonra  durumu güncelleme:  

  Burada  K  Kalman  kazancı  matrisi  ve  P  ise kestirimin  hassasiyeti  ile  ilgili  bilgiyi  içinde barındıran  durum  kestirim  kovaryans matrisidir.  Bu  filtre  ile  daha  fazla  bilgi  ve detaylar [2] no’lu makaleden elde edilebilir.     Kalman  filtresi  çoğunlukla doğrusal bir yapıya sahip  olduğu  için   matris  ters  alma  işlemleri hariç kolay hesaplamalar  içerir. Ayrıca Kalman 

filtresinin  ikinci derecen hata metriğine  sahip süreç  durumları  için  optimal  bir  kestirici olduğu kanıtlanabilir [2].  3.3 Genişletilmiş Kalman Filtresi 

Gerçek  dünyadaki  çoğu  süreç  maalesef doğrusal  değildir;  dolayısıyla  Kalman  filtresi vasıtası  ile  tahmin edilebilmeleri  için doğrusal hale  getirilmelidir.  Genişletilmiş  Kalman Filtresi  (GKF)  bu  problemi,  f  ve  h fonksiyonlarının  tahmini  durum  etrafında Jacobian  matrisini  hesaplayarak  çözer.    Bu matris  durumun  etrafına merkezlenen model fonksiyonunun  eğrisini  verir.  Jacobian matrisi bir  vektörün  bütün  kısmi  türevlerini içermektedir. 

 

 

 

Şekil  5:  GKF’nin  doğrusal  olmayan  bir fonksiyonu  Gauss  dağılımının  ortalaması etrafında  doğrusal  hale  getirmesi  ve  sonra doğrusal  hale  getirilmiş  bu model  vasıtası  ile ortalama ve kovaryansı yayması. 

 

  

Genişletilmiş  Kalman  filtresi  tahmini  duruma           dayalı  olarak  zamanla  değişiklik  gösteren (zamana  bağlı)  F  ve  H  dışında  normal  bir Kalman  filtresi  gibi  çalışır.  Gerekli hesaplamalar aşağıda sırasıyla gösterilmiştir:  

Ölçümler alınmadan önce bir  sonraki durumu tahmin etme:  

  Ölçümler  alındıktan  sonra  durumu güncelleme:  

  Burada  K  Kalman  kazancı  matrisi  ve  P  ise kestirimin  hassasiyeti  ile  ilgili  bilgiyi  içinde barındıran  durum  kestirim  kovaryans matrisidir. 

 

3.4 Unscented Kalman Filtresi 

3.4.1  Giriş 

Rastgele  Gauss  değişkenlerinin    doğrusal olmayan  bir  fonksiyon  üzerinde  ilerletilmesi problemi  unscented  dönüşüm  olarak adlandırılan  başka  bir  yöntemle  ele  alınabilir. Bu  dönüşüm,  fonksiyonları  doğrusal  hale getirmek  yerine  belirli  bir  noktalar  kümesini kullanır  ve  bu  noktaları  doğrusal  olmayan fonksiyon  üzerinde  ilerletir.  Bu  noktalar,  ortalaması, kovaryansı ve muhtemelen yüksek dereceli momentleri  rastgele Gauss değişkeni ile  eşleşecek  şekilde  seçilir.  Ortalama  ve kovaryans,  fonksiyon  üzerindeki  bu noktalardan  klasik  fonksiyon doğrusallaştırma yöntemine  göre  daha  doğru  sonuç  verecek şekilde yeniden hesaplanabilir. Bu yöntemdeki temel  fikir;    fonksiyonun  kendisinin  yerine olasılık  dağılımına  yaklaşım  sağlamaktır.  Bu strateji  genel  olarak  hem  hesaplama karmaşıklığını  azaltır  hem  de  daha  doğru  ve hızlı  sonuçlar  ortaya  çıkararak  kestirim doğruluğunu artırır.  

3.4.2  Genel Bilgi 

Unscented  dönüşümün  temelini  teşkil  eden metod  ilk  olarak  Julier, Uhlmann  ve Durrant‐

Wyte  tarafından  [10]  ve  [11]  no’lu makalelerde  ortaya  konmuştur.  Julier, Uhlmann  ve  Durrant‐Wyte    bu  metotta  N boyutlu  rastgele  bir  Gauss  değişkenin  sigma noktaları  olarak  adlandırılınan  2N+1  adet örnek  ile  nasıl    ifade  edilebildiğinin  taslak olarak  ortaya  koymuşlardır.    Yaklaşım sağladıkları Gauss dağılımı  ile aynı kovaryansa olacak  şekilde  üç  noktayı  seçebilmek  için karekök  matrisi  ve  kovaryans  tanımlanndan faydalanmışlardır. Bu noktalar  simetrik olacak şekilde seçilerek   çarpıklık önlenir. Bu yöntem ile  yaklaştırım  hatası  düşük  olur  ve  ancak dördüncü  ve  daha  yüksek  momentlerden kaynaklanır. 

Unscented  dönüşümün  Kalman  filtrelemede kullanımı  daha  sonra  Julier  ve  Uhlmann tarafından  Unscented  Kalman  Filtresi formunda  [8]  no’lu  makalede  ortaya koyulmuştur.  Unscented  Kalman  Filtresi  bir sonraki  durumu  sigma  noktaları  kullanarak tahmin  etmektedir.  Bu  filtre,  [16]  no’lu makalede daha detaylı olarak analiz edilmiştir. 

Unscented Kalman Filtresi ile ilgili kısıtlama; bu filtrenin durum uzayındaki noktalar arasındaki uzaklığı  ifade eden  sigma noktalarının güvenli yayılımının  daha  düşük  bir  limite  sahip olmasıdır.  Sigma  noktası  yayılımlarının  bu limitin  altında  olması  pozitif  yarı‐tanımlı korelasyon matrisler elde etmeyi garanti altına almaz.  Ayrıca  bu  uzaklık  durum  uzayının boyutuna  bağlı  olarak  artar  ve  bu  şekilde ortaya  çıkan  yüksek  sigma  yayılımı  lokal olmayan  özelliklerin  örneklemesine  yol açabileceğinden  yüksek  derecede  doğrusal olmayan  modellerde  çeşitli  problemler meydana getirebilir. 

Bu  yüzden  burada ortaya  konan  yöntem,  asıl unscented  dönüşümden  farklı  olarak  ek  bir  ayarlama  parametresi  ,α,  içeren  ölçekli unscented dönüşüm  [6] üzerine  yapılanmıştır. Bu  parametre,  sigma  noktalarının  yayılımını pozitif  yarı‐tanımlı  korelasyon  matrisler sağlamayı garanti altına alarak kontrol etmede kullanılır. Bu  sayede yüksek boyutlu modeller için bile dar bir sigma noktası yayılımı  ile  lokal olmayan etkilerden kaçınılabilir.  

 

 

Şekil  6:  UKF’nin  Gauss  dağılımlı  sigma noktalarının  doğrusal  olmayan  bir  fonksiyon üzerinde  ilerletilmesi,ve  sonuçların  ortalama ve  kovaryanslarını  hesaplayarak  yeniden  bir Gauss dağılım oluşturulması  

3.4.3  Takviyeli Durum  (Augmented State) 

 Gauss  olmayan  ya  da  aditif  olmayan gürültüleri  oluşturmak  için  gürültünün doğrusal  olmayan  bir  usul  ile  ele  alınabilir olması  unscented  dönüşüm  yaklaşımının  bir başka  avantajıdır.  Bu  stratejideki  işlem gürültünün  fonksiyonlar  üzerinde ilerletilmesini  içerir. İlk olarak durum vektörü, gürültü  kaynaklarını  hesaba  katmak maksadıyla  genişletilir,  bir  başka  deyişle takviye  edilr.  Bu  yöntem  ilk  olarak  Julier tarafından  [7]  no’lu  makalede  ortaya koyulmuştur,  daha  sonra  ise    Merwe tarafından  [16]  no’lu  makalede  daha  detaylı olarak  incelenmiştir.  Bir  sonraki  aşamada  ise gürültü  değerlerini  de  içeren  bu  takviyeli durumdan (xa)   sigma noktası örnekleri seçilir. Bu  işlemin  net  sonucu  ;  süreç  ve  ölçüm gürültülerinin  doğrusal  olmayan  etkilerinin durumun  geri  kalanı  ile  aynı  doğrulukla  elde edilmesi , ve dolayısıyla  aditif olmayan gürültü kaynakları için doğruluğu artırılmasıdır. 

3.4.4  Filtre Formülasyonu 

Filtre,    durum  vektörünün  L  boyutuna  kadar genişletilmesi,  takviye  edilmesi    ile  başlar. Burada  L  orijinal  durum  vektörü,  model gürültüsü  ve  ölçüm  gürültü  boyutlarının toplamıdır.  Benzer  şekilde  kovaryans  matrisi  de  L2  boyutunda  bir  matris  olacak  şekilde 

genişletilir.  Bu  biçimde  takviye  edilen  durum tahmin  vektörü  xa  ve  kovaryans matrisi Pa  şu şekilde ifade edilir : 

 

Bir  sonraki  aşama  genişletilen  (ekli) durumun ortalama  ve  kovaryansını  yakalayan  2L+1 sigma noktasının oluşturulmasını içermektedir. χa matrisi  bu  noktaları  içerecek  şekilde  seçilir ve sutünları şu şekilde hesaplanır : 

 

i alt simgesi kovaryans matrisinin karekökünün 4i’nci  sutünuna  karşılık  gelmektedir.    0<α≤1 aralığında  olan  α  parametresi  sigma  noktası yayılımını belirler. Bu parametre lokal olmayan etkilerden  sakınmak  için  genelde  çok  küçük seçilir ve tipik değeri 0.001 civarındadır.  

Elde  edilen    matrisi  bu  halde  durumu 

içeren  ,  örneklenmiş  süreç  gürültüsünü 

içeren   ve örneklenmiş ölçüm gürültüsünü 

içeren   sütunlarına  ayrıştırılabilir.  

ak 1−χ

                   

xk 1−χ

wk 1−χvk 1−χ

                                        

Her bir  sigma noktası bir bağıl değere  atanır. Bu  bağıl  değerler,    sigma  noktaları momentlerinin  Gauss  dağılımı  varsayımı altında  modellerin  Taylor  serisi  açılımı    ile karşılaştırılarak  [9]  no’lu  makalede  de belirtildiği  gibi  elde  edilir.  Elde  edilen  bağıl değerler,  ortalama  (m)  ve  kovaryans  (c) tahminlerinde şu şekilde kullanılır :  

  

4 Simetrik bir matrisin karekökü  tipik olarak düşük olan üçgen Cholesky dağılımı vasıtası ile hesaplanır. P  matrisinin  karekök  matrisi  A    ise  P=AAT 

formundadır. 

Daha sonra  filtre, sigma noktalarını durum ve ölçüm  (gözlem) modelleri üzerinde  ilerleterek ve elde edilen  sonuçların ağırlıklı ortalama ve kovaryans  matrislerini  hesaplayarak  bir sonraki durumu tahmin eder :  

 

Tahminler    gelen  yeni ölçümlerle  güncellenir. Güncelleme  işleminde  ilk  olarak  ölçüm kovaryans ve durum‐ölçüm çapraz korelasyon matrisleri hesaplanır daha  sonra bu matrisler Kalman kazancını belirlemede kullanılır :  

 

Deneysel  sonuçlar  Unscented  Kalman filtrelerinin durum modelinin üçüncü derecede Taylor  serisi açılımına yakın sonuçlar  verdiğini ;buna karşın,  genişletilmiş Kalman filtrelerinin sadece  birinci  derece  doğrusallaştırma seviyesinde  bir  doğruluğa  sahip  olduğunu göstermektedir[16].    [14]  no’lu makalede  bu iki  tip  filtrenin  doğruluk  performansları karşılaştırılmıştır.  

Unscented  Kalman  filtresinde  hesaplama açısından en çok dikkat gerektiren kısım sigma noktalarının hesaplamasında kullanılan matris karekök  alma  kısmıdır.  Kovaryans  matrisinin Cholesky  çarpanlarına  ayrılması  ya  da matris köşegenleştirme  işleminin  yapılması  bu 

problemin  çözümünde    kullanılabilir  ;  fakat, sadece  kovaryans  matrisi  kareköklerinin yayılımını  kapsayan  doğrudan  karekök yaklaşımı hesaplama  açısından daha  etkin bir çözüm  sunar.  [15] no’lu makalede Merwe bu şekilde bir yaklaşım ortaya koymaktadır. 

 

3.5 Diğer Yaklaşımlar 

Merkezi Fark Kalman Filtresi (MFKF),  [4]  no’lu makalede  Gauss  dağılıma  sahip  olasılık yoğunluk  fonksiyonlarının  doğrusal  olmayan fonksiyonlar  üzerinde  ilerletilmesinde kullanılabilecek  alternatif  bir  Kalman formülasyonu  olarak  önerilmiştir.    [13]  no’lu makalede bu  formülasyonun  esasen  farklı bir yapısı olmasına rağmen, yapılan testlerde elde edilen  sonuçların  UKF  sonuçları  ile  ayırt edilemez  bir  şekilde  benzerlik  taşıdığı  ifade edilmektedir. Bu yüzden Merkezi Fark Kalman Filtresi  (MFKF)  bu  makalede  kapsam  dışı tutulmuştur. 

Tanecik  (Partikül)  filtreleri  [1]:  Çeşitli  türldeki Kalman  filtreleri  çoğunlukla  yüksek doğruluğa sahip  kestirimler  sağlamasına  karşın  bazı kestirim  problemlerine  uygun  olmadığı durumlar  da  olabilir.  Bu  durum,    Kalman filtrelerinin  sadece  Gauss  olasılıklarını modelleyebilmesinden  ve dolayısıyla çarpık ya da  farklı  formlardaki  dağılımlar  için  uygun olmamasından kaynaklanır.   Bu yüzden Gauss olmayan  dağılımları  da  kapsamak  için  daha genel  bir  yaklaşıma  ihtiyaç  vardır.  Önem örneklemesini  kullanan  ardışık  Monte‐Carlo simulasyonlarına  dayalı    tanecik  (partikül) filtreleri  bu  durumlarda  sıklıkla  kullanılan  iyi bir alternatif yaklaşımdır. Bu makalede sürecin olasılık yoğunluk fonksiyonun tamamına değil,   sadece  en  olası  durumunun  kestirimine odaklanılmıştır  ; bu nedenle  tanecik  (partikül) filtresi  yaklaşımının  burada  ele  alınmasına gerek  yoktur.    Ayrıca,  tanecik  (partikül) filtreleri  Kalman  filtrelerine  göre  çoğunlukla daha  karmaşık  hesaplamalar  içermesinden dolayı  bu  tip  filtrelerin  gerçek  zamanlı düzeneklerde kullanılması oldukça güçtür. 

 

 

4 Deneyler 

Bu kısımda ele alınan deneyler, doğrusal süreç modellerine  ve  doğrusal  olmayan  ölçüm modellerine  sahip  pratik  takip  uygulamaları için GKF  ve UKF  performanslarının  arasındaki farklılıkları ortaya çıkarmayı amaçlamaktadır. 

 

Bu  modelde  radarın  (0,0)  koornidat noktasında  konuşlandığı  ve  n1,  n2  ölçüm gürültüleri  varyanslarının  sırasıyla  200  ve 0.003 olduğu varsayılmıştır. 4.1 Süreç Modeli 

Doğrusal  olarak  modellenmiş  ve  beyaz gürültülü  ivmeye  sahip   bir uçak   bu  kısımda ele alınacak deneyin temelini oluşturmaktadır. Doğrusal  model,  konum  ve  hız  için  ikili Wiener5 süreci biçiminde oluşturulmuştur. 

xxyyxx avvpvp === ,, &&  ve   modeli 

aşağıdaki  Ts=1  zaman  aralıklı  kesikli  zaman süreç modelini ortaya çıkarır : 

xy av =&

 

 

Şekil 7: Radarla uçak takibi 

 

 4.3 Üçgenleme Metodu ile Takip6 

Üçgenleme ya da nirengi metodu  ile  takip;  iki gözlem evi tarafından hedefin kendilerine olan uzaklıkları  (d1  ve  d2)  gözlemleyen  doğrusal olmayan bir ölçüm modeli ile modellenebilir : 

Buradaki ax ve ay  ivmeleri 0.5 varyanslı  ilşkisiz beyaz gürültülü olarak modellenmiştir.  

Sürecin,  aşağıdaki  durumda  başladığı varsayılmıştır: 

 

Kestirim  doğruluğu  için  karesel  hata  metriği ise;  

   

olarak tanımlanmıştır. 

 

4.2 Radarla Takip 

Radarla  takip  ya  da  radarla  izleme  hedefin uzaklığını  (d)  ve  açısını  (θ)  gözlemleyen doğrusal olmayan bir ölçüm/gözlem modeli ile modellenebilir: 

                                                                                                                       5 Wiener süreçleri beyaz gürültü ile entegre 

süreçlerdir. 

 

Bu modelde gözlem evlerinin sırasıyla (‐300,0) ve (300,0) koornidat noktalarında konuşlandığı ve  n1,  n2 ölçüm  gürültüleri  varyanslarının her ikisinin de 200 olduğu varsayılmıştır. 

 

Şekil 8: Üçgenleme metodu ile uçak takibi 

 6 ing.: Tracking by Triangulation 

Bu  tarz  bir  yapılanış,  belirli  bir  konumdan gelen  ölçümlerin  tamamının  her  seferinde       x  ekseni  üzerinde  aynı  konuma  karşılık geleceğinden  bir  belirsizlik  problemi  ortaya çıkarır.  Fakat  süreç  model  tahmininin  bu belirsizlik problemini çözmeye yardımcı olması yüksek  ihtimalle  bu  durumu  büyük  bir  sorun olmaktan çıkaracaktır. 

 

Şekil 9: Hedefin izlediği rota için UKF kullanılarak yapılan deneylerde elde edilen gözlemler, gerçek rota ve kestirilen rota 

 

4.4 Sonuçlar 

İkili  simulasyon/kestirim  deneyi  10000  kere gerçekleştirilmiştir.  Her  seferinde  simule edilen uçağın  izlediği rota 80 zaman adımında hem  GKF  hem  de  UKF  gözlem  modelleri kullanılarak  kestirilmiştir.  Kestirim  doğruluğu sonuçları  Tablo  1’de  ,  doğruluk  dağılım grafikleri ise Şekil 10’da gösterilmiştir. 

 

Model  GKF Ortalama Karesel Hata 

(OKH) 

UKF Ortalama Karesel Hata 

(OKH) Radar  174.4 (5.00)  116.9 (0.363) 

Üçgenleme  185.2 (3.15)  183.1 (2.81)  

Tablo 1:  Kestirimler için ortalama karesel hata değerleri (1000 simulasyon sonrası doygunluğa ulaşan hata değerleri).  Doğruluk varyansları da parantez içinde verilmiştir. 

 

Şekil 10: Kestirim Doğruluk Dağılımları 

 

Ortalama Karesel Hata (OKH) kestirim varyansı  merkezi  limit  teoreminin  geçerliliği  varsayımı altında  deneysel    hata  dağılımı  kullanılarak hesaplanmıştır: 

 

VAR(OKH) = VAR(hata) / N  

 

Yüksek  derecede  doğrusal  olmayan  arkus‐tanjant  içeren  ölçümlere  sahip  radar  modeli sadece Pythagoras ölçümlere sahip üçgenleme modeline  göre  GKF  ve  UKF  kestirim doğrulukları  arasında  daha  büyük  farklılık göstermektedir.    Bu  durum,  her  iki modelde de  hatanın  1000’in  üzerinde  olduğu  birkaç kestirim  için  UKF’nin  daha  gürbüz  bir  yapı sergilediğini gösterir. 

 

 

 

 

 

 

5 Sonuç 

Unscented  Kalman  filtresinin  (UKF) doğruluğunu  doğrusal    olmayan  süreç modelleri için araştıran bir kaç makale [16],[5], olmasına  karşın,  bunların  hiç  biri  özellikle doğrusal  olmayan  ölçüm  modellerinin doğruluğuna hitap etmemektedir. 

Bu  nedenle  bu  makalede  doğrusal  olmayan ölçümlere  sahip  doğrusal  durum  uzay modelleri  için  UKF  kestirim  doğruluk performansı    GKF’ye  göre  göreceli  olarak karşılaştırılmıştır.  Deneysel  sonuçlar  radar  ile takip modeli  için  UKF  ve  GKF  performansları arasında  ciddi  bir  fark  olduğunu,  buna  karşın üçgenleme  metodu  ile  takip  modeli  için önemli bir fark olmadığını göstermektedir.  Bu durum  iki  model  arasındaki  doğrusallık derecesi  farkından  kaynaklanmaktadır; üçgenleme  metodu  ile  takip  modeli  radar modeline  göre  daha  doğrusal  bir  yapıya sahiptir.  Bu  yüzden,  doğrusallık  derecesi oldukça  düşük  doğrusal  olmayan  ölçüm modelleri  için    UKF  kullanımını  GKF’ye  göre daha  avantajlıdır.  Bu  çıkarım  [16]  no’lu makalede  sunulan  UKF  kullanımı  ile  ilgili argümanlarla örtüşmektedir. 

Kestirim  doğruluk  dağılım  grafikleri  ,  iki kestiricinin  her  iki  model  için  de  büyük hatalara  sahip  kestirimler  hariç  benzer sonuçlar  verdiğini  ortaya  koymaktadır.  Buna göre  UKF’nin  GKF’ye  göre  daha  gürbüz7 (sağlam) bir kestirici olduğu söylenebilir. 

 

Kaynakça 

[1] S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp. A  tutorial  on  particle  filters  for  on‐line  non‐linear/non‐gaussian bayesian  tracking.  IEEE  Transactions  on  Signal Processing, 50(2):174–188, February 2002.  [2]  Robert  Grover  Brown  and  Patrick  Y.  C.  Hwang. Introduction  to  Random  Signals  and  Applied  Kalman Filtering, 3rd Edition. Prentice Hall,1996.  [3]  Chi‐Tsong  Chen.  Linear  System  Theory  and  Design, third edition. Oxford University Press,1999.  

                                                            7 ing.: robust 

[4]  K.  Ito  and  K.  Xiong.  Gaussian  filters  for  nonlinear filtering  problems.  Kazufumi  Ito  and  Kaiqi  Xiong, Gaussian  Filters  for  Nonlinear  Filtering  Problems,  IEEE Transactions  on  Automatic  Control,  vol.  45,  no.  5,  pp. 910–927, May 2000.  [5]  Joseph  J. and LaViola  Jr. A comparison of unscented and extended kalman filtering for estimating quaternion motion.  Proceedings  of  the  2004  American  Control Conference, IEEE Press,2190‐2195, June 2004.  [6]  Simon  Julier.  The  scaled  unscented  transform. Proceedings  of  the  2002  American  Control Conference,IEEE Press, May 2002.  [7] Simon Julier and Jeffrey Uhlmann. A general method for  approximating  nonlinear  transformations  of probability distributions. University of Oxford, November 1996.  [8] Simon Julier and Jeffrey Uhlmann. A new extension of the  kalman  filter  to  nonlinear  systems.  Int.  Symp. Aerospace/Defense  Sensing,  Simul.  And  Controls, Orlando, FL, 1997.  [9] Simon Julier and Jeffrey Uhlmann. Unscented filtering and  nonlinear  estimation.  Proceedings  of  the  IEEE, March 2004.  [10]  Simon  Julier,  Jeffrey Uhlmann,  and Hugh Durrant‐Whyte. A new approach  for  filtering nonlinear systems. Proceedings  of  the  1995  American  Control  Conference, IEEE Press, June 1995.  [11]  Ben  Quine,  Jeffrey  Uhlmann,  and  Hugh  Durrant‐Whyte.  Implicit  jacobians  for  linearised state estimation in nonlinear systems. Proceedings of the 1995 American Control Conference, IEEE Press, June 1995.  [12]  Charles W.  Therrien. Discrete Random  Signals  and Statistical Signal Processing. Prentice Hall, 1992.  [13] Rudolph van der Merwe. Sigma‐point kalman filters for probabilistic inference in dynamic statespace models. Workshop on Advances  in Machine Learning, Montreal., June 2003.  [14] Rudolph van der Merwe and Eric Wan. Sigmapoint kalman filters for integrated navigation.  [15] Rudolph van der Merwe and Eric Wan. The square‐root  unscented  kalman  filter  for  state  and  parameter‐estimation. Proceedings of  the  International Conference on Acoustics, Speech, and Signal Processing (ICASSP), Salt Lake City, Utah, May 2001.  [16]  Eric  Wan  and  Rudolph  van  der  Merwe.  The unscented kalman filter for nonlinear estimation.Proc. of IEEE  Symposium  2000  (AS‐SPCC),  Lake  Louise,  Alberta, Canada, October 2000. 

 

Comparison of Kalman Filter Estimation Approaches forState Space Models with Nonlinear Measurements

Fredrik OrderudSem Sælands vei 7-9, NO-7491 Trondheim

Abstract

The Extended Kalman Filter (EKF) has long been thede-facto standard for nonlinear state space estimation[11], primarily due to its simplicity, robustness andsuitability for realtime implementations. However, analternative approach has emerged over the last fewyears, namely the unscented Kalman filter (UKF). Thisfilter claims both higher accuracy and robustness fornonlinear models. Several papers have investigated theaccuracy of UKF for nonlinear process models, butnone has addresses the accuracy for nonlinear mea-surement models in particular. This paper claims tobridge this gap by comparing the performance of EKFto UKF for two tracking models having nonlinear mea-surements.

1 Introduction

The problem ofstate estimationconcerns the task ofestimating the state of a process while only havingaccess to noisy and/or inaccurate measurements fromthat process. It is a very ubiquitous problem setting,encountered in almost every discipline within scienceand engineering.

The most commonly used type of state estimator is theKalman filter. It is an optimal estimator for linear sys-tems, but unfortunately very few systems in real worldare linear. A common approach to overcome this prob-lem is to linearize the system first before using theKalman filter, resulting in an extended Kalman filter.This linearization does however pose some problems,e.g. it can result in nonstable estimates [8]. The de-velopment of better estimator algorithms for nonlin-ear systems has therefore attracted a great deal of in-terest in the scientific community, because improve-ments here will undoubtedly have great impact in awide range of engineering fields.

Notation

Algebraic letters are distinguished based on their ap-pearance: Normal (a) denotes scalars, bold (a) denotesvectors and uppercase (A) denotes matrices. Sub-scripts (xa) denote discrete time. Conditional sub-scripts (xa|b) denote statex in time a, given measure-ments up to timeb. A ‘hat’ superscript ( ˆa) denotes anestimated value, known only with a certain belief.

2 Background

A state space model is a mathematical model of aprocess, where the process’statex is represented bya numerical vector. State-space models actually con-sists of two separate models: theprocess model, whichdescribes how the state propagates in time based on ex-ternal influences, such as input and noise; and themea-surement model, which describe how measurementszare taken from the process, typically simulating noisyand/or inaccurate measurements.

2.1 General State Space Model

The most general form of state-space models is thenonlinear model.This model does typically consist of two functions,fandh:

xk+1 = f (xk,uk,wk)zk = h(xk,vk)

which govern state propagation and measurements, re-spectively.u is process input, andw andv are state andmeasurement noise vectors, respectively whilek is thediscrete time.State-space models are remarkably usable for mod-elling almost all sorts of processes.f andh are usuallybased upon a set of discretized differential equations,governing the dynamics of and observations from theprocess.

w

xz -1

zh(x,v)

v

f(x,u,w)

k

k

k

k

k

x

x

k+1u k

Figure 1:A general state-space model. z−1 is the unitdelay function known from the Z-transform in digitalsignal processing.

2.2 Linear State Space Model

A linear state-space model is a model where the func-tions f andh are linear in both state and input. Thefunctions can then be expressed by using the matricesF , B and H, reducing state propagation calculationsto linear algebra. Overall this results in the followingstate-space model:

xk+1 = Fkxk +Bkuk +wk

zk = Hkxk +vk

u xz -1

z+ +H

v

F

k k

k

kx k+1

k

k

kw

Bk

Figure 2:A linear state-space model

This linear model is easier both to calculate and an-alyze. Enabling modellers to investigate propertiessuch as controllability, observability and frequency re-sponse [3].Linear state models are either based on inherently lin-ear processes, or simply a linearized versions of a non-linear process by means of a first order Taylor approx-imation.

3 State Estimation

State estimation concerns the problem of estimatingthe probability density function (pdf) for the state ofa process which is not directly observable. This in-volves both predicting the next state based on the cur-rent, and updating/correcting this prediction based onnoisy measurements taken.

3.1 Recursive Bayesian Estimation

The most general form for state estimation is knownas recursive Bayesian estimation [1]. This is the op-

timal way of predicting a state pdf for any process,given a system and a measurement model. In this sec-tion we will discuss this estimator, which recursivelycalculates a new estimate for each time-step, based onthe estimate for the previous timestep and new mea-surements.Recursive Bayesian estimation works by simulatingthe process, while at the same time adjusting it to ac-count for new measurementsz, taken from the realprocess. The calculations are performed recursivelyin a two step procedure. First, the next state is pre-dicted, by extrapolating the current state onto nexttime step using state propagation beliefp(xk|xk−1) ob-tained from functionf . Secondly, this prediction iscorrected using measurement likelihoodp(zk|xk) ob-tained from functionh, taking new measurements intoaccount.The Chapman-Kolmogorov equation is first used tocalculate a prior pdf for statexk, based on measure-ments up to timek−1:

p(xk|zk−1) =Z

p(xk|xk−1)p(xk−1|zk−1)dxk−1

Bayes rule is then used to calculate the updated pdffor statexk, after taking measurements up to timekinto account:

p(xk|zk) =p(zk|xk)p(xk|zk−1)R

p(zk|xk)p(xk|zk−1)dxk

p(x |z ) zk

updatepredict

kk-1

p(x |z )k k

Figure 3:Recursive Bayesian estimator loop

Unfortunately, this method does not scale very wellin practice, mainly due to the large state space formultidimensional state vectors. Calculating the priorprobability of each point in this state space involvesa multidimensional integral, which quickly becomesintractable as the state space grows. Computers arealso limited to calculation of the pdf in discrete pointin state space, requiring a discretization of the statespace. This technique is therefore mainly consideredas a theoretic foundation for state estimation in gen-eral. Bayesian estimation by means of computersis only possible if either the state space can be dis-cretized, or if certain limitations apply for the model.

3.2 Kalman Filter

The problem of state estimation can be made tractableif we put certain constrains on the process model, byrequiring bothf andh to be linear functions, and thenoise termsw andv to be uncorrelated, Gaussian andwhite with zero mean. Put in mathematical notation,we then have the following constraints:

f (xk,uk,wk) = Fkxk +Bkuk +wk

h(xk,vk) = Hxk +vk

wk ∼ N(0,Qk) vk ∼ N(0,Rk)

E(wiwTj ) = Qiδ(i− j) E(vivT

j ) = Riδ(i− j)

E(wkvTk ) = 0

whereQ andRare covariance matrices, describing thesecond-order properties of the state- and measurementnoise. The constraints described above reduces thestate model to:

xk+1 = Fkxk +Bkuk +wk

zk = Hkxk +vk

whereF , B andH are matrices, possible time depen-dent.As the model is linear and input is Gaussian, we knowthat the state and output will also be Gaussian [12].The state and output pdf will therefore always be nor-mally distributed, where mean and covariance are suf-ficient statistics. This implies that it is not necessaryto calculate a full state pdf any more, a mean vectorxand covariance matrixP for the state will suffice.

x zPk|k-1

updatepredict

x k|k P

kk|k-1

k|k

Figure 4:Kalman filter loop

The recursive Bayesian estimation technique is thenreduced to theKalman filter, wheref andh is replacedby the matricesF , B andH. The Kalman filter is, justas the Bayesian estimator, decomposed into two steps:predict and update. The actual calculations requiredare:Predict next state, before measurements are taken:

xk|k−1 = Fkxk−1|k−1 +Bkuk

Pk|k−1 = FkPk−1|k−1FTk +Qk

Update state, after measurements are taken:

Kk = Pk|k−1HTk (HkPk|k−1HT

k +Rk)−1

xk|k = xk|k−1 +Kk(zk−Hkxk|k−1)

Pk|k = (I −KkHk)Pk|k−1

whereK is the Kalman gain matrix, used in the up-date observer, andP is the covariance matrix for thestate estimate, containing information about the accu-racy of the estimate. More details and background forthis filter can be found in [2].The Kalman filter is quite easy to calculate, due to thefact that it is mostly linear, except for a matrix inver-sion. It can also be proved that the Kalman filter is anoptimal estimator of process state, given a quadraticerror metric [2].

3.3 Extended Kalman Filter

Most processes in real life are unfortunately not linear,and therefore needs to be linearized before they can beestimated by means of a Kalman filter. The extendedKalman filter (EKF) solves this problem by calculatingthe Jacobian1 of f andh around the estimated state,which in turn yields a trajectory of the model functioncentered around this state.

x

y y=f(x)

Figure 5:Illustration of how the Extended Kalman fil-ter linearizes a nonlinear function around the mean ofa Gaussian distribution, and thereafter propagates themean and covariance through this linearized model

Fk =∂ f (x,u,w)

∂x

∣∣∣∣xk|k,uk,0

Hk =∂h(x,v)

∂x

∣∣∣∣xk|k−1,0

1The Jacobian is the matrix of all partial derivatives of a vector

The extended Kalman filter works almost like a regularKalman filter, except forF andH, which vary in timebased on the estimated statex. The actual calculationsrequired are:Predict next state, before measurements are taken:

xk|k−1 = f (xk−1|k−1,uk,0)

Pk|k−1 = FkPk−1|k−1FTk +Qk

Update state, after measurements are taken:

Kk = Pk|k−1HTk (HkPk|k−1HT

k +Rk)−1

xk|k = xk|k−1 +Kk(zk−h(xk|k−1,0))

Pk|k = (I −KkHk)Pk|k−1

whereK is the Kalman gain matrix, used in the updateobserver, andP is the covariance matrix for the stateestimate, containing information about the accuracy ofthe estimate.

3.4 Unscented Kalman Filter

3.4.1 Introduction

The problem of propagating Gaussian random vari-ables through a nonlinear function can also be ap-proached using another technique, namely theun-scented transform. Instead of linearizing the func-tions, this transform uses a set of points, and propa-gates them through the actual nonlinear function, elim-inating linearization altogether. The points are chosedsuch that their mean, covariance, and possibly alsohigher order moments, match the Gaussian randomvariable. Mean and covariance can be recalculatedfrom the propagated points, yielding more accurate re-sults compared to ordinary function linearizaton. Theunderlying idea is also to approximate the probabil-ity distribution instead of the function. This strategytypically does both decrease the computational com-plexity, while at the same time increasing estimate ac-curacy, yielding faster, more accurate results.

3.4.2 Background

The underlying method of unscented transform wasfirst proposed by Uhlmann et al. in [11] and [10],where they laid out the framework for representinga Gaussian random variable inN dimensions using2N + 1 samples, calledsigma points. They utilizedthe matrix square root and covariance definitions to se-lect these points in such a way that they had the same

covariance as the Gaussian they approximated. Skew-ness was avoided by selecting the points in a symmet-ric way, such that any approximation error would onlyoriginate from the fourth and higher moments.Usage of the unscented transform in Kalman filter-ing was then presented by Julier in [8], where he in-troduced theUnscented Kalman filter(UKF), whichapproximates the state estimate using sigma points.Later, it was analyzed more in depth in [16].A limitation associated with the unscented Kalman fil-ter is that it has a lower bound on thesafe spreadofthe sigma points, meaning the distance between thepoints in state space. Sigma point spreads below thisbond are not guaranteed to yield positive semidefinitecorrelation matrices. This distance also increases withthe dimension of the state space, a limitation that maycause problems in highly nonlinear models, since highsigma point spread may result in sampling of non-localfeatures.The technique presented here is therefore based on thescaled unscented transform[6], which provides an ad-ditional tuning parmeter,α, compared to the originalunscented transform. This parameter is used to arbi-trary control the spread of the sigma-points, while atthe same time guaranteeing positive semidefinite co-variance matrices. Even models of high dimensonalitycan then keep a tight sigma point spread to avoid non-local effects.

3.4.3 Augmented state

The unscented transform approach also has anotheradvantage, namely that noise can be treated in a non-linear fashion to account for non-Gaussian or non-additive noises. The strategy for doing so involvespropagation of noise through the functions by first aug-menting the state vector to also include noise sources,a technique first introduced by Julier in [7], and laterrefined more in depth by Merwe in [16]. Sigma pointsamples are then selected from the augmented state,xa, which also includes noise values. The net resultis that any nonlinear effects of process and measure-ment noise are captured with the same accuracy as therest of the state, which in turn increases accuracy fornon-additive noise sources.

3.4.4 Filter Formulation

The filter starts by augmenting the state vector toLdimensions, whereL is the sum of dimensions in theoriginal state-vector, model noise and measurementnoise. The covariance matrix is similarly augmented

x

y y=f(x)

Figure 6: Illustration of how the unscented Kalmanfilter propagates sigma-points from a Gaussian distri-bution through a nonlinear function, and recreates aGaussian distribution, by calculating the mean and co-variance of the results

to aL2 matrix. Together this forms the augmented stateestimate vectorxa and covariance matrixPa:

xak−1 =

xk−1

0w

0v

Pa

k−1 = E{(xak−1− xa

k−1)(xak−1− xa

k−1)T}

=

Pk−1 0 00 Qk−1 00 0 Rk−1

The next step consists of creating 2L+1 sigma-pointsin such a way that they together captures the full meanand covariance of the augmented state. Theχa matrixis chosen to contain these points, and its columns arecalculated as follows:

χa0,k−1 = xa

k−1 i = 0

χai,k−1 = xa

k−1 +(α√

LPak−1)i , i = 1...,L

χai,k−1 = xa

k−1− (α√

LPak−1)i−L, i = L+1...,2L

where subscripti means thei-th column of the squareroot of the covariance matrix2. The α parameter, inthe interval 0< α≤ 1, determines sigma-point spread.This parameter is typically quite low, often around0.001, to avoid non-local effects.The resultingχa

k−1 matrix can now be decomposedvertically into theχx

k−1 rows, which contains the state;the χw

k−1 rows, which contains sampled process noise

2The square root of a symmetric matrix is typically calculatedby means of a lower triangular Cholesky decomposition. Thesquare rootA of matrixP is then on the formP = AAT .

and theχvk−1 rows, which contains sampled measure-

ment noise.Each sigma-point is also assigned a weight. Theseweight are derived by comparing the moments of thesigma-points with a Taylor series expansion of themodels while assuming a Gaussian distribution, as de-rived in [9]. The resulting weights for mean (m) andcovariance (c) estimates then becomes:

w(m)0 = 1− 1

α2 i = 0

w(c)0 = 4− 1

α2 −α2 i = 0

w(m)i = w(c)

i =1

2α2Li = 1...,2L

The filter then predicts next state by propagating thesigma-points through the state and measurement mod-els, and then calculating weighted averages and covari-ance matrices of the results:

χxk|k−1 = f (χx

k−1,uk,χwk−1)

xk|k−1 =2L

∑i=0

w(m)i χx

k|k−1

Pk|k−1 =2L

∑i=0

w(c)i [χx

k|k−1− xk|k−1][χxk|k−1− xk|k−1]T

Zk|k−1 = h(χxk|k−1,χ

vk−1)

zk|k−1 =2L

∑i=0

w(m)i Zi,k|k−1

The predictions are then updated with new measure-ments by first calculating the measurement covari-ance and state-measurement cross correlation matri-ces, which are then used to determine the Kalman gain:

Pzz=2L

∑i=0

w(c)i [Zi,k|k−1− zk|k−1][Zi,k|k−1− zk|k−1]T

Pxz =2L

∑i=0

w(c)i [χx

i,k|k−1− xk|k−1][Zi,k|k−1− zk|k−1]T

Kk = PxzP−1zz

xk|k = xk|k−1 +Kk(zk− zk|k−1)

Pk|k = Pk|k−1−KkPyyKTk

Experimental results indicate [16] that UnscentedKalman filters yield results comparable to a third or-der Taylor series expansion of the state-model, whileExtended Kalman filters of course only are accurate toa first order linearization. Consult [14] for a compari-son of accuracy between the two kinds of filters.

The most computationally demanding part of the Un-scented Kalman filter is the matrix square-root usedto calculate sigma points. Matrix diagonalization orCholesky factorization of the covariance matrix can beused to solve this problem, but a more direct squareroot approach, propagating only the square-roots ofthe covariance matrices, offers higher computationallyefficiency. Merwe et al. proposes a approach for doingthis in [15].

3.5 Other Approaches

Central difference Kalman filter(CDKF) proposed in[4] offers an alternative Kalman formulation for prop-agating Gaussianpdfs through nonlinear functions.This formulation, although it is different, remains verymuch like UKF and is reported to perform indistin-guishable from UKF in all tests performed [13]. TheCDKF is therefore not addressed by this paper.Particle filters[1]: While the various types of Kalmanfilters often offers superb estimation accuracy, thereare situations where they are not suited for the task.This problem relates to the fact that all Kalman fil-ters are constrained to only model Gaussian probabil-ities, and are therefore incapable of handling skewedor multimodal distributions. A more general ap-proach is therefore needed when trying to estimatenon-Gaussian distributions. Particle filters, which arebased on sequential Monte-Carlo simulations usingimportance sampling, can then often be a good alter-native. Particle filters are not addressed here, sincethis paper focuses on estimating only the most proba-ble state of a process, and not the entire process pdf,thus rendering particle filters superfluous. Particle fil-ters are also much more computationally demandingthan Kalman filters, often making them intractable forusage in real-time settings.

4 Experiments

The experiments described here aims at determiningwhether there are any difference between EKF andUKF for practical tracking applications, having linearprocess models and nonlinear measurement models.

4.1 Process Model

The basis for the experiment is an aeroplane, modelledlinearly as a dual Wiener process3 for position and ve-locity respectively, driven by white noise acceleration.

3Wiener processes are integrated white noise

The model ˙px = vx, py = vy, vx = ax andvy = ay yieldsthe following discrete time process model when as-suming zero order hold with timestepTs = 1.

px

py

vx

vy

k+1

=

1 0 1 00 1 0 10 0 1 00 0 0 1

px

py

vx

vy

k

+

00ax

ay

k

where the accelerationsax anday are modelled as un-correlated white noise with a variance of 0.5.The process is assumed to start in the following state:

x0 = [−200 200 4 0]T

with the squared error metric(x− x)T(x− x) for esti-mation accuracy.

4.2 Tracking by Radar

Radar tracking can be modelled with a measurementmodel observing distance and angle to the target:[

dΘ

]=

[ √p2

x + p2y

atan(py/px)

]+

[n1

n2

]where the measurement model is clearly nonlinear.The radar is assumed to be positioned in the coordi-nates(0,0) with measurement noisesn1 andn1, havinga variance of 200 and 0.003, respectively.

d

Figure 7: Tracking of plane motion by means of aradar

4.3 Tracking by Triangulation

Tracking by triangulation can similarly be modelledwith a measurement model observing distances to thetarget from two observators:[

d1

d2

]=

[ √(px− p1x)2 +(py− p1y)2√(px− p2x)2 +(py− p2y)2

]+

[n1

n2

]where the measurement model is also clearly nonlin-ear. The observators are assumed to be positioned in

d1 d2

Figure 8:Tracking of plane motion by means of trian-gulation

the coordinates(−300,0) and(300,0), with measure-ment noisesn1 andn2, both having a variance of 200.This configuration also poses an ambiguity problem,since measurements coming from a given position willbe equal to the same position flipped about the x-axis.This will probably not be a big problem, since theprocess model prediction will help resolving this am-biguity.

−200 −150 −100 −50 0 50 100

150

200

250

300

350

Observed, true and estimated trajectory

observationstrue trajectoryestimated trajectory

Figure 9: Example illustration of observations, truetrajectory and estimated trajectory for the experimentsusing the UKF

4.4 Results

A dual simulation/estimation experiment was run10000 times. Each time, a simulated plane trajectorywere estimated over 80 time steps, by both EKF andUKF for both of the observation models. The estima-tion accuracy results are shown in the table below, withaccuracy distribution plots in the accompanying figure.The MSE estimate variance is calculated from the em-pirical error distribution, using

VAR(MSE) = VAR(error)/N

Model EKF MSE UKF MSERadar 174.4 (5.00) 116.9 (0.363)

Triangulation 185.2 (3.15) 183.1 (2.81)

Table 1: Mean squared error (MSE) for the estima-tions, saturating errors above 1000. Accuracy vari-ance is given in parenthesis.

101

102

103

0

500

1000Error distribution for radar tracking

ekfukf

101

102

103

0

500

1000Error distribution for triangulation tracking

ekfukf

Figure 10:Estimation accuracy distributions

assuming validity of the central limit theorem.The radar model, having measurements involving thehighly nonlinear arcus-tangent, shows a wider differ-ence in the estimation accuracy between EKF andUKF, compared to the triangulation model which onlyhas Pythagoras’ measurements, being significantlymore linear. It can further be seen that UKF seemsto show a higher degree of robustness, having fewerestimates with errors above 1000 for both of the mod-els.

5 Conclusion

Several papers have investigated the accuracy of UKFfor nonlinear process models [16], [5], but none hasaddresses the accuracy for nonlinear measurementmodels in particular.This paper did therefore compare the relative estima-tion accuracy of UKF compared to EKF for linear statespace models with nonlinear measurements. The em-pirical results shows a significant difference for theradar model, but not for the tracking model. This isbelieved to be caused by the difference in nonlinear-ity between the two models, having a highly nonlin-ear radar model and a relatively more linear trackingmodel. The relative advantage of using UKF doestherefore seem to increase with the degree of nonlin-earity in the measurement model. This finding is con-

sistent with the arguments for using the UKF presentedin [16].The estimation error distribution plots show that thetwo estimators yield quite similar results for bothmodels, with the most significant exception being theamount of estimates having severely large errors. Thisleads us to the conclusion of UKF being a more robustestimator than EKF.Statement of reproducibility: All program code re-quired to reproduce the results shown in this paper arefreely available upon request by contacting the author.

References

[1] S. Arulampalam, S. Maskell, N. Gordon, andT. Clapp. A tutorial on particle filters for on-linenon-linear/non-gaussian bayesian tracking.IEEETransactions on Signal Processing, 50(2):174–188, February 2002.

[2] Robert Grover Brown and Patrick Y. C. Hwang.Introduction to Random Signals and AppliedKalman Filtering, 3rd Edition. Prentice Hall,1996.

[3] Chi-Tsong Chen. Linear System Theory andDesign, third edition. Oxford University Press,1999.

[4] K. Ito and K. Xiong. Gaussian filters for nonlin-ear filtering problems.Kazufumi Ito and KaiqiXiong, Gaussian Filters for Nonlinear FilteringProblems, IEEE Transactions on Automatic Con-trol, vol. 45, no. 5, pp. 910–927, May 2000.

[5] Joseph J. and LaViola Jr. A comparison of un-scented and extended kalman filtering for esti-mating quaternion motion.Proceedings of the2004 American Control Conference, IEEE Press,2190-2195, June 2004.

[6] Simon Julier. The scaled unscented transform.Proceedings of the 2002 American Control Con-ference, IEEE Press, May 2002.

[7] Simon Julier and Jeffrey Uhlmann. A generalmethod for approximating nonlinear transforma-tions of probability distributions.University ofOxford, November 1996.

[8] Simon Julier and Jeffrey Uhlmann. A new exten-sion of the kalman filter to nonlinear systems.Int.Symp. Aerospace/Defense Sensing, Simul. andControls, Orlando, FL, 1997.

[9] Simon Julier and Jeffrey Uhlmann. Unscentedfiltering and nonlinear estimation.Proceedingsof the IEEE, March 2004.

[10] Simon Julier, Jeffrey Uhlmann, and HughDurrant-Whyte. A new approach for filteringnonlinear systems. Proceedings of the 1995American Control Conference, IEEE Press, June1995.

[11] Ben Quine, Jeffrey Uhlmann, and Hugh Durrant-Whyte. Implicit jacobians for linearised state es-timation in nonlinear systems.Proceedings of the1995 American Control Conference, IEEE Press,June 1995.

[12] Charles W. Therrien.Discrete Random Signalsand Statistical Signal Processing. Prentice Hall,1992.

[13] Rudolph van der Merwe. Sigma-point kalman fil-ters for probabilistic inference in dynamic state-space models.Workshop on Advances in Ma-chine Learning, Montreal., June 2003.

[14] Rudolph van der Merwe and Eric Wan. Sigma-point kalman filters for integrated navigation.

[15] Rudolph van der Merwe and Eric Wan. Thesquare-root unscented kalman filter for state andparameter-estimation. Proceedings of the In-ternational Conference on Acoustics, Speech,and Signal Processing (ICASSP), Salt Lake City,Utah, May 2001.

[16] Eric Wan and Rudolph van der Merwe. Theunscented kalman filter for nonlinear estima-tion. Proc. of IEEE Symposium 2000 (AS-SPCC),Lake Louise, Alberta, Canada, October 2000.