KalmanFilter.c

typedef struct {
	float varVolt;  // среднее отклонение (ищем в excel)
	float varProcess; // скорость реакции на изменение (подбирается вручную)
	float Pc;
	float G;
	float P;
	float Xp;
	float Zp;
	float Xe;
} KalmanFilterTypeDef;

// Инициализация фильтра
void KalmanFilterInit(KalmanFilterTypeDef *Filter, float Volt, float Process) {
	Filter->varVolt = Volt;  // среднее отклонение (ищем в excel)
	Filter->varProcess = Process; // скорость реакции на изменение (подбирается вручную)
	Filter->Pc = 0.0;
	Filter->G = 0.0;
	Filter->P = 1.0;
	Filter->Xp = 0.0;
	Filter->Zp = 0.0;
	Filter->Xe = 0.0;
}

// Добавление нового значения
float KalmanFilter(KalmanFilterTypeDef *Filter, float NewData) {  //функция фильтрации
	Filter->Pc = Filter->P + Filter->varProcess;
	Filter->G = Filter->Pc / (Filter->Pc + Filter->varVolt);
	Filter->P = (1 - Filter->G) * Filter->Pc;
	Filter->Xp = Filter->Xe;
	Filter->Zp = Filter->Xp;
	Filter->Xe = Filter->G * (NewData - Filter->Zp) + Filter->Xp; // "фильтрованное" значение
	return(Filter->Xe);
}