Расчет траектории движения по данным трехмерного датчика движения

Есть мгновенные данные в кватернионах от 3-осевого магнитного датчика движения (они в диапазоне от -1 до 1), нужно посчитать точки траектории в пространстве, по которой будет двигаться датчик на базе исторических данных. Также нужно посчитать расстояние и вектор направления между двумя любыми точками траектории.

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