diff --git a/CMakeLists.txt b/CMakeLists.txt index bf7a08c560..93a686c2ba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_module( STACK_MAX 6000 COMPILE_FLAGS SRCS + airdata/WindEstimator.cpp attitude_fw/ecl_controller.cpp attitude_fw/ecl_pitch_controller.cpp attitude_fw/ecl_roll_controller.cpp diff --git a/airdata/WindEstimator.cpp b/airdata/WindEstimator.cpp new file mode 100644 index 0000000000..25736d761d --- /dev/null +++ b/airdata/WindEstimator.cpp @@ -0,0 +1,356 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file WindEstimator.cpp + * A wind and airspeed scale estimator. + */ + +#include "WindEstimator.hpp" + + +bool +WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas) +{ + // do no initialise if ground velocity is low + // this should prevent the filter from initialising on the ground + if (sqrtf(velI(0) * velI(0) + velI(1) * velI(1)) < 3.0f) { + return false; + } + + const float v_n = velI(0); + const float v_e = velI(1); + + // estimate heading from ground velocity + const float heading_est = atan2f(v_e, v_n); + + // initilaise wind states assuming zero side slip and horizontal flight + _state(w_n) = velI(w_n) - tas_meas * cosf(heading_est); + _state(w_e) = velI(w_e) - tas_meas * sinf(heading_est); + _state(tas) = 1.0f; + + // compute jacobian of states wrt north/each earth velocity states and true airspeed measurement + float L0 = v_e * v_e; + float L1 = v_n * v_n; + float L2 = L0 + L1; + float L3 = tas_meas / (L2 * sqrtf(L2)); + float L4 = L3 * v_e * v_n + 1.0f; + float L5 = 1.0f / sqrtf(L2); + float L6 = -L5 * tas_meas; + + matrix::Matrix3f L; + L.setZero(); + L(0, 0) = L4; + L(0, 1) = L0 * L3 + L6; + L(1, 0) = L1 * L3 + L6; + L(1, 1) = L4; + L(2, 2) = 1.0f; + + // get an estimate of the state covariance matrix given the estimated variance of ground velocity + // and measured airspeed + _P.setZero(); + _P(w_n, w_n) = velIvar(0); + _P(w_e, w_e) = velIvar(1); + _P(tas, tas) = 0.0001f; + + _P = L * _P * L.transpose(); + + // reset the timestamp for measurement rejection + _time_rejected_tas = 0; + _time_rejected_beta = 0; + + return true; +} + +void +WindEstimator::update(uint64_t time_now) +{ + if (!_initialised) { + return; + } + + // run covariance prediction at 1Hz + if (time_now - _time_last_update < 1000 * 1000 || _time_last_update == 0) { + if (_time_last_update == 0) { + _time_last_update = time_now; + } + + return; + } + + float dt = (float)(time_now - _time_last_update) * 1e-6f; + _time_last_update = time_now; + + float q_w = _wind_p_var; + float q_k_tas = _tas_scale_p_var; + + float SPP0 = dt * dt; + float SPP1 = SPP0 * q_w; + float SPP2 = SPP1 + _P(0, 1); + + matrix::Matrix3f P_next; + + P_next(0, 0) = SPP1 + _P(0, 0); + P_next(0, 1) = SPP2; + P_next(0, 2) = _P(0, 2); + P_next(1, 0) = SPP2; + P_next(1, 1) = SPP1 + _P(1, 1); + P_next(1, 2) = _P(1, 2); + P_next(2, 0) = _P(0, 2); + P_next(2, 1) = _P(1, 2); + P_next(2, 2) = SPP0 * q_k_tas + _P(2, 2); + _P = P_next; +} + +void +WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const matrix::Vector3f &velI, + const matrix::Vector2f &velIvar) +{ + matrix::Vector2f velIvar_constrained = { math::max(0.01f, velIvar(0)), math::max(0.01f, velIvar(1)) }; + + if (!_initialised) { + // try to initialise + _initialised = initialise(velI, velIvar_constrained, true_airspeed); + return; + } + + // don't fuse faster than 10Hz + if (time_now - _time_last_airspeed_fuse < 100 * 1000) { + return; + } + + _time_last_airspeed_fuse = time_now; + + // assign helper variables + const float v_n = velI(0); + const float v_e = velI(1); + const float v_d = velI(2); + + const float k_tas = _state(tas); + + // compute kalman gain K + const float HH0 = sqrtf(v_d * v_d + (v_e - w_e) * (v_e - w_e) + (v_n - w_n) * (v_n - w_n)); + const float HH1 = k_tas / HH0; + + matrix::Matrix H_tas; + H_tas(0, 0) = HH1 * (-v_n + w_n); + H_tas(0, 1) = HH1 * (-v_e + w_e); + H_tas(0, 2) = HH0; + + matrix::Matrix K = _P * H_tas.transpose(); + + const matrix::Matrix S = H_tas * _P * H_tas.transpose() + _tas_var; + + K /= (S._data[0][0]); + // compute innovation + const float airspeed_pred = _state(tas) * sqrtf((v_n - _state(w_n)) * (v_n - _state(w_n)) + (v_e - _state(w_e)) * + (v_e - _state(w_e)) + v_d * v_d); + + _tas_innov = true_airspeed - airspeed_pred; + + // innovation variance + _tas_innov_var = S._data[0][0]; + + bool reinit_filter = false; + bool meas_is_rejected = false; + + meas_is_rejected = check_if_meas_is_rejected(time_now, _tas_innov, _tas_innov_var, _tas_gate, _time_rejected_tas, reinit_filter); + + reinit_filter |= _tas_innov_var < 0.0f; + + if (meas_is_rejected || reinit_filter) { + if (reinit_filter) { + _initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), true_airspeed); + } + + // we either did a filter reset or the current measurement was rejected so do not fuse + return; + } + + // apply correction to state + _state(w_n) += _tas_innov * K(0, 0); + _state(w_e) += _tas_innov * K(1, 0); + _state(tas) += _tas_innov * K(2, 0); + + // update covariance matrix + _P = _P - K * H_tas * _P; + + run_sanity_checks(); +} + +void +WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att) +{ + if (!_initialised) { + _initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length()); + return; + } + + // don't fuse faster than 10Hz + if (time_now - _time_last_beta_fuse < 100 * 1000) { + return; + } + + _time_last_beta_fuse = time_now; + + const float v_n = velI(0); + const float v_e = velI(1); + const float v_d = velI(2); + + // compute sideslip observation vector + float HB0 = 2.0f * q_att(0); + float HB1 = HB0 * q_att(3); + float HB2 = 2.0f * q_att(1); + float HB3 = HB2 * q_att(2); + float HB4 = v_e - w_e; + float HB5 = HB1 + HB3; + float HB6 = v_n - w_n; + float HB7 = q_att(0) * q_att(0); + float HB8 = q_att(3) * q_att(3); + float HB9 = HB7 - HB8; + float HB10 = q_att(1) * q_att(1); + float HB11 = q_att(2) * q_att(2); + float HB12 = HB10 - HB11; + float HB13 = HB12 + HB9; + float HB14 = HB13 * HB6 + HB4 * HB5 + v_d * (-HB0 * q_att(2) + HB2 * q_att(3)); + float HB15 = 1.0f / HB14; + float HB16 = (HB4 * (-HB10 + HB11 + HB9) + HB6 * (-HB1 + HB3) + v_d * (HB0 * q_att(1) + 2.0f * q_att(2) * q_att(3))) / + (HB14 * HB14); + + matrix::Matrix H_beta; + H_beta(0, 0) = HB13 * HB16 + HB15 * (HB1 - HB3); + H_beta(0, 1) = HB15 * (HB12 - HB7 + HB8) + HB16 * HB5; + H_beta(0, 2) = 0; + + // compute kalman gain + matrix::Matrix K = _P * H_beta.transpose(); + + const matrix::Matrix S = H_beta * _P * H_beta.transpose() + _beta_var; + + K /= (S._data[0][0]); + + // compute predicted side slip angle + matrix::Vector3f rel_wind(velI(0) - _state(w_n), velI(1) - _state(w_e), velI(2)); + matrix::Dcmf R_body_to_earth(q_att); + rel_wind = R_body_to_earth.transpose() * rel_wind; + + if (fabsf(rel_wind(0)) < 0.1f) { + return; + } + + // use small angle approximation, sin(x) = x for small x + const float beta_pred = rel_wind(1) / rel_wind(0); + + _beta_innov = 0.0f - beta_pred; + _beta_innov_var = S._data[0][0]; + + bool reinit_filter = false; + bool meas_is_rejected = false; + + meas_is_rejected = check_if_meas_is_rejected(time_now, _beta_innov, _beta_innov_var, _beta_gate, _time_rejected_beta, + reinit_filter); + + reinit_filter |= _beta_innov_var < 0.0f; + + if (meas_is_rejected || reinit_filter) { + if (reinit_filter) { + _initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length()); + } + + // we either did a filter reset or the current measurement was rejected so do not fuse + return; + } + + // apply correction to state + _state(w_n) += _beta_innov * K(0, 0); + _state(w_e) += _beta_innov * K(1, 0); + _state(tas) += _beta_innov * K(2, 0); + + // update covariance matrix + _P = _P - K * H_beta * _P; + + run_sanity_checks(); +} + +void +WindEstimator::run_sanity_checks() +{ + for (unsigned i = 0; i < 3; i++) { + if (_P(i, i) < 0.0f) { + // ill-conditioned covariance matrix, reset filter + _initialised = false; + return; + } + + // limit covariance diagonals if they grow too large + if (i < 2) { + _P(i, i) = _P(i, i) > 25.0f ? 25.0f : _P(i, i); + + } else if (i == 2) { + _P(i, i) = _P(i, i) > 0.1f ? 0.1f : _P(i, i); + } + } + + if (!PX4_ISFINITE(_state(w_n)) || !PX4_ISFINITE(_state(w_e)) || !PX4_ISFINITE(_state(tas))) { + _initialised = false; + return; + } + + // constrain airspeed scale factor, negative values physically do not make sense + _state(tas) = math::max(0.0f, _state(tas)); + + // attain symmetry + for (unsigned row = 0; row < 3; row++) { + for (unsigned column = 0; column < row; column++) { + float tmp = (_P(row, column) + _P(column, row)) / 2; + _P(row, column) = tmp; + _P(column, row) = tmp; + } + } +} + +bool +WindEstimator::check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, uint64_t &time_meas_rejected, + bool &reinit_filter) +{ + if (innov * innov > gate_size * gate_size * innov_var) { + time_meas_rejected = time_meas_rejected == 0 ? time_now : time_meas_rejected; + + } else { + time_meas_rejected = 0; + } + + reinit_filter = time_now - time_meas_rejected > 5 * 1000 * 1000 && time_meas_rejected != 0; + + return time_meas_rejected != 0; +} \ No newline at end of file diff --git a/airdata/WindEstimator.hpp b/airdata/WindEstimator.hpp new file mode 100644 index 0000000000..b5292ef87c --- /dev/null +++ b/airdata/WindEstimator.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file WindEstimator.hpp + * A wind and airspeed scale estimator. + */ + +#pragma once + +#include + +class WindEstimator +{ +public: + WindEstimator() = default; + ~WindEstimator() = default; + + // no copy, assignment, move, move assignment + WindEstimator(const WindEstimator &) = delete; + WindEstimator &operator=(const WindEstimator &) = delete; + WindEstimator(WindEstimator &&) = delete; + WindEstimator &operator=(WindEstimator &&) = delete; + + void update(uint64_t time_now); + + void fuse_airspeed(uint64_t time_now, float true_airspeed, const matrix::Vector3f &velI, + const matrix::Vector2f &velIvar); + void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att); + + void get_wind(float wind[2]) + { + wind[0] = _state(w_n); + wind[1] = _state(w_e); + } + + bool is_estimate_valid() { return _initialised; } + + bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, uint64_t &time_meas_rejected, + bool &reinit_filter); + + float get_tas_scale() { return _state(tas); } + 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; } + void get_wind_var(float wind_var[2]) + { + wind_var[0] = _P(0, 0); + wind_var[1] = _P(1, 1); + } + + void set_wind_p_noise(float wind_sigma) { _wind_p_var = wind_sigma * wind_sigma; } + void set_tas_scale_p_noise(float tas_scale_sigma) { _tas_scale_p_var = tas_scale_sigma * tas_scale_sigma; } + void set_tas_noise(float tas_sigma) { _tas_var = tas_sigma * tas_sigma; } + void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; } + void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; } + void set_beta_gate(uint8_t gate_size) {_beta_gate = gate_size; } + +private: + enum { + w_n = 0, + w_e, + tas + }; ///< enum which can be used to access state. + + matrix::Vector3f _state; ///< state vector + matrix::Matrix3f _P; ///< state covariance matrix + + float _tas_innov{0.0f}; ///< true airspeed innovation + float _tas_innov_var{0.0f}; ///< true airspeed innovation variance + + float _beta_innov{0.0f}; ///< sideslip innovation + float _beta_innov_var{0.0f}; ///< sideslip innovation variance + + bool _initialised{false}; ///< True: filter has been initialised + + float _wind_p_var{0.1f}; ///< wind process noise variance + float _tas_scale_p_var{0.0001f}; ///< true airspeed scale process noise variance + float _tas_var{1.4f}; ///< true airspeed measurement noise variance + float _beta_var{0.5f}; ///< sideslip measurement noise variance + uint8_t _tas_gate{3}; ///< airspeed fusion gate size + uint8_t _beta_gate{1}; ///< sideslip fusion gate size + + uint64_t _time_last_airspeed_fuse = 0; ///< timestamp of last airspeed fusion + uint64_t _time_last_beta_fuse = 0; ///< timestamp of last sideslip fusion + uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction + uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected + uint64_t _time_rejected_tas = + 0; ///