我们要使用IMU数据，必须对数据进行预处理，卡尔曼滤波就是很好的方式。
卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
其原理可以参考b站的视频：
从放弃到精通！卡尔曼滤波从理论到实践~_哔哩哔哩_bilibili
#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内置滤波器实现滤波。
将主机和车载ros系统连接在同一个局域网下，用ip地址对二者进行连接。可以通过MATLAB自带的demo，选取需要发布的信息。
下载MATLAB-control库，找到建立好的卡尔曼滤波器的simulink模块，将输入端与imu接口连接，后面的可视化模块就能显示滤波后的效果。
模块和效果图如下：
