robot_pose_ekf 代码流程详解

odom_estimation_node.cpp中

步骤1： main.cpp 中
 

OdomEstimationNode my_filter_node;

步骤2：构造函数OdomEstimationNode 中

  //ekf filter,OdomEstimation,创建系统模型和测量更新模型
my_filter_.setOutputFrame(output_frame_);
my_filter_.setBaseFootprintFrame(base_footprint_frame_);

在此根据my_filter_ 的定义OdomEstimation my_filter_;，跳到odom_estimation.cpp的构造函数
步骤3：在odom_estimation.cpp的构造函数中

// 初始化系统模型噪音的均值和协方差矩阵
ColumnVector sysNoise_Mu(6);  sysNoise_Mu = 0;
SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2);
// 采用均值和协方差矩阵生成代表系统不确定性的高斯分布
Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);

// 创造里程计测量更新模型
// 初始化代表测量噪音的高斯分布
ColumnVector measNoiseOdom_Mu(6);  measNoiseOdom_Mu = 0;
SymmetricMatrix measNoiseOdom_Cov(6);  measNoiseOdom_Cov = 0;
for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
// 初始化代表测量更新线性模型本身的高斯分布
Matrix Hodom(6,6);  Hodom = 0;
Hodom(1,1) = 1;    Hodom(2,2) = 1;    Hodom(6,6) = 1;
// 用线性模型和噪音生成pdf以及测量更新模型
odom_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
odom_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_);

步骤4：
在此初始化结束，回到构造函数OdomEstimationNode中，
odomCallback 处理odom的数据到odom_meas_
然后spin函数中开始处理
 

my_filter_.update(odom_active_, imu_active_,gps_active_, vo_active_,  filter_stamp_, diagnostics)
在此跳到OdomEstimation中的update函数
// 首先只加上系统噪音，其中filter_是BFL::ExtendedKalmanFilter*
ColumnVector vel_desi(2); vel_desi = 0;
filter_->Update(sys_model_, vel_desi);
然后
// 给里程计系统模型更新这次的测量值 BFL::ExtendedKalmanFilter*
filter_->Update(odom_meas_model_, odom_rel);

此时就会跳到bfl 包中的扩展卡尔曼滤波器，由于extendedkalmanfilter.h 的是继承kalmanfilter.h，而kalmanfilter.h是继承filter.h，update 函数定义在filter.h中
分别是
Filter::Update(SystemModel* const sysmodel,
              const SVar& u)
{
  SVar s; MVar z;
  return this->UpdateInternal(sysmodel,u,NULL,z,s);
}

template bool
Filter:: Update(MeasurementModel* const measmodel,
               const MVar& z)
{
  SVar u; SVar s;
  return this->UpdateInternal(NULL,u,measmodel,z,s);
}

最终UpdateInternal 是定义且实现在kalmanfilter.h
 bool
  KalmanFilter::UpdateInternal(SystemModel* const sysmodel,
    const ColumnVector& u,
    MeasurementModel* const measmodel,
    const ColumnVector& z, const ColumnVector& s)
  {
    if (sysmodel != NULL)
      {
    SysUpdate(sysmodel,u);
      }
    if (measmodel != NULL)
      {
    MeasUpdate(measmodel,z,s);
      }
    return true;
  }

步骤5：
SysUpdate 与MeasUpdate 都在extendedkalmanfilter.cpp

void
ExtendedKalmanFilter::SysUpdate(SystemModel* const sysmodel,
                                const ColumnVector& u)
{
    //为了计算条件概率密度函数的期望值
  _x = _post->ExpectedValueGet();
  _J = ((AnalyticSys*)sysmodel)->PredictionGet(u,_x);
  //求第i个条件参数求偏导数
  _F = ((AnalyticSys*)sysmodel)->df_dxGet(u,_x);
  _Q = ((AnalyticSys*)sysmodel)->CovarianceGet(u,_x);

  CalculateSysUpdate(_J, _F, _Q);
}

void
ExtendedKalmanFilter::MeasUpdate(MeasurementModel* const measmodel,
                                 const ColumnVector& z,
                     const ColumnVector& s)
{
  // allocate measurement for z.rows() if needed
  AllocateMeasModelExt(z.rows());
  //为了计算条件概率密度函数的期望值
  _x = _post->ExpectedValueGet();
  //计算先验
  (_mapMeasUpdateVariablesExt_it->second)._Z = ((AnalyticMeas*)measmodel)->PredictionGet(s,_x);
  //求第i个条件参数求偏导数
  (_mapMeasUpdateVariablesExt_it->second)._H = ((AnalyticMeas*)measmodel)->df_dxGet(s,_x);
  //计算高斯噪声误差
  (_mapMeasUpdateVariablesExt_it->second)._R = ((AnalyticMeas*)measmodel)->CovarianceGet(s,_x);

  CalculateMeasUpdate(z, (_mapMeasUpdateVariablesExt_it->second)._Z, (_mapMeasUpdateVariablesExt_it->second)._H, (_mapMeasUpdateVariablesExt_it->second)._R);
}

步骤6：
PredictionGet 等函数实现在analyticmeasurementmodel_gaussianuncertainty.cpp 中(高级搜索得到)
 

 Matrix
  AnalyticMeasurementModelGaussianUncertainty::df_dxGet(const ColumnVector& u, const ColumnVector& x)
  {
    MeasurementPdfGet()->ConditionalArgumentSet(0,x);
    if (MeasurementPdfGet()->NumConditionalArgumentsGet() == 2) MeasurementPdfGet()->ConditionalArgumentSet(1,u);
    return dynamic_cast(MeasurementPdfGet())->dfGet(0);
  }


  ColumnVector
  AnalyticMeasurementModelGaussianUncertainty::PredictionGet(const ColumnVector& u, const ColumnVector& x)
  {
    MeasurementPdfGet()->ConditionalArgumentSet(0,x);
    if (MeasurementPdfGet()->NumConditionalArgumentsGet() == 2) MeasurementPdfGet()->ConditionalArgumentSet(1,u);
    return MeasurementPdfGet()->ExpectedValueGet();
  }


  SymmetricMatrix
  AnalyticMeasurementModelGaussianUncertainty::CovarianceGet(const ColumnVector& u, const ColumnVector& x)
  {
    MeasurementPdfGet()->ConditionalArgumentSet(0,x);
    if (MeasurementPdfGet()->NumConditionalArgumentsGet() == 2) MeasurementPdfGet()->ConditionalArgumentSet(1,u);
    return dynamic_cast(MeasurementPdfGet())->CovarianceGet();
  }

步骤6：最后CalculateMeasUpdate 计算更新
 

void
  KalmanFilter::CalculateMeasUpdate(const ColumnVector& z, const ColumnVector& Z, const Matrix& H, const SymmetricMatrix& R)
  {
    // allocate measurement for z.rows() if needed
    AllocateMeasModel(z.rows());

    (_mapMeasUpdateVariables_it->second)._postHT =   (Matrix)(_post->CovarianceGet()) * H.transpose() ;
    (_mapMeasUpdateVariables_it->second)._S_Matrix =  H * (_mapMeasUpdateVariables_it->second)._postHT;
    (_mapMeasUpdateVariables_it->second)._S_Matrix += (Matrix)R;

    // _K = covariance * H' * S(-1)
    (_mapMeasUpdateVariables_it->second)._K =  (_mapMeasUpdateVariables_it->second)._postHT * ( (_mapMeasUpdateVariables_it->second)._S_Matrix.inverse());

    // calcutate new state gaussian
    // Mu = expectedValue + K*(z-Z)
    (_mapMeasUpdateVariables_it->second)._innov = z-Z;
     _Mu_new  =  (_mapMeasUpdateVariables_it->second)._K * (_mapMeasUpdateVariables_it->second)._innov  ;
     _Mu_new  +=  _post->ExpectedValueGet() ;
    // Sigma = post - K*H*post
    _Sigma_temp = (_post->CovarianceGet());
    _Sigma_temp_par = (_mapMeasUpdateVariables_it->second)._K * H ;
    _Sigma_temp -=  _Sigma_temp_par * (Matrix)(_post->CovarianceGet());
    // convert to symmetric matrix
    _Sigma_temp.convertToSymmetricMatrix(_Sigma_new);

    // set new state gaussian
    PostMuSet   ( _Mu_new );
    PostSigmaSet( _Sigma_new );

    /*
      cout << "H:\n" << H << endl;
      cout << "R:\n" << R << endl;
      cout << "Z:\n" << Z << endl;
      cout << "inov:\n" << z-Z << endl;
      cout << "S:\n" << S << endl;
      cout << "S.inverse:\n" << S.inverse() << endl;
      cout << "K:\n" << K << endl;
      cout << "Mu_new:\n" << Mu_new << endl;
      cout << "sigma_new\n" << Sigma_new << endl;
    */
  }