对IMU数据进行卡尔曼滤波

ego_vehicle

已于 2023-03-19 11:56:29 修改

阅读量3.3k

点赞数 3

文章标签：算法 c++ 自动驾驶

于 2023-03-19 11:55:28 首次发布

本文链接：https://blog.csdn.net/qq_53576285/article/details/129648293

版权

我们要使用IMU数据，必须对数据进行预处理，卡尔曼滤波就是很好的方式。

1.卡尔曼滤波

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

其原理可以参考b站的视频：

从放弃到精通！卡尔曼滤波从理论到实践~_哔哩哔哩_bilibili

2.代码实现

#include <ros/ros.h>
#include <fstream>
#include "sensor_msgs/Imu.h"
#include "Eigen/Dense"

double a_x; //x方向的加速度
double a_y; //y方向的加速度
double a_z; //z方向的加速度

Eigen::MatrixXd X = Eigen::MatrixXd(3,1); //创建一个3*1矩阵

using namespace std;

class kalman_filter
{
private:
    Eigen::MatrixXd A; //系统状态矩阵
    Eigen::MatrixXd P; //协方差
    Eigen::MatrixXd Q; //测量过程噪音（预测）
    Eigen::MatrixXd R; //真实传感器噪音
    Eigen::MatrixXd H; //测量矩阵

    bool isinitized = false; //判断是否进行了初始化

public:
    kalman_filter();
    Eigen::MatrixXd predictAndupdate( Eigen::MatrixXd x,Eigen::MatrixXd z);
    ~kalman_filter();
};

Eigen::MatrixXd kalman_filter::predictAndupdate(Eigen::MatrixXd x,Eigen::MatrixXd z)
{
    if(!isinitized)
    {
        P = Eigen::MatrixXd(3,3); //协方差 3*3矩阵
        P<< 1,0,0,
            0,1,0,
            0,0,1; //协方差的初始化
        isinitized=true;
    }
    x = A*x; // 状态一步预测方程
    P = A*P*(A.transpose())+Q; //一步预测协方差阵
    Eigen::MatrixXd  K = P*(H.transpose())*((H*P*(H.transpose())+R).inverse()); //kalman增益
    x = x+K*(z-H*x); //状态更新：
    int x_size = x.size();
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
    P = (I-K*H)*P; // 协方差阵更新：
    return x;
}

kalman_filter::kalman_filter()
{
//参数初始化设置
    A=Eigen::MatrixXd(3,3); //系统状态矩阵
    A<< 1,0,0,
        0,1,0,
        0,0,1;
    H=Eigen::MatrixXd(3,3); //测量矩阵
    H<< 1,0,0,
        0,1,0,
        0,0,1;
    Q=Eigen::MatrixXd(3,3); //（预测）过程噪音
    Q<< 0.03,0,0,
        0,0.03,0,
        0,0,0.03;
    R=Eigen::MatrixXd(3,3); //真实传感器噪音
    R<<3.65,0,0,            //R太大，卡尔曼滤波响应会变慢
       0,3.65,0,
       0,0,3.65;     
}

kalman_filter::~kalman_filter(){}
//订阅发布者主要订阅imu信息并调用前面的卡尔曼滤波处理后再发布信息
class SubscribeAndPublish
{
public:
    SubscribeAndPublish()
    {
        imu_info_sub = n.subscribe("/imu/data_raw", 10, &SubscribeAndPublish::imuInfoCallback,this);
        IMU_kalman_pub = n.advertise<sensor_msgs::Imu>("/imu_kalman", 10);
    }
    void imuInfoCallback(const sensor_msgs::Imu::ConstPtr& msg)
    {
        // 第一次将接收到的消息打印出来
        if(first)
        {
            // 三个方向的线性加速度
            double a_x_m = msg->linear_acceleration.x;
            double a_y_m = msg->linear_acceleration.y;
            double a_z_m = msg->linear_acceleration.z;
            // 用X存第一帧三个方向的线性加速度
            X<<a_x_m,a_y_m,a_z_m;
            // 第一次不用卡尔曼滤波，直接发布出去，
            sensor_msgs::Imu M;
            M.linear_acceleration.x = a_x_m;
            M.linear_acceleration.y = a_y_m;
            M.linear_acceleration.z = a_z_m;
            M.header = msg->header;
            IMU_kalman_pub.publish(M);
            first=false;
            cout<< "first farm"<< endl;
        }
        else
        {
            double a_x_m = msg->linear_acceleration.x;
            double a_y_m = msg->linear_acceleration.y;
            double a_z_m = msg->linear_acceleration.z;
            Eigen::MatrixXd z;
            z = Eigen::MatrixXd(3,1);
            z<<a_x_m,a_y_m,a_z_m;
            kalman_filter kf;   //kalman_filter创建的对象kf，之后就进行卡尔曼滤波
            Eigen::MatrixXd x_new = kf.predictAndupdate(X,z);//z当前时刻的量测值，X
            X = z;

            // 整理成适合话题发布的形式
            a_x = x_new(0,0);
            a_y = x_new(1,0);
            a_z = x_new(2,0);
            sensor_msgs::Imu M;
            M.linear_acceleration.x = a_x;
            M.linear_acceleration.y = a_y;
            M.linear_acceleration.z = a_z;
            M.header = msg->header;
            cout<< "linear_acceleration.x = " << a_x <<"linear_acceleration.y = " << a_y <<"linear_acceleration.z = " << a_z << endl;
            IMU_kalman_pub.publish(M);
        }
    }
private:
    ros::NodeHandle n;
    ros::Publisher IMU_kalman_pub;
    ros::Subscriber imu_info_sub;
    bool first=true;
};
//接收IMU话题，通过卡尔曼滤波参数进行数据处理，然后发布处理后的数据作为一个话题。
int main(int argc, char **argv)
{
    ros::init(argc, argv, "subscribe_and_publish");

    SubscribeAndPublish SAPObject;

    ros::spin();
    return 0;
}

获得发布后的数据后，需要对于数据作图处理，ros中的作图工具不是很好，可以通过录制bag包的方式，再将.bag文件转为.csv的格式通过命令：rostopic echo -b <BAGFILE> -p <TOPIC> > <output>.csv

该代码用的最基础卡尔曼滤波算法，且没有进行调参。MATLAB/simulink中自带有封装好了的卡尔曼滤波代码，通过ros/tool工具可以直接接收IMU的信息，用MATLAB内置滤波器实现滤波。