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维的状态向量
卡尔曼滤波的公式贴一下

$$\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-KH){\bar{\Sigma }}_k\end{array}$$
其他自己推导吧
我放一下我做的两个函数，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;
}

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