Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 22:32:11 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

move implementation of updateAidSrcStatus to cpp file, saves flash

Browse Source
This commit is contained in:
Marco Hauswirth
2024-09-23 11:42:37 +02:00
committed by Daniel Agar
parent b41811b145
commit e3f138862a
2 changed files with 79 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+1 -74
View File
@@ -1135,80 +1135,7 @@ private:
void updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t &timestamp_sample,
const float &observation, const float &observation_variance,
const float &innovation, const float &innovation_variance,
float innovation_gate = 1.f) const
{
bool innovation_rejected = false;
const float test_ratio = sq(innovation) / (sq(innovation_gate) * innovation_variance);
if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) {
const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f);
static constexpr float tau = 0.5f;
const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f);
// test_ratio_filtered
if (PX4_ISFINITE(status.test_ratio_filtered)) {
status.test_ratio_filtered += alpha * (matrix::sign(innovation) * test_ratio - status.test_ratio_filtered);
} else {
// otherwise, init the filtered test ratio
status.test_ratio_filtered = test_ratio;
}
// innovation_filtered
if (PX4_ISFINITE(status.innovation_filtered)) {
status.innovation_filtered += alpha * (innovation - status.innovation_filtered);
} else {
// otherwise, init the filtered innovation
status.innovation_filtered = innovation;
}
// limit extremes in filtered values
static constexpr float kNormalizedInnovationLimit = 2.f;
static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit);
if (test_ratio > kTestRatioLimit) {
status.test_ratio_filtered = math::constrain(status.test_ratio_filtered, -kTestRatioLimit, kTestRatioLimit);
const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance);
status.innovation_filtered = math::constrain(status.innovation_filtered, -innov_limit, innov_limit);
}
} else {
// invalid timestamp_sample, reset
status.test_ratio_filtered = test_ratio;
status.innovation_filtered = innovation;
}
status.test_ratio = test_ratio;
status.observation = observation;
status.observation_variance = observation_variance;
status.innovation = innovation;
status.innovation_variance = innovation_variance;
if ((test_ratio > 1.f)
|| !PX4_ISFINITE(test_ratio)
|| !PX4_ISFINITE(status.innovation)
|| !PX4_ISFINITE(status.innovation_variance)
) {
innovation_rejected = true;
}
status.timestamp_sample = timestamp_sample;
// if any of the innovations are rejected, then the overall innovation is rejected
status.innovation_rejected = innovation_rejected;
// reset
status.fused = false;
}
float innovation_gate = 1.f) const;
// state was reset to aid source, keep observation and update all other fields appropriately (zero innovation, etc)
template <typename T>
src/modules/ekf2/EKF/ekf_helper.cpp
+78
View File
@@ -1116,3 +1116,81 @@ bool Ekf::measurementUpdate(VectorState &K, const VectorState &H, const float R,
fuse(K, innovation);
return true;
}
void Ekf::updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t &timestamp_sample,
const float &observation, const float &observation_variance,
const float &innovation, const float &innovation_variance,
float innovation_gate) const
{
bool innovation_rejected = false;
const float test_ratio = sq(innovation) / (sq(innovation_gate) * innovation_variance);
if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) {
const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f);
static constexpr float tau = 0.5f;
const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f);
// test_ratio_filtered
if (PX4_ISFINITE(status.test_ratio_filtered)) {
status.test_ratio_filtered += alpha * (matrix::sign(innovation) * test_ratio - status.test_ratio_filtered);
} else {
// otherwise, init the filtered test ratio
status.test_ratio_filtered = test_ratio;
}
// innovation_filtered
if (PX4_ISFINITE(status.innovation_filtered)) {
status.innovation_filtered += alpha * (innovation - status.innovation_filtered);
} else {
// otherwise, init the filtered innovation
status.innovation_filtered = innovation;
}
// limit extremes in filtered values
static constexpr float kNormalizedInnovationLimit = 2.f;
static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit);
if (test_ratio > kTestRatioLimit) {
status.test_ratio_filtered = math::constrain(status.test_ratio_filtered, -kTestRatioLimit, kTestRatioLimit);
const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance);
status.innovation_filtered = math::constrain(status.innovation_filtered, -innov_limit, innov_limit);
}
} else {
// invalid timestamp_sample, reset
status.test_ratio_filtered = test_ratio;
status.innovation_filtered = innovation;
}
status.test_ratio = test_ratio;
status.observation = observation;
status.observation_variance = observation_variance;
status.innovation = innovation;
status.innovation_variance = innovation_variance;
if ((test_ratio > 1.f)
|| !PX4_ISFINITE(test_ratio)
|| !PX4_ISFINITE(status.innovation)
|| !PX4_ISFINITE(status.innovation_variance)
) {
innovation_rejected = true;
}
status.timestamp_sample = timestamp_sample;
// if any of the innovations are rejected, then the overall innovation is rejected
status.innovation_rejected = innovation_rejected;
// reset
status.fused = false;
}