卡尔曼滤波c++

//Kalfilter滤波数据处理
/**
 * @brief PrcTools::Kalfilter
 * @param v 输出值
 * @param x 上一个历元的解，InitX:X的初始值,不知道初始值就输入零向量，对应XKF值
 * @param Qx 上一个历元接对应的方差，P
 * @param F 状态转移矩阵 1
 * @param Qw 协方差矩阵，反映状态受外界影响的大小Q
 * @param B 系数阵，观测矩阵H
 * @param R 观测值的方差阵，反映测量的精度，取决于传感器
 * @param l 输入测量得到的数据Z
 */
void Kalfilter(   VectorXd *v,
                            VectorXd *x,
                            MatrixXd *Qx,
                      const MatrixXd &F,
                      const MatrixXd &Qw,
                      const MatrixXd &B,
                      const MatrixXd &R,
                      const VectorXd &L)
{
    (*x) = F*(*x);//一步预测
    MatrixXd M = F*(*Qx)*F.transpose() + Qw;//一步预测误差方差阵P
    MatrixXd K = (M*B.transpose())*(B*M*B.transpose()+R).inverse();//滤波增益矩阵（权重）

    MatrixXd I = MatrixXd::Identity(Qx->rows(), Qx->cols());//单位矩阵
    (*Qx) = (I-K*B) * M;
    (*Qx) = 0.5 * ((*Qx)+Qx->transpose());

    (*v) = L - B*(*x);
    (*x) = (*x) + K*(*v);    //TODOu: Deal with large KV by iteration.
    (*v) = B*(*x) - L;
}