Как получить скорость вращения вокруг 2 осей по заданному набору векторов положения

Я пытался поместить это как комментарий, но там есть ограничение по длине. Итак, некоторые уточняющие вопросы/промежуточные выводы:

Спасибо за цифры! Таким образом, это выглядит из вашего 4-го рисунка выше, на котором показаны синусоидальные волны с двумя осями xyz, и из вашей схемы машины, как будто у вас есть три системы координат: первая - это земная система координат, назовите ее x1, y1, z1, как вы показываете это на диаграмме "картинка машины". Второй кадр, назовем его x2,y2,z2, вращается вокруг осей x1 и x2 (они остаются параллельными). Третий кадр x3,y3,z3 вращается вокруг оси z2 (=z3). Ваши акселерометры зафиксированы в системе координат x3,y3,z3.

В вашем наборе данных с одной осью z3=z2 выровнено с землей z1 и вращается вокруг z, так что x3 и y3 вращаются вокруг силы тяжести в квадратурных синусоидальных волнах.

Во втором наборе данных внешний подвес x1=x2 вращается с постоянной скоростью, что приводит к идеальной синусоидальной волне на акселерометре z, в то время как внутренний подвес z3=z2 также вращается, возможно, с постоянной скоростью, но теперь акселерометры на Амплитуды x3 и y3 модулируются путем умножения на косинус угла поворота x1/x2.

Все это звучит правильно?

Еще одна вещь, которую нам всегда нужно знать при оценке скорости по измерениям положения, — это какая-то модель или концепция того, как ваша система изменяется во времени: можем ли мы предположить некоторое максимальное угловое ускорение? Или мы можем предположить, что, как только скорость вращения достигнет определенной скорости, она станет постоянной? Это станет особенно важным при попытке сшить угол карданного подвеса z2/z3 в моменты времени, когда угол x1/x2 проходит через +/-pi/2 радиана, где угол z2/z3 становится на мгновение ненаблюдаемым, поскольку акселерометры x и y не работают. ортогонален вектору силы тяжести и будет показывать только шум. Это также поможет нам решить, поднялся ли подвес x1/x2 до pi/2 и снова опустился, или продолжал вращаться в том же направлении до > pi/2, потому что оба движения выглядят одинаково на z-акселерометре, а угол z2/z3 там не наблюдается.

Простой ответ:

Use two-argument arctangent.
The roll angle is atan2(ay, ax).
The pitch angle is atan2(az, sqrt(ax*ax + ay*ay)).
Then time-difference these to get angle rates.

У этого упрощенного решения есть ряд проблем, но это хорошее место для начала.

Вероятно, ключ, который вам нужен, заключается в следующем: вы должны преобразовать ускорения x и y из координат измерения в координаты машины, прежде чем оценивать шаг вокруг оси x машины. Это требует, чтобы вы сначала знали угол крена (относительно оси z машины). В синтаксисе Matlab,

[x_machine; y_machine] = [cos(roll) -sin(roll); sin(roll) cos(roll)] * [x_meas; y_meas].
z_machine = z_meas, always.

Учитывая x, y, z в координатах машины, вы можете напрямую оценить угол наклона и скорость относительно оси x машины:

pitch = atan2(z_machine, -y_machine)  (right hand rule about the machine x axis; positive acceleration pointing down); 
pitch_rate = -asin((xyz_i cross xyz_i-1)_x) / dt_i, 

где на английском языке скорость вычисляется из арксинуса компонента x_machine векторного произведения последнего вектора ускорения координаты машины с предыдущим, деленного на время между ними (1/8 секунды в вашем случае). Тот же подход работает для оценки крена и скорости крена (относительно оси z машины):

roll = atan2(-x_meas, -y_meas) * cos(pitch) / abs(cos(pitch));  
roll_rate = -asin((xyz_meas_i cross xyz_meas_i-1)_z) * cos(pitch) / abs(cos(pitch)) / dt_i.

Это проблема курицы и яйца, где вам нужно знать тангаж и крен, чтобы оценить тангаж и крен и их скорость. Таким образом, вам нужно начать с хорошего предположения о правильных углах тангажа и крена (в пределах 15 градусов или около того должно быть хорошо). Все измерения зашумлены, поэтому оценки тоже будут. Скорость оценивает особенно так. Таким образом, вы захотите отфильтровать оценки во времени. Распространяйте оценки углов тангажа и крена во времени, используя отфильтрованные оценки угловой скорости. Кроме того, ваши оценки угла крена и скорости становятся чистым шумом, поскольку шаг близок к +/-pi/2, поэтому вам, вероятно, следует уменьшить входные данные для фильтров крена на что-то вроде cos ^ 2 (шаг).