卡尔曼滤波及数据融合：PX4-EKF源码分析

注：笔者只是在自己学习的过程中遇到了很多问题，在网上也没有看到可以解释得太清楚明白的资料，所以将一些心得和笔记写出来，后来人如果可以从中得到一些帮助甚感荣幸。因为非科班出身自学，如果有错误的地方希望大佬们不吝赐教。

卡尔曼滤波

提起导航制导，必然避不开卡尔曼滤波，原理都很好解释经典五步，但是怎么用，为什么有这五步才是关键。

• 卡尔曼滤波整体体现为：预测+更新

  • 预测：通过状态转移函数计算当前值
  • 更新：融合当前观测状态：计算两个高斯分布融合后的均值和协方差矩阵

• 卡尔曼滤波的经典五步：

  • 状态一步预测： ${\hat{X}}_{k/k-1}={\Phi }_{k,k-1}{\hat{X}}_{k-1}$
  • 状态估计： ${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$, 其中$K_k$就是卡尔曼增益
  • 滤波增益： $K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}=P_k{H}_k^T{R}_k^{-1}$
  • 一步预测均方误差：$P_{k/k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T$
  • 估计均方误差： $P_k=(I-K_k{H}_k)P_{k/k-1}$

这五步就是卡尔曼滤波经典的五步计算，计算流程图如下所示：

注，流程图来源《卡尔曼滤波与组合导航原理》出版社：西北工业大学
整体学习参考教材也是这本书

详细推导过程如下：

• 首先描述一组状态使用：

  $X_k={\Phi }_{k,k-1}{X}_{k-1}+{\Gamma }_{k-1}{W}_{k-1}$ ; $Z_k=H_k{X}_k+V_k$

  实际上卡尔曼滤波就是用观测修正预测

• 滤波计算回路：

  • 状态预测：${\hat{X}}_{k/k-1}={\Phi }_{k,k-1}{\hat{X}}_{k-1}$，是根据k-1时刻的状态估计k时刻的状态，即根据k-1个观测量$Z_1,Z_2...Z_{k-1}$估计k时刻的状态。

    $\because X_k={\Phi }_{k,k-1}+{\Gamma }_{k-1}{W}_{k-1}$，噪声$W_{k-1}$与观测$Z_1,Z_2...Z_{k-1}$不相关，只影响$X_k$

    $\therefore E^{\ast }[\Gamma W_{k-1}/Z_1,Z_2...Z_{k-1}]=0$

    又$\because {\hat{X}}_{k/k-1}=E^{\ast }[X_k/Z_1,Z_2...Z_{k-1}]=E^{\ast }[({\Phi }_{k,k-1}{\hat{X}}_{k-1}+\Gamma W_{k-1})/Z_1,Z_2...Z_{k-1}]$

    $\therefore {\hat{X}}_{k/k-1}=E^{\ast }[{\Phi }_{k,k-1}{\hat{X}}_{k-1}/Z_1,Z_2...Z_{k-1}]={\Phi }_{k,k-1}{E}^{\ast }[{\hat{X}}_{k-1}/Z_1,Z_2...Z_{k-1}]={\Phi }_{k,k-1}{\hat{X}}_{k-1}$

    可计算观测：${\hat{Z}}_{k/k-1}=H_k{\hat{X}}_{k/k-1}$

    因此可以得出一个由于使用${\hat{X}}_{k/k-1}$代替$X_k$引起的误差：${\tilde{X}}_{k/k-1}=X_k-{\hat{X}}_{k/k-1}$

  • 状态估计：${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$，如上述流程图所示，这一步实际上是根据右边增益计算回路得到的K去修正预测的状态。

    根据上一步，先计算一个观测误差：${\tilde{Z}}_{k/k-1}=Z_k-{\hat{Z}}_{k/k-1}=H_k{X}_k+V_k-H_k{\hat{X}}_{k/k-1}=H_k(X_k-{\hat{X}}_{k/k-1})+V_k=H_k{\tilde{X}}_{k/k-1}+V_k$

    这个观测误差在滤波中被称为残差，包含一步预测的信息，因此可以通过加权将${\tilde{X}}_{k/k-1}$分离出来修正${\hat{X}}_{k/k-1}$

    即${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k{\tilde{Z}}_{k/k-1}={\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$

• 增益计算回路：

  上面推到了${\hat{X}}_k$就引出了一个问题：$K_k$如何确定，这里倒着推：

  这里就需要知道一个问题：卡尔曼滤波的本质就是线性最小方差估计，是其递推形式

  所以对于方差阵$P_k$，优化目标是：$min$ $P_k=E[{\tilde{X}}_k{\tilde{X}}_k^T]$

  • 估计均方误差： $P_k=(I-K_k{H}_k)P_{k/k-1}$。对于均方误差阵有$P_k=E[{\tilde{X}}_k{\tilde{X}}_k^T]=(I-K_k{H}_k)P_{k/k-1}(I-K_k{H}_k{)}^T+K_k{R}_k{K}_k^T$

    推到这里，要求$P_k$，需要知道两个值$K_k，P_{k/k-1}$

  • 滤波增益：$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}=P_k{H}_k^T{R}_k^{-1}$

    目标是求得滤波增益阵$K_k$ ,优化目标是最小化$P_k=E[{\tilde{X}}_k{\tilde{X}}_k^T]$，则使用增量$\delta P_k,\delta K_k$来描述增益阵与均方误差阵有：$P_k+\delta P_k=(I-(K_k+\delta K_k)H_k)P_{k/k-1}(I-(K_k+\delta K_k)H_k{)}^T+(K_k+\delta K_k)R_k(K_k+\delta K_k{)}^T$

    带入$P_k$等式，整体求解就是利用极值原理，最后得到一个条件，当$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$时$\delta P_k$最小。

    推到这里，$H_k,R_k$已知，为了求$K_k$,需要先求$P_{k/k-1}$

  • 一步预测均方误差：$P_{k/k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T$

    在进行状态预测时有提到一个误差：由于使用${\hat{X}}_{k/k-1}$代替$X_k$引起的误差：${\tilde{X}}_{k/k-1}=X_k-{\hat{X}}_{k/k-1}$，一步预测均方误差阵就是用来描述这个误差的$P_{k/k-1}=E^{\ast }[{\tilde{X}}_{k/k-1}{\tilde{X}}_{k/k-1}^T]$

    又$\because X_k={\Phi }_{k,k-1}{X}_{k-1}+{\Gamma }_{k-1}{W}_{k-1}$，

    $\therefore P_{k/k-1}=E^{\ast }[{\tilde{X}}_{k/k-1}{\tilde{X}}_{k/k-1}^T]=E[({\Phi }_{k,k-1}{\tilde{X}}_{k-1}+{\Gamma }_{k-1}{W}_{k-1})({\Phi }_{k,k-1}{\tilde{X}}_{k-1}+{\Gamma }_{k-1}{W}_{k-1}{)}^T]$

    同上所述：$W_{k-1}$只影响$X_k$，而${\tilde{X}}_{k/k-1}=X_k-{\hat{X}}_{k/k-1}$，$\therefore$ $W_{k-1}$与${\tilde{X}}_{k/k-1}$不相关，且$E[W_{k-1}]=0$（高斯噪声）

    $\therefore P_{k/k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T$

至此，所有推导结束，计算流程清晰可见。

PX4-EKF:

（开源的，源码自己去github）
=> ekf.cpp

init() 确定延时，调用reset()设置初始状态

reset() 初始化参数，并调用resetGpsDriftCheckFilters()重置GPS漂移检查滤波器

update() 如果buffer中的IMU数据已经更新，则进行更新：

• predictState(); //状态预测

• predictCovariance(); //协方差矩阵预测，写在covariance.cpp

• controlFusionModes(); //观测数据的控制融合，写在control.cpp

• runTerrainEstimator(); //run a separate filter for terrain estimation,写在terrain_estimator.cpp

• runYawEKFGSF(); //run EKF-GSF yaw estimator,写在ekf_helper.cpp

• calculateOutputStates

predictState():状态预测L.259

• IMU误差修正corrected_delta_ang：ekf.cpp L.262-267

  • corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.delta_ang_bias;
  • constrainStates():EKF状态约束 ekf_helper.cpp L.563,该函数中对_state.delta_ang_bias进行约束：_state.delta_ang_bias = matrix::constrain(_state.delta_ang_bias, -delta_ang_bias_limit, delta_ang_bias_limit);
  • 减去地球自转产生的角速度分量:
    • corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
  • 同理，得到一个corrected_delta_vel_ef地理系下的速度误差修正量
  • 算出一个过滤后的水平加速度_accel_lpf_NE：暂时没发现是干什么用的

• 接下来进行速度和位置处理：

  • 先取上一时刻的速度，用上面计算得到的corrected_delta_vel_ef对速度进行修正，之后在z轴补偿重力加速度

  • 之后使用梯形积分预测位置

• 调用constrainStates()进行EKF状态约束

• 计算一个平均更新时间_dt_ekf_avg,在calculateOutputStates()中被调用

calculateOutputStates：L.323

存进Buffer的内容：

用已知的误差对经过卡尔曼滤波的IMU的信息修正并得到一个预测量，将该预测量存入Buffer

• delta_angle

// correct delta angles for bias offsets
const float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg;
// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
const Vector3f delta_angle(imu.delta_ang - _state.delta_ang_bias * dt_scale_correction + _delta_angle_corr);
//使用delta_angle对四元数进行修正，得到新的output quaternions

_output_new.quat_nominal = _output_new.quat_nominal * dq;

• 对速度，位置，积分进行估计，如果IMU状态更新，则将所有值存入Buffer。L.328-394

修正算法：

在确定IMU信息更新之后，取Buffer里最早的数据进行对预测量进行修正。（即：新的最优估计=之前最优估计的预测量+已知外界影响的修正）

此处的修正采用PD控制。L.397-462

  if (_imu_updated) {
  	_output_buffer.push(_output_new);
  	_output_vert_buffer.push(_output_vert_new);
  // get the oldest INS state data from the ring buffer
  // this data will be at the EKF fusion time horizon
  // TODO: there is no guarantee that data is at delayed fusion horizon
  //       Shouldnt we use pop_first_older_than?
  const outputSample &output_delayed = _output_buffer.get_oldest();
  const outputVert &output_vert_delayed = _output_vert_buffer.get_oldest();
  					
  // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
  const Quatf q_error((_state.quat_nominal.inversed() * output_delayed.quat_nominal).normalized());
  					
  // convert the quaternion delta to a delta angle
  const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f;
  					
  const Vector3f delta_ang_error{scalar * q_error(1), scalar * q_error(2), scalar * q_error(3)};
  					
  // calculate a gain that provides tight tracking of the estimator attitude states and
  // adjust for changes in time delay to maintain consistent damping ratio of ~0.7
  const float time_delay = fmaxf((imu.time_us - _imu_sample_delayed.time_us) * 1e-6f, _dt_imu_avg);
  const float att_gain = 0.5f * _dt_imu_avg / time_delay;
  					
  // calculate a corrrection to the delta angle
  // that will cause the INS to track the EKF quaternions
  _delta_angle_corr = delta_ang_error * att_gain;
  _output_tracking_error(0) = delta_ang_error.norm();
  					
  /*
   * Loop through the output filter state history and apply the corrections to the velocity and position states.
   * This method is too expensive to use for the attitude states due to the quaternion operations required
   * but because it eliminates the time delay in the 'correction loop' it allows higher tracking gains
   * to be used and reduces tracking error relative to EKF states.
   */
  					
  // Complementary filter gains
  const float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
  const float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
  					
  // calculate down velocity and position tracking errors
  const float vert_vel_err = (_state.vel(2) - output_vert_delayed.vert_vel);
  const float vert_vel_integ_err = (_state.pos(2) - output_vert_delayed.vert_vel_integ);
  					
  // calculate a velocity correction that will be applied to the output state history
  // using a PD feedback tuned to a 5% overshoot
  const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f;
  					
  applyCorrectionToVerticalOutputBuffer(vert_vel_correction);
  					
  // calculate velocity and position tracking errors
  const Vector3f vel_err(_state.vel - output_delayed.vel);
  const Vector3f pos_err(_state.pos - output_delayed.pos);
  					
  _output_tracking_error(1) = vel_err.norm();
  _output_tracking_error(2) = pos_err.norm();
  					
  // calculate a velocity correction that will be applied to the output state history
  _vel_err_integ += vel_err;
  const Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;
  					
  // calculate a position correction that will be applied to the output state history
  _pos_err_integ += pos_err;
  const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
  					
  applyCorrectionToOutputBuffer(vel_correction, pos_correction);

}

此处用到了两个函数：

• applyCorrectionToVerticalOutputBuffer(vert_vel_correction);
• applyCorrectionToOutputBuffer(vel_correction, pos_correction);

如上图所示，修正算法得到的速度和位置量将用于对INS数据及Buffer数据进行修正，角度差量数据将被添加进IMU数据。

整个部分用到了两个Buffer，一个是OutputBuffer，另一个是VerticalOutputBuffer

applyCorrectionToVerticalOutputBuffer(vert_vel_correction);

计算一个修正应用到vert_vel，使vert_vel_integ跟踪EKF向下位置状态在融合时间范围使用的替代算法是用于vel和pos状态跟踪。该算法对垂向状态历史进行修正，并利用修正后的垂向历史向前传播垂向积分。这提供了另一种垂直速度输出，更接近位置的一阶导数，但相对于EKF状态降低了跟踪。

算法过程：循环垂直输出滤波器的状态历史，从最老数据的开始，对垂直速度状态应用修正，并使用修正后的垂直速度向前传播vert_vel_integ，并更新_output_vert_new。

applyCorrectionToOutputBuffer(vel_correction, pos_correction);

直接将修正量加进Buffer数据，最后更新一个最新的_output_new

注意：状态预测是EKF状态预测，最后计算的输出状态是预测状态+已知量的修正。predictState()及calculateOutputStates()在update()中调用

controlFusionModes()

该部分进行数据融合，controlFusionModes()函数在control.cpp文件中，该函数里是这样写的：

	// check for height sensor timeouts and reset and change sensor if necessary
	controlHeightSensorTimeouts();

	// control use of observations for aiding
	controlMagFusion();
	controlOpticalFlowFusion();
	controlGpsFusion();
	controlAirDataFusion();
	controlBetaFusion();
	controlDragFusion();
	controlHeightFusion();

	// Additional data odoemtery data from an external estimator can be fused.
	controlExternalVisionFusion();

	// Additional horizontal velocity data from an auxiliary sensor can be fused
	controlAuxVelFusion();

	// Fake position measurement for constraining drift when no other velocity or position measurements
	controlFakePosFusion();

	// check if we are no longer fusing measurements that directly constrain velocity drift
	update_deadreckoning_status();

在头文件中关于几个融合函数的解释是这样的：

	// control fusion of range finder observations
	void controlRangeFinderFusion();

	// control fusion of air data observations
	void controlAirDataFusion();

	// control fusion of synthetic sideslip observations
	void controlBetaFusion();

	// control fusion of multi-rotor drag specific force observations
	void controlDragFusion();

	// control fusion of pressure altitude observations
	void controlBaroFusion();

	// control fusion of fake position observations to constrain drift
	void controlFakePosFusion();

	// control fusion of auxiliary velocity observations
	void controlAuxVelFusion();

挨个看融合函数：

controlMagFusion()：磁场数据融合

写在mag_control.cpp中

整体流程：

• 确保使用磁力计数据进行融合：L.44-73

• if (noOtherYawAidingThanMag() && _mag_data_ready) ：如果确定没有其它外部导航数据且磁力计数据已经准备好：

  • 选择融合模式：

    • MAG_FUSE_TYPE_AUTO：该模式下方向或3D磁力计融合的选择将是自动的，调用selectMagAuto()函数：

      void Ekf::selectMagAuto()
      {
      	check3DMagFusionSuitability();
      	canUse3DMagFusion() ? startMag3DFusion() : startMagHdgFusion();
      }
      

    • MAG_FUSE_TYPE_HEADING：该模式下官方解释为：简单的偏航角融合将一直使用。这是不准确的，但较少受地球场变形的影响，不应用于-60至+60度范围以外的俯仰角：

      void Ekf::startMagHdgFusion()
      {
      	stopMag3DFusion();
      	_control_status.flags.mag_hdg = true;
      }
      

    • MAG_FUSE_TYPE_3D：该模式下磁力计三轴融合将一直被使用。这是更准确的，但更受局部地球场扭曲的影响：

      void Ekf::startMag3DFusion()
      {
      	if (!_control_status.flags.mag_3D) {
      		stopMagHdgFusion();
      		zeroMagCov();
      		loadMagCovData();
      		_control_status.flags.mag_3D = true;
      	}
      }
      

      和上面MAG_FUSE_TYPE_HEADING模式互锁了一下

  • if (_control_status.flags.in_air){

    checkHaglYawResetReq();
    runInAirYawReset();
    runVelPosReset();
    //在起飞之后重置磁力计偏航角、速度和位置以避免地面环境中的干扰
    

    }

  • checkMagDeclRequired();
    checkMagInhibition();
    runMagAndMagDeclFusions();
    

    • checkMagDecRequired()：官方解释是如果我们使用三轴磁力计融合，但没有外部的NE辅助，那么赤纬必须作为观测进行融合，以防止长期的航向漂移。当有gps辅助时，赤纬融合是可选的，但建议防止车辆长时间静止的问题。设置一个_control_status.flags.mag_dec

    • checkMagInhibition()：设置一个_mag_inhibit_yaw_reset_req

    • runMagAndMagDeclFusions()：该函数根据前面设置的融合模式进行函数调用

      void Ekf::runMagAndMagDeclFusions()
      {
      	if (_control_status.flags.mag_3D) {
      		run3DMagAndDeclFusions();
      	} else if (_control_status.flags.mag_hdg) {
      		fuseHeading();
      	}
      }
      

• 接下来分别理解两种融合模式：

  • run3DMagAndDeclFusions()：

    如果地磁重置了就重新计算协方差，否则直接调用fuseMag(),整个融合过程是先计算一个更新，然后再调用measurementUpdate()，之后在该函数中调用fuse()对状态进行更新

  • fuseHeading()：

    整体思路如上，只是计算更新的方式不一样最后都是计算一个增益然后调用fuse()对状态进行更新

相关函数：

• noOtherYawAidingThanMag()：函数解释是在使用视觉或者gps导航时不融合磁力计数据，即如果在这两种状态下该函数会返回0

fuse()：ekf融合函数

void Ekf::fuse(const Vector24f &K, float innovation)
{
	_state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation;
	_state.quat_nominal.normalize();
	_state.vel -= K.slice<3, 1>(4, 0) * innovation;
	_state.pos -= K.slice<3, 1>(7, 0) * innovation;
	_state.delta_ang_bias -= K.slice<3, 1>(10, 0) * innovation;
	_state.delta_vel_bias -= K.slice<3, 1>(13, 0) * innovation;
	_state.mag_I -= K.slice<3, 1>(16, 0) * innovation;
	_state.mag_B -= K.slice<3, 1>(19, 0) * innovation;
	_state.wind_vel -= K.slice<2, 1>(22, 0) * innovation;
}

注：笔者主要目标在于理解滤波和融合，其它部分写的不太详细，见谅。
另外，上回扒aruco的时候有手算了DLT和P3P，改天有时间的话再写写放上来吧，公式太多了好难打QAQ