卡尔曼滤波及数据融合:PX4-EKF源码分析
注:笔者只是在自己学习的过程中遇到了很多问题,在网上也没有看到可以解释得太清楚明白的资料,所以将一些心得和笔记写出来,后来人如果可以从中得到一些帮助甚感荣幸。因为非科班出身自学,如果有错误的地方希望大佬们不吝赐教。
卡尔曼滤波
提起导航制导,必然避不开卡尔曼滤波,原理都很好解释经典五步,但是怎么用,为什么有这五步才是关键。
-
卡尔曼滤波整体体现为:
预测+更新
- 预测:通过状态转移函数计算当前值
- 更新:融合当前观测状态:计算两个高斯分布融合后的均值和协方差矩阵
-
卡尔曼滤波的经典五步:
- 状态一步预测: X ^ k / k − 1 = Φ k , k − 1 X ^ k − 1 \hat{X}_{k/k-1}=\Phi_{ {k,k-1}}\hat{X}_{k-1} X^k/k−1=Φk,k−1X^k−1
- 状态估计: X ^ k = X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) \hat{X}_{k}=\hat{X}_{k/k-1}+K_{k}(Z_{k}-H_{k} \hat{X}_{k/k-1}) X^k=X^k/k−1+Kk(Zk−HkX^k/k−1), 其中 K k K_{k} Kk就是卡尔曼增益
- 滤波增益: 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/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} Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1=PkHkTRk−1
- 一步预测均方误差: P k / k − 1 = Φ k , k − 1 P k − 1 Φ k , k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T 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} Pk/k−1=Φk,k−1Pk−1Φk,k−1T+Γk−1Qk−1Γk−1T
- 估计均方误差: P k = ( I − K k H k ) P k / k − 1 P_{k}=(I-K_{k}H_{k})P_{k/k-1} Pk=(I−KkHk)Pk/k−1
这五步就是卡尔曼滤波经典的五步计算,计算流程图如下所示:
注,流程图来源《卡尔曼滤波与组合导航原理》出版社:西北工业大学
整体学习参考教材也是这本书
详细推导过程如下:
-
首先描述一组状态使用:
X k = Φ k , k − 1 X k − 1 + Γ k − 1 W k − 1 X_{k}=\Phi_{k,k-1}X_{k-1}+\Gamma_{k-1}W_{k-1} Xk=Φk,k−1Xk−1+Γk−1Wk−1 ; Z k = H k X k + V k Z_{k}=H_{k}X_{k}+V_{k} Zk=HkXk+Vk
实际上卡尔曼滤波就是用观测修正预测
扫描二维码关注公众号,回复: 15852971 查看本文章 -
滤波计算回路:
-
状态预测
: X ^ k / k − 1 = Φ k , k − 1 X ^ k − 1 \hat{X}_{k/k-1}=\Phi_{ {k,k-1}}\hat{X}_{k-1} X^k/k−1=Φk,k−1X^k−1,是根据k-1时刻的状态估计k时刻的状态,即根据k-1个观测量 Z 1 , Z 2 . . . Z k − 1 Z_{1},Z_{2}...Z_{k-1} Z1,Z2...Zk−1估计k时刻的状态。∵ X k = Φ k , k − 1 + Γ k − 1 W k − 1 \because X_{k}=\Phi_{k,k-1}+\Gamma_{k-1}W_{k-1} ∵Xk=Φk,k−1+Γk−1Wk−1,噪声 W k − 1 W_{k-1} Wk−1与观测 Z 1 , Z 2 . . . Z k − 1 Z_{1},Z_{2}...Z_{k-1} Z1,Z2...Zk−1不相关,只影响 X k X_{k} Xk
∴ E ∗ [ Γ W k − 1 / Z 1 , Z 2 . . . Z k − 1 ] = 0 \therefore E^{*}[\Gamma W_{k-1}/Z_{1},Z_{2}...Z_{k-1}]=0 ∴E∗[ΓWk−1/Z1,Z2...Zk−1]=0
又 ∵ X ^ k / k − 1 = E ∗ [ X k / Z 1 , Z 2 . . . Z k − 1 ] = E ∗ [ ( Φ k , k − 1 X ^ k − 1 + Γ W k − 1 ) / Z 1 , Z 2 . . . Z k − 1 ] \because\hat{X}_{k/k-1}=E^{*}[X_{k}/Z_{1},Z_{2}...Z_{k-1}]=E^{*}[(\Phi_{ {k,k-1}}\hat{X}_{k-1}+\Gamma W_{k-1})/Z_{1},Z_{2}...Z_{k-1}] ∵X^k/k−1=E∗[Xk/Z1,Z2...Zk−1]=E∗[(Φk,k−1X^k−1+ΓWk−1)/Z1,Z2...Zk−1]
∴ X ^ k / k − 1 = E ∗ [ Φ k , k − 1 X ^ k − 1 / Z 1 , Z 2 . . . Z k − 1 ] = Φ k , k − 1 E ∗ [ X ^ k − 1 / Z 1 , Z 2 . . . Z k − 1 ] = Φ k , k − 1 X ^ k − 1 \therefore \hat{X}_{k/k-1}=E^{*}[\Phi_{ {k,k-1}}\hat{X}_{k-1}/Z_{1},Z_{2}...Z_{k-1}]=\Phi_{ {k,k-1}}E^{*}[\hat{X}_{k-1}/Z_{1},Z_{2}...Z_{k-1}]=\Phi_{ {k,k-1}}\hat{X}_{k-1} ∴X^k/k−1=E∗[Φk,k−1X^k−1/Z1,Z2...Zk−1]=Φk,k−1E∗[X^k−1/Z1,Z2...Zk−1]=Φk,k−1X^k−1
可计算观测: Z ^ k / k − 1 = H k X ^ k / k − 1 \hat{Z}_{k/k-1}=H_{k}\hat{X}_{k/k-1} Z^k/k−1=HkX^k/k−1
因此可以得出一个由于使用 X ^ k / k − 1 \hat{X}_{k/k-1} X^k/k−1代替 X k X_{k} Xk引起的误差: X ~ k / k − 1 = X k − X ^ k / k − 1 \widetilde{X}_{k/k-1}=X_{k}-\hat{X}_{k/k-1} X k/k−1=Xk−X^k/k−1
-
状态估计
: X ^ k = X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) \hat{X}_{k}=\hat{X}_{k/k-1}+K_{k}(Z_{k}-H_{k}\hat{X}_{k/k-1}) X^k=X^k/k−1+Kk(Zk−HkX^k/k−1),如上述流程图所示,这一步实际上是根据右边增益计算回路得到的K去修正预测的状态。根据上一步,先计算一个观测误差: Z ~ k / k − 1 = Z k − Z ^ k / k − 1 = H k X k + V k − H k X ^ k / k − 1 = H k ( X k − X ^ k / k − 1 ) + V k = H k X ~ k / k − 1 + V k \widetilde{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}\widetilde{X}_{k/k-1}+V_{k} Z k/k−1=Zk−Z^k/k−1=HkXk+Vk−HkX^k/k−1=Hk(Xk−X^k/k−1)+Vk=HkX k/k−1+Vk
这个观测误差在滤波中被称为残差,包含一步预测的信息,因此可以通过加权将 X ~ k / k − 1 \widetilde{X}_{k/k-1} X k/k−1分离出来修正 X ^ k / k − 1 \hat{X}_{k/k-1} X^k/k−1
即 X ^ k = X ^ k / k − 1 + K k Z ~ k / k − 1 = X ^ k = X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) \hat{X}_{k}=\hat{X}_{k/k-1}+K_{k}\widetilde{Z}_{k/k-1}=\hat{X}_{k}=\hat{X}_{k/k-1}+K_{k}(Z_{k}-H_{k}\hat{X}_{k/k-1}) X^k=X^k/k−1+KkZ k/k−1=X^k=X^k/k−1+Kk(Zk−HkX^k/k−1)
-
-
增益计算回路:
上面推到了 X ^ k \hat{X}_{k} X^k就引出了一个问题: K k K_{k} Kk如何确定,这里倒着推:
这里就需要知道一个问题:卡尔曼滤波的本质就是线性最小方差估计,是其递推形式
所以对于方差阵 P k P_{k} Pk,优化目标是: m i n min min P k = E [ X ~ k X ~ k T ] P_{k}=E[\widetilde{X}_{k} \widetilde{X}_{k}^{T}] Pk=E[X kX kT]
-
估计均方误差
: P k = ( I − K k H k ) P k / k − 1 P_{k}=(I-K_{k}H_{k})P_{k/k-1} Pk=(I−KkHk)Pk/k−1。对于均方误差阵有 P k = E [ X ~ k 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}=E[\widetilde{X}_{k} \widetilde{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} Pk=E[X kX kT]=(I−KkHk)Pk/k−1(I−KkHk)T+KkRkKkT推到这里,要求 P k P_{k} Pk,需要知道两个值 K k , P k / k − 1 K_{k},P_{k/k-1} Kk,Pk/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/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} Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1=PkHkTRk−1目标是求得滤波增益阵 K k K_{k} Kk ,优化目标是最小化 P k = E [ X ~ k X ~ k T ] P_{k}=E[\widetilde{X}_{k}\widetilde{X}_{k}^{T}] Pk=E[X kX kT],则使用增量 δ P k , δ K k \delta P_{k},\delta K_{k} δPk,δKk来描述增益阵与均方误差阵有: P k + δ P k = ( I − ( K k + δ K k ) H k ) P k / k − 1 ( I − ( K k + δ K k ) H k ) T + ( K k + δ K k ) R k ( K k + δ K k ) T 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} Pk+δPk=(I−(Kk+δKk)Hk)Pk/k−1(I−(Kk+δKk)Hk)T+(Kk+δKk)Rk(Kk+δKk)T
带入 P k P_{k} Pk等式,整体求解就是利用
极值原理
,最后得到一个条件,当 K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 K_{k}={P}_{k/k-1}H_{k}^{T}{(H_{k}P_{k/k-1}H_{k}^{T}+R_{k})}^{-1} Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1时 δ P k \delta P_{k} δPk最小。推到这里, H k , R k H_{k},R_{k} Hk,Rk已知,为了求 K k K_{k} Kk,需要先求 P k / k − 1 P_{k/k-1} Pk/k−1
-
一步预测均方误差
: P k / k − 1 = Φ k , k − 1 P k − 1 Φ k , k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T 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} Pk/k−1=Φk,k−1Pk−1Φk,k−1T+Γk−1Qk−1Γk−1T在进行状态预测时有提到一个误差:由于使用 X ^ k / k − 1 \hat{X}_{k/k-1} X^k/k−1代替 X k X_{k} Xk引起的误差: X ~ k / k − 1 = X k − X ^ k / k − 1 \widetilde{X}_{k/k-1}=X_{k}-\hat{X}_{k/k-1} X k/k−1=Xk−X^k/k−1,一步预测均方误差阵就是用来描述这个误差的 P k / k − 1 = E ∗ [ X ~ k / k − 1 X ~ k / k − 1 T ] P_{k/k-1}=E^{*}[\widetilde{X}_{k/k-1} \widetilde{X}_{k/k-1}^{T}] Pk/k−1=E∗[X k/k−1X k/k−1T]
又 ∵ X k = Φ k , k − 1 X k − 1 + Γ k − 1 W k − 1 \because X_{k}=\Phi_{k,k-1}X_{k-1}+\Gamma_{k-1}W_{k-1} ∵Xk=Φk,k−1Xk−1+Γk−1Wk−1,
∴ P k / k − 1 = E ∗ [ X ~ k / k − 1 X ~ k / k − 1 T ] = E [ ( Φ k , k − 1 X ~ k − 1 + Γ k − 1 W k − 1 ) ( Φ k , k − 1 X ~ k − 1 + Γ k − 1 W k − 1 ) T ] \therefore P_{k/k-1}=E^{*}[\widetilde{X}_{k/k-1} \widetilde{X}_{k/k-1}^{T}]=E[(\Phi_{k,k-1}\widetilde{X}_{k-1}+\Gamma_{k-1}W_{k-1})(\Phi_{k,k-1}\widetilde{X}_{k-1}+\Gamma_{k-1}W_{k-1})^{T}] ∴Pk/k−1=E∗[X k/k−1X k/k−1T]=E[(Φk,k−1X k−1+Γk−1Wk−1)(Φk,k−1X k−1+Γk−1Wk−1)T]
同上所述: W k − 1 W_{k-1} Wk−1只影响 X k X_{k} Xk,而 X ~ k / k − 1 = X k − X ^ k / k − 1 \widetilde{X}_{k/k-1}=X_{k}-\hat{X}_{k/k-1} X k/k−1=Xk−X^k/k−1, ∴ \therefore ∴ W k − 1 W_{k-1} Wk−1与 X ~ k / k − 1 \widetilde{X}_{k/k-1} X k/k−1不相关,且 E [ W k − 1 ] = 0 E[W_{k-1}]=0 E[Wk−1]=0(高斯噪声)
∴ P k / k − 1 = Φ k , k − 1 P k − 1 Φ k , k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T \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} ∴Pk/k−1=Φk,k−1Pk−1Φk,k−1T+Γk−1Qk−1Γk−1T
-
至此,所有推导结束,计算流程清晰可见。
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