【算法】基于STM32的MPU6050卡尔曼滤波算法（入门级）

原创

已于 2022-07-15 23:34:06 修改 · 3.6w 阅读

108

850

于 2022-05-13 19:41:22 首次发布

本文介绍了如何在RT-Thread平台上，利用卡尔曼滤波算法对MPU6050的加速度和陀螺仪数据进行融合，以修正yaw轴缺失的限制，计算并优化pitch和roll轴的角度数据。关键步骤包括先验估计、协方差矩阵更新和卡尔曼增益计算。源代码和相关链接可供参考。

1. 简介

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。详情见：卡尔曼滤波简介
MPU6050的解算主要有三种姿态融合算法：四元数法、一阶互补算法和卡尔曼滤波算法。我们常用的DMP库使用的是四元数法，本文采用卡尔曼滤波算法，使用RT-Thread国产操作系统，利用env工具进行串口、模拟IIC环境配置，使用10ms的线程进行卡尔曼滤波解算。

2. 设计思想

因为MPU6050没有包含磁力计，故无法对yaw轴运用卡尔曼滤波算法。利用MPU6050中加速度传感器采集到的xyz轴的加速度和陀螺仪采集到的xyz轴的角速度，进行进一步处理，得到pitch轴和roll轴的原始角度，利用原始角度和角速度进行卡尔曼滤波处理，最终得到滤波后的角度数据。

3. 流程图

在这里插入图片描述

4. 计算公式及源代码

在此公布所有计算公式和部分源代码，所有源代码请见最下方的下载链接

卡尔曼参数

static float Q_angle = 0.001;		//角度数据置信度，角度噪声的协方差
static float Q_gyro  = 0.003;		//角速度数据置信度，角速度噪声的协方差  
static float R_angle = 0.5;			//加速度计测量噪声的协方差
static float dt      = 0.01;		//采样周期即计算任务周期10ms
static float Q_bias;				//Q_bias:陀螺仪的偏差
static float K_0, K_1;				//卡尔曼增益  K_0:用于计算最优估计值  K_1:用于计算最优估计值的偏差
static float PP[2][2] = {
   
    {
   
    1, 0 },{
   
    0, 1 } };//过程协方差矩阵P，初始值为单位阵

（1）进行先验估计计算

先验估计方程，公式1：X(k|k-1) = AX(k-1|k-1) + BU(k) + (W(k))
应用到本文得：
在这里插入图片描述
其中newGyro代表陀螺仪测得得角速度，代码如下↓

	/*
	1. 先验估计
* * *公式1：X(k|k-1) = AX(k-1|k-1) + BU(k) + (W(k))

		X = (Angle,Q_bias)
		A(1,1) = 1,A(1,2) = -dt
		A(2,1) = 0,A(2,2) = 1
		注：上下连“[”代表矩阵
		预测当前角度值：
		[ angle ] 	[1 -dt][ angle ]   [dt]
		[ Q_bias] = [0  1 ][ Q_bias] + [ 0] * newGyro(加速度计测量值)
		故
		angle = angle - Q_bias*dt + newGyro * dt
		Q_bias = Q_bias
	*/
	pitch_kalman += (gyro - Q_bias) * dt; //状态方程,角度值等于上次最优角度加角速度减零漂后积分

（2）预测协方差矩阵

公式2：P(k|k-1)=AP(k-1|k-1)A^T + Q
由先验估计有系统参数
在这里插入图片描述
系统过程协方差Q定义

令D( angle ) = Q_angle ，D( Q_bias ) = Q_gyro
设上一次预测协方差矩阵为P(k-1)
在这里插入图片描述
本次预测协方差矩阵P(k)

将以上参数带入预测协方差公式得：

代码如下↓

	/*
	2. 预测协方差矩阵
* * *公式2：P(k|k-1)=AP(k-1|k-1)A^T + Q 
	*/
	//由于dt^2太小，故dt^2省略
	PP[0][0

标签

#算法 #stm32 #arm

最低0.47元/天解锁文章