Оценка линейного перемещения с помощью акселерометра, интегрирование данных акселерометра во время разгона Опции

[serjj]

Всем доброго.
Есть идея определять вектор перемещения твердого тела, имеющего 6 степеней свободы с помощью акселеромета. Если точнее, то определять проекцию вектора на плоскость OXY (в инерциальной Земной системе координат) в период разгона объекта. Т.е. есть следующие условия: линейное ускорение порядка g, объект может поворачиваться или вращаться вокруг своих осей, время измерения - секунды (пока идёт разгон).

Моя идея была следующая. С помощью fusion фильтра получаю набор Эйлеровых параметров Q = [q0 q1 q2 q3], т.е. кватернион, который задаёт мгновенное радиальное положение объекта. Далее усреднённый вектор данных акселерометра, асс, измеренного в связанной системе координат, преобразую в вектор acci относительно Земной системы координат:
acci = (Q * [0 acc]) *conj(Q), выкидываю acci(0) (переход от кватерниона к вектору) и acci(3) (z составляющая), а далее численное интегрирование два раза, чтобы получить искомое перемещение. Чтобы понять разгоняемся мы или нет, при интегрировании к модулю вектора ускорения применяется некоторый порог.

С получением acci особых проблем не возникло, но вот дальше пока не очень понятно. Проверял идею пока только "на столе", т.е. с STMовской отладкой, он что-то мереет, но это точно не то, что я ожидаю. При интегрировании за dt взял период сэмплирования акселерометра, ~10 мс (это же период обновления fusion фильтра). Скорость уверено уходит за 10 м/с. Есть предположение, что подход должен таки работать при длительном (относительно перида сэмплирования, т.е секунды) и резком разгоне (т.е. линейное ускорение порядка g). При первом рассмотрении гугл говорит, что таки да для малых движений (например смартфон в руках человека) алгоритм врёт и очень сильно. Плюс даже если он не врёт сначала, то обязательно начнёт врать потом (т.к. скорость уйдёт). Но вот как будет для обозначенных мною условий? Есть ли у кого какие прогнозы и может быть кто нибудь поделится опытом по оценке векторов линейных движений (с угловыми вроде бы разобрался)? Или может быть это все от лукавого и магнетометр + априорная скорость наше всё?

Зы: поставить дополнительно измеритель скорости не предлагать) для решения задачи есть два датчика от ST - гироскоп и акселерометр+магнетометр.

Зыы: еще заметил такую интересность, алгоритм как-то реагирует на разгон, но совершенно не хочет на торможение, т.е. не сбрасывает скорость после остановки.