1. 简介
卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。详情见:卡尔曼滤波简介
MPU6050的解算主要有三种姿态融合算法:四元数法 、一阶互补算法和卡尔曼滤波算法。我们常用的DMP库使用的是四元数法,本文采用卡尔曼滤波算法,使用RT-Thread国产操作系统,利用env工具进行串口、模拟IIC环境配置,使用10ms的线程进行卡尔曼滤波解算。
2. 设计思想
因为MPU6050没有包含磁力计,故无法对yaw轴运用卡尔曼滤波算法。利用MPU6050中加速度传感器采集到的xyz轴的加速度和陀螺仪采集到的xyz轴的角速度,进行进一步处理,得到pitch轴和roll轴的原始角度,利用原始角度和角速度进行卡尔曼滤波处理,最终得到滤波后的角度数据。
3. 流程图
![在这里插入图片描述](https://img-blog.csdnimg.cn/452242c8ffeb4630b9771f662da5d67e.png)
4. 计算公式及源代码
在此公布所有计算公式和部分源代码,所有源代码请见最下方的下载链接
卡尔曼参数
static float Q_angle = 0.001;
static float Q_gyro = 0.003;
static float R_angle = 0.5;
static float dt = 0.01;
static float Q_bias;
static float K_0, K_1;
static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
(1)进行先验估计计算
先验估计方程,公式1:X(k|k-1) = AX(k-1|k-1) + BU(k) + (W(k))
应用到本文得:
![在这里插入图片描述](https://img-blog.csdnimg.cn/bca12a45d8f74d09bf8f2661e9ea339e.png)
其中newGyro代表陀螺仪测得得角速度,代码如下↓
pitch_kalman += (gyro - Q_bias) * dt;
(2)预测协方差矩阵
公式2:P(k|k-1)=AP(k-1|k-1)A^T + Q
由先验估计有系统参数
![在这里插入图片描述](https://img-blog.csdnimg.cn/9ba9165e4c254146802f9a58213c12ff.png)
系统过程协方差Q定义
![在这里插入图片描述](https://img-blog.csdnimg.cn/355996fa724345f7931b828bb552fb37.png)
令D( angle ) = Q_angle ,D( Q_bias ) = Q_gyro
设上一次预测协方差矩阵为P(k-1)
![在这里插入图片描述](https://img-blog.csdnimg.cn/7e29f396c84f4cb3988927c0955f1821.png)
本次预测协方差矩阵P(k)
![在这里插入图片描述](https://img-blog.csdnimg.cn/5e018c7e29e5412daa39c55985a0668b.png)
将以上参数带入预测协方差公式得:
![在这里插入图片描述](https://img-blog.csdnimg.cn/d4008b3114c64dbaaa6f989a5521be75.png)
代码如下↓
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:
![在这里插入图片描述](https://img-blog.csdnimg.cn/c125ae45fc344c55b4f49f329fb050ec.png)
带入本文得:
![在这里插入图片描述](https://img-blog.csdnimg.cn/329b5d6fa02d4a5cae4bd3d093e41df8.png)
代码如下↓
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)]
带入本文得:
![在这里插入图片描述](https://img-blog.csdnimg.cn/5b6249e930f44734853fc00766dc86b0.png)
代码如下↓
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)
带入本文得
![在这里插入图片描述](https://img-blog.csdnimg.cn/84d3c7412d954c49924b29dc86d9dc92.png)
代码如下↓
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)
{
static float Q_bias;
static float K_0, K_1;
static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
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];
}
卡尔曼滤波效果
![在这里插入图片描述](https://img-blog.csdnimg.cn/77565d2af69448e39c7b87e8fddafc75.png)
完整工程Gitee链接:
https://gitee.com/lebron-meng/rt-thread-kalman-filtering.git
参考资料:
图说卡尔曼滤波,一份通俗易懂的教程
从放弃到精通!卡尔曼滤波从理论到实践~
CH32读取MPU6050姿态数据(卡尔曼滤波法)
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)