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 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 www.kalmanfilter.net sayfasını aldım. Eğer ingilizce biliyorsanız buradan da anlayabilirsiniz. Gerçekten güzel anlatılmis.

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;

Önerilen makaleler

27 Yorum

  1. Avatar

    Konuyu çok güzel anlatmışsınız. Yazınızı okuyunca çok daha iyi anladım. Elinize sağlık.

  2. Avatar

    Süpersin Yusuf
    Benim gibi vasatlar için teorik olarak değilde bir örnek üzerindende anlatman mümkün mü? Değişkenlere rakamlar vererek

    Çalışmalarında başarılar dilerim

    1. admin

      Teşekkür ederim. Müsait olduğumda daha detaylı ela alırım inşallah.

  3. Avatar

    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]);

    Merhaba admin burada eşitliklerin sağ tarafındaki Container.error_covariance[][]’lar Container.pre_error_covariance[][] olmayacak mı? yani eski kovaryansları kullanmayacak mıyız?

    1. admin

      Merhaba,

      pre error covariance bu fonksiyonla hesaplanıyor; void KalmanFilter::calculatePreErrorCovariance(KalmanContainer &Container).

      1. Avatar

        Teşekkürler admin anladım. Ayrıca bitirme tezim için yapmam gereken önemli bir kısımdı sayende kodu yazabildim.

        1. admin

          Yardımcı olabildiysem ne mutlu.:)

      2. Avatar

        Selam ,
        Asic tasarımcısıyım. Dronelar için PID Motor Sürücü tasarlıyorum. Buna kalman filtresi uygulamayı düşünüyorum. Sizce bu model uygun mudur?

        1. admin

          Merhaba, Filtrenin değişken ve kontrol vektörlerinin gerçek uygulamada denenip düzgün ayarlanması gerek. Yani gerçekte uygulayıp denedikten sonra tasarıma ekleyin derim.

  4. Avatar

    http://www.barissamanci.net/Makale/26/accelerometer-gyroscope-imu-nedir/

    Yusuf hocam,
    bu sitede konu çok güzel anlatılmış,ancak örnek bir çözüm olsaydı teoriyi anlardım,
    bu konuyu örnek bir çözümle somutlaştırmanız mümkün mü,
    yani sizden yazılanları koda çevirmerizi değilde,problemin matematiksel bir çözüm nasıl olur,somutlaştırma derken onu kastettim,
    mesala,akselorometre kordinat ekseni ile gyro eksenini çakıştırmış,yani bunu nasıl yapmış anlayamadım.

    1. admin

      Merhaba, Yazıya göz attım. Güzel açıklayıcı bir yazı olmuş. Akselorometre ile gyro eksenini çakıştırmaktan kasıt, gyro verilerini kullanılabilir şekle dönüştürmektir. Yani, kalman filtresi ile ilgili değil bu kısım. Verileri kullanılabilecek formata dönüştürüyor orda. Bu dönüşüm formülü sensörün datasheetinde veya kılavuzunda yazar. Daha sonra kalman filtresi uyguluyor. Benim yaptığım örnekte herhangi bir değer üzerine kalman filtresi uygulanıyor. Bu gyro olabilir, sıcaklık sensörü olabilir veya farklı bir veri.

      1. Avatar

        Yusuf hocam, öğrendiklerime göre; akselorometre verileri ile gyro verileri Kalman filtresi ile birlestiriliyor( sensör füzyonu ) bu sayede her iki sensör birbirini tamamlıyor.bu iş için tamamlayıcı filtre kullananda var.bu konuda github’da hazır kod da var.fakat bu işi anlamak istiyorum

        1. admin

          Bu filtreyi uygulayalı baya oldu. Tekrar konuyu hatırlamam ve tekrar okumam gerekiyor. Onun için müsait bir zamanda tekrar bir literatür taraması yapip daha yeni bir yazı yazarak komuyu tazeleyebilirim belki.

          1. Avatar

            Bilgi, yorumlama sizlere nasip olmuş, O, ilminizi daha da artırsın inşallah . Sizden gelecek ilmi nasibim olarak değer lendiririm.

          2. admin

            Yorumunuz için teşekkür ederim. Bilgi her isteyene nasip olur inşallah.

  5. Avatar

    Sensör füzyonu kalman filtresiyle yapılıyor, kalmana göre basit olduğu için complemrnter-tamamlayıcı filtre kullananda var.

    1. admin

      Kalman anlaşılması o kadar zor bir yöntem değil aslında. Temel mantığı şu; milisaniyede bir değer alıyorsanız ve bu değerler belli aralıklarda bozuluyorsa, zaman ilerledikçe değerlerinizi ortalamaya sabitlemeye çalışıyorsunuz. Eğer değişim çok fazla olursa bu sefer yeni ortalamaya göre sabitlemeye çalışıyorsunuz. Yani ortalama alma olayına biraz istatistik bilgisi biraz sofistike bir yaklaşım kazandırılmış.

  6. Avatar

    Programci degilim, biraz ilgilendim programlama ile ancak acemi gözü ile baktığımda zannimca Bu örnek çalışmada eksik olduğunu düşünmüyorum. Barissamanci.net sitesindeki anlatılan gyro ve akselorometre ham verilerini işleyen kod yok, dizi değişkenleri tanımlanmamış

  7. Avatar

    Merhabalar, Simulink’te 6-Dof model üzerinden çıkan stateleri kullanarak rüzgar tahmini yapacağım. Elimde GPS’den 3 hız bilgisi ve pitot tüpünden gelen hava hız büyüklüğü var. Bu verileri Extended Kalman Filtresinde kullanarak rüzgar hızını kestireceğim. Literatürde hep EKF kullanılmış nonlineer çıktılar olduğu için ancak nereye baksam hep algoritma yazıyor. Simulink’e uyarlama konusunda zorlanıyorum. Elimde hazır veriler var bu verileri füzyon yapıp modele giren rüzgar verisi ile karşılaştıracağım. Yardımıcı olabilir misiniz?

  8. Avatar

    Yazınız için teşekkürler. Fakat kodunuzu alıp VisualStudioyada yazdığımda hata aldım. Mpu6050yi Kalman filtresi kullanarak filtrelemek istiyorum. Bunu nasıl yapabilirim?

    1. admin

      Merhaba,

      Ne hatası aldığınızı söylerseniz belki o şekilde yardımcı olmaya çalışabilirim.

    2. Avatar

      Ben de MPU6050 sensörünü kalman filtresini kullanarak filtrelemek istiyorum bu konuda fikir sahibi olduysanız beni de bilgilendirir misiniz ?

      1. admin

        Merhaba, Yazıda bahsedilen filtre tüm sensörler için uygulanabilir.

  9. Avatar

    Çok güzel bir anlatım olmuş ilaç gibi geldi teşekkürler.

Bir cevap yazın

E-posta hesabınız yayımlanmayacak.