Компенсация панорамирования/наклона IMU

Я собираю квадрокоптер с IMU для ориентации в 3D и ультразвуковым дальномером для измерения расстояния до земли. По каким-то механическим причинам IMU не совмещен с осью квадрокоптера. Дальномер направлен вниз перпендикулярно плоскости коптера. IMU имеет левую систему координат, где ось X направлена ​​вниз, ось Z назад, а Y вправо. IMU предоставляет кватернион для описания ориентации. Следующий рисунок иллюстрирует настройку.

Есть две проблемы, с которыми я столкнулся:

1. Чтобы получить ориентацию коптера, мне нужно настроить кватернион из IMU.
2. Чтобы получить реальную высоту (расстояние до земли), мне нужно принять во внимание поворот и наклон основания, чтобы отрегулировать измерения с помощью дальномера.

Я использую библиотеку numpy-quaternion. Чтобы решить первую проблему (отрегулировать кватернион IMU, чтобы получить ориентацию коптера), я поместил коптер на известную горизонтальную поверхность, получил 5 кватернионов от IMU, усреднил их, затем создал кватернион без вращений и вычислил разностный кватернион, как предложено здесь:

qs = quaternion.as_quat_array(sensor_data)
avg_imu_pos = np.average(qs)
base_q = quaternion.from_rotation_vector([[0, 0, 0]])[0]
diff = base_q * avg_imu_pos.conjugate()

Затем, чтобы получить положение коптера, я умножаю рассчитанную разницу на кватернион из IMU:

orientation_true = diff * orientation

где orientation — кватернион, полученный от IMU. Насколько я могу судить, этот шаг работает правильно. Я сделал простую визуализацию, и она мне кажется правильной.

Проблема, которую я пока не могу решить, это как отрегулировать измерение дальномера. Я сделал для него следующую функцию:

def correct_distance(self, measured_distance: float, orientation: quaternion) -> float:
    q1 = quaternion.from_rotation_vector([[0, 0, 0]])[0]
    diff = q1.conjugate() * orientation
    if diff.w < -1:
        diff.w = -1
    elif diff.w > 1:
        diff.w = 1
    angle = 2 * math.acos(diff.w)
    return measured_distance * math.cos(angle)

Здесь предполагается, что orientation — это кватернион, описывающий ориентацию коптера, т.е. скорректированный кватернион IMU. Идея заключалась в том, чтобы вычислить угол между не повернутым кватернионом и кватернионом, описывающим положение коптера. Затем используйте этот угол для корректировки измерения расстояния, умножив его на cos (угол).

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

Итак, мой вопрос: правильна ли моя общая стратегия, и если да, заметили ли вы какую-либо ошибку, мешающую мне правильно отрегулировать измерение расстояния?