ROS下自己手写了一个卡尔曼滤波器以及一些参数的说明

     因为要平滑IMU的加速度计输出的数据，包括x，y和z轴的加速度，所以这里我只用了三维。按照自己对卡尔曼滤波的理解自己写了个c++工程，如果有错误请指出。

     项目源码：GitHub - JackJu-HIT/KalmanFilter: 基于自己之前搭好的ROS机器人平台，自己写了卡尔曼滤波算法对IMU的加速度xyz进行滤波处理。

     下面代码就是核心：预测+测量更新。

Eigen::MatrixXd kalman_filter::predictAndupdate(Eigen::MatrixXd x,Eigen::MatrixXd z)
{
  if(!isinitized)
  {
  P=Eigen::MatrixXd(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());
  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;

}

  参数初始化设置：

kalma