二十几天前写的卡尔曼滤波库,之前是好用的,现在调用后发现输出的角度抖动很厉害,不像滤波后的数据。
.h文件
#ifndef KALMAN_H
#define KANMAN_H
class Kalman{
public:
Kalman(); //构造函数 初始化对象
float Kalman_Filter(float newAngle,float newGyro); //获取并处理mpu6050信息
private:
float Q_angle; //加速度计噪声协方差
float Q_gyro; //陀螺仪噪声协方差
float R_angle; //角度测量噪声协方差
float angle; //最终得到的角度
float bias; //陀螺仪零偏
float gyro; //最终得到的角速度
float dt; //卡尔曼滤波采样时间
float p[2][2]; //先验或最优估计协方差矩阵
};
#endif
.cpp文件
#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include "Kalman.h"
Kalman::Kalman()
{
Q_angle = 0.001f;
Q_gyro = 0.003f;
R_angle = 0.03f;
angle = 0.0f;
bias = 0.0f;
dt = 0.0055f;
p[0][0] = 1.0f;
p[0][1] = 0.0f;
p[1][0] = 0.0f;
p[1][1] = 1.0f; //p初始值取1
}
float Kalman::Kalman_Filter(float newAngle,float newGyro)
{
float x = p[0][0] + R_angle;
float K[2];
float y = newAngle - angle;
gyro = newGyro - bias;
angle += gyro * dt; //预测当前角度
p[0][0] += (Q_angle - p[0][1] - p[1][0]) * dt;
p[0][1] -= p[1][1] * dt;
p[1][0] -= p[1][1] * dt;
p[1][1] += Q_gyro * dt; //计算预测估计协方差矩阵
K[0] = p[0][0] / x;
K[1] = p[1][0] / x; //计算卡尔曼增益
angle += K[0] * y;
bias += K[1] * y; //计算当前最优估计值
float Plast0 = p[0][0];
float Plast1 = p[0][1];
p[0][0] -= K[0] * Plast0;
p[0][1] -= K[0] * Plast1;
p[1][0] -= K[1] * Plast0;
p[1][1] -= K[1] * Plast1; //更新协方差矩阵
return angle;
}