尔曼滤波

前言MPU6050寄存器代码详解mpu6050.cmpu6050.h

使用说明

本篇文章基于卡尔曼滤波的原理详解与公式推导来详细的解释下如何使用卡尔曼滤波来解算MPU6050的姿态

参考资料Github_mpu6050

我们简单介绍下MPU6050驱动里设计到的寄存器详情见MPU-60X0寄存器手册

Who

为数不多的默认值不为0的寄存器其默认值为0x68也即MPU6050的设备地址通过读取该寄存器的值来判断识别到的设备是否是MPU6050

Power

通过该寄存器配置电源模式、时钟源选择、设备复位和睡眠模式等功能。

Sample

|---------------------------------------------------------------------------------*

Copyright

https://github.com/TKJElectronics/KalmanFilter*

|---------------------------------------------------------------------------------*/#include

math.h

57.295779513082320876798154814105

(180/π)其中

寄存器)用于配置电源模式、时钟源选择、设备复位和睡眠模式等功能。

#define

MPU6050_Read_Accel(I2C_HandleTypeDef

*I2Cx,

i2c_timeout。

HAL_I2C_Mem_Read(I2Cx,

MPU6050_ADDR,

Rec_Data[1]);DataStruct-Accel_Y_RAW

Rec_Data[3]);DataStruct-Accel_Z_RAW

Rec_Data[5]);/***

(由于Z轴加速度计的值会受重力、传感器零飘、传感器精度与校准的影响导致在水平放置时Accel_Z_RAW

16384而是实际值。

修正参数就是根据实际情况测出的用来矫正零飘和其他影响的校正因子

}void

MPU6050_Read_Gyro(I2C_HandleTypeDef

*I2Cx,

i2c_timeout。

HAL_I2C_Mem_Read(I2Cx,

MPU6050_ADDR,

Rec_Data[1]);DataStruct-Gyro_Y_RAW

Rec_Data[3]);DataStruct-Gyro_Z_RAW

Rec_Data[5]);/***

MPU6050_Read_Temp(I2C_HandleTypeDef

*I2Cx,

字节数据高字节和低字节HAL_I2C_Mem_Read(I2Cx,

MPU6050_ADDR,

并使用卡尔曼滤波器对预测角度与测量角度(加速度计和陀螺仪数据)进行融合以计算俯仰角pitch和滚转角roll。

**

param

结构体的指针存储读取到的原始传感器数据以及处理后的数据如加速度、角速度、角度等*/

void

MPU6050_Read_All(I2C_HandleTypeDef

*I2Cx,

字节加速度、温度、角速度数据HAL_I2C_Mem_Read(I2Cx,

MPU6050_ADDR,

Rec_Data[1]);DataStruct-Accel_Y_RAW

Rec_Data[3]);DataStruct-Accel_Z_RAW

Rec_Data[5]);//

Rec_Data[9]);DataStruct-Gyro_Y_RAW

Rec_Data[11]);DataStruct-Gyro_Z_RAW

原始数据

卡尔曼滤波通过结合系统的预测值和测量值来修正角度和偏置的估计减少噪声对结果的影响。

*

param

卡尔曼滤波器结构体指针包含预测角度、预测偏置、角度协方差、偏置协方差、噪声协方差及协方差矩阵*

param

角度测量值读取加速度计的三轴加速度分量再计算反正切得到角度测量值*

param

角速度“实际“值他只是可以看作角速度实际值实际上是角速度测量值。

只不过因陀螺仪精度问题而有过程噪声。

可以理解为匀变速直线运动的状态方程中必须有

速度v

/*---------------------预测阶段--------------------------*///

预测角度//

预测偏置协方差/*---------------------更新阶段--------------------------*///

更新卡尔曼增益//

根据卡尔曼增益更新角度和偏置的估计值修正预测阶段的误差Kalman-angle

K[0]

传感器的结构和函数包括加速度计、陀螺仪、温度传感器的读取函数*

INC_GY521_H_

MPU6050_Read_Accel(I2C_HandleTypeDef

*I2Cx,

MPU6050_Read_Gyro(I2C_HandleTypeDef

*I2Cx,

MPU6050_Read_Temp(I2C_HandleTypeDef

*I2Cx,

MPU6050_Read_All(I2C_HandleTypeDef

*I2Cx,

使用卡尔曼滤波器计算角度根据新测得的角度和角速度更新滤波器返回平滑的角度值

double

loop(void){MPU6050_Read_All(hi2c1,

MPU6050);HAL_Delay

}经过卡尔曼滤波后得到的数据(最优估计)为MPU6050.KalmanAngleX和MPU6050.KalmanAngleY