fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 14:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

WindEstimator: use state indexing enum consistently

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-01-17 14:06:59 +01:00
committed by Daniel Agar
parent 21ddb04856
commit 15335b194a
2 changed files with 13 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/wind_estimator/WindEstimator.cpp
+10 -10
View File
@@ -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;
src/lib/wind_estimator/WindEstimator.hpp
+3 -3
View File
@@ -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