Python處理加速度和陀螺儀數據計算姿態角

通過x、y、z加速度和陀螺儀計算姿態角（歐拉角）

#coding:utf-8

import math

#IMU算法更新


Kp = 100 #比例增益控制加速度計/磁強計的收斂速度
Ki = 0.002 #積分增益控制陀螺偏差的收斂速度
halfT = 0.001 #採樣週期的一半

#傳感器框架相對於輔助框架的四元數(初始化四元數的值)
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
    global q1
    global q2
    global q3
    global exInt
    global eyInt
    global ezInt
    # print(q0)
    
    #測量正常化
    norm = math.sqrt(ax*ax+ay*ay+az*az)
    #單元化
    ax = ax/norm
    ay = ay/norm
    az = az/norm
    
    #估計方向的重力
    vx = 2*(q1*q3 - q0*q2)
    vy = 2*(q0*q1 + q2*q3)
    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3
    
    #錯誤的領域和方向傳感器測量參考方向之間的交叉乘積的總和
    ex = (ay*vz - az*vy)
    ey = (az*vx - ax*vz)
    ez = (ax*vy - ay*vx)
    
    #積分誤差比例積分增益
    exInt += ex*Ki
    eyInt += ey*Ki
    ezInt += ez*Ki
    
    #調整後的陀螺儀測量
    gx += Kp*ex + exInt
    gy += Kp*ey + eyInt
    gz += Kp*ez + ezInt
    
    #整合四元數
    q0 += (-q1*gx - q2*gy - q3*gz)*halfT
    q1 += (q0*gx + q2*gz - q3*gy)*halfT
    q2 += (q0*gy - q1*gz + q3*gx)*halfT
    q3 += (q0*gz + q1*gy - q2*gx)*halfT
    
    #正常化四元數
    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
    return pitch,roll,yaw

參考博客 == https://www.amobbs.com/thread-5492189-1-1.html