numpy 如何在EKF滤波器中将四元数转换为欧拉角?

pgpifvop  于 2023-08-05  发布在  其他
关注(0)|答案(1)|浏览(66)

我正在使用扩展卡尔曼滤波器,并试图找到方向从mag,accel和gyr。我得到了四元数,但我想把它转换成欧拉角。我该怎么做呢?我也要导入一些东西吗?这是代码:

import numpy as np
from ahrs.filters import QUEST
from ahrs.filters import EKF
from ahrs.common.orientation import acc2q
acc_data=np.array([[1,3,5.8]],dtype=float)
gyr_data=np.array([[2,4,8]],dtype=float)
mag_data=np.array([[1,2,3]],dtype=float)
ekf = EKF(gyr=gyr_data, acc=acc_data, mag=mag_data)
num_samples = 1             # Assuming sensors have 1000 samples each
Q = np.zeros((num_samples, 4))  # Allocate array for quaternions
Q[0] = acc2q(acc_data[0])       # First sample of tri-axial accelerometer
for t in range(1, num_samples):
    
    Q[t] = ekf.update(Q[t-1], gyr_data[t], acc_data[t])

字符串

qrjkbowd

qrjkbowd1#

import numpy as np
from math import atan2, asin

def quaternion_to_euler(q):
    roll = atan2(2*(q[0]*q[1] + q[2]*q[3]), 1 - 2*(q[1]**2 + q[2]**2))
    pitch = asin(2*(q[0]*q[2] - q[3]*q[1]))
    yaw = atan2(2*(q[0]*q[3] + q[1]*q[2]), 1 - 2*(q[2]**2 + q[3]**2))
    return np.array([roll, pitch, yaw])

字符串
使用四元数_to_Euler函数将四元数转换为EulerAngular :

euler_angles = quaternion_to_euler(Q)


source

相关问题