Вы попробуйте менять направление вращения случайным и независимым генератором (ГСЧ) , чтобы он попадал в разные моменты и не был синхронизован в выполнением основной программы .Кроме того ,хороший алгоритм должен исходить из того что могут быть высокочастотные помехи ,которые сравнимы по длительности с фронтами и которые сглаживать фильтром нельзя ,что бы не потерять основные импульсы с энкодера при больших скоростях .