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

reset state to unknown on init

Browse Source
This commit is contained in:
Marco Hauswirth
2025-08-26 15:08:09 +02:00
parent 670d19c382
commit bb17333bf4
4 changed files with 15 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
+4 -4
View File
@@ -43,13 +43,13 @@ using namespace matrix;
void RangeFinderConsistencyCheck::init(const float &z, const float &z_var, const float &dist_bottom, void RangeFinderConsistencyCheck::init(const float &z, const float &z_var, const float &dist_bottom,
const float &dist_bottom_var) const float &dist_bottom_var)
{ {
float p[4] = {z_var, 0.f, 0.f, z_var + dist_bottom_var}; float p[4] = {z_var, z_var, z_var, z_var + dist_bottom_var};
_P = Matrix<float, RangeFilter::size, RangeFilter::size>(p); _P = SquareMatrix<float, RangeFilter::size>(p);
sym::RangeValidationFilter(&_H); _H = sym::RangeValidationFilter<float>();
_x(RangeFilter::z.idx) = z; _x(RangeFilter::z.idx) = z;
_x(RangeFilter::terrain.idx) = z - dist_bottom; _x(RangeFilter::terrain.idx) = z - dist_bottom;
_initialized = true; _initialized = true;
_state = _test_ratio_lpf.getState() < 1.f ? KinematicState::UNKNOWN : KinematicState::INCONSISTENT; _state = KinematicState::UNKNOWN;
_test_ratio_lpf.reset(2.f); _test_ratio_lpf.reset(2.f);
_t_since_first_sample = 0.f; _t_since_first_sample = 0.f;
_test_ratio_lpf.setAlpha(0.2f); _test_ratio_lpf.setAlpha(0.2f);
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+1 -2
View File
@@ -72,8 +72,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f); const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f);
if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) {
&& _rng_consistency_check.isNotKinematicallyInconsistent()) {
_rng_consistency_check.reset(); _rng_consistency_check.reset();
} }
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
+1 -1
View File
@@ -766,6 +766,6 @@ generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_va
generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"]) generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"])
generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"]) generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"])
generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"]) generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"])
generate_px4_function(range_validation_filter, output_names=["H"]) generate_px4_function(range_validation_filter, output_names=None)
generate_px4_state(State, tangent_idx) generate_px4_state(State, tangent_idx)
src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h
+9 -9
View File
@@ -18,10 +18,10 @@ namespace sym {
* Args: * Args:
* *
* Outputs: * Outputs:
* H: Matrix22 * res: Matrix22
*/ */
template <typename Scalar> template <typename Scalar>
void RangeValidationFilter(matrix::Matrix<Scalar, 2, 2>* const H = nullptr) { matrix::Matrix<Scalar, 2, 2> RangeValidationFilter() {
// Total ops: 0 // Total ops: 0
// Input arrays // Input arrays
@@ -29,15 +29,15 @@ void RangeValidationFilter(matrix::Matrix<Scalar, 2, 2>* const H = nullptr) {
// Intermediate terms (0) // Intermediate terms (0)
// Output terms (1) // Output terms (1)
if (H != nullptr) { matrix::Matrix<Scalar, 2, 2> _res;
matrix::Matrix<Scalar, 2, 2>& _h = (*H);
_h.setZero(); _res.setZero();
_h(0, 0) = 1; _res(0, 0) = 1;
_h(1, 0) = 1; _res(1, 0) = 1;
_h(1, 1) = -1; _res(1, 1) = -1;
}
return _res;
} // NOLINT(readability/fn_size) } // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size) // NOLINTNEXTLINE(readability/fn_size)