fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 18:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

move ekf2 Matrix helper utilities to mathlib

Browse Source
This commit is contained in:
Daniel Agar
2022-08-05 09:58:07 -04:00
parent 6ebc88fed7
commit bce4237963
11 changed files with 173 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_control.cpp
+2 -2
View File
@@ -70,7 +70,7 @@ void Ekf::controlMagFusion()
// compute mag heading innovation (for estimator_aid_src_mag_heading logging)
const Vector3f mag_observation = mag_sample.mag - _state.mag_B;
const Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth);
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
const Vector3f mag_earth_pred = R_to_earth * mag_observation;
resetEstimatorAidStatus(_aid_src_mag_heading);
@@ -355,7 +355,7 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
} else if (_control_status.flags.mag_hdg && !_is_yaw_fusion_inhibited) {
// Rotate the measurements into earth frame using the zero yaw angle
Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth);
Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B);