为什么卡尔曼滤波器失灵了？

Question

二十几天前写的卡尔曼滤波库，之前是好用的，现在调用后发现输出的角度抖动很厉害，不像滤波后的数据。

.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;
}

Answer 1

以下回答参考 皆我百晓生、券券喵儿 等免费微信小程序相关内容作答，并由本人整理回复。

从你的描述来看，你可能遇到了以下问题：

1. 滤波器的精度下降：这可能是由于卡尔曼滤波器的参数设置不当或者输入信号不稳定导致的。在你的KalmanFilter类中，你可以检查这些参数是否正确，并且考虑使用更先进的卡尔曼滤波算法（例如基于状态空间的方法）来提高精度。

2. 数据处理不正确：如果在调用Kalman_Filter()函数时，输入的新角度和新陀螺仪读数不是按照正确的格式和顺序传递给函数的，那么也会导致数据处理错误。确保所有输入的数据都是按正确的格式和顺序传递给函数的。

3. 噪声影响：如果你的传感器有高动态范围的噪音（例如加速度计和陀螺仪），并且这个噪声的影响超过了卡尔曼滤波器的稳定性限制，那么你也可能会遇到性能下降的问题。

4. 预测与实际误差：如果你的预测值和实际值之间的误差很大，那么也会影响滤波器的准确性。确保你的预测值和实际值之间没有太大的差异。

解决这些问题的方法包括但不限于：

• 确保所有的输入数据都是正确的格式和顺序；
• 使用更先进的卡尔曼滤波算法；
• 调整卡尔曼滤波器的参数，以适应不同的环境条件；
• 在处理输入数据时，尽可能地减少噪声的影响。

如果你提供了更多关于你的系统、传感器以及如何调用Kalman_Filter()函数的具体细节，我可以提供更具体的帮助和建议。