MPU6050卡尔曼滤波C语言代码实现详解
写在前面:目前网上很多卡尔曼滤波的c语言代码有一些小问题,且不方便移植,写这个博客的目前是想提供一个直接复制粘贴就能使用的c语言代码,理论推导部分请参考别的博客。
结构体定义
typedef struct
{
float dt; //采样时间
float angle_f; //角度滤波后
float angle_m; //角度测量
float wb_m; //角速度测量
float wb_f; //角速度滤波后
float q_bias; //角速度offset
float P[2][2]; //协方差矩阵
float Q_angle; // Q矩阵
float Q_gyro;
float R_angle; // R矩阵
} Kalman_pm_st;
结构体初始化(赋值)
void Kalman_Init(Kalman_pm_st* Kalman_pm)
{
/*注意:只需要调节Q矩阵[Q_angle,Q_gyro]和R矩阵[R_angle]即可*/
Kalman_pm->P[0][0] = 1.0f;
Kalman_pm->P[0][1] = 0.0f;
Kalman_pm->P[1][0] = 0.0f;
Kalman_pm->P[1][1] = 1.0f;
Kalman_pm->dt = 0.001f;
Kalman_pm->Q_angle = 0.001f;
Kalman_pm->Q_gyro = 0.003f;
Kalman_pm->R_angle = 0.5f;
Kalman_pm->q_bias = 0.0f;
Kalman_pm->angle_f = 0.0f;
}
卡尔曼滤波函数
void Kalman_Filter(Kalman_pm_st *kalman_pm)
{
float angle_err; //先验误差
float angle_; //先验估计
float Pdot[2][2]; //先验误差协方差矩阵
float K_0;
float K_1; //卡尔曼增益
/*先验估计*/
angle_ = kalman_pm->angle_f + (kalman_pm->wb_m - kalman_pm->q_bias) * kalman_pm->dt; //先验估计
angle_err = kalman_pm->angle_m - angle_; //先验误差
/*先验误差协方差矩阵*/
Pdot[0][0] = kalman_pm->P[0][0] + kalman_pm->Q_angle - (kalman_pm->P[0][1] + kalman_pm->P[1][0]) * kalman_pm->dt; // Q_angle->Q1
Pdot[0][1] = kalman_pm->P[0][1] - (kalman_pm->P[1][1]) * kalman_pm->dt;
Pdot[1][0] = kalman_pm->P[1][0] - (kalman_pm->P[1][1]) * kalman_pm->dt;
Pdot[1][1] = kalman_pm->P[1][1] + kalman_pm->Q_gyro; // Q_gyro->Q2
/*卡尔曼增益*/
K_0 = Pdot[0][0] / (Pdot[0][0] + kalman_pm->R_angle);
K_1 = Pdot[1][0] / (Pdot[0][0] + kalman_pm->R_angle);
/*后验估计*/
kalman_pm->angle_f = angle_ + K_0 * angle_err; //最优角度
kalman_pm->q_bias += K_1 * angle_err; //最优角速度偏差
kalman_pm->wb_f = kalman_pm->wb_m - kalman_pm->q_bias; //最优角速度
/*更新误差协方差矩阵*/
kalman_pm->P[0][0] = Pdot[0][0] - K_0 * Pdot[0][0];
kalman_pm->P[0][1] = Pdot[0][1] - K_0 * Pdot[0][1];
kalman_pm->P[1][0] = Pdot[1][0] - K_1 * Pdot[0][0];
kalman_pm->P[1][1] = Pdot[1][1] - K_1 * Pdot[0][1];
}
使用方法:
Kalman_pm_st Kalman_pm; //定义结构体
Kalman_Init(&Kalman_pm); //给结构体赋初值和修改参数
/*参数输入*/
Kalman_pm.angle_m=yaw_acc; //加速度计求解角度的值(偏航角)
Kalman_pm.wb_m=wz_gyro; //陀螺仪的角速度(偏航角速度)
Kalman_Filter(&Kalman_pm); //调用卡尔曼滤波
float yaw_angle=Kalman_pm.angle_f; //滤波后的角度
float yaw_w=Kalman_pm.wb_f; //滤波后的角速度