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

Что такое СЕТАП?

Фильтр сейчас выглядит так:

varTerm = 0.14;
varProcess = 0.0005;
Pc = 0.0;
RG = 0.0;
RP = 1.0;
Xp = mV2048;
Zp = mV2048;
Xe = mV2048;

float filter_KM(float val_1) { // kalman
Pc = RP + varProcess;
RG = Pc/(Pc + varTerm);
RP = (1-RG)*Pc;
Xp = Xe;
Zp = Xp;
Xe = RG*(val_1-Zp)+Xp;
return(Xe);
}

Переменная mV2048 - это вот та предустановка, что бы фильтр сразу попадал в нужное состояние.
Добавил R в начало переменных G и P, это потому что в 8051 переменная P системная переменная, нельзя ее использовать для других.

 

Функция setup()

Запускается -один раз при старте системы