1、融合流程

1.1、状态方程(求F和B)

状态方程一般形式如下(是一个微分方程)x服从一个高斯分布

状态量由预测给出(本例中是由IMU预测)

其中

状态量的误差

状态量误差的微分形式为

所以他们的转换矩阵F为

 IMU自身误差

 加速度误差，角速度误差，bias误差

 除加速度误差与状态量相差一个旋转矩阵外i，其他均无关，所以B为

1.2、观测方程(求G和C)

观测方程一般形式

 y为观测，包括位置和失准角

观测量与状态量的一三项相同，其他无关，误差项n为其在xyz的分量

1.3、观测方程中加入编码器

编码器观测的是在b系下观测的线速度加入到观测方程后为观测变为

其中第二项需要推导其与状态量误差的关系

 所以新的G矩阵为在原来的基础之上加上编码器观测的线速度的转换关系

 矩阵C为误差关系矩阵为单位矩阵即可

1.4、 在观测方程中加入运动约束

 1.5、卡尔曼预测更新

在得到预测方程的F和B以及观测方程的G和C之后就可以进行卡尔曼滤波了；在这之前还要进行初始化和惯性解算，此处就不介绍了

第一个方程意义：已知k-1时刻的状态误差项，可以通过F和B预测出此时的状态误差项

第二个方程意义:已知k-1时刻状态量误差分布的协方差矩阵，以及误差项分布协方差矩阵，可以预测出k时刻的协方差矩阵；

蓝色代表k-1时刻，我们通过预测得到了k时刻状态量误差(粉色部分)；但这并不一定非常准确所以我们需要用观测方程来进一步修正他

1.6、加入观测后更新

 首先第一个式子：通过PK(下尖)（代表通过预测得到的k时刻的协方差也叫先验）和G，C，R得到卡尔曼增益。

再计算出后验协方差矩阵Pk(上尖)和后验状态量误差(分布的均值)

后验用图形可以表示为

 两者融合

如果没有后验等于先验（左侧是后验右侧是先验）

1.7、计算后验位姿与状态量清零

2 、作业

 本次作业与上一章相似，只有少部分不同

在观测方程中加入运动约束，其他不变

2.1、融合编码器

void ErrorStateKalmanFilter::CorrectErrorEstimationPoseVel(
    const Eigen::Matrix4d &T_nb, const Eigen::Vector3d &v_b, const Eigen::Vector3d &w_b,
    Eigen::VectorXd &Y, Eigen::MatrixXd &G, Eigen::MatrixXd &K
) {
    //
    // TODO: set measurement:
    //
  Eigen::Vector3d  v_b_  =  v_b;           //  measurment   velocity  (body  系) ， 融入速度 (vx 取自 惯导)
 
    Eigen::Vector3d  dp  =  pose_.block<3,  1>(0,  3)  -   T_nb.block<3,  1>(0,  3);
    Eigen::Matrix3d  dR  =  T_nb.block<3,  3>(0, 0).transpose() *  pose_.block<3, 3>(0, 0) ;
    Eigen::Vector3d  dv  =   pose_.block<3,  3>(0, 0).transpose() *vel_  -  v_b_ ;                  //  delta v       严格意义上来说，这里的观测是，惯导给的vx
    // TODO: set measurement equation:
    Eigen::Vector3d  dtheta  =  Sophus::SO3d::vee(dR  -  Eigen::Matrix3d::Identity() );
    YPoseVel_.block<3, 1>(0, 0)  =  dp;          //    delta  position 
    YPoseVel_.block<3, 1>(3, 0)  =  dv;           //   delta   velocity 
    YPoseVel_.block<3, 1>(6, 0)  =  dtheta;          //   失准角
    Y = YPoseVel_;
    //   set measurement  G
    GPoseVel_.setZero();
    GPoseVel_.block<3, 3>(0, kIndexErrorPos)  =  Eigen::Matrix3d::Identity();
    GPoseVel_.block<3, 3>(3, kIndexErrorVel)   =   pose_.block<3,  3>(0, 0).transpose();
    GPoseVel_.block<3, 3>(3, kIndexErrorOri)   =   Sophus::SO3d::hat( pose_.block<3,  3>(0, 0).transpose() *vel_  ) ;
    GPoseVel_.block<3 ,3>(6, kIndexErrorOri)   =  Eigen::Matrix3d::Identity();        
    G  =   GPoseVel_;     
    //   set measurement  C
    CPoseVel_.setIdentity();
    Eigen::MatrixXd  C  =   CPoseVel_;
    // TODO: set Kalman gain:
    Eigen::MatrixXd R = RPoseVel_;    //  观测噪声
    K =  P_  *  G.transpose() * ( G  *  P_  *  G.transpose( )  +  C * RPoseVel_*  C.transpose() ).inverse() ;
}

 与仅有雷达观测时有几处不同

观测量y不同，添加了编码器观测的线速度。G不同，C也进行了扩充，不过还是单位矩阵

YPoseVel_.block<3, 1>(0, 0)  =  dp;          //    delta  position 
YPoseVel_.block<3, 1>(3, 0)  =  dv;           //   delta   velocity 
YPoseVel_.block<3, 1>(6, 0)  =  dtheta;          //   失准角

1. Eigen::Vector3d dv = pose_.block<3, 3>(0, 0).transpose() _vel_ - v_b_ ;

GPoseVel_.setZero();
    GPoseVel_.block<3, 3>(0, kIndexErrorPos)  =  Eigen::Matrix3d::Identity();
    GPoseVel_.block<3, 3>(3, kIndexErrorVel)   =   pose_.block<3,  3>(0, 0).transpose();
    GPoseVel_.block<3, 3>(3, kIndexErrorOri)   =   Sophus::SO3d::hat( pose_.block<3,  3>(0, 0).transpose() *vel_  ) ;
    GPoseVel_.block<3 ,3>(6, kIndexErrorOri)   =  Eigen::Matrix3d::Identity();

2.2、添加运动约束(伪观测)

void ErrorStateKalmanFilter::CorrectErrorEstimationPosiVel(
    const Eigen::Matrix4d &T_nb, const Eigen::Vector3d &v_b, const Eigen::Vector3d &w_b,
    Eigen::VectorXd &Y, Eigen::MatrixXd &G, Eigen::MatrixXd &K
) {
    Eigen::Vector3d  v_b_  =  {v_b[0],  0,  0};           //  measurment   velocity  (body  系) ， 伪观测 (vy 、vz  = 0)
 
    Eigen::Vector3d  dp  =  pose_.block<3,  1>(0,  3)  -   T_nb.block<3,  1>(0,  3);
    Eigen::Vector3d  dv  =   pose_.block<3,  3>(0, 0).transpose() *vel_  -  v_b ;                  //  delta v  ,  v_x 来自轮速里程计
    Eigen::Vector3d  dtheta  =  Sophus::SO3d::vee(dR  -  Eigen::Matrix3d::Identity() );
 
    // TODO: set measurement equation:
    YPosiVel_.block<3, 1>(0, 0)  =  dp;          //    delta  position 
    YPosiVel_.block<3, 1>(3, 0)  =  dv;           //   delta   velocity  
    YPoseVel_.block<3, 1>(6, 0)  =  dtheta;          //   失准角
    Y = YPosiVel_;
    //   set measurement  G
    GPosiVel_.setZero();
    GPosiVel_.block<3, 3>(0, kIndexErrorPos)  =  Eigen::Matrix3d::Identity();
    GPosiVel_.block<3, 3>(3, kIndexErrorVel)   =   pose_.block<3,  3>(0, 0).transpose();
    GPosiVel_.block<3, 3>(3, kIndexErrorOri)   =   Sophus::SO3d::hat( pose_.block<3,  3>(0, 0).transpose() *vel_  ) ;
    G  =   GPosiVel_;     
    //   set measurement  C
    CPosiVel_.setIdentity();
    Eigen::MatrixXd  C  =   CPosiVel_;
    // TODO: set Kalman gain:
    Eigen::MatrixXd R = RPosiVel_;    //  观测噪声
    K =  P_  *  G.transpose() * ( G  *  P_  *  G.transpose( )  +  C * R*  C.transpose() ).inverse() ;
}

仅仅是添加了一个运动约束，在侧向和天向速度为0；添加这两个约束进观测方程，因为没有任何观测，所以叫伪观测 ；

2.3、结果