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

AttPosEKF: Replace custom min/max functions with c++ standard

Browse Source
This commit is contained in:
Johan Jansen
2015-02-05 13:18:32 +01:00
parent 022208e998
commit 7287dc3c4c
2 changed files with 12 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+11 -33
View File
@@ -3,6 +3,7 @@
#include <stdio.h>
#include <stdarg.h>
#include <math.h>
#include <algorithm>
#ifndef M_PI_F
#define M_PI_F ((float)M_PI)
@@ -1805,7 +1806,7 @@ void AttPosEKF::FuseOptFlow()
Vector3f relVelSensor;
// Perform sequential fusion of optical flow measurements only with valid tilt and height
flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng);
flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9];
bool validTilt = Tnb.z.z > 0.71f;
if (validTilt)
@@ -2070,7 +2071,7 @@ void AttPosEKF::OpticalFlowEKF()
} else {
return;
}
distanceTravelledSq = min(distanceTravelledSq, 100.0f);
distanceTravelledSq = std::min(distanceTravelledSq, 100.0f);
Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
}
@@ -2110,7 +2111,7 @@ void AttPosEKF::OpticalFlowEKF()
varInnovRng = 1.0f/SK_RNG[1];
// constrain terrain height to be below the vehicle
flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
// estimate range to centre of image
range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2];
@@ -2130,7 +2131,7 @@ void AttPosEKF::OpticalFlowEKF()
}
// constrain the states
flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
// correct the covariance matrix
float nextPopt[2][2];
@@ -2139,8 +2140,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] = maxf(nextPopt[0][0],0.0f);
Popt[1][1] = maxf(nextPopt[1][1],0.0f);
Popt[0][0] = std::max(nextPopt[0][0],0.0f);
Popt[1][1] = std::max(nextPopt[1][1],0.0f);
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
Popt[1][0] = Popt[0][1];
}
@@ -2179,7 +2180,7 @@ void AttPosEKF::OpticalFlowEKF()
vel.z = statesAtFlowTime[6];
// constrain terrain height to be below the vehicle
flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng);
flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
// estimate range to centre of image
range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z;
@@ -2247,7 +2248,7 @@ void AttPosEKF::OpticalFlowEKF()
}
// constrain the states
flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng);
flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
// correct the covariance matrix
for (uint8_t i = 0; i < 2 ; i++) {
@@ -2263,8 +2264,8 @@ void AttPosEKF::OpticalFlowEKF()
}
// prevent the state variances from becoming negative and maintain symmetry
Popt[0][0] = maxf(nextPopt[0][0],0.0f);
Popt[1][1] = maxf(nextPopt[1][1],0.0f);
Popt[0][0] = std::max(nextPopt[0][0],0.0f);
Popt[1][1] = std::max(nextPopt[1][1],0.0f);
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
Popt[1][0] = Popt[0][1];
}
@@ -2286,29 +2287,6 @@ void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATE
}
}
float AttPosEKF::sq(float valIn)
{
return valIn*valIn;
}
float AttPosEKF::maxf(float valIn1, float valIn2)
{
if (valIn1 >= valIn2) {
return valIn1;
} else {
return valIn2;
}
}
float AttPosEKF::min(float valIn1, float valIn2)
{
if (valIn1 <= valIn2) {
return valIn1;
} else {
return valIn2;
}
}
// Store states in a history array along with time stamp
void AttPosEKF::StoreStates(uint64_t timestamp_ms)
{
src/modules/ekf_att_pos_estimator/estimator_22states.h
+1 -5
View File
@@ -274,11 +274,7 @@ public:
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static float sq(float valIn);
static float maxf(float valIn1, float valIn2);
static float min(float valIn1, float valIn2);
static inline float sq(float valIn) {return valIn * valIn;}
void OnGroundCheck();