ROS中使用EIGEN库写卡尔曼滤波的方法

以前一直在使用robot_pose_ekf这个包在做卡尔曼滤波，但是读过源码后发现，这个包做的状态量只加了位置x,y,z和角度roll,pitch,yaw这6个量（当然其实这6个量就够了，我在实际使用中用这个包做UWB融合odom/imu还是很准的，前提是有一个比较精确的协方差矩阵）。
当时看了源码之后就想自己做一个卡尔曼滤波，把速度、角速度、加速度等各种量都融合进去，当时想的越多越准嘛。不过一直没时间自己做（可能是懒吧）。
现在因为某些原因还是完成了这项工作，在这里贴一点代码，给大家一点思路或者方法（其实写起来很简单的，没有我当时想象的这么复杂）。
首先需要使用矩阵库Eigen，一般有ros的ubuntu基本都是有这个库的，如何使用参考ROS 中配置使用Eigen库
我做的状态量是位置（x,y）,角度（theta）,速度（v）,加速度（w），5维的状态向量
卡尔曼滤波的公式贴一下

X ˉ k = A k X k Σ ˉ k = A k X k A k T K = Σ ˉ k H ( H Σ ˉ k H T + z Σ ) − 1 X k + 1 = X ˉ k + K ( Z k − H X ˉ k ) Σ k + 1 = ( I − K H ) Σ ˉ k \begin{array}{l} \bar{X}_{k}=A_{k} X_{k} \\ \bar{\Sigma}_{k}=A_{k} X_{k} A_{k}^{T} \\ K=\bar{\Sigma}_{k} H\left(H \bar{\Sigma}_{k} H^{T}+_{z} \Sigma\right)^{-1} \\ X_{k+1}=\bar{X}_{k}+K\left(Z_{k}-H \bar{X}_{k}\right) \\ \Sigma_{k+1}=(I-K H) \bar{\Sigma}_{k} \end{array} Xˉk​=Ak​Xk​Σˉk​=Ak​Xk​AkT​K=Σˉk​H(HΣˉk​HT+z​Σ)−1Xk+1​=Xˉk​+K(Zk​−HXˉk​)Σk+1​=(I−KH)Σˉk​​
其他自己推导吧
我放一下我做的两个函数，emmmmmm，函数也自己读：

//EKF状态预测STATIC
void friendfind::KalmanPredictor(VectorXd& X, Matrix<double,5,5>& cov, double T) {
    Matrix<double, 5, 5> A;
    A << 1, 0, -T * X(3) * sin(X(2)), T* cos(X(2)), 0,
                0, 1, T * X(3) * cos(X(2)), T* sin(X(2)), 0,
                0, 0, 1, 0, T,
                0, 0, 0, 1, 0,
                0, 0, 0, 0, 1;
    X = A * X;
    cov = A * cov * A.adjoint();
        //cout << myfriend[i](0) << "," << myfriend[i](1) << endl;
}
//EKF观测校正static
void friendfind::KalmanUpdate(VectorXd& X, Matrix<double,5,5>& cov, const Matrix<double,5,5>& H, const VectorXd& Z, const Matrix<double,5,5>& covZ) {
    Matrix<double,5,5> K = cov * H * (H * cov * H + covZ).inverse();
    //cout << K << endl << cov << endl << covZ << endl;;
    X = X + K * (Z - H * X);
    cout << "X = (" << X(0) << " , " << X(1) << "), Z = (" << Z(0) << " , " <<Z(1) << ")" << endl;
    cov = (Matrix<double, 5, 5>::Identity() - K * H) * cov;
}

最后，总结一下，如果传感器精度不够高，没必要自己做卡尔曼滤波，如果有特殊需求另说。