六轴数据转换四元数

四元数法
6轴计算

import math

IMU算法更新
#Kp = 100 # 比例增益控制加速度计/磁强计的收敛速度
#Ki = 0.002 # 积分增益控制陀螺偏差的收敛速度
#halfT = 0.001 # 采样周期的一半

Kp = 2.0 # 比例增益控制加速度计/磁强计的收敛速度
Ki = 0.005 # 积分增益控制陀螺偏差的收敛速度
halfT = 0.5 # 采样周期的一半

传感器框架相对于辅助框架的四元数(初始化四元数的值)
q0 = 1
q1 = 0
q2 = 0
q3 = 0

由Ki缩放的积分误差项(初始化)
exInt = 0
eyInt = 0
ezInt = 0

def Update_IMU(ax, ay, az, gx, gy, gz):
global q0, q1, q2, q3
global exInt, eyInt, ezInt

# 测量正常化
norm = math.sqrt(ax * ax + ay * ay + az * az)
# 单元化
ax = ax / norm
ay = ay / norm
az = az / norm

# 估计方向的重力
vx = q1 * q3 - q0 * q2
vy = q0 * q1 + q2 * q3
vz = q0 * q0 - 0.5 + q3 * q3

# 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex = (ay * vz - az * vy)
ey = (az * vx - ax * vz)
ez = (ax * vy - ay * vx)

# 积分误差比例积分增益
exInt += ex * Ki * (1/50)
eyInt += ey * Ki * (1/50)
ezInt += ez * Ki * (1/50)

# 调整后的陀螺仪测量
gx += Kp * ex + exInt
gy += Kp * ey + eyInt
gz += Kp * ez + ezInt

# 整合四元数
gx *= 0.5*(1/50)
gy *= 0.5*(1/50)
gz *= 0.5*(1/50)
q0 += (-q1 * gx - q2 * gy - q3 * gz)
q1 += (q0 * gx + q2 * gz - q3 * gy)
q2 += (q0 * gy - q1 * gz + q3 * gx)
q3 += (q0 * gz + q1 * gy - q2 * gx)

# 正常化四元数
norm = math.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)
q0 /= norm
q1 /= norm
q2 /= norm
q3 /= norm

# 获取欧拉角 pitch、roll、yaw
#pitch = math.asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3
#roll = math.atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3
#yaw = math.atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3

pitch = -math.asin(2 * q1 * q3 + 2 * q0 * q2) * 57.3
roll = math.atan2(2 * q1 * q2 - 2 * q0 * q3, 2 * q0 * q0 + 2 * q1 * q1 - 1) * 57.3
yaw = math.atan2(2 * q2 * q3 - 2 * q0 * q1, 2 * q0 * q0 + 2 * q3 * q3 - 1) * 57.3

return pitch, roll, yaw