Hata Durumu Kalman Filtresine (ESKF) Giriş ve Temelleri

Bir drone havada süzülürken nereye baktığını, ne kadar hızlı gittiğini ve nerede olduğunu bilmek zorundadır. Bu derste bunu kestiren en yaygın yöntemlerden birini temelden kuracağız: hata durumu Kalman filtresi, kısaca ESKF. Önce varacağımız yeri gösterelim. Bütün ders tek bir fikrin etrafında döner: gerçek durumu iki parçaya ayırmak. Soldaki x t, gerçek durumdur: aracın o anki hali. Onu hiçbir zaman doğrudan bilemeyiz. Ortadaki x, nominal durumdur. Büyüktür, doğrusal değildir ve her sensör örneğiyle hızla ilerletilir. Ama içinde gürültü ya da belirsizlik yoktur. Sağdaki delta x ise hata durumudur: nominalin gerçekten ne kadar saptığı. Küçüktür, neredeyse doğrusaldır ve bütün belirsizliği o taşır. Bu ayrımın neden bu kadar işe yaradığını görmek için önce standart Kalman filtresine kısaca dönelim. Genişletilmiş Kalman filtresi, yani EKF, iki adımda çalışır. Tahmin adımında durumu bir model f ile ileri taşırız ve kovaryans P büyür. Güncelleme adımında bir ölçüm z gelir. Kalman kazancı K, ölçüme ne kadar güveneceğimizi söyler. Sonra kestirimi, ölçüm farkıyla orantılı bir miktar ekleyerek düzeltiriz ve belirsizlik küçülür. Dikkat edin, düzeltme bir toplamadır. Konum ya da hız için bu tamamen doğaldır. Peki ya yönelim için? Yönelimi birim kuaterniyon q ile tutalım. Birim kuaterniyonlar dört boyutlu uzayda eğri bir yüzey oluşturur. Burada onun iki boyutlu benzerini, birim çemberi çiziyoruz. Bu sarı nokta, aracın şu anki yönelimidir. Geçerli her yönelim tam olarak çemberin üzerinde durur; ne içinde, ne dışında. Şimdi EKF gibi davranıp düzeltmeyi doğrudan ekleyelim. Kırmızı ok düz bir çizgide ilerler ve ucu çemberin dışına düşer. Toplamın normu artık bir değildir. Bu nesne bir yönelim bile değildir; onu zorla yeniden normalleştirmek ancak bir yamadır. İkinci sorun sayılardadır. Kuaterniyonun dört sayısı vardır, ama yönelimin yalnızca üç serbestlik derecesi. Bu yüzden dörde dört kovaryans matrisi tekil hale gelir. Üç açıyla, yani Euler açılarıyla çalışsak bu kez başka bir duvara çarparız: doksan derecede tekillik, meşhur gimbal kilidi. Hata için yalnızca üç sayı kullanalım: delta teta. ESKF düzeltmeyi kuaterniyona eklemek yerine, bu üç sayıyı yönelimin hemen yanındaki düz bir uzayda, teğet doğruda tutar. Yeşil ok, teğet doğrultudaki küçük bir döndürmedir. Teğet doğru düz olduğu için orada toplama, çıkarma ve kovaryans tamamen sorunsuzdur. Ardından bu düzeltmeyi çembere sararız. Kuaterniyon çarpımı noktayı, okun uzunluğu kadar yay boyunca kaydırır ve nokta çemberin üzerinde kalır. Hata hep sıfırın yakınında tutulduğu için üç açı asla doksan dereceye yaklaşmaz ve tekillik hiç görünmez. Küçük açı varsayımının bize verdiği hediye budur.

Şimdi bu iki parçayı tek bir açı üzerinde izleyelim. Yatay eksen zaman, dikey eksen açıdır. Mavi eğri, aracın gerçek açısıdır. Sarı eğri nominal açıdır. Jiroskop ölçümlerini toplayarak ilerler, ama jiroskobun küçük bir sapması olduğu için gerçek açıdan yavaşça uzaklaşır. Aradaki fark, şu kırmızı çizgi, hata durumudur: delta teta. Gerçek açı, nominal açıya bu küçük farkın eklenmesiyle elde edilir. Eksendeki her çentik bir ölçüm anıdır. Ölçüm geldiğinde filtre farkı kestirir ve nominali gerçeğe geri iter; fark bu sıçramalarda sıfıra döner ve hiçbir zaman büyüyemez. Şimdi iki noktanın birlikte yürüyüşünü izleyelim. Aralarındaki boşluk büyür, ölçüm anında kapanır, sonra yeniden büyür. Ama hep küçük kalır. İşte ESKF'nin temel fikri: büyük ve doğrusal olmayan hareketi nominal taşır, filtre ise yalnızca sıfıra yakın küçük farkı kestirir. Küçük bir şeyi doğrusallaştırmak çok daha güvenlidir. Gerçek bir seyir sisteminde durum tek bir açıdan ibaret değildir. Nominal durum konum, hız, kuaterniyon ve iki sensör sapmasından oluşur: toplam on altı sayı. Hata durumu aynı parçaları taşır, tek bir farkla. Kuaterniyonun yerinde üç boyutlu delta teta durur. Bu yüzden toplam on beş sayıdır. Konum ve hız düz uzayda yaşar, bu yüzden onlarda birleştirme sıradan bir toplamadır. Yönelimde ise toplama yoktur. Gerçek kuaterniyon, nominal kuaterniyonun küçük bir delta q ile çarpılmasıdır. Sensör sapmaları da düz uzaydadır ve yine toplanır. Kısacası artı işareti çoğu yerde gerçekten artıdır; yalnızca yönelimde çarpıma dönüşür. Peki delta q nasıl bir şeydir? Bir eksen etrafında phi açısı kadar dönüşü temsil eder: yarım açının kosinüsü ve sinüsü. Şimdi açının küçük olduğunu kullanalım. Yarım açının kosinüsü yaklaşık bir, sinüsü ise yaklaşık yarım açının kendisidir. Böylece delta q, bir ve yarım delta teta olur. Dördüncü sayı kendiliğinden belirlenir; geriye bağımsız üç sayı kalır. Aynı yaklaşım döndürme matrisi için de geçerlidir: birim matris artı delta tetanın çapraz çarpım matrisi. ESKF'nin bütün Jacobianları bu basit formdan çıkar. Hata hep küçük tutulduğu için bu yaklaşımlar her adımda doğru kalır.

Filtrenin ilk adımı tahmindir ve iki ayrı iş yapar. Önce nominal durumu, ataletsel ölçüm biriminden gelen ivme ve açısal hız ölçümleriyle ileri taşırız. Konum, hız ve ivmeyle ilerler. İvmeölçerin okuduğu değerden sapmayı çıkarır, dünya eksenlerine döndürür ve yerçekimini ekleriz. Yönelim için jiroskop okumasından sapmayı çıkarır, kalan küçük dönüşü kuaterniyona çarparız. Sapmalar ise bu adımda sabit kalır. Dikkat edin: bu denklemlerde ne gürültü var ne de kovaryans. Nominal durum en iyi tahminimizi, yüksek hızda ve doğrusal olmayan haliyle taşır. Şimdi hata durumuna geçelim. Az önce bir düzeltme yapılıp sıfırlandığı için kestirilen hata tam sıfırdır: sarı nokta. Sıfırı doğrusal bir matrisle ilerletirseniz yine sıfır elde edersiniz. Yani tahminde hata kestirimi hiç değişmez; değişen tek şey ondan ne kadar emin olduğumuzdur. Bu matris, F x, hata dinamiğinin Jacobianıdır ve küçük açı yaklaşımı sayesinde basit bloklardan oluşur. Örneğin açı hatası döndürülür ve jiroskop sapmasının hatası kadar kayar. Kovaryans ise her adımda büyür: eski belirsizlik F x ile taşınır, üstüne süreç gürültüsü Q eklenir. Kırmızı disk bu belirsizliği gösteriyor. Ölçüm gelmedikçe her tahmin adımı diski biraz daha şişirir. Nominal sapmaya devam eder ve biz ondan giderek daha az emin oluruz. Tahminin özeti şu: nominal ilerler, hata kestirimi sıfırda kalır, kovaryans büyür. Bu büyümeyi durduracak tek şey ölçümdür.

Tahmin bitti; elimizde ilerlemiş bir nominal durum ve şişmiş bir belirsizlik var. Çember üzerindeki sarı nokta nominal yönelim, kırmızı disk de onun etrafındaki belirsizliktir. Şimdi bir ölçüm geliyor, örneğin bir manyetometreden ya da konum alıcısından. Ölçüm, gerçek durumun bir fonksiyonu artı gürültüdür. Filtre hata durumunu kestirdiği için Jacobianı hata durumuna göre almalıyız. Zincir kuralı bunu iki parçaya böler: ölçümün gerçek duruma, gerçek durumun da hataya göre türevi. Bu ikinci parça neredeyse birim matristir; yalnızca yönelim bloğu küçük açı formundan gelen basit bir matristir. Kazanç K ise bildiğimiz formüldür. Kazancı ölçüm farkıyla çarptığımızda hata durumunun kestirimini elde ederiz. Ortalaması artık sıfır değildir: yeşil ok, teğet doğrudaki düzeltmedir. Aynı anda belirsizlik küçülür, çünkü ölçüm bize bilgi verdi. Kırmızı disk daralır. Ama dikkat edin: düzeltme şu an hâlâ teğet doğruda duruyor. Nominal durum henüz değişmedi. Üçüncü adım enjeksiyondur: kestirilen hatayı nominal duruma katarız. Konum ve hız için bu basit bir toplamadır. Yönelim için ise teğetteki düzeltmeyi kuaterniyon çarpımıyla çembere sararız. Sarı nokta yay boyunca kayar ve yine tam çemberin üzerinde durur. Son adım sıfırlamadır. Hata artık nominalin içine aktarıldığı için hata kestirimi yeniden sıfır olur. Kovaryans da yeni yönelimin teğet uzayına taşınır. Bu dönüşüm G, küçük düzeltmelerde birim matrise çok yakındır; çoğu uygulamada doğrudan birim alınır. Ve döngü baştan başlar: yeni ölçümler gelene kadar tahmin, ölçüm geldiğinde güncelleme, ardından enjeksiyon ve sıfırlama.

Bütün parçaları bir araya koyalım. ESKF her döngüde dört adım atar. Tahmin adımında nominal ilerler ve kovaryans büyür. Güncelleme adımında ölçüm, hata kestirimini ve kovaryansı düzeltir. Enjeksiyon, kestirilen hatayı nominale aktarır. Sıfırlama ise hatayı yeniden sıfıra çeker ve döngü kapanır. Son olarak standart EKF ile farkları tek bir tabloda toplayalım. Yönelimi EKF dört sayılı kuaterniyonla doğrudan kestirir; ESKF ise yalnızca üç sayılık delta tetayı kestirir. Bu yüzden EKF'nin yönelim kovaryansı dörde dört ve tekildir; ESKF'ninki üçe üç ve düzgündür. EKF büyük ve hızla değişen bir durum etrafında doğrusallaştırır. ESKF ise her zaman sıfıra yakın bir hata etrafında, yani yaklaşımların en doğru olduğu yerde. Ve birim norm kısıtı: EKF onu sonradan zorla düzeltir, ESKF'de ise çarpımsal enjeksiyon onu kendiliğinden korur. Hata durumu Kalman filtresi büyük işi nominale, belirsizliği küçük hataya bırakır. Bu basit ayrım, onu insansız hava araçlarından uzay araçlarına kadar standart bir yöntem yapar.