Фильтр Калмана для одной переменной

Ребята, доброго времени суток!

Мой уровень программирования не столь высок, что бы разобраться самому в актуальной проблеме. Уверен, перед многими вставала задача фильтрации данных от датчика и не редко бывает что нужно обработать всего одну переменную. Фильтр Калмана при решении таких задач дает замечательный результат. Нашел интересную статью про ФИЛЬТ КАЛМАНА ДЛЯ ОДНОЙ ПЕРЕМЕННОЙ. Вот здесь http://habrahabr.ru/post/140274/ (надеюсь за рекламу ресурса не сочтут ).
Так вот, можно ли из кода представленного в этой статье собрать библиотеку для реализации такого фильтра? Я побродил по просторам интернета и так не нашел чего-то близкого. В основном встречается фильтр Калмана для кадрокоптеров. Так может найдтся здесь умелец, который подарит сообществу Отечественных Arduino-разработчиков столь ценную библиотеку ?
P.S. И вообще может быть Amperke завести банк библиотек? )

Приожение:

Код (Text):

class KalmanFilterSimple1D
    {
        public double X0 {get; private set;} // predicted state
        public double P0 { get; private set; } // predicted covariance

        public double F { get; private set; } // factor of real value to previous real value
        public double Q { get; private set; } // measurement noise
        public double H { get; private set; } // factor of measured value to real value
        public double R { get; private set; } // environment noise

        public double State { get; private set; }
        public double Covariance { get; private set; }

        public KalmanFilterSimple1D(double q, double r, double f = 1, double h = 1)
        {
            Q = q;
            R = r;
            F = f;
            H = h;
        }

        public void SetState(double state, double covariance)
        {
            State = state;
            Covariance = covariance;
        }

        public void Correct(double data)
        {
            //time update - prediction
            X0 = F*State;
            P0 = F*Covariance*F + Q;

            //measurement update - correction
            var K = H*P0/(H*P0*H + R);
            State = X0 + K*(data - H*X0);
            Covariance = (1 - K*H)*P0;          
        }
    }



    // Применение...

    var fuelData = GetData();
    var filtered = new List<double>();

    var kalman = new KalmanFilterSimple1D(f: 1, h: 1, q: 2, r: 15); // задаем F, H, Q и R
    kalman.SetState(fuelData[0], 0.1); // Задаем начальные значение State и Covariance
    foreach(var d in fuelData)
    {
        kalman.Correct(d); // Применяем алгоритм

        filtered.Add(kalman.State); // Сохраняем текущее состояние
    }

С Уважением,
Погорелов Николай ))

 

Ребят, где может быть ошибка ?

Код (Text):

KalmanFilterSimple1D.h

#ifndef _KalmanFilterSimple1D_h
#define _KalmanFilterSimple1D_h


class KalmanFilterSimple1D
    {
        public double X0 { get; private set; } // predicted state
        public double P0 { get; private set; } // predicted covariance

        public double F { get; private set; } // factor of real value to previous real value
        public double Q { get; private set; } // measurement noise
        public double H { get; private set; } // factor of measured value to real value
        public double R { get; private set; } // environment noise

        public double State { get; private set; }
        public double Covariance { get; private set; }

        public KalmanFilterSimple1D(double q, double r, double f = 1, double h = 1);

        public void SetState(double state, double covariance);
   
        public void Correct(double data);

    };

#endif

Код (Text):

KalmanFilterSimple1D.cpp

#include "KalmanFilterSimple1D.h"

void KalmanFilterSimple1D::KalmanFilterSimple1D(double q, double r, double f = 1, double h = 1)
    {
            Q = q;
            R = r;
            F = f;
            H = h;
    }

void KalmanFilterSimple1D::SetState(double state, double covariance)
        {
            State = state;
            Covariance = covariance;
        }

void KalmanFilterSimple1D::Correct(double data)
        {
            //time update - prediction
            X0 = F*State;
            P0 = F*Covariance*F + Q;

            //measurement update - correction
            var K = H*P0/(H*P0*H + R);
            State = X0 + K*(data - H*X0);
            Covariance = (1 - K*H)*P0;        
        }

При компиляции выдает ошибки:

In file included from sketch_jan23a.ino:1:
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:7: error: expected `:' before 'double'
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:7: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:8: error: expected `:' before 'double'
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:8: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:10: error: expected `:' before 'double'
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:10: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:11: error: expected `:' before 'double'
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:11: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:12: error: expected `:' before 'double'
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:12: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:13: error: expected `:' before 'double'
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:13: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:15: error: expected `:' before 'double'
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:15: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:16: error: expected `:' before 'double'
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:16: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:18: error: expected `:' before 'KalmanFilterSimple1D'
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:20: error: expected `:' before 'void'
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:22: error: expected `:' before 'void'

 

Класс по ссылке написан на C#, а не на С++.

 

В части { get; private set; } ?

 

Часть ошибок ушла вот при таком объявлении public: Спасибо, но ошибки все равно есть

Код (Text):

#ifndef _KalmanFilterSimple1D_h
#define _KalmanFilterSimple1D_h


class KalmanFilterSimple1D
    {
        public: double X0 { get; private set; } // predicted state
        public: double P0 { get; private set; } // predicted covariance

        public: double F { get; private set; } // factor of real value to previous real value
        public: double Q { get; private set; } // measurement noise
        public: double H { get; private set; } // factor of measured value to real value
        public: double R { get; private set; } // environment noise

        public: double State { get; private set; }
        public: double Covariance { get; private set; }

        public: KalmanFilterSimple1D(double q, double r, double f = 1, double h = 1);

        public: void SetState(double state, double covariance);
   
        public: void Correct(double data);

    };

#endif

In file included from sketch_jan23a.ino:1:
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:7: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:8: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:10: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:11: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:12: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:13: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:15: error: function definition does not declare parameters
C:\Program Files\Arduino\libraries\KalmanFilterSimple1D/KalmanFilterSimple1D.h:16: error: function definition does not declare parameters

 

Прежде чем заниматься библиотекой разобрались бы лучше с классом в основном скетче.
Язык программирования не тот, ошибок дофига.
Вот это, хотя бы, компилится.

Код (Text):

 class KalmanFilterSimple1D
 class KalmanFilterSimple1D
  {
  private:
  double X0; // predicted state
  double P0; // predicted covariance
  double F;  // factor of real value to previous real value
  double Q;  // measurement noise
  double H;  // factor of measured value to real value
  double R;  // environment noise
  double State, Covariance;
  public:
  KalmanFilterSimple1D(double q, double r, double f, double h);
  void SetState(double state, double covariance);
  void Correct(double data);
  };
KalmanFilterSimple1D::KalmanFilterSimple1D(double q, double r, double f, double h)
  {
  this->Q = q;
  this->R = r;
  this->F = f;
  this->H = h;
  }

void KalmanFilterSimple1D::SetState(double state, double covariance)
  {
  this->State = state;
  this->Covariance = covariance;
  }

void KalmanFilterSimple1D::Correct(double data)
  {
  //time update - prediction
  X0 = F*State;
  P0 = F*Covariance*F + Q;
  //measurement update - correction
  double K = H*P0/(H*P0*H + R);
  State = X0 + K*(data - H*X0);
  Covariance = (1 - K*H)*P0;  
  }
 

По-логике
Не понятно, зачем X0 и P0 должны быть членами класса. Они каждый раз рассчитываются заново в Correct, а потом никаким другим методом из значения не вызываются.
В Correct я просто поленился перед каждым членом класса поставить this->. Хотя надо бы.

 

Посмотрел статью. А что простое апериодическое звено первого порядка не подойдет?
Всего-то формула

Код (Text):

  Y0:=(K*X0+(Tf/SAMPLE_T*Y1))/((Tf/SAMPLE_T)+1);
  Y1:=Y0;

Tf - постоянная времени фильтра,
SAMPLE_T - время квантования (период измерения). должен быть в одной размерности Tf.
К-коэффициент звена. для фильтра =1.
X0 - входное значение.
Y0 - Выходное значение.
Y1- Выходное значение в предыдущий период квантования.
Он хоть проще.
Кальмана в float-е ардуино заколебется считать. А от использования double пользы никакой. Те же 4 байта float.

 

Я так понимаю это уравнение какого-то другого фильтра ?? Вы предлагаете его как более простой инструмент, что бы не заморачиваться с Калманом ?

 

Хорошее предложение! Профессионалу, наверняка, сразу очевидно, в чем разница и преимущество кажого из подходов перед другим. Для меня вот 5 минут назад и апериодичное звено первого порядка было неизвестным объектом https://ru.wikipedia.org/wiki/Апериодическое_звено.. Тогда предлагаю сравнить этот алгоритм и все же довести до ума Калмана. На LabView обязуюсь проверить, сравнить и выложить результаты. Но может стоит выдать библиотеку в руки разработчикам. Уверен, она будет полезна. А если Ваше предложение не уступит или окажется лучше, то это тоже будет большим результатом и хорошим инструментом для отечественных Arduino-разработчиков, энтузиастов.

 

Код (C++):

// Filter temperature sensor readings using the Kalman process

const int sensorPin = A0;  // named constant for the pin the sensor is connected to

// kalman variables
float varVolt = 1.12184278324081E-05;  // variance determined using excel and reading samples of raw sensor data
float varProcess = 1e-8;
float Pc = 0.0;
float G = 0.0;
float P = 1.0;
float Xp = 0.0;
float Zp = 0.0;
float Xe = 0.0;

void setup() {
  // open a serial connection to display values
  Serial.begin(9600);
}

void loop() {
  int sensorVal = analogRead(sensorPin);     // read the value on AnalogIn pin 0 and store it in a variable
  float voltage = sensorVal * 5.0 / 1023.0;  // convert the ADC reading to voltage

  // kalman process
  Pc = P + varProcess;
  G = Pc/(Pc + varVolt);    // kalman gain
  P = (1-G)*Pc;
  Xp = Xe;
  Zp = Xp;
  Xe = G*(voltage-Zp)+Xp;   // the kalman estimate of the sensor voltage
   
  Serial.print(voltage,5);
  Serial.print(",");
  Serial.print(Xe,5);
  Serial.println();

}
 

если вдруг кому нужно для квадрокоптера или других целей)))

 

Внесу скромный вклад.
Представленный в предыдущем посте код запаковал в модуль kalman.js:

Код (Javascript):

var kalman = function(variance, varProcess) {
    this.variance = variance || 1.12184278324081/10000;
    this.varProcess = varProcess || 1/10000000;
    // console.log("variance = ", this.variance);
    // console.log("varProcess = ", this.varProcess);

    this.Pc = 0.0;
    this.G = 0.0;
    this.P = 1.0;
    this.Xp = 0.0;
    this.Zp = 0.0;
    this.Xe = 0.0;
}
kalman.prototype.set = function(variance, varProcess){
    this.variance = variance || 1.12184278324081/10000;
    this.varProcess = varProcess || 1/10000000;
    // console.log("variance = ", this.variance);
    // console.log("varProcess = ", this.varProcess);
}

kalman.prototype.update = function(vol) {
    this.Pc = this.P + this.varProcess;
    this.G = this.Pc/(this.Pc + this.variance);
    this.P = (1-this.G)*this.Pc;
    this.Xp = this.Xe;
    this.Zp = this.Xp;
    this.Xe = this.G*(vol-this.Zp) + this.Xp;
    return this.Xe;
}

exports.connect = function(variance, varProcess) {
    return new kalman(variance, varProcess);
};

 

Пользоваться этим так:

Код (Javascript):

var cl = console.log;
var Pin = A0;
var ow = new OneWire(Pin);
var sensor = require("DS18B20").connect(ow);
var KalmanFilter = require("kalman").connect;

var filter = new KalmanFilter(0.1,0.01);

/*  Variants:
var filter = new KalmanFilter();
var filterA = new KalmanFilter(0,0.5);
var filterB = new KalmanFilter(0.1111);
var filterC = new KalmanFilter(1,2);
*/
// filter.set(1,1);

setInterval(function() {
  sensor.getTemp(function (temp) {
      var filteredX = filter.update(temp);
      cl(temp.toFixed(2)," - ", filteredX.toFixed(2));
  });
}, 3000);

Работает. Осталось понять как...

 

[/URL][/IMG] [/URL][/IMG] в процессе эксплуатации пришёл к выводу, что

Код (C++):

Xe = G*(voltage-Zp)+Xp;

требует введение "коэффициента"

Код (C++):

Xe = G*(voltage-Zp)+Xp +3;

+3 - величина найденная случайно, исходя из требуемой задачи. Находил по картине из штатного плоттера, добиваясь нужной плавности.
Почему коэффициент в кавычках? Причина проста, являясь истинным программистом платы ардуино, изменял все параметры в строках кода.
[/URL] [/IMG]

 

и с +3
http://vfl.ru/fotos/19d588aa16026799.html

простите, не смог нормально вставить

 

Код (C++):

// переменные для калмана "ВЕРХНЕГО" нагревателя
float varTerm1 = 0.25;  // среднее отклонение (ищем в excel)
float varProcess1 = 0.025; // скорость реакции на изменение (подбирается вручную)
float Pc1 = 0.0;
float G1 = 0.0;
float P1 = 1.0;
float Xp1 = 0.0;
float Zp1 = 0.0;
float Xe1 = 0.0;

// переменные для калмана "НИЖНЕГО" нагревателя
float varTerm2 = 0.25;  // среднее отклонение (ищем в excel)
float varProcess2 = 0.025; // скорость реакции на изменение (подбирается вручную)
float Pc2 = 0.0;
float G2 = 0.0;
float P2 = 1.0;
float Xp2 = 0.0;
float Zp2 = 0.0;
float Xe2 = 0.0;
// переменные для калмана



        Input1 = filter1(thermocouple1.readCelsius());
        Input2 = filter2(thermocouple2.readCelsius());



  float filter1(float val_1) {  //функция фильтрации показений "Верхней" термопары
  Pc1 = P1 + varProcess1;
  G1 = Pc1/(Pc1 + varTerm1);
  P1 = (1-G1)*Pc1;
  Xp1 = Xe1;
  Zp1 = Xp1;
  Xe1 = G1*(val_1-Zp1)+Xp1; // "фильтрованное" значение
  return(Xe1);
}
  float filter2(float val_2) {  //функция фильтрации показений "Нижней" термопары
  Pc2 = P2 + varProcess2;
  G2 = Pc2/(Pc2 + varTerm2);
  P2 = (1-G2)*Pc2;
  Xp2 = Xe2;
  Zp2 = Xp2;
  Xe2 = G2*(val_2-Zp2)+Xp2; // "фильтрованное" значение
  return(Xe2);
  }

 

Такой код применил для ИК паяльной станции
Здесь есть и графики http://forum.amperka.ru/threads/ИК-...560-Доработка-скетча-ars_v2_lilium_jsn.10176/

 

vector99, подскажите, как называются переменные Pc2, G2, P2, Xp2, Zp2? В чем их смысл?
Хочу этот пример запустить на своей плате под 8051 проц.
Но у меня другая задача - нужно шум подавить в потоке данных от АЦП.

 

Я нашол это здесь:

 

Мда...
Новые слова: Ардуино, СериалПлоттер...
Ладно, почитаю в интернете.

 

vector99 - по-моему. переменные Zp1 и Zp2 лишние, их можно заменить на Xp1 и Xp2 соответсвенно, формула станет проще

в первом посте этой ветки есть ссылка на статью на Хабре, там все относительно понятно обьяснено.

 

vector99, "дернул" подпрограмму float filter1(float val_1) - без изменений.
Вставил для фильтрации потока данных с АЦП.
На вход АЦП прицепил стабилитрон 2.049 В.
Стабилитрон немного шумит, АЦП немного шумит - в целом все подходяще.
Что выяснилось, так это переменная Xe1 при инициализации требует предустановки, в то значение - какое ожидается. Иначе фильтр начинает "шлепать" с нуля, и может это делать достаточно долго.
Измеряю входное значение, еще до запуска фильтра - 50 реализаций вычисляю среднеарифметическое и присваиваю это - Xe1. При запуске фильтр сразу стартует с нужного места. В моем случае это значение 2049.560 мВ.
В общем и целом фильтр работает и у меня тоже.
Хочу запустить плату из серии STM32 под Ардуино.
Ну и там на ней попробую этот проект тоже запустить - в режиме "побыстрей".

 

Может нужно это делать в СЕТАПЕ.