IMU：姿态解算算法集合

最新推荐文章于 2025-10-28 20:14:00 发布

原创

最新推荐文章于 2025-10-28 20:14:00 发布 · 1.4w 阅读

269 ·

CC 4.0 BY-SA版权

文章标签：

#c语言 #IMU #姿态解算 #无人机 #MPU6050

文章目录

- - 一、IMU原理
  - 二、源码

一、IMU原理

二、源码

源文件：

#include "IMU.h"
#include "math.h"
////////////////////////////////////////////////////////////////////////////////
#define Kp 1.6f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.001f                      // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.001f                   // half the sample period 采样周期的一半
#define Gyro_Gr		0.0010653f		//角速度变成弧度 此参数对应陀螺2000度每秒
#define Gyro_G 		0.0610351f		//角速度变成度   此参数对应陀螺2000度每秒	
#define FILTER_NUM 	20

float 	AngleOffset_Rol=0,AngleOffset_Pit=0;
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;  // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error

FLOAT_XYZ 	Acc,Acc_out,Gyro;	              //把陀螺仪的各通道读出的数据，转换成弧度制
FLOAT_ANGLE Att_Angle;

void Prepare_Data(FLOAT_XYZ *acc_in,FLOAT_XYZ *acc_out)
{
   
   
	static uint8_t 	filter_cnt=0;
	static int16_t	ACC_X_BUF[FILTER_NUM],ACC_Y_BUF[FILTER_NUM],ACC_Z_BUF[FILTER_NUM];
	int32_t temp1=0,temp2=0,temp3=0;
	uint8_t i;

	ACC_X_BUF[filter_cnt] = acc_in->X;
	ACC_Y_BUF[filter_cnt] = acc_in-