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

ekf2: auto-generate state vector size constant

Browse Source
This commit is contained in:
bresch
2023-09-19 09:37:50 -04:00
committed by Daniel Agar
parent 8a9a303354
commit dac337efc4
8 changed files with 20 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+5 -5
View File
@@ -283,14 +283,14 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// keep previous covariance
for (unsigned i = 0; i < State::mag_I.dof; i++) {
unsigned row = State::mag_I.idx + i;
for (unsigned col = 0; col < _k_num_states; col++) {
for (unsigned col = 0; col < State::size; col++) {
nextP(row, col) = nextP(col, row) = P(row, col);
}
}
for (unsigned i = 0; i < State::mag_B.dof; i++) {
unsigned row = State::mag_B.idx + i;
for (unsigned col = 0; col < _k_num_states; col++) {
for (unsigned col = 0; col < State::size; col++) {
nextP(row, col) = nextP(col, row) = P(row, col);
}
}
@@ -308,14 +308,14 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// keep previous covariance
for (unsigned i = 0; i < State::wind_vel.dof; i++) {
unsigned row = State::wind_vel.idx + i;
for (unsigned col = 0 ; col < _k_num_states; col++) {
for (unsigned col = 0 ; col < State::size; col++) {
nextP(row, col) = nextP(col, row) = P(row, col);
}
}
}
// covariance matrix is symmetrical, so copy upper half to lower half
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned row = 0; row < State::size; row++) {
for (unsigned column = 0 ; column < row; column++) {
P(row, column) = P(column, row) = nextP(column, row);
}
@@ -515,7 +515,7 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP)
{
bool healthy = true;
for (int i = 0; i < _k_num_states; i++) {
for (int i = 0; i < State::size; i++) {
if (P(i, i) < KHP(i, i)) {
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
healthy = false;