近期在做模拟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);//源数据串口输出波型
}
复制即可使用，方便曾经和我一样踩坑的朋友！
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
