赞
踩
写在前面:目前网上很多卡尔曼滤波的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; //滤波后的角速度
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。