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

22 state estimator: Fix min/max #3022

This commit is contained in:
Lorenz Meier 2015-10-19 22:38:16 +02:00
parent 9288b8fa70
commit a554333841
1 changed files with 10 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -43,7 +43,6 @@
#include <stdio.h>
#include <stdarg.h>
#include <cmath>
#include <algorithm>
#ifndef M_PI_F
#define M_PI_F static_cast<float>(M_PI)
@ -1851,7 +1850,7 @@ void AttPosEKF::FuseOptFlow()
Vector3f relVelSensor;
// Perform sequential fusion of optical flow measurements only with valid tilt and height
flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng);
float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9];
bool validTilt = Tnb.z.z > 0.71f;
if (validTilt)
@ -2111,7 +2110,7 @@ void AttPosEKF::OpticalFlowEKF()
} else {
return;
}
distanceTravelledSq = std::min(distanceTravelledSq, 100.0f);
distanceTravelledSq = fmin(distanceTravelledSq, 100.0f);
Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
}
@ -2151,7 +2150,7 @@ void AttPosEKF::OpticalFlowEKF()
varInnovRng = 1.0f/SK_RNG[1];
// constrain terrain height to be below the vehicle
flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng);
// estimate range to centre of image
range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2];
@ -2171,7 +2170,7 @@ void AttPosEKF::OpticalFlowEKF()
}
// constrain the states
flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng);
// correct the covariance matrix
float nextPopt[2][2];
@ -2180,8 +2179,8 @@ void AttPosEKF::OpticalFlowEKF()
nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
// prevent the state variances from becoming negative and maintain symmetry
Popt[0][0] = std::max(nextPopt[0][0],0.0f);
Popt[1][1] = std::max(nextPopt[1][1],0.0f);
Popt[0][0] = fmax(nextPopt[0][0],0.0f);
Popt[1][1] = fmax(nextPopt[1][1],0.0f);
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
Popt[1][0] = Popt[0][1];
}
@ -2220,7 +2219,7 @@ void AttPosEKF::OpticalFlowEKF()
vel.z = statesAtFlowTime[6];
// constrain terrain height to be below the vehicle
flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng);
// estimate range to centre of image
range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z;
@ -2288,7 +2287,7 @@ void AttPosEKF::OpticalFlowEKF()
}
// constrain the states
flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng);
// correct the covariance matrix
for (uint8_t i = 0; i < 2 ; i++) {
@ -2304,8 +2303,8 @@ void AttPosEKF::OpticalFlowEKF()
}
// prevent the state variances from becoming negative and maintain symmetry
Popt[0][0] = std::max(nextPopt[0][0],0.0f);
Popt[1][1] = std::max(nextPopt[1][1],0.0f);
Popt[0][0] = fmax(nextPopt[0][0],0.0f);
Popt[1][1] = fmax(nextPopt[1][1],0.0f);
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
Popt[1][0] = Popt[0][1];
}