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

More debug in filter

Browse Source
This commit is contained in:
Lorenz Meier
2014-05-11 14:42:33 +02:00
parent e098591a58
commit 5f392c80ad
1 changed files with 25 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/estimator.cpp
+25 -10
View File
@@ -1268,7 +1268,7 @@ void AttPosEKF::FuseMagnetometer()
for (uint8_t i = 0; i < n_states; i++) {
H_MAG[i] = 0.0f;
}
uint8_t indexLimit;
unsigned indexLimit;
// Perform sequential fusion of Magnetometer measurements.
// This assumes that the errors in the different components are
@@ -1281,11 +1281,11 @@ void AttPosEKF::FuseMagnetometer()
// Limit range of states modified when on ground
if(!onGround)
{
indexLimit = 22;
indexLimit = n_states;
}
else
{
indexLimit = 13;
indexLimit = 13 + 1;
}
// Sequential fusion of XYZ components to spread processing load across
@@ -1483,7 +1483,7 @@ void AttPosEKF::FuseMagnetometer()
if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0)
{
// correct the state vector
for (uint8_t j= 0; j<=indexLimit; j++)
for (uint8_t j= 0; j < indexLimit; j++)
{
states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
}
@@ -1500,7 +1500,7 @@ void AttPosEKF::FuseMagnetometer()
// correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the
// number of operations
for (uint8_t i = 0; i<=indexLimit; i++)
for (uint8_t i = 0; i < indexLimit; i++)
{
for (uint8_t j = 0; j <= 3; j++)
{
@@ -1522,9 +1522,9 @@ void AttPosEKF::FuseMagnetometer()
}
}
}
for (uint8_t i = 0; i<=indexLimit; i++)
for (uint8_t i = 0; i < indexLimit; i++)
{
for (uint8_t j = 0; j<=indexLimit; j++)
for (uint8_t j = 0; j < indexLimit; j++)
{
KHP[i][j] = 0.0f;
for (uint8_t k = 0; k <= 3; k++)
@@ -1541,9 +1541,9 @@ void AttPosEKF::FuseMagnetometer()
}
}
}
for (uint8_t i = 0; i <= indexLimit; i++)
for (uint8_t i = 0; i < indexLimit; i++)
{
for (uint8_t j = 0; j <= indexLimit; j++)
for (uint8_t j = 0; j < indexLimit; j++)
{
P[i][j] = P[i][j] - KHP[i][j];
}
@@ -2001,7 +2001,22 @@ void AttPosEKF::CovarianceInit()
float AttPosEKF::ConstrainFloat(float val, float min, float max)
{
return (val > max) ? max : ((val < min) ? min : val);
float ret;
if (val > max) {
ret = max;
ekf_debug("> max: %8.4f, val: %8.4f", min);
} else if (val < min) {
ret = min;
ekf_debug("< min: %8.4f, val: %8.4f", max);
} else {
ret = val;
}
if (!isfinite) {
ekf_debug("constrain: non-finite!");
}
return ret;
}
void AttPosEKF::ConstrainVariances()