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

clean up

Browse Source
This commit is contained in:
Jacob Dahl 2025-09-22 17:23:43 -08:00
parent 1e12fe13d4
commit ca3ad3d63d
4 changed files with 15 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/CMakeLists.txt
View File

@ -241,7 +241,7 @@ px4_add_module(
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
-fno-associative-math
-DDEBUG_BUILD
#-DDEBUG_BUILD
#-O0
INCLUDES
EKF

19
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
View File

@ -46,13 +46,12 @@ void RangeFinderConsistencyCheck::init(const float z, const float z_var, const f
{
printf("RangeFinderConsistencyCheck::init\n");
_P = sym::RangeValidationFilterPInit(z_var, dist_bottom_var);
_Ht = sym::RangeValidationFilterH<float>();
_p = sym::RangeValidationFilterPInit(z_var, dist_bottom_var);
_ht = sym::RangeValidationFilterH<float>();
_x(RangeFilter::z.idx) = z;
_x(RangeFilter::terrain.idx) = z - dist_bottom;
_initialized = true;
// _test_ratio_lpf.reset(0.f);
_test_ratio = 0.f;
_t_since_first_sample = 0.f;
}
@ -75,8 +74,8 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
// prediction step
_time_last_update_us = time_us;
_x(RangeFilter::z.idx) -= dt * vz;
_P(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var;
_P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += _terrain_process_noise;
_p(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var;
_p(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += _terrain_process_noise;
// iterate through both measurements (z and dist_bottom)
const Vector2f measurements{z, dist_bottom};
@ -84,7 +83,7 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) {
// dist_bottom
Vector2f H = _Ht;
Vector2f H = _ht;
float R = dist_bottom_var;
// z, direct state measurement
@ -101,8 +100,8 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
// for H as col-vector:
// innovation variance S = H^T * P * H + R
// kalman gain K = P * H / S
const float S = (H.transpose() * _P * H + R)(0, 0);
Vector2f K = (_P * H / S);
const float S = (H.transpose() * _p * H + R)(0, 0);
Vector2f K = (_p * H / S);
if (measurement_idx == 0) {
K(RangeFilter::z.idx) = 1.f;
@ -110,8 +109,6 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
} else if (measurement_idx == 1) {
_innov = y;
const float test_ratio = fminf(sq(y) / (sq(_gate) * S), 4.f); // limit to 4 to limit sensitivity to outliers
// _test_ratio_lpf.setParameters(dt, _t_to_init);
// _test_ratio_lpf.update(sign(_innov) * test_ratio);
_test_ratio = sign(_innov) * test_ratio;
}
@ -124,7 +121,7 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
Matrix2f I;
I.setIdentity();
Matrix2f IKH = I - K.multiplyByTranspose(H);
_P = IKH * _P * IKH.transpose() + (K * R).multiplyByTranspose(K);
_p = IKH * _p * IKH.transpose() + (K * R).multiplyByTranspose(K);
}
evaluateState(dt, vz, vz_var);

10
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
View File

@ -40,7 +40,9 @@
#ifndef EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP
#define EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP
#include <mathlib/math/filter/AlphaFilter.hpp>
#include <mathlib/math/Functions.hpp>
using namespace math;
class RangeFinderConsistencyCheck final
{
@ -54,7 +56,6 @@ public:
UNKNOWN = 2
};
// float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; }
float getTestRatioLpf() const { return _test_ratio; }
float getInnov() const { return _initialized ? _innov : 0.f; }
@ -75,15 +76,14 @@ private:
void init(float z, float z_var, float dist_bottom, float dist_bottom_var);
void evaluateState(float dt, float vz, float vz_var);
float _terrain_process_noise{0.0f};
matrix::SquareMatrix<float, 2> _P{};
matrix::Vector2f _Ht{};
matrix::SquareMatrix<float, 2> _p{};
matrix::Vector2f _ht{};
matrix::Vector2f _x{};
bool _initialized{false};
float _innov{0.f};
float _innov_var{0.f};
uint64_t _time_last_update_us{0};
static constexpr float time_constant{1.f};
// AlphaFilter<float> _test_ratio_lpf{time_constant};
float _test_ratio{0.f};

4
src/modules/ekf2/EKF/covariance.cpp
View File

@ -105,8 +105,7 @@ void Ekf::initialiseCovariance()
#endif // CONFIG_EKF2_WIND
#if defined(CONFIG_EKF2_TERRAIN)
// use the ground clearance value as our uncertainty
// TODO: ^ why? wouldn't the noise value make more sense?
// use the ground clearance value as our uncertainty. TODO: wouldn't it make more sense to use the noise value instead?
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.ekf2_rng_noise));
#endif // CONFIG_EKF2_TERRAIN
}
@ -230,7 +229,6 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
#if defined(CONFIG_EKF2_RANGE_FINDER)
// TODO: explain
_rng_consistency_check.set_terrain_process_noise(terrain_process_noise);
#endif // CONFIG_EKF2_RANGE_FINDER
}