四元数法求姿态角，六轴

coding:utf-8

四元数法

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

# 错