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

estimator: Move symmetry force outside of non-related loop, improving efficiency and resolving a locals warning.

Browse Source
This commit is contained in:
Lorenz Meier
2014-06-29 12:13:40 +02:00
parent d93a64dd09
commit 2b32414709
1 changed files with 11 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+11 -11
View File
@@ -887,20 +887,20 @@ void AttPosEKF::CovariancePrediction(float dt)
// propagate
for (unsigned i = 0; i <= 13; i++) {
P[i][i] = nextP[i][i];
}
// force symmetry for observable states
// force zero for non-observable states
for (unsigned i = 1; i < n_states; i++)
// force symmetry for observable states
// force zero for non-observable states
for (unsigned i = 1; i < n_states; i++)
{
for (uint8_t j = 0; j < i; j++)
{
for (uint8_t j = 0; j < i; j++)
{
if ((i > 13) || (j > 13)) {
P[i][j] = 0.0f;
} else {
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
}
P[j][i] = P[i][j];
if ((i > 13) || (j > 13)) {
P[i][j] = 0.0f;
} else {
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
}
P[j][i] = P[i][j];
}
}