(学习)用mahony算法解算mpu6050得到姿态角

我这个菜鸟在暑假打算做一架四轴无人机,选用的材料其中必然包含了mpu6050，但是就连本菜鸟都知道mpu6050只能输出原始数据而不能输出真实的姿态角,网上有许多种解算姿态角的方法,本菜鸟选用的是四元数解算姿态角,并用mahony算法得出的,效果如图:,听大佬说,没有磁力计加以校准,似乎yaw角的偏移是不可避免的,我参考了许多大佬的文章,最终也只能解算出这个样子,希望以后能解算的更好吧,在此附上源码,希望以后能解算得更好吧

#include <math.h>
#include "IMU.h"
#include "mpu6050.h"
const float kp=23;
const float halfT=0.005;//半次采样时间
#define G 9.8f
const float ki=0.005;





//基于mahony算法解算四元数得到姿态角的AHRS的算法,后续可以写写直接用卡尔曼滤波解算姿态角的AHRS算法，但需要磁力计加以辅助解算yaw角
angle GetAngle(int16_t *ax,int16_t *gx,angle xyz,int16_t *Acccorrect,int16_t *Gyrocorrect){
float norm;
float accx,accy,accz,grox,groy,groz;
float ex,ey,ez;

static float q0=1;
static float q1=0;
static float q2=0;
static float q3=0;
static float exInt=0.0;
static float eyInt=0.0;
static float ezInt=0.0;
float q0q0,q0q1,q0q2,q0q3,q1q1,q1q2,q1q3,q2q2,q2q3,q3q3;
q0q0=q0*q0;/*提前算好，方便后续运算，加快运算速度*/
q0q1=q0*q1;
q0q2=q0*q2;
q0q3=q0*q3;
q1q1=q1*q1;
q1q2=q1*q2;
q1q3=q1*q3;
q2q3=q2*q3;
q2q2=q2*q2;
q3q3=q3*q3;
accx=(float)(ax[0]-Acccorrect[0])/16384.0f*G;/*读取原始数据*/
accy=(float)(ax[1]-Acccorrect[1])/16384.0f*G;
accz=(float)(ax[2]-Acccorrect[2])/16384.0f*G;
grox=(gx[0]-Gyrocorrect[0])*0.061/57.3;
groy=(gx[1]-Gyrocorrect[1])*0.061/57.3;
groz=(gx[2]-Gyrocorrect[2])*0.061/57.3;

norm=sqrt(accx*accx+accy*accy+accz*accz);/*加速度归一化*/
accx/=norm;
accy/=norm;
accz/=norm;

float vx=2*(q1q3-q0q2);/*重力分量的提取*/
float vy=2*(q0q1+q2q3);
float vz=q0q0+q3q3-q1q1-q2q2;

ex=(accy*vz-accz*vy);/*解算重力分量引起的误差*/
ey=(accz*vx-accx*vz);
ez=(accx*vy-accy*vx);

exInt+=ki*ex;/*误差的累计，用于纠正角速度*/
eyInt+=ki*ey;
ezInt+=ki*ez;

grox+=(kp*ex+exInt);
groy+=(kp*ey+eyInt);
groz+=(kp*ez+ezInt);

//grox+=(factor*ex/halfT+exInt);
//groy+=(factor*ey/halfT+eyInt);
//groz+=(factor*ez/halfT+ezInt);

q0=q0+(-q1*grox-q2*groy-q3*groz)*halfT;/*更新四元数*/
q1=q1+(q0*grox+q2*groz-q3*groy)*halfT;
q2=q2+(q0*groy-q1*groz+q3*grox)*halfT;
q3=q3+(q0*groz+q1*groy-q2*grox)*halfT;

norm=sqrt(q0q0+q1q1+q2q2+q3q3);/*四元数归一化*/
q0=q0/norm;
q1=q1/norm;
q2=q2/norm;
q3=q3/norm;

xyz.pitch=-asin(2*q1q3-2*q0q2)*57.3;/*利用旋转矩阵与四元数表示旋转矩阵相同的特点，取对应余子式解算角度*/
xyz.roll=atan2(2*q2q3+2*q0q1,1-2*q1q1-2*q2q2)*57.3;
xyz.yaw=atan2(2*q1q2+2*q0q3,1-2*q2q2-2*q3q3)*57.3;

return xyz;


}