From 15335b194a128938f80d96941f2c7a6992bf5884 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 17 Jan 2023 14:06:59 +0100 Subject: [PATCH] WindEstimator: use state indexing enum consistently Signed-off-by: Silvan Fuhrer --- src/lib/wind_estimator/WindEstimator.cpp | 20 ++++++++++---------- src/lib/wind_estimator/WindEstimator.hpp | 6 +++--- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index 7b3fd720f7..7143a551ba 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -95,9 +95,9 @@ WindEstimator::update(uint64_t time_now) _time_last_update = time_now; matrix::Matrix3f Qk; - Qk(0, 0) = _wind_psd * dt; - Qk(1, 1) = Qk(0, 0); - Qk(2, 2) = _tas_scale_psd * dt; + Qk(INDEX_W_N, INDEX_W_N) = _wind_psd * dt; + Qk(INDEX_W_E, INDEX_W_E) = Qk(INDEX_W_N, INDEX_W_N); + Qk(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = _tas_scale_psd * dt; _P += Qk; } @@ -137,9 +137,9 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const } // apply correction to state - _state(INDEX_W_N) += _tas_innov * K(0, 0); - _state(INDEX_W_E) += _tas_innov * K(1, 0); - _state(INDEX_TAS_SCALE) += _tas_innov * K(2, 0); + _state(INDEX_W_N) += _tas_innov * K(INDEX_W_N, 0); + _state(INDEX_W_E) += _tas_innov * K(INDEX_W_E, 0); + _state(INDEX_TAS_SCALE) += _tas_innov * K(INDEX_TAS_SCALE, 0); // update covariance matrix _P = _P - K * H_tas * _P; @@ -182,9 +182,9 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const } // apply correction to state - _state(INDEX_W_N) += _beta_innov * K(0, 0); - _state(INDEX_W_E) += _beta_innov * K(1, 0); - _state(INDEX_TAS_SCALE) += _beta_innov * K(2, 0); + _state(INDEX_W_N) += _beta_innov * K(INDEX_W_N, 0); + _state(INDEX_W_E) += _beta_innov * K(INDEX_W_E, 0); + _state(INDEX_TAS_SCALE) += _beta_innov * K(INDEX_TAS_SCALE, 0); // update covariance matrix _P = _P - K * H_beta * _P; diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp index 4c7f405552..462f88ff26 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -76,12 +76,12 @@ public: // invert scale (CAS = IAS * scale), protect agains division by 0, constrain to [0.1, 10] float get_tas_scale() { return 1.f / math::constrain(_state(INDEX_TAS_SCALE), 0.1f, 10.0f); } - float get_tas_scale_var() { return _P(2, 2); } + float get_tas_scale_var() { return _P(INDEX_TAS_SCALE, INDEX_TAS_SCALE); } float get_tas_innov() { return _tas_innov; } float get_tas_innov_var() { return _tas_innov_var; } float get_beta_innov() { return _beta_innov; } float get_beta_innov_var() { return _beta_innov_var; } - matrix::Vector2f get_wind_var() { return matrix::Vector2f{_P(0, 0), _P(1, 1)}; } + matrix::Vector2f get_wind_var() { return matrix::Vector2f{_P(INDEX_W_N, INDEX_W_N), _P(INDEX_W_E, INDEX_W_E)}; } bool get_wind_estimator_reset() { return _wind_estimator_reset; } // unaided, the state uncertainty (diagonal of sqrt(P)) grows by the process noise spectral density every second