Yusuf Bülbül

An Engineer

Kalman Filtresi Teorisi ve Örnek C++ Kodu

Örneğin bir sensörden ölçüm yaptınız ve ölçümünüzde elde ettiğiniz değerler gürültü sebebiyle çok net değil.  Çok değişken sonuçlar elde ettiğiniz için tasarladığınız sistemin stabil olması da oldukça zorlaşıyor tabi ki. Sıradan bir yazılımcı bu durumda hemen elde ettiği değerlerin ortalamasını alıp gürültüyü biraz daha azaltmaya çalışır tıpkı ilk başta benim yaptığım gibi. Fakat bu bir çözüm olmaz.  Sonunda tilkinin dönüp dolaşıp geleceği yer tabi ki kalman filtresi olur.

Kalman abimiz, bu yöntemi 1930 yılında bulup bizi bu karın ağrısından kurtardı. Aslında yöntemin matematiksel mantığı göründüğü kadar zor değil ama ah şu matematikçilerin gözü kör olsun ki, kullandıkları ifade ve gösterimler kafaları karıştırıyor bazen.  Anlayana kadar göbeğimiz çatlayabiliyor. Teorinin en temelinde bizim başta yaptığımız gibi ortalama alma mantığı var. Fakat bu ortalamayı alırken bazı istatistiksel yaklaşımlarla daha sofistike bir değer olması sağlanmaya çalışılıyor.  Şimdi teoriyi anlatmaya başlayalım.

Öncelikle referans olarak bu sayfayı aldım. Eğer ingilizce biliyorsanız bu yazıyı da okuyarak anlayabilirsiniz. Fakat ben ekstradan yorum ve bilgi kattım. Kalman filtersi iki aşamadan oluşuyor. Bunları (Time Update) ve (Measurement Update) olarak ayırıyoruz. “Time Update” bölümünde bir önceki ölçüm ile ilgili değerler ile bazı değişkenler hesaplıyoruz. “Measurement Update” bölümünde ise bu değişkenlerle yeni ölçüm değerimizi hesaplıyoruz.  Denklemlerimiz şu şekilde;

Time Update;

Measurement Update

İlk bakışta korkutucu gözüküyor. Ama kalemi elinize alıp denklemleri kendiniz yazarak anlamaya çalışınca daha iyi anlaşılıyor. Öncelikle aşamaları iyi okumak gerekiyor. Dikkat ettiyseniz denklemlerde aynı ifadelerin 3 farklı gösterimi mevcut. Bu gösterimler değişkenin durumunu yani “state”‘ini belirtiyor.

Örneğin, Xk değerine bakalım. Bu değer aslında tahmin edilen değerdir. Fakat Xk-1|k-1 durumu, bir önceki tahmin edilen değer, Xk|k-1, “time update” aşamasında hesaplanan tahmini değer ki buna “priori state” de deniyor.  Xk|k ise şu anda hesaplamak istediğimiz tahmin edilen değeri ifade ediyor. Aynı şekilde P hata kovaryansıdır. Hata kovaryansı, doğru değeri tahmin etmek için kullanılıyor.  K ise en önemli değişkenimiz olan Kalman sabitidir.

B vektörü kontrol sinyalini ifade eder. F geçiş vektörüdür. Q varyans matrix’idir. H ise gözlem modeli olarak adlandırılır.

Eğer ölçüm yaptığınız sensör ya da sistem bir kontrol sistemine bağlıysa, örneğin bir robot koluna bağlı jiroskop gibi; bu durumda tahmin ettiğiniz değerler jiroskopun üzerinde bulunan kontrol mekanızmasının hareketine göre de değişecektir. Bu durumda kontrol girişini yani B vektörünü de hesaba katmak gerekiyor. Ya da ölçümleriniz çevresel belli diğer etmenlere( konum ve zaman gibi) bağlı olduğunda bunları da hesaba katmak için F vektörünü modellemezmiz yani geçiş vektörü oluşturmanız gerekiyor.  Q ve H vektörleri aslında direk ortama bağlı sabitlerdir. Mesela çok fazla gürültü varsa Q matrisini yüksek değerden seçmek gerekir.  H ise gerçek hayatta her zaman 1 olarak alınır.

Teoriyi anlamak adına ikiye bölerek anlatmakta fayda görüyorum. İlk önce denklemlerin en sade ve basit halini alalım. Denklemlerdeki kafa karıştıran F, B, H gibi vektörleri hesaba katmayıp skaler bir hesap yapalım ve denklemlerimizi şöyle düzenlemiş olalım.

Bu da demek oluyor ki ölçüm yaptığımız sensörümüz, herhangi bir kontrol girişine bağlı değil ve belli çevresel etmenlere göre değişmiyor. Bu denklemlerin en can alıcı olanı 4 . deklemdir. Burada Zk ölçülen değer, Xk ise tahmin edilen değeri tanımlar. Aslında bu denklem bize, doğru olarak kabul etmemiz gereken değerin, ölçülen değer ile bir önceki tahmin edilen değer arasında bir yerlerde olması gerektiğini söylüyor. Eğer Kk değerini 1/2 alırsanız, sonucun bir önceki değer ile şu anki değerin ortalaması çıktığını görürsünüz. Buradaki büyü, doğru kabul etmemiz gereken değerin acaba bir önceki ölçüm değerine mi yoksa şu anki ölçülen değere mi yakın olması gerektiğini söyleyen Kk yani kalman sabitini bulmaktan ibarettir.  Kalman sabiti ise hata kovaryansının etraftaki gürültü varyansı(R) ile kendi toplamına oranıdır.(5 Denklem) Hata kovaryansı ne kadar küçük olursa, bir önceki değere daha fazla güvenmemiz gerektiğini anlarız. Hata kovaryansı arttıkça doğru kabul ettiğimiz değerin mevcut ölçülen değere yakın olması gerekir.   Aslında kulağa mantıklı geliyor.  Eğer ölçülen değer bir anda çok farklı değerlerde değişirse benim doğru kabul etmem gereken değerin bir önceki değere daha yakın olması gerekiyor. Bu yüzden ani değişimlerde ölçtüğüm değerin hata kovaryansı ile tölere ediyorum.

Fakat burada şöyle bir çıkmaz var; görüldüğü gibi hata kovaryansı ile ölçümlerin arasındaki hata farklarının herhangi bir bağlantısı mevcut değil. Hata kovaryansını başlangıçta “1” kabul ettiğimizi varsayarsak, her ölçümde hata kovaryansı giderek küçülüyor. Bu da demek oluyor ki, zaman ilerledikçe ölçüm yaptığım bir önceki değere daha fazla yaklaşıyorum. Bir süre sonra ise bir önceki değerin her zaman aynısını ölçmüş olacağım.  Yani bir süre sonra sensörünüz size farklı değerler bile veriyor olsa sizin tahmin ettiğiniz değer sabit olmaya başlayacak.

Bu handikaptan kurtulmanın yolu, denklemlere bir geçiş matrisi yani “F” ve kontrol modeli olan “B” matrisini eklemekle olabiliyor. Bu durumda denklemlerimiz vektörel bir hal alıyor.  Ve her değişkeni vektörel olarak tekrar tanımlamak icap ediyor.

 

 

 

Tahmin edilen değeri vetörel olarak iki değişkenle ifade ediyorum. Bunlar θ yani tahmin edilen değer ve θb yani bir önceki tahmin edilen değer ile şimdiki arasındaki hata farkı; diğer ismiyle Bias değeri.   Bir sonraki ise F yani geçiş matrisi ve B kontrol giriş matrisi.  Bu matrisler ilk bakışta oldukça anlamsız gelebilir.  Fakat denklemlere yerine koyup işlem yapınca daha anlamlı oluyor. Ayrıca bu matrisler farklı uygulamalara göre farklı modellenebilir. Bu uygulama için geçiş matrisi ve kontrol matrisini koymamızdaki amaç, ölçümlerin zamana göre türevini alarak değişimin ne kadar büyük ya da hangi yönde olduğunu tahmin edebilmeyi sağlamasından kaynaklanıyor.

Time Update 1. Denklem;

Aslında bu denklem ile bir önceki denklemlerde yaşadığımız handikaptan kurtulmuş oluyoruz. Şöyle ki, aslında bizim handikapımızın zamanla, hata kovaryansının düşmesi ve zamanla hep sabit değer okumaya başlamamdan kaynaklanıyordu. Fakat şimdi denklemime bir de türev ve integral ekledim.  Yukarıdaki matrisin ilk satırındaki denklem bize şunu söylüyor; eğer sensörümde okuduğum değerler bir yöne doğru artıyor ya da azalıyorsa o halde bu artış ya da azalış benim tahmin edeceğim değere yansıtılmalı. Şöyle ki sensörden okuduğum birim zamandaki değerinden hata miktarını çıkarttığımda doğru kabul edebileceğim bir artış ya da azalma belirteci bulurum. Bunu bir zaman aralığı ile çarptığımda aslında bu bana sensör değerlerindeki artışı ya da azalışı veriyor. Bunu da tahmin edeceğim değere ekliyorum ki her zaman sabit bir değer okumuş olmayayım. ( Bu arada denklemdeki “θk” değişkeni birim zamandaki ölçümdür yani aslında ölçümün zamana göre türevidir. ) 

Time Update 2.Denklem;

Denklemlerimiz artık vektörel hale geldiğinden hata kovaryansı da vektörel bir hal almalı. Buradaki Q varians değerlerini biz kendimiz ortamın gürültüsünü tahmin ederek veriyoruz.

Measurement Update 1. Denklem;

Göründüğü üzere aslında vektörleri ihmal ederek yazdığımız denklemden çok farkı yok. Sadece denklemi vektörel hale getirdiğimiz için ekstra gösterimler işin içerisine girmiş oluyor.

Measurement Update 2. Denklem;

Measurement 3. Denklem;

 

Aslında bu denklemlerde çok büyük değişimler görünmüyor.  En büyük değişim Time update 1. denkleme eklediğimiz kontrol girişi ve geçiş matrisini eklemek oldu. Böylelikle Hata kovaryansının okuduğumuz değeri zaman geçtikçe sabit hale getirmesini önlemiş olduk.

C++ için yazdığım kalman filter sınıfının kodları şu şekilde;

kalmanfilter.h;

#ifndef KALMANFILTER_H
#define KALMANFILTER_H

#include "process.h"
#include "queue"
struct KalmanGain
{
    double measurement_kalman_gain = 0;
    double error_kalman_gain = 0;
};

struct EstimatedValue
{
    double estimated_measurement = 0;
    double estimated_error = 0;
};

struct KalmanContainer
{

    KalmanGain kalman_gain;
    EstimatedValue estimated_value;
    EstimatedValue pre_estimated_value;
    double error_covariance[2][2] = {{1.0, 1.0}, {1.0, 1.0}};
    double pre_error_covariance[2][2] = {{1.0, 1.0}, {1.0, 1.0}};

    double measured_value = 0;
    double measured_value_persecond = 0;
    double time_interval = 0;
    double noise_variance = 0.9;
    double error_variance = 0.9;
    double measurement_variance = 0.9;

    double now = 0;
    double last = 0;

    KalmanContainer operator =(double Value)
    {

        measured_value = Value;

        now = (double)(clock()) / 1000000;

        time_interval = now - last;

        measured_value_persecond = measured_value / time_interval;

        last = now;

        return *this;

    }

};

class KalmanFilter
{



public:
    KalmanFilter();
    cv::Point takeKalmanFilter(const cv::Point &NoisyPoint);

private:

    KalmanContainer gmPointXKalman;
    KalmanContainer gmPointYKalman;

    void calculateKalmanGain(KalmanContainer &Container); //Measurement step1
    void calculateErrorCovariance(KalmanContainer &Container); //Mesaurement step2
    void calculateEstimatedValue(KalmanContainer &Container); //Measurement step3

    void calculatePreEstimateValue(KalmanContainer &Container); //timeUpdate step1
    void calculatePreErrorCovariance(KalmanContainer &Container); //timeUpdate step2

    void timeUpdate(KalmanContainer &Container);
    void measurmentUpdate(KalmanContainer &Container);

};

#endif // KALMANFILTER_H

 

kalmanfilter.cpp;

#include "kalmanfilter.h"



KalmanFilter::KalmanFilter()
{

}



cv::Point KalmanFilter::takeKalmanFilter(const cv::Point &NoisyPoint)
{

    gmPointXKalman = (double)NoisyPoint.x;
    gmPointYKalman = (double)NoisyPoint.y;

    timeUpdate(gmPointXKalman);
    timeUpdate(gmPointYKalman);

    measurmentUpdate(gmPointXKalman);
    measurmentUpdate(gmPointYKalman);

    return cv::Point(gmPointXKalman.estimated_value.estimated_measurement,
                     gmPointYKalman.estimated_value.estimated_measurement);

}



void KalmanFilter::calculatePreEstimateValue(KalmanContainer &Container)
{


    Container.estimated_value.estimated_measurement = Container.pre_estimated_value.estimated_measurement +
             Container.time_interval*(Container.measured_value_persecond - Container.pre_estimated_value.estimated_error);

    Container.estimated_value.estimated_error = Container.pre_estimated_value.estimated_error;
}



void KalmanFilter::calculatePreErrorCovariance(KalmanContainer &Container)
{

    Container.error_covariance[0][0] += Container.time_interval*(Container.time_interval*Container.pre_error_covariance[1][1]
            - Container.pre_error_covariance[0][1] - Container.pre_error_covariance[1][0] + Container.measurement_variance);
    Container.error_covariance[0][1] -= Container.time_interval*Container.pre_error_covariance[1][1];

    Container.error_covariance[1][0] -= Container.time_interval*Container.pre_error_covariance[1][1];
    Container.error_covariance[1][1] += Container.time_interval*Container.measurement_variance;

}



void KalmanFilter::calculateEstimatedValue(KalmanContainer &Container)
{

//    std::cout << "estimated_error: " << std::to_string(Container.estimated_value.estimated_measurement);
//    std::cout << " - error_kalman_gain:" << std::to_string(Container.pre_estimated_value.estimated_measurement);
//    std::cout << " - estimated_error:" << std::to_string(Container.pre_estimated_value.estimated_error) << std::endl;

    Container.estimated_value.estimated_measurement += Container.kalman_gain.measurement_kalman_gain *
                                                    (Container.measured_value - Container.estimated_value.estimated_measurement);

    Container.estimated_value.estimated_error += Container.kalman_gain.error_kalman_gain *
                                                    (Container.measured_value - Container.estimated_value.estimated_measurement);

    Container.pre_estimated_value.estimated_measurement = Container.estimated_value.estimated_measurement;
    Container.pre_estimated_value.estimated_error = Container.estimated_value.estimated_error;

}



void KalmanFilter::calculateKalmanGain(KalmanContainer &Container)
{

    Container.kalman_gain.measurement_kalman_gain = (double)Container.error_covariance[0][0]
            / (Container.error_covariance[0][0] + Container.noise_variance);

    Container.kalman_gain.error_kalman_gain = (double)Container.error_covariance[1][0]
            / (Container.error_covariance[0][0] + Container.noise_variance);

}



void KalmanFilter::calculateErrorCovariance(KalmanContainer &Container)
{
    Container.error_covariance[0][0] -=
            (Container.kalman_gain.measurement_kalman_gain*Container.error_covariance[0][0]);

    Container.error_covariance[0][1] -=
            (Container.kalman_gain.measurement_kalman_gain*Container.error_covariance[0][1]);

    Container.error_covariance[1][0] -=
            (Container.kalman_gain.error_kalman_gain*Container.error_covariance[0][0]);

    Container.error_covariance[1][1] -=
            (Container.kalman_gain.error_kalman_gain*Container.error_covariance[0][1]);


    for(int i=0; i<2; i++)
        for(int k=0; k<2; k++)
            Container.pre_error_covariance[i][k] = Container.error_covariance[i][k];
}



void KalmanFilter::timeUpdate(KalmanContainer &Container)
{

    calculatePreEstimateValue(Container); // step 1
    calculatePreErrorCovariance(Container); //step 2

}



void KalmanFilter::measurmentUpdate(KalmanContainer &Container)
{

    calculateKalmanGain(Container); // step1

    calculateErrorCovariance(Container); // step2

    calculateEstimatedValue(Container); // step3


}


 

Kodda görüldüğü üzere  aşağıdaki değerleri sabit olarak tanımlıyoruz. Bu değerleri kendi uygulamanıza göre değiştirebilirsiniz.  Gürültü fazlaysa bu değerleri bire yakın azsa sıfıra yakın tutarsanız daha iyi olur.  Kolay Gelsin.

double noise_variance = 0.9;

double error_variance = 0.9;

double measurement_variance = 0.9;

 

Bir cevap yazın

E-posta hesabınız yayımlanmayacak. Gerekli alanlar * ile işaretlenmişlerdir

Copyright © Tüm Hakları Saklıdır.