Эффективная интеграция данных гироскопа скорости с Python ⇐ Python

[Anonymous]

Я работаю с 3-осевым гироскопом и хотел бы интегрировать данные о скорости, чтобы определить ориентацию моего гироскопа как функцию времени. Гироскоп скорости измеряет мгновенный вектор вращения, который можно превратить в небольшое вращение dR ~ w*dt. Базовую интеграцию можно выполнить, отслеживая текущее вращение R и применяя мгновенное dR на каждом временном шаге. Я написал функцию Python, которая делает это:
from scipy.spatial.transform import Rotation as R
def rate_int_py(gyro_rates, R0, dt):
R_curr = R0
out = list()
for i, w in enumerate(gyro_rates):
dR = R.from_rotvec(np.deg2rad(w * dt))
R_curr = dR * R_curr #note left mult
out.append(R_curr)
return R.concatenate(out)

Однако эта функция не особенно быстра, и я надеялся получить большую скорость «numpy», а не скорость «Python». Я попытался сделать это с помощью numpy.cumprod, но получил немного другой ответ. Я считаю, что проблема в том, что композиция вращения не является коммутативной, и cumprod накапливает «вправо», а не влево, умножая текущее вращение на следующий dR. Вот код этой попытки:
from scipy.spatial.transform import Rotation as R
import numpy as np
def rate_int_scipy(gyro_rates, R0, dt):
rots = R.from_rotvec(gyro_rates*dt, degrees=True)
rots = R.concatenate([R0] + list(rots))
res = np.cumprod(rots, axis=0)
res = R.concatenate(res)
return res[1:]

В любом случае эта попытка была спорной, поскольку numpy при воздействии на неродные типы numpy по умолчанию использует «обычный» Python, и поэтому эта функция в любом случае работает медленнее, чем нативная функция Python.
Как люди работают с данными гироскопа в Python? Я не думаю, что мой вариант использования вообще нишевый. В итоге я написал программу на C, которая делает то, что мне нужно, но я бы предпочел использовать чистый Python.
--- Минимально воспроизводимый пример ---
По просьбе комментатора я прилагаю небольшой пример. Я генерирую некоторые поддельные данные ориентации и беру производную, чтобы восстановить их с помощью своих функций
from scipy.spatial.transform import Rotation as R
import numpy as np
from time import time

t = np.linspace(0, 1, 100000)
dt = t[1] - t[0]
#define truth orientation over time
theta = R.from_rotvec(np.vstack((2 * t + 100, 3 * t + 50, 10.0 * t - 100)).T)
# compute the angular rates: theta(t) = w(t) * theta(t-1) => w(t) = theta(t) * theta(t-1)^-1
# this is what a rate gyro measures
omega = []
for i in range(1, len(t)):
delta_rot = theta * theta[i-1].inv()
delta_angle = delta_rot.as_rotvec(degrees=True)
omega.append(delta_angle / ( dt))
omega = np.array(omega)

t1 = time()
orientation_py = rate_int_py(omega, theta[0], dt)
t2 = time()
print(f"rate_int_py took {t2 - t1} seconds")
#0.8s on my machine
t1 = time()
orientation_scipy = rate_int_scipy(omega, theta[0], dt)
t2 = time()
print(f"rate_int_scipy took {t2 - t1} seconds")
# 1.1s on my machine

assert np.allclose(orientation_py.as_rotvec(), theta[1:].as_rotvec())
assert np.allclose(orientation_scipy.as_rotvec(), theta[1:].as_rotvec()) # this fails!


Подробнее здесь: https://stackoverflow.com/questions/798 ... ith-python