PX4源码版本commit hash41c6dcaf705f40983a196fc53b39b69ce2788146代码结构src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp预处理// mag对齐标志if(!_control_status_prev.flags.in_air_control_status.flags.in_air){_control_status.flags.mag_aligned_in_flightfalse;}// 是否可用_fc.mag.available_params.ekf2_mag_type!static_castint32_t(MagFuseType::NONE);// 是否使用if(!_fc.mag.intended()){stopMagFusion();return;}状态估计// 噪声初始化// XYZ Measurement uncertainty. Need to consider timing errors for fast rotationsconstfloatR_MAGmath::max(sq(_params.ekf2_mag_noise),sq(0.01f));// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gainsVector3f mag_innov;Vector3f innov_var;// Observation jacobian and Kalman gain vectorsVectorState H;sym::ComputeMagInnovInnovVarAndHx(_state.vector(),P,mag_sample.mag,R_MAG,FLT_EPSILON,mag_innov,innov_var,H);updateAidSourceStatus(aid_src,mag_sample.time_us,// sample timestampmag_sample.mag,// observationVector3f(R_MAG,R_MAG,R_MAG),// observation variancemag_innov,// innovationinnov_var,// innovation variancemath::max(_params.ekf2_mag_gate,1.f));// innovation gate世界磁场模型( WMM)更新boolwmm_updatedfalse;if(global_origin().isInitialized()){boolorigin_newer_than_last_mag(global_origin().getProjectionReferenceTimestamp()aid_src.time_last_fuse);if(global_origin_valid()(origin_newer_than_last_mag||(isLocalHorizontalPositionValid()isTimedOut(_wmm_mag_time_last_checked,10e6)))){if(updateWorldMagneticModel(_gpos.latitude_deg(),_gpos.longitude_deg())){wmm_updatedtrue;}_wmm_mag_time_last_checked_time_delayed_us;}elseif(origin_newer_than_last_mag){// use global origin to update WMMif(updateWorldMagneticModel(global_origin().getProjectionReferenceLat(),global_origin().getProjectionReferenceLon())){wmm_updatedtrue;}}}合成计算z轴若飞控参数 EKF2_SYNT_MAG_Z 为enabled且 WMM 数据有效用 XY 测量 地磁场模型计算合成 Z 分量避免强磁干扰if(_params.ekf2_synt_mag_z(_params.ekf2_decl_typeGeoDeclinationMask::USE_GEO_DECL)(_wmm_earth_field_gauss.isAllFinite()_wmm_earth_field_gauss.longerThan(0.f))){mag_sample.mag(2)calculate_synthetic_mag_z_measurement(mag_sample.mag,_wmm_earth_field_gauss);_control_status.flags.synthetic_mag_ztrue;}else{_control_status.flags.synthetic_mag_zfalse;}融合模式决策{constboolmag_consistent_or_no_ne_aiding_control_status.flags.mag_heading_consistent||!isNorthEastAidingActive();constboolcommon_conditions_passing_control_status.flags.mag((_control_status.flags.yaw_alignmag_consistent_or_no_ne_aiding)||(!_control_status.flags.ev_yaw!_control_status.flags.yaw_align))!_control_status.flags.mag_fault!_control_status.flags.mag_field_disturbed!_control_status.flags.ev_yaw!_control_status.flags.gnss_yaw(!_control_status.flags.yaw_manual||_control_status.flags.mag_aligned_in_flight)!_control_status.flags.constant_pos;_control_status.flags.mag_3Dcommon_conditions_passing(_params.ekf2_mag_typestatic_castint32_t(MagFuseType::AUTO))_control_status.flags.mag_aligned_in_flight;_control_status.flags.mag_hdgcommon_conditions_passing((_params.ekf2_mag_typestatic_castint32_t(MagFuseType::HEADING))||(_params.ekf2_mag_typestatic_castint32_t(MagFuseType::AUTO)!_control_status.flags.mag_3D));}if(_control_status.flags.mag_dec){// observation variance (rad**2)constfloatR_DECLsq(0.5f);if((_params.ekf2_decl_typeGeoDeclinationMask::USE_GEO_DECL)PX4_ISFINITE(_wmm_declination_rad)){// using declination from the world magnetic modelfuseDeclination(_wmm_declination_rad,R_DECL,update_all_states,update_tilt);}elseif((_params.ekf2_decl_typeGeoDeclinationMask::SAVE_GEO_DECL)PX4_ISFINITE(_params.ekf2_mag_decl)(fabsf(_params.ekf2_mag_decl)0.f)){// using previously saved declinationfuseDeclination(math::radians(_params.ekf2_mag_decl),R_DECL,update_all_states,update_tilt);}else{// if there is no aiding coming from an inertial frame we need to fuse some declination// even if we dont know the value, its better to fuse 0 than nothingfloatdeclination_rad0.f;fuseDeclination(declination_rad,R_DECL);}}}解决地磁干扰问题下面是地磁干扰处理的完整决策与操作流程图轻微干扰中度干扰严重干扰保留航向完全禁用检测到地磁干扰干扰强度评估启用合成Z轴EKF2_SYNT_MAG_Z enabled调大传感器噪声EKF2_MAG_NOISE 增大切换融合模式EKF2_MAG_TYPE 调整使用WMM模型计算Z轴XY测量 地磁场模型降低磁力计权重增加观测方差R_MAG选择融合策略MAG_HDG模式仅使用航向信息MAG_NONE模式停止磁力计融合参数调整完成状态估计更新卡尔曼滤波融合输出稳定姿态干扰问题解决根据代码逻辑首先使能EKF2_SYNT_MAG_Z然后调大传感器噪声修改融合方式为。