Хорошо, в основном, рыскание, тангаж и крен - это углы Эйлера , с ними вы уже получили свою матрицу вращения.Кватернионы схожи с этим, с их помощью вы также можете вычислить необходимую матрицу вращения.
Если у вас есть матрицы вращения R_i для каждого момента i в вашем интервале l=20secs.Чем эти повороты являются относительными к тому, который применяется при R_(i-1), вы можете рассчитать их повороты относительно первой позиции.Итак, A_i = R_1*...*R_i, но в конце концов вы также можете просто сохранить новое направление движения (расчеты сейфов).
При условии, что сначала направление движения будет d_0 = (1,0,0).Вы можете вычислить следующее по d_i = R_i*d_(i-1) (всегда норма d_(i-1), потому что оно может уменьшиться или увеличиться из-за ошибки).Первая позиция p, ваша начальная скорость v_0 = (0,0,0) и, наконец, ускорение a_i.Вам нужно рассчитать векторную скорость v_i для каждого момента:
v_i = v_(i-1) + l*a_i*A_i*d_0 = v_(i-1) + l*a_i*d_i
Теперь вы в основном знаете, куда вы движетесь и какую скорость используете, поэтому ваша позиция p_i на данный момент i определяется как:
`p_i = p_0 + l * ( v_1 + v_2 + ... + v_i)`
Для единиц:
a_i = [m/s^2]^3
v_i = [m/s]^3
p_i = [m]^3
Точность
Теперь несколько моментов с точностью вашей позициирасчет (просто если вы хотите знать, насколько хорошо это будет работать).Предположим, у вас есть ошибка e>= ||R_i*v-w|| (где w - правильный вектор).в данных вы рассчитываете матрицы вращения с.Ваша ошибка умножается сама собой, поэтому ваша ошибка в i моменте составляет e_i <= e^i.
Тогда, поскольку вы применяете к ней l и a_i, она становится:
f_i <= l*a_i*e^i
Но вытакже добавляется ошибка при добавлении скорости, так что теперь ее g_i <= f_1+...+f_i.И да, вы также складываете для позиции (обе суммы превышают i):
h_i <= g_1+...+g_i = ΣΣ (l*a_i*e^i) = l* ΣΣ (a_i*e^i)
Так что это в основном максимальная разница от вашей позиции p_i до правильной позиции w (||p_i - w|| <= h_i).
Это все еще не принимает во внимание, что вы не получаете правильное ускорение от вашего устройства (я не знаю, как они обычно делают это), потому что правильным будет:
a_i = ||∫a_i(t) dt|| (where a_i(t) is vectorial now)
И вам нужно будет рассчитать разницу в направлении (матрицу вращения) следующим образом:
Δd_i = (∫a_i(t) dt)/a_i (again a_i(t) is vectorial)
Таким образом, кроме ошибок, которые вы получаете из-за ошибки ваших вращений с вашего устройства (и из арифметики с плавающей запятой)), у вас есть ошибка в вашем ускорении, я не буду вычислять это сейчас, но вы замените a_i = a_i + b_i.
Так что я почти уверен, что это будет далеко от реальной позиции.Вы даже должны принять во внимание, что ваша скорость может отличаться от нуля, когда она должна быть!
Но, как уже говорилось, я действительно хотел бы знать, какую точность вы получите после ее реализации, это то, что всегда сохраняетсяя от этого попробую.