對於姿態角解算，目前我所知道的只有兩種，一種是dmp庫解算出姿態角、一種是演算法進行姿態解算；
本次說的是姿態解算，採集的是六軸的原始資料，用python進行姿態解算；廢話不多說，看程式，有註解的。。
import math
import csv
kp=100 #比例增益控制加速度計/磁強計的收斂速度
ki=0.002 #積分增益控制陀螺儀偏差的收斂速度
halft=0.001 #取樣週期的一半
#感測器框架相對於輔助框架的四元數（初始化四元數的值）
q0=1
q1=0
q2=0
q3=0
#由ki縮放的積分誤差項（初始化）
exint=0
eyint=0
ezint=0
'''舉例子：「-0.31,-0.11,-0.95,-26,70,9」
#三軸加速度原始資料
ax=-0.31
ay=-0.11
az=-0.95
#三軸角速度原始資料
gx=-26
gy=70
gz=9
'''#讀取data.csv的資料然後把對比結果資料寫入到data.txt檔案中
f0=open("data.txt",'w',encoding='utf-8')
f0.write("\n")
csvname="data.csv"
with open(csvname,"r") as f:
csvhandle = csv.reader(f)
for row in csvhandle:
aax=row[0] #原始加速度x軸資料
aay=row[1] #原始加速度y軸資料
aaz=row[2] #原始加速度z軸資料
ggx=row[3] #原始角速度x軸資料
ggy=row[4] #原始角速度y軸資料
ggz=row[5] #原始角速度z軸資料
#把csv的字串轉化為float型
ax=float(aax)
ay=float(aay)
az=float(aaz)
gx=float(ggx)
gy=float(ggy)
gz=float(ggz)
#單元化
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
#列印姿態角資訊以及把姿態角資料寫到txt文字中
print("pry:%.3f,%.3f,%.3f"%(pitch,roll,yaw))
f0.write("pitch:%.3f;roll:%.3f;yaw:%.3f"%(pitch,roll,yaw))
f0.write("\n")
Python 資料處理
將檔案切分，存入列表 strip split with open james.txt as jaf data jaf.readline james data.strip split 資料檔案為 2 34,3 21,2.34,2.45,3.01,2 01,2 01,3 10,2 22 print ja...
Python 資料處理
本場 chat 為 python 資料處理課程，包括 python 基礎知識 極簡教程 提公升 python 執行效率的方法 爬蟲簡介 scrapy selenium 自動化測試框架 簡易分布式 redis 分詞程式設計 jieba 資料儲存 本地資料上傳 hive 通過本場 chat 讀者可學到以...
Python資料處理
用傳統的電子 來處理資料不僅效率低下，而且無法處理某些格式的資料，對於混亂或龐大的資料集更是束手無策。本書將教你如何利用語法簡單 容易上手的python輕鬆處理資料。作者通過循序漸進的練習，詳細介紹如何有效地獲取 清洗 分析與呈現資料，如何將資料處理過程自動化，如何安排檔案編輯與清洗任務，如何處理更...