Конечно, давай разбираться с этими нюансами. Алгоритм чистый, но если немного подкрутим пороги и нормализуем вес слияния датчиков, то, возможно, выжмем процент. Выложи логи, посмотрим, где там шум прячется.