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

1. 简介

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

2. 设计思想

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

3. 流程图

【算法】基于STM32的MPU6050卡尔曼滤波算法(入门级)_第1张图片

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))
应用到本文得:
【算法】基于STM32的MPU6050卡尔曼滤波算法(入门级)_第2张图片
其中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定义
【算法】基于STM32的MPU6050卡尔曼滤波算法(入门级)_第3张图片

令D( angle ) = Q_angle ,D( Q_bias ) = Q_gyro
设上一次预测协方差矩阵为P(k-1)
【算法】基于STM32的MPU6050卡尔曼滤波算法(入门级)_第4张图片
本次预测协方差矩阵P(k)
【算法】基于STM32的MPU6050卡尔曼滤波算法(入门级)_第5张图片
将以上参数带入预测协方差公式得:
【算法】基于STM32的MPU6050卡尔曼滤波算法(入门级)_第6张图片
代码如下↓

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

(3)建立测量方程

系统测量方程 Z(k) = HX(k) + V(k),其中系统测量系数 H = [1, 0]。
因为陀螺仪输出自带噪声,所以measure = newAngle。

(4)计算卡尔曼增益

卡尔曼增益系数方程,公式3:
【算法】基于STM32的MPU6050卡尔曼滤波算法(入门级)_第7张图片
带入本文得:
【算法】基于STM32的MPU6050卡尔曼滤波算法(入门级)_第8张图片
代码如下↓

	/*
		4. 计算卡尔曼增益
* * *公式3:Kg(k)= P(k|k-1)H^T/(HP(k|k-1)H^T+R)
				Kg = (K_0,K_1) 对应angle,Q_bias增益
				H = (1,0)
	*/
	K_0 = PP[0][0] / (PP[0][0] + R_angle);
	K_1 = PP[1][0] / (PP[0][0] + R_angle);

(5)计算当前最优化估计值

最优化估计值方程,公式4:X(k|k) = X(k|k-1) + kg(k)[z(k) - HX(k|k-1)]
带入本文得:
在这里插入图片描述
代码如下↓

	/*
		5. 计算当前最优化估计值
* * *公式4:X(k|k) = X(k|k-1) + kg(k)[z(k) - HX(k|k-1)]
		angle = angle + K_0*(newAngle - angle)
		Q_bias = Q_bias + K_1*(newAngle - angle)
	*/
		
	pitch_kalman = pitch_kalman + K_0 * (acc - pitch_kalman);
	Q_bias = Q_bias + K_1 * (acc - pitch_kalman);

(6)更新协方差矩阵

根据误差协方差得到公式5:P(k|k)=[I-Kg(k)H]P(k|k-1)
带入本文得
在这里插入图片描述
代码如下↓

	/*
		6. 更新协方差矩阵
* * *公式5:P(k|k)=[I-Kg(k)H]P(k|k-1)
	*/
	PP[0][0] = PP[0][0] - K_0 * PP[0][0];
	PP[0][1] = PP[0][1] - K_0 * PP[0][1];
	PP[1][0] = PP[1][0] - K_1 * PP[0][0];
	PP[1][1] = PP[1][1] - K_1 * PP[0][1];

下面是roll轴完整处理代码

void Kalman_Cal_Roll(float acc,float gyro) //卡尔曼滤波roll轴计算				
{
	static float Q_bias;	//Q_bias:陀螺仪的偏差  Angle_err:角度偏量 
	static float K_0, K_1;	//卡尔曼增益  K_0:用于计算最优估计值  K_1:用于计算最优估计值的偏差 t_0/1:中间变量
	static float PP[2][2] = { { 1, 0 },{ 0, 1 } };//过程协方差矩阵P,初始值为单位阵
	roll_kalman += (gyro - Q_bias) * dt; //状态方程,角度值等于上次最优角度加角速度减零漂后积分
	PP[0][0] = PP[0][0] + Q_angle - (PP[0][1] + PP[1][0])*dt;
	PP[0][1] = PP[0][1] - PP[1][1]*dt;
	PP[1][0] = PP[1][0] - PP[1][1]*dt;
	PP[1][1] = PP[1][1] + Q_gyro;
	K_0 = PP[0][0] / (PP[0][0] + R_angle);
	K_1 = PP[1][0] / (PP[0][0] + R_angle);
	roll_kalman = roll_kalman + K_0 * (acc - roll_kalman);
	Q_bias = Q_bias + K_1 * (acc - roll_kalman);
	PP[0][0] = PP[0][0] - K_0 * PP[0][0];
	PP[0][1] = PP[0][1] - K_0 * PP[0][1];
	PP[1][0] = PP[1][0] - K_1 * PP[0][0];
	PP[1][1] = PP[1][1] - K_1 * PP[0][1];
}

卡尔曼滤波效果

【算法】基于STM32的MPU6050卡尔曼滤波算法(入门级)_第9张图片

完整工程Gitee链接:
https://gitee.com/lebron-meng/rt-thread-kalman-filtering.git

参考资料:
图说卡尔曼滤波,一份通俗易懂的教程
从放弃到精通!卡尔曼滤波从理论到实践~
CH32读取MPU6050姿态数据(卡尔曼滤波法)

你可能感兴趣的:(Algorithm,C语言,算法,stm32,arm)