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

reset state to unknown on init

Browse Source
This commit is contained in:
Marco Hauswirth 2024-12-30 10:18:24 +01:00
parent 670d19c382
commit bb17333bf4
4 changed files with 15 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -43,13 +43,13 @@ using namespace matrix;
void RangeFinderConsistencyCheck::init(const float &z, const float &z_var, const float &dist_bottom,
const float &dist_bottom_var)
{
float p[4] = {z_var, 0.f, 0.f, z_var + dist_bottom_var};
_P = Matrix<float, RangeFilter::size, RangeFilter::size>(p);
sym::RangeValidationFilter(&_H);
float p[4] = {z_var, z_var, z_var, z_var + dist_bottom_var};
_P = SquareMatrix<float, RangeFilter::size>(p);
_H = sym::RangeValidationFilter<float>();
_x(RangeFilter::z.idx) = z;
_x(RangeFilter::terrain.idx) = z - dist_bottom;
_initialized = true;
_state = _test_ratio_lpf.getState() < 1.f ? KinematicState::UNKNOWN : KinematicState::INCONSISTENT;
_state = KinematicState::UNKNOWN;
_test_ratio_lpf.reset(2.f);
_t_since_first_sample = 0.f;
_test_ratio_lpf.setAlpha(0.2f);

3
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
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);
if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion
&& _rng_consistency_check.isNotKinematicallyInconsistent()) {
if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) {
_rng_consistency_check.reset();
}

2
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
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_y_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)

18
src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter.h
View File

@ -18,10 +18,10 @@ namespace sym {
* Args:
*
* Outputs:
* H: Matrix22
* res: Matrix22
*/
template <typename Scalar>
void RangeValidationFilter(matrix::Matrix<Scalar, 2, 2>* const H = nullptr) {
matrix::Matrix<Scalar, 2, 2> RangeValidationFilter() {
// Total ops: 0
// Input arrays
@ -29,15 +29,15 @@ void RangeValidationFilter(matrix::Matrix<Scalar, 2, 2>* const H = nullptr) {
// Intermediate terms (0)
// Output terms (1)
if (H != nullptr) {
matrix::Matrix<Scalar, 2, 2>& _h = (*H);
matrix::Matrix<Scalar, 2, 2> _res;
_h.setZero();
_res.setZero();
_h(0, 0) = 1;
_h(1, 0) = 1;
_h(1, 1) = -1;
}
_res(0, 0) = 1;
_res(1, 0) = 1;
_res(1, 1) = -1;
return _res;
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)