当所有N个EKF的速度新息都超限时,EKFGSF_yaw 会reset。 细节3: 从这个3状态水平运动EKF的新息中,可以明确航向可观性来自加速度的原由,这一点在完整INS/GPS EKF中原理是相同,可以体会一下。所以可以说,对INS/GPS系统,需要通过加速运动来提供yaw的可观性。但是,如果是其他传感器组合,那么上文提到的“航向可观性来...
EKF: Create a EKF-GSF yaw estimator class 14e1924 EKF: add emergency yaw reset functionality c8c1904 EKF: remove un-used function a566eb0 EKF: Ensure required constants are defined for all builds c59daad EKF: Fix CI build error e9dd2ef ...
1 change: 1 addition & 0 deletions 1 src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp Original file line numberDiff line numberDiff line change @@ -245,6 +245,7 @@ void Ekf::controlMagFusion() if (reset_heading) { _control_status.flags.yaw_align = true; resetAidSource...
(yaw_delta[i] - yaw_delta.mean(), 2); } } } void EKFGSF::reset() { ekf_gsf_vel_fuse_started_ = false; gsf_yaw_variance_ = INFINITY; } void EKFGSF::ahrsPredictYaw(const uint8_t model_index, const float &gyro_z, const float delta_ang_dt) { float ang_rate = gyro_z - ...
();// Update states using magnetometer or external yaw sensor dataSelectMagFusion();// Update states using GPS and altimeter dataSelectVelPosFusion();// Update states using range beacon dataSelectRngBcnFusion();// Update states using optical flow dataSelectFlowFusion();// Update states using ...
AHRS_CUSTOM_YAW,0 AHRS_EKF_TYPE,2 AHRS_GPS_GAIN,1 AHRS_GPS_MINSATS,6 AHRS_GPS_USE,1 AHRS_ORIENTATION,0 AHRS_RP_P,0.2 AHRS_TRIM_X,-0.009350876 AHRS_TRIM_Y,0.02558022 AHRS_TRIM_Z,0 AHRS_WIND_MAX,0 AHRS_YAW_P,0.2 ANGLE_MAX,4500 ARMING_ACCTHRESH,0.75 ARMING_CH...
();// Update states using magnetometer or external yaw sensor dataSelectMagFusion();// Update states using GPS and altimeter dataSelectVelPosFusion();// Update states using range beacon dataSelectRngBcnFusion();// Update states using optical flow dataSelectFlowFusion();// Update states using ...
&& !_control_status.flags.gps_yaw; Expand All@@ -92,7 +89,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star if(continuing_conditions_passing && _control_status.flags.yaw_align) { if(mag_sample.reset||checkHaglYawResetReq()) { ...
For example if you have vision + GNSS, but at takeoff you don't have a good fix (maybe you're inside) we fully use the EV frame (ev_pos + ev_yaw) and ignore mag (!yaw_align). Later if GNSS becomes good we reset yaw and start (cs_yaw_align=true, cs_gps=true) and ev_yaw...
} _state_reset_status{}; ///< reset event monitoring structure containing velocity, position, height and yaw reset informationfloat _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekfVector3f _ang_rate_delayed_raw; ///< uncorrected angular rate vector at fusion time...