fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 07:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

saved_mag_ef_cov to Squarematrix2f

Browse Source
This commit is contained in:
kamilritz
2020-07-27 21:19:25 +02:00
committed by Mathieu Bresciani
parent 7609bc69b2
commit 15d360f446
2 changed files with 4 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.h
+2 -1
View File
@@ -50,6 +50,7 @@ public:
static constexpr uint8_t _k_num_states{24}; ///< number of EKF states static constexpr uint8_t _k_num_states{24}; ///< number of EKF states
typedef matrix::Vector<float, _k_num_states> Vector24f; typedef matrix::Vector<float, _k_num_states> Vector24f;
typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f; typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f;
typedef matrix::SquareMatrix<float, 2> Matrix2f;
Ekf() = default; Ekf() = default;
virtual ~Ekf() = default; virtual ~Ekf() = default;
@@ -475,7 +476,7 @@ private:
uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec) uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
float _saved_mag_bf_variance[4] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) float _saved_mag_bf_variance[4] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
float _saved_mag_ef_covmat[2][2] {}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) Matrix2f _saved_mag_ef_covmat; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
bool _velpos_reset_request{false}; ///< true when a large yaw error has been fixed and a velocity and position state reset is required bool _velpos_reset_request{false}; ///< true when a large yaw error has been fixed and a velocity and position state reset is required
gps_check_fail_status_u _gps_check_fail_status{}; gps_check_fail_status_u _gps_check_fail_status{};
EKF/ekf_helper.cpp
+2 -10
View File
@@ -1517,11 +1517,7 @@ void Ekf::saveMagCovData()
} }
// save the NE axis covariance sub-matrix // save the NE axis covariance sub-matrix
for (uint8_t row = 0; row <= 1; row ++) { _saved_mag_ef_covmat = P.slice<2, 2>(16, 16);
for (uint8_t col = 0; col <= 1; col ++) {
_saved_mag_ef_covmat[row][col] = P(row + 16,col + 16);
}
}
} }
void Ekf::loadMagCovData() void Ekf::loadMagCovData()
@@ -1531,11 +1527,7 @@ void Ekf::loadMagCovData()
P(index + 18,index + 18) = _saved_mag_bf_variance[index]; P(index + 18,index + 18) = _saved_mag_bf_variance[index];
} }
// re-instate the NE axis covariance sub-matrix // re-instate the NE axis covariance sub-matrix
for (uint8_t row = 0; row <= 1; row ++) { P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat;
for (uint8_t col = 0; col <= 1; col ++) {
P(row + 16,col + 16) = _saved_mag_ef_covmat[row][col];
}
}
} }
void Ekf::stopGpsFusion() void Ekf::stopGpsFusion()