[滑动窗口/卡尔曼滤波]用C++/ROS写一个简单的速度滤波器

需求背景

在ROS相关的小车进行导航时，往往会使用navigation包里的move_base作为小车的速度输出。move_base为我们内置了一些局部路径规划器如dwa和teb等，但这些速度规划器的输出可能不够平滑，因此我们可以使用速度滤波器来改善这些情况

理论

卡尔曼滤波器

卡尔曼滤波器是一种用于估计系统状态的优雅数学方法，其在控制系统、导航、机器人技术等领域广泛应用。它通过结合传感器测量和系统模型，提供了对真实状态的最优估计。

步骤

• 初始化：定义系统的状态向量、状态转移矩阵、控制输入、测量矩阵等。还需要初始化状态估计值和协方差矩阵。

• 预测（预测步）：根据状态转移矩阵和控制输入，预测下一个状态的估计值和协方差矩阵。

• 更新（校正步）：使用测量值来校正预测的状态估计。计算卡尔曼增益，用于将预测值与测量值进行加权平均，得到最终的状态估计值和协方差矩阵。

• 重复：不断交替执行预测步和更新步，以持续地优化状态估计。

优缺点

• 优点：对于线性系统，卡尔曼滤波器提供了最优的状态估计。能够处理噪声和不确定性，具有较高的鲁棒性。
• 缺点：需要调状态矩阵的参，一般对于我们这种情况有两种调法，一是把状态矩阵视为对角阵，认为各个变量是相互独立的，二是根据v=v+at的表达式来写状态矩阵

滑动窗口滤波

滑动窗口滤波器是一种简单但有效的信号处理方法，用于去除噪声和平滑数据。它通过计算窗口内数据的平均值或中值，减少噪声的影响。

步骤

• 选择窗口大小：定义滑动窗口的大小，即在多少个数据点内计算平均值或中值。

• 移动窗口：从数据序列的起始点开始，将窗口按照指定的大小进行滑动。

• 计算平均值或中值：在每个窗口位置，计算窗口内数据的平均值或中值，作为当前位置的估计值。

• 重复：继续移动窗口并计算估计值，直到窗口滑过整个数据序列。

优缺点

• 优点：实现简单，计算效率高。最后输出的速度特别丝滑
• 缺点：需要调参窗口的长度，如果长度太大，则会增加时延，减慢系统的响应

代码

下面的代码是订阅了"/cmd_vel"话题，经过滤波器后，向"/cmd_vel_"话题输出，当然，更优雅的方式应该是把这段代码写到局部路径规划器中

#include <Eigen/Dense>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <vector>

// 卡尔曼滤波
// 卡尔曼滤波器类
// 卡尔曼滤波器类
class KalmanFilter {
public:
    KalmanFilter(){};
    void init(double dt, double acc_noise) {
        // 状态转移矩阵 A
        A << 1, dt, 0, 1;
        // 状态协方差矩阵 P
        P << 1, 0, 0, 1;
        // 测量矩阵 H
        H << 1, 0;
        // 测量噪声协方差矩阵 R
        R << acc_noise;
        // 状态噪声协方差矩阵 Q
        Q << pow(dt, 4) / 4.0, pow(dt, 3) / 2.0, pow(dt, 3) / 2.0, pow(dt, 2);
        // 初始状态估计
        x << 0, 0;
    }

    double filter(double acc) {
        // 预测
        x_hat = A * x;
        P_hat = A * P * A.transpose() + Q;
        // 更新
        K = P_hat * H.transpose() * (H * P_hat * H.transpose() + R).inverse();
        x = x_hat + K * (acc - H * x_hat);
        P = (Eigen::Matrix2d::Identity() - K * H) * P_hat;
        return x(0);
    }

private:
    double dt;          // 采样时间间隔
    Eigen::Matrix2d A;         // 状态转移矩阵
    Eigen::Matrix2d P;         // 状态协方差矩阵
    Eigen::Matrix<double, 1, 2> H;  // 测量矩阵
    Eigen::Matrix<double, 1, 1> R;  // 测量噪声协方差矩阵
    Eigen::Matrix2d Q;         // 状态噪声协方差矩阵
    Eigen::Vector2d x;         // 状态估计
    Eigen::Vector2d x_hat;     // 预测状态估计
    Eigen::Matrix2d P_hat;     // 预测状态协方差矩阵
    Eigen::Matrix<double, 2, 1> K;  // 卡尔曼增益矩阵
};

// 滑动窗口滤波
// 滑动平均滤波器类
class MovingAverageFilter {
public:
    MovingAverageFilter(){};
    void init(int size) {
        m_size = size;
        m_sum = 0;
        m_data.reserve(size);
    }

    double filter(double value) {
        if (m_data.size() == m_size) {
            m_sum -= m_data.front();
            m_data.erase(m_data.begin());

        }
        m_data.push_back(value);
        m_sum += value;
        return m_sum / m_data.size();
    }

private:
    int m_size;                // 滤波器窗口大小
    std::vector<double> m_data;     // 数据存储数组
    double m_sum;              // 数据累加和
};

// 速度滤波类
class VelFilter {
    private: 
        ros::NodeHandle nh_;

        ros::Subscriber subSpeed = nh_.subscribe<geometry_msgs::Twist> ("/cmd_vel", 5, &VelFilter::speedHandler, this);
        ros::Publisher pubSpeed = nh_.advertise<geometry_msgs::Twist> ("/cmd_vel_", 5);
        geometry_msgs::Twist now_vel_;
        MovingAverageFilter filter_x;
        MovingAverageFilter filter_y;
        MovingAverageFilter filter_z;
        KalmanFilter kal_filter_x;
        KalmanFilter kal_filter_y;
        KalmanFilter kal_filter_z;

    public:
        VelFilter()
        {
            // 创建滤波器
            filter_x.init(5);
            filter_y.init(5);
            filter_z.init(10);
            //kal_filter_x.init(0.01, 0.5);
            //kal_filter_y.init(0.01, 0.5);
            //kal_filter_z.init(0.01, 0.5);
        }
        void speedHandler(const geometry_msgs::Twist::ConstPtr& speed)
        {
            now_vel_.linear.x = filter_x.filter(speed->linear.x);
            now_vel_.linear.y = filter_y.filter(speed->linear.y);
            now_vel_.angular.z = filter_z.filter(speed->angular.z);
            // now_vel_.linear.x = kal_filter_x.filter(speed->linear.x);
            // now_vel_.linear.y = kal_filter_y.filter(speed->linear.y);
            // now_vel_.angular.z = kal_filter_z.filter(speed->angular.z);
            pubSpeed.publish(now_vel_);
        }

}; 




int main(int argc, char** argv)
{
  ros::init(argc, argv, "vel_filter");

  VelFilter velfilter;
  ros::Rate rate(100);
  bool status = ros::ok();
  while (status) {
    ros::spinOnce();
  }
}