卡尔曼滤波解算欧拉角（去积分漂移版本）

最新推荐文章于 2025-05-11 00:48:25 发布

阿让啊

最新推荐文章于 2025-05-11 00:48:25 发布

阅读量2.5k

点赞数 20

CC 4.0 BY-SA版权

分类专栏： IMU 文章标签：算法

本文链接：https://blog.csdn.net/kivenx/article/details/137033189

IMU 专栏收录该内容

2 篇文章

订阅专栏

文章描述了一个项目中如何通过IIC接口从QMI8658六轴传感器读取数据，进行零漂移和积分漂移去除，然后利用卡尔曼滤波和均值滤波技术融合加速度计和陀螺仪数据，计算并输出横滚角、俯仰角。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

近期在做模拟IIC读取QMI8658六轴传感器数据，滤波融合解算姿态角：

项目要求：

①去除零漂移、

②去除陀螺仪积分漂移、

③输出横滚角roll、俯仰角Pitch（无磁力计故此无yaw角），角度单位（度）

先看结果：

因为是个人座面未完全水平，近似为0，输出稳定，没有积分漂移！

收敛速度可调节卡尔曼中协方差Q、R值。

在QMI数据读取中采样了10位数据求平均的均值滤波：

/****************************************************************************************
*   函数名: Filter_WindowFloat 窗口/均值滤波
*   功能说明: 计算数组平均值
*   形   参1: *arr 要滤波的数组指针
*   形   参2: num 滤波长度
*   返回值: 无
*****************************************************************************************/
float Filter_WindowFloat( short *arr, unsigned short num )
{
float sum = 0.0, vol ;
unsigned short i ;

for( i=0 ; i<num; i++ )
{
sum += arr[i] ;
}
vol = sum / num ;
return( vol ) ;
}

其次，主函数main中采用均值求零漂：

其main函数部分代码如下：

typedef struct
{
    short accelOffset[3]; // 加速度计XYZ数据均值
    short gyroOffset[3];  // 陀螺仪XYZ数据均值
} IMU_Filter_mean;          // 陀螺仪和加速度计的XYZ均值数据
IMU_Filter_mean Imu_Mean; // 陀螺仪和加速度计的XYZ均值数据
Accelerometer_Angle_Du A   ccelerometer_Angle_data; // 加速度计姿态角.数据结构体
IMU_Filter_mean_sum Imu_Sum;                            // 陀螺仪和加速度计数据求和

short OffData[6];                         // 存储IMU原始数据

float IMURoll;    // 卡尔曼滤波后的横滚角
float IMUPitch; // 卡尔曼滤波后的俯仰角
// 主函数
void main()
{
    unsigned char states_Init = 0;
    unsigned char states = 0;
    unsigned short offset_count = 0;

    // 系统时钟120M,外设高频时钟16M
    SystemInit(120);          // 系统时钟初始化
    systick_delay_init(120U); // 系统初始化

    USART1_Init(); // 串口1初始化

    I2C_GPIO_Init(); // 软件I2C初始化

    states_Init = Qmi8658_init(); // QMI8658C初始化

// 初始化函数,将QMI8658C_Data_float_Old内存块，全部设置为0x00
    memset(&QMI8658C_Data_float_Old, 0x00, sizeof(QMI8658C_Data_float));

// 计算加速度计、陀螺仪的均值
    //  计算零偏
    for (short i = 0; i < 10000; i++)
    {
        Gyro_Offset_Calibration(OffData);                           // 读出加速度计、陀螺仪数据
        Imu_Sum.gyro_offsets_sum[0] += QMI8658C_AGM_float.GX_rads; // 陀螺仪X数据之和
        Imu_Sum.gyro_offsets_sum[1] += QMI8658C_AGM_float.GY_rads;
        Imu_Sum.gyro_offsets_sum[2] += QMI8658C_AGM_float.GZ_rads;
        Imu_Sum.accel_offsets_sum[0] += QMI8658C_AGM_float.AX_g; // 加速度计X数据之和
        Imu_Sum.accel_offsets_sum[1] += QMI8658C_AGM_float.AY_g;
        Imu_Sum.accel_offsets_sum[2] += QMI8658C_AGM_float.AZ_g;
        offset_count++;
        if (offset_count > 500)
        {
            Imu_Mean.gyroOffset[0] = Imu_Sum.gyro_offsets_sum[0] / offset_count; // 陀螺仪X数据均值
            Imu_Mean.gyroOffset[1] = Imu_Sum.gyro_offsets_sum[1] / offset_count;
            Imu_Mean.gyroOffset[2] = Imu_Sum.gyro_offsets_sum[2] / offset_count;
            Imu_Mean.accelOffset[0] = Imu_Sum.accel_offsets_sum[0] / offset_count; // 加速度计X数据均值
            Imu_Mean.accelOffset[1] = Imu_Sum.accel_offsets_sum[0] / offset_count;
            Imu_Mean.accelOffset[2] = Imu_Sum.accel_offsets_sum[0] / offset_count;
            // Imu_Mean.accelOffset[2] -= 9.81; // Z轴去除重力加速度常量

            offset_count = 0;
            Imu_Sum.gyro_offsets_sum[0] = 0;
            Imu_Sum.gyro_offsets_sum[1] = 0;
            Imu_Sum.gyro_offsets_sum[2] = 0;
            Imu_Sum.accel_offsets_sum[0] = 0;
            Imu_Sum.accel_offsets_sum[1] = 0;
            Imu_Sum.accel_offsets_sum[2] = 0;
            break;
        }
    }

while (1)
    {

        OffData[0] -= Imu_Mean.gyroOffset[0];  // X轴陀螺仪原始数据减去零偏
        OffData[1] -= Imu_Mean.accelOffset[1]; // Y轴陀螺仪原始数据减去零偏
        OffData[2] -= Imu_Mean.gyroOffset[2];  // Z轴陀螺仪原始数据减去零偏
        // 去除加速度计零偏
        OffData[3] -= Imu_Mean.accelOffset[0]; // X轴加速度原始数据减去零偏
        OffData[4] -= Imu_Mean.accelOffset[1]; // Y轴加速度原始数据减去零偏
        OffData[5] -= Imu_Mean.accelOffset[2]; // Z轴加速度原始数据减去零偏

        Angle_Calcu(&IMURoll, &IMUPitch); // 卡尔曼滤波数据处理

        fprintf(USART1_STREAM, "Roll = %.6f", IMURoll);
        fprintf(USART1_STREAM, "/");
        fprintf(USART1_STREAM, "Pitch = %.6f", IMUPitch
        systick_delay_ms(10);
    }
}

重点来了以下为卡尔曼滤波源码：

float realVals[4];  //原始数据储存

float Accel_x;	     		//X轴加速度值暂存
float Accel_y;	    		//Y轴加速度值暂存
float Accel_z;	     		//Z轴加速度值暂存

float Angle_x_temp;  		//由加速度计算的x倾斜角度
float Angle_y_temp;  		//由加速度计算的y倾斜角度
float Angle_Z_temp;  		//由加速度计算的y倾斜角度
float Angle_Y_Final; 		//Y最终倾斜角度

float Gyro_x;				//X轴陀螺仪数据暂存
float Gyro_y;        		//Y轴陀螺仪数据暂存
float Gyro_z;		 		//Z轴陀螺仪数据暂存

float Roll,Pitch,Yaw,Gyro_X;

float Angle_Y_Final;			//解算后横滚角
float Angle_X_Final;			//解算后俯仰角
float Angle_Z_Final;			//解算后航向角

//卡尔曼参数
float Q_angle = 0.001;		//角度数据置信度，角度噪声的协方差  //0.002
float Q_gyro  = 0.003;		//角速度数据置信度，角速度噪声的协方差  //0.005
float R_angle = 0.5;	  	//加速度计测量噪声的协方差              //0.03
float dt      = 0.005;		  //滤波算法计算周期，由定时器定时20ms  //0.005
char  C_0     = 1;			  //H矩阵值
float Q_bias_X, Q_bias, Angle_err;	//Q_bias_X:陀螺仪的X轴偏差 Q_bias：陀螺仪Y轴偏差  Angle_err:角度偏量
float PCt_0, PCt_1, E;		//计算的过程量
float K_0, K_1, t_0, t_1;	//卡尔曼增益  K_0:用于计算最优估计值  K_1:用于计算最优估计值的偏差 t_0/1:中间变量
float P[4] ={0,0,0,0};	  //过程协方差矩阵的微分矩阵，中间变量
float PP[2][2] = { { 1, 0 },{ 0, 1 } };//过程协方差矩阵P

float Kalman_Filter_X(float Accel,float Gyro) //卡尔曼函数
{

	//步骤一，先验估计
	//公式：X(k|k-1) = AX(k-1|k-1) + BU(k)
	//X = (Angle,Q_bias_X)
	//A(1,1) = 1,A(1,2) = -dt
	//A(2,1) = 0,A(2,2) = 1
	Angle_X_Final += (Gyro - Q_bias_X) * dt; //状态方程,角度值等于上次最优角度加角速度减零漂后积分

	//步骤二，计算过程协方差矩阵的微分矩阵
	//公式：P(k|k-1)=AP(k-1|k-1)A^T + Q
	//Q(1,1) = cov(Angle,Angle)	Q(1,2) = cov(Q_bias_X,Angle)
	//Q(2,1) = cov(Angle,Q_bias_X)	Q(2,2) = cov(Q_bias_X,Q_bias_X)


	//2023.11.24 更新
	P[0]=dt*PP[1][1] - PP[0][1] - PP[1][0] + Q_angle;
	//P[0]= Q_angle - PP[0][1] - PP[1][0]; //Q_angle
	P[1]= -PP[1][1];// 先验估计误差协方差
	P[2]= -PP[1][1];
	P[3]= Q_gyro;//Q_gyro
	PP[0][0] += P[0] * dt;
	PP[0][1] += P[1] * dt;
	PP[1][0] += P[2] * dt;
	PP[1][1] += P[3] * dt;

	//步骤三，计算卡尔曼增益
	//公式：Kg(k)= P(k|k-1)H^T/(HP(k|k-1)H^T+R)
	//Kg = (K_0,K_1) 对应Angle,Q_bias_X增益
	//H = (1,0)	可由z=HX+v求出z:Accel

	PCt_0 = C_0 * PP[0][0];
	PCt_1 = C_0 * PP[1][0];
	E = R_angle + C_0 * PCt_0;//R_angle
	K_0 = PCt_0 / E;
	K_1 = PCt_1 / E;
	Angle_err = Accel - Angle_X_Final;	//Z(k)先验估计 计算角度偏差
	//步骤四，后验估计误差协方差
	//公式：P(k|k)=(I-Kg(k)H)P(k|k-1)
	//也可写为：P(k|k)=P(k|k-1)-Kg(k)HP(k|k-1)
	t_0 = PCt_0;
	t_1 = C_0 * PP[0][1];
	PP[0][0] -= K_0 * t_0;
	PP[0][1] -= K_0 * t_1;
	PP[1][0] -= K_1 * t_0;
	PP[1][1] -= K_1 * t_1;

	//步骤五，计算最优角速度值
	//公式：X(k|k)= X(k|k-1)+Kg(k)(Z(k)-X(k|k-1))

	Angle_X_Final += K_0 * Angle_err;	 //后验估计，给出最优估计值
	Q_bias_X        += K_1 * Angle_err;	 //后验估计，跟新最优估计值偏差
	Gyro_x         = Gyro - Q_bias_X;
	return Angle_X_Final;

}

float Kalman_Filter_Y(float Accel,float Gyro)
{
	Angle_Y_Final += (Gyro - Q_bias) * dt;
	//P[0]=Q_angle - PP[0][1] - PP[1][0];
	P[0]=dt*PP[1][1] - PP[0][1] - PP[1][0] + Q_angle;
	P[1]=-PP[1][1];
	P[2]=-PP[1][1];
	P[3]=Q_gyro;
	PP[0][0] += P[0] * dt;
	PP[0][1] += P[1] * dt;
	PP[1][0] += P[2] * dt;
	PP[1][1] += P[3] * dt;
	Angle_err = Accel - Angle_Y_Final;
	PCt_0 = C_0 * PP[0][0];
	PCt_1 = C_0 * PP[1][0];
	E = R_angle + C_0 * PCt_0;
	K_0 = PCt_0 / E;
	K_1 = PCt_1 / E;
	t_0 = PCt_0;
	t_1 = C_0 * PP[0][1];
	PP[0][0] -= K_0 * t_0;
	PP[0][1] -= K_0 * t_1;
	PP[1][0] -= K_1 * t_0;
	PP[1][1] -= K_1 * t_1;
	Angle_Y_Final	+= K_0 * Angle_err;
	Q_bias	+= K_1 * Angle_err;
	Gyro_y   = Gyro - Q_bias;
  return Angle_Y_Final;
}

//读取数据预处理
void Angle_Calcu(float*IMURoll,float*IMUPitch)
{

	//1.原始数据读取
	Gyro_Offset_Calibration(OffData);	//得到加速度传感器、陀螺仪数据

	Accel_x = (float)OffData[3];//x轴加速度值暂存
	Accel_y = (float)OffData[4];//y轴加速度值暂存
	Accel_z = (float)OffData[5];//z轴加速度值暂存

	Gyro_x  = (float)OffData[0];//x轴陀螺仪值暂存
	Gyro_y  = (float)OffData[1];//y轴陀螺仪值暂存
	Gyro_z  = (float)OffData[2];//z轴陀螺仪值暂存

		//2.角加速度原始值处理过程
	//加速度传感器设置范围为±2g。换算关系：2^16/4 = 16384LSB/g
	if(Accel_x<32764) realVals[0]=Accel_x/16384;  else  realVals[0]=1-(Accel_x-49152)/16384;//计算x轴加速度
	if(Accel_y<32764) realVals[1]=Accel_y/16384;  else  realVals[1]=1-(Accel_y-49152)/16384;//计算y轴加速度
	if(Accel_z<32764) realVals[2]=Accel_z/16384;  else  realVals[2]=  (Accel_z-49152)/16384;//计算z轴加速度



	//加速度反正切公式计算三个轴和水平面坐标系之间的夹角
	Angle_y_temp=(atan(realVals[0]/realVals[2]))*180/PI;  //picth
	Angle_x_temp=(atan(realVals[1]/realVals[2]))*180/PI;  //roll


	//判断计算后角度的正负号
	if(Accel_x<32764) Angle_x_temp = +Angle_x_temp;
	if(Accel_x>32764) Angle_x_temp = -Angle_x_temp;
	if(Accel_y<32764) Angle_y_temp = +Angle_y_temp;
	if(Accel_y>32764) Angle_y_temp = -Angle_y_temp;

	//3.角速度原始值处理过程
	//陀螺仪设置范围为±128deg/s。换算关系：2^16/256=256LSB/(°/S)
	计算角速度
	if(Gyro_x<32768) Gyro_x=-(Gyro_x/256.0);
	if(Gyro_x>32768) Gyro_x=+(65535-Gyro_x)/256.0;

	if(Gyro_y<32768) Gyro_y=-(Gyro_y/256.0);
	if(Gyro_y>32768) Gyro_y=+(65535-Gyro_y)/256.0;

	if(Gyro_z<32768) Gyro_z=-(Gyro_z/256.0);
	if(Gyro_z>32768) Gyro_z=+(65535-Gyro_z)/256.0;

    /*计算微分时间,使用时间戳*/
	/*now = millis();					//当前时间（ms）
	dt = (now - lasttime)/1000.0;  //微分时间（s）
	lasttime = now;					//上一次采样时间（ms）
	*/
//-------卡尔曼滤波融合-----------------------
	//4.调用卡尔曼函数
	*IMURoll=Kalman_Filter_X(Angle_x_temp,Gyro_x);  //卡尔曼滤波计算X倾角
	*IMUPitch=Kalman_Filter_Y(Angle_y_temp,Gyro_y);  //卡尔曼滤波计算Y倾角
	//printf("%f,%f,%f,%f\n",Angle_x_temp,Angle_y_temp,IMURoll,IMUPitch);//源数据串口输出波型
}

复制即可使用，方便曾经和我一样踩坑的朋友！