dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

new fixed wing estimator: Fix the symmetry force step of the covariance prediction.

This commit is contained in:
Lorenz Meier 2014-03-23 18:28:13 +01:00
parent a0ceeee9ef
commit 5693a9d3b6
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/fw_att_pos_estimator/estimator.cpp
View File

@ -876,7 +876,7 @@ void CovariancePrediction(float dt)
nextP[20][19] = P[20][19];
nextP[20][20] = P[20][20];
for (uint8_t i=0; i<= 20; i++)
for (unsigned i = 0; i < n_states; i++)
{
nextP[i][i] = nextP[i][i] + processNoise[i];
}
@ -942,7 +942,16 @@ void CovariancePrediction(float dt)
P[i][i] = nextP[i][i];
}
ForceSymmetry();
// force symmetry for observable states
for (unsigned i = 1; i < n_states; i++)
{
for (uint8_t j = 0; j < i; j++)
{
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
P[j][i] = P[i][j];
}
}
}
ConstrainVariances();