From f65653a3911c0a87db68da1e41b60163815d96c2 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Thu, 27 Jun 2024 09:41:56 +0200 Subject: [PATCH] battery: add internal resistance estimation --- .../init.d/airframes/4601_droneblocks_dexi_5 | 1 - msg/BatteryStatus.msg | 8 ++ src/lib/battery/battery.cpp | 111 +++++++++++++----- src/lib/battery/battery.h | 24 ++-- src/lib/battery/module.yaml | 26 +--- src/lib/matrix/matrix/SquareMatrix.hpp | 1 + src/modules/ekf2/EKF/ekf.h | 1 - 7 files changed, 109 insertions(+), 63 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/4601_droneblocks_dexi_5 b/ROMFS/px4fmu_common/init.d/airframes/4601_droneblocks_dexi_5 index c1484d0b417c..d7f985d1d03e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4601_droneblocks_dexi_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4601_droneblocks_dexi_5 @@ -25,7 +25,6 @@ param set-default BAT1_CAPACITY 4000 param set-default BAT1_N_CELLS 6 param set-default BAT1_V_EMPTY 3.3 -param set-default BAT1_V_LOAD_DROP 0.5 param set-default BAT_AVRG_CURRENT 13 # Square quadrotor X PX4 numbering diff --git a/msg/BatteryStatus.msg b/msg/BatteryStatus.msg index 66fcffa0e68f..b13f721a5aa1 100644 --- a/msg/BatteryStatus.msg +++ b/msg/BatteryStatus.msg @@ -76,3 +76,11 @@ float32 design_capacity # The design capacity of the battery uint16 average_time_to_full # The predicted remaining time until the battery reaches full charge, in minutes uint16 over_discharge_count # Number of battery overdischarge float32 nominal_voltage # Nominal voltage of the battery pack + +float32 internal_resistance_estimate # [Ohm] Internal resistance per cell estimate +float32 ocv_estimate # [V] Open circuit voltage estimate +float32 ocv_estimate_filtered # [V] Filtered open circuit voltage estimate +float32 volt_based_soc_estimate # [0, 1] Normalized volt based state of charge estimate +float32 voltage_prediction # [V] Predicted voltage +float32 prediction_error # [V] Prediction error +float32 estimation_covariance_norm # Norm of the covariance matrix diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 21c4b906f367..213d8fc017e1 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -46,6 +46,7 @@ #include using namespace time_literals; +using namespace matrix; Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source) : ModuleParams(parent), @@ -53,10 +54,9 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, _source(source) { const float expected_filter_dt = static_cast(sample_interval_us) / 1_s; - _voltage_filter_v.setParameters(expected_filter_dt, 1.f); - _current_filter_a.setParameters(expected_filter_dt, .5f); _current_average_filter_a.setParameters(expected_filter_dt, 50.f); - _throttle_filter.setParameters(expected_filter_dt, 1.f); + _ocv_filter_v.setParameters(expected_filter_dt, 1.f); + _cell_voltage_filter_v.setParameters(expected_filter_dt, 1.f); if (index > 9 || index < 1) { PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index); @@ -81,9 +81,6 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, snprintf(param_name, sizeof(param_name), "BAT%d_CAPACITY", _index); _param_handles.capacity = param_find(param_name); - snprintf(param_name, sizeof(param_name), "BAT%d_V_LOAD_DROP", _index); - _param_handles.v_load_drop = param_find(param_name); - snprintf(param_name, sizeof(param_name), "BAT%d_R_INTERNAL", _index); _param_handles.r_internal = param_find(param_name); @@ -97,29 +94,36 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, _param_handles.bat_avrg_current = param_find("BAT_AVRG_CURRENT"); updateParams(); + + // Internal resistance estimation initializations + _RLS_est(0) = OCV_DEFAULT * _params.n_cells; + _RLS_est(1) = R_DEFAULT * _params.n_cells; + _estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells; + _estimation_covariance(0, 1) = 0.f; + _estimation_covariance(1, 0) = 0.f; + _estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells; + _estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0), + 2.f) + powf(_estimation_covariance(1, 1), 2.f)); } void Battery::updateVoltage(const float voltage_v) { _voltage_v = voltage_v; - _voltage_filter_v.update(voltage_v); } void Battery::updateCurrent(const float current_a) { _current_a = current_a; - _current_filter_a.update(current_a); } void Battery::updateBatteryStatus(const hrt_abstime ×tamp) { if (!_battery_initialized) { - _voltage_filter_v.reset(_voltage_v); - _current_filter_a.reset(_current_a); + resetInternalResistanceEstimation(_voltage_v, _current_a); } // Require minimum voltage otherwise override connected status - if (_voltage_filter_v.getState() < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) { + if (_voltage_v < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) { _connected = false; } @@ -132,7 +136,7 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp) sumDischarged(timestamp, _current_a); _state_of_charge_volt_based = - calculateStateOfChargeVoltageBased(_voltage_filter_v.getState(), _current_filter_a.getState()); + calculateStateOfChargeVoltageBased(_voltage_v, _current_a); if (!_external_state_of_charge) { estimateStateOfCharge(); @@ -149,9 +153,7 @@ battery_status_s Battery::getBatteryStatus() { battery_status_s battery_status{}; battery_status.voltage_v = _voltage_v; - battery_status.voltage_filtered_v = _voltage_filter_v.getState(); battery_status.current_a = _current_a; - battery_status.current_filtered_a = _current_filter_a.getState(); battery_status.current_average_a = _current_average_filter_a.getState(); battery_status.discharged_mah = _discharged_mah; battery_status.remaining = _state_of_charge; @@ -167,6 +169,14 @@ battery_status_s Battery::getBatteryStatus() battery_status.warning = _warning; battery_status.timestamp = hrt_absolute_time(); battery_status.faults = determineFaults(); + battery_status.internal_resistance_estimate = _internal_resistance_estimate; + battery_status.ocv_estimate = _voltage_v + _internal_resistance_estimate * _params.n_cells * _current_a; + battery_status.ocv_estimate_filtered = _ocv_filter_v.getState(); + battery_status.volt_based_soc_estimate = math::interpolate(_ocv_filter_v.getState() / _params.n_cells, + _params.v_empty, _params.v_charged, 0.f, 1.f); + battery_status.voltage_prediction = _voltage_prediction; + battery_status.prediction_error = _prediction_error; + battery_status.estimation_covariance_norm = _estimation_covariance_norm; return battery_status; } @@ -213,27 +223,69 @@ float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const f // remaining battery capacity based on voltage float cell_voltage = voltage_v / _params.n_cells; - // correct battery voltage locally for load drop to avoid estimation fluctuations - if (_params.r_internal >= 0.f && current_a > FLT_EPSILON) { - cell_voltage += _params.r_internal * current_a; - - } else { - vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; - _vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint); - const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz); - const float throttle = thrust_setpoint.length(); + // correct battery voltage locally for load drop according to internal resistance and current + if (current_a > FLT_EPSILON) { + updateInternalResistanceEstimation(voltage_v, current_a); - _throttle_filter.update(throttle); + if (_params.r_internal >= 0.f) { // Use user specified internal resistance value + cell_voltage += _params.r_internal * current_a; - if (!_battery_initialized) { - _throttle_filter.reset(throttle); + } else { // Use estimated internal resistance value + cell_voltage += _internal_resistance_estimate * current_a; } - // assume linear relation between throttle and voltage drop - cell_voltage += throttle * _params.v_load_drop; } - return math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f); + _cell_voltage_filter_v.update(cell_voltage); + return math::interpolate(_cell_voltage_filter_v.getState(), _params.v_empty, _params.v_charged, 0.f, 1.f); +} + +void Battery::updateInternalResistanceEstimation(const float voltage_v, const float current_a) +{ + Vector2f x{1, -current_a}; + _voltage_prediction = (x.transpose() * _RLS_est)(0, 0); + _prediction_error = voltage_v - _voltage_prediction; + const Vector2f gamma = _estimation_covariance * x / (LAMBDA + (x.transpose() * _estimation_covariance * x)(0, 0)); + const Vector2f RSL_est_temp = _RLS_est + gamma * _prediction_error; + const Matrix2f estimation_covariance_temp = (_estimation_covariance + - Matrix(gamma) * (x.transpose() * _estimation_covariance)) / LAMBDA; + const float estimation_covariance_temp_norm = + sqrtf(powf(estimation_covariance_temp(0, 0), 2.f) + + 2.f * powf(estimation_covariance_temp(1, 0), 2.f) + + powf(estimation_covariance_temp(1, 1), 2.f)); + + if (estimation_covariance_temp_norm < _estimation_covariance_norm) { // Only update if estimation improves + _RLS_est = RSL_est_temp; + _estimation_covariance = estimation_covariance_temp; + _estimation_covariance_norm = estimation_covariance_temp_norm; + _internal_resistance_estimate = + math::max(_RLS_est(1) / _params.n_cells, 0.f); // Only use positive values + + } else { // Update OCV estimate with IR estimate + _RLS_est(0) = voltage_v + _RLS_est(1) * current_a; + } + + _ocv_filter_v.update(voltage_v + _internal_resistance_estimate * _params.n_cells * current_a); +} + +void Battery::resetInternalResistanceEstimation(const float voltage_v, const float current_a) +{ + _RLS_est(0) = voltage_v; + _RLS_est(1) = R_DEFAULT * _params.n_cells; + _estimation_covariance.setZero(); + _estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells; + _estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells; + _estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0), + 2.f) + powf(_estimation_covariance(1, 1), 2.f)); + _internal_resistance_estimate = R_DEFAULT; + _ocv_filter_v.reset(voltage_v + _internal_resistance_estimate * _params.n_cells * current_a); + + if (_params.r_internal >= 0.f) { // Use user specified internal resistance value + _cell_voltage_filter_v.reset(voltage_v / _params.n_cells + _params.r_internal * current_a); + + } else { // Use estimated internal resistance value + _cell_voltage_filter_v.reset(voltage_v / _params.n_cells + _internal_resistance_estimate * current_a); + } } void Battery::estimateStateOfCharge() @@ -354,7 +406,6 @@ void Battery::updateParams() param_get(_param_handles.v_charged, &_params.v_charged); param_get(_param_handles.n_cells, &_params.n_cells); param_get(_param_handles.capacity, &_params.capacity); - param_get(_param_handles.v_load_drop, &_params.v_load_drop); param_get(_param_handles.r_internal, &_params.r_internal); param_get(_param_handles.source, &_params.source); param_get(_param_handles.low_thr, &_params.low_thr); diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 91edbe058053..9d24b7bd4448 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -58,7 +58,6 @@ #include #include #include -#include /** * BatteryBase is a base class for any type of battery. @@ -118,7 +117,6 @@ class Battery : public ModuleParams param_t v_charged; param_t n_cells; param_t capacity; - param_t v_load_drop; param_t r_internal; param_t low_thr; param_t crit_thr; @@ -132,7 +130,6 @@ class Battery : public ModuleParams float v_charged; int32_t n_cells; float capacity; - float v_load_drop; float r_internal; float low_thr; float crit_thr; @@ -155,7 +152,6 @@ class Battery : public ModuleParams void computeScale(); float computeRemainingTime(float current_a); - uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::SubscriptionData _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)}; uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; @@ -167,12 +163,11 @@ class Battery : public ModuleParams uint8_t _priority{0}; bool _battery_initialized{false}; float _voltage_v{0.f}; - AlphaFilter _voltage_filter_v; + AlphaFilter _ocv_filter_v; + AlphaFilter _cell_voltage_filter_v; float _current_a{-1}; - AlphaFilter _current_filter_a; AlphaFilter _current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight. - AlphaFilter _throttle_filter; float _discharged_mah{0.f}; float _discharged_mah_loop{0.f}; float _state_of_charge_volt_based{-1.f}; // [0,1] @@ -183,4 +178,19 @@ class Battery : public ModuleParams bool _armed{false}; bool _vehicle_status_is_fw{false}; hrt_abstime _last_unconnected_timestamp{0}; + + // Internal Resistance estimation + void updateInternalResistanceEstimation(const float voltage_v, const float current_a); + void resetInternalResistanceEstimation(const float voltage_v, const float current_a); + matrix::Vector2f _RLS_est; // [Open circuit voltage estimate [V], Total internal resistance estimate [Ohm]]^T + matrix::Matrix2f _estimation_covariance; + float _estimation_covariance_norm{0.f}; + float _internal_resistance_estimate{0.005f}; // [Ohm] Per cell estimate of the internal resistance + float _voltage_prediction{0.f}; // [V] Predicted voltage of the estimator + float _prediction_error{0.f}; // [V] Error between the predicted and measured voltage + static constexpr float LAMBDA = 0.95f; // [0, 1] Forgetting factor (Tuning parameter for the RLS algorithm) + static constexpr float R_DEFAULT = 0.005f; // [Ohm] Initial per cell estimate of the internal resistance + static constexpr float OCV_DEFAULT = 4.2f; // [V] Initial per cell estimate of the open circuit voltage + static constexpr float R_COVARIANCE = 0.1f; // Initial per cell covariance of the internal resistance + static constexpr float OCV_COVARIANCE = 1.5f; // Initial per cell covariance of the open circuit voltage }; diff --git a/src/lib/battery/module.yaml b/src/lib/battery/module.yaml index 27723a7a4b63..1b6ee5802fb4 100644 --- a/src/lib/battery/module.yaml +++ b/src/lib/battery/module.yaml @@ -39,33 +39,11 @@ parameters: instance_start: 1 default: [4.05, 4.05, 4.05] - BAT${i}_V_LOAD_DROP: - description: - short: Voltage drop per cell on full throttle - long: | - This implicitly defines the internal resistance - to maximum current ratio for the battery and assumes linearity. - A good value to use is the difference between the - 5C and 20-25C load. Not used if BAT${i}_R_INTERNAL is - set. - - type: float - unit: V - min: 0.07 - max: 0.5 - decimal: 2 - increment: 0.01 - reboot_required: true - num_instances: *max_num_config_instances - instance_start: 1 - default: [0.1, 0.1, 0.1] - BAT${i}_R_INTERNAL: description: short: Explicitly defines the per cell internal resistance for battery ${i} long: | - If non-negative, then this will be used in place of - BAT${i}_V_LOAD_DROP for all calculations. + If non-negative, then this will be used instead of the online estimated internal resistance. type: float unit: Ohm @@ -76,7 +54,7 @@ parameters: reboot_required: true num_instances: *max_num_config_instances instance_start: 1 - default: [0.005, 0.005, 0.005] + default: [-1.0, -1.0, -1.0] BAT${i}_N_CELLS: description: diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp index 76cb9a3ebc47..0cd20bc5bc4e 100644 --- a/src/lib/matrix/matrix/SquareMatrix.hpp +++ b/src/lib/matrix/matrix/SquareMatrix.hpp @@ -618,6 +618,7 @@ SquareMatrix choleskyInv(const SquareMatrix &A) return L_inv.T() * L_inv; } +using Matrix2f = SquareMatrix; using Matrix3f = SquareMatrix; using Matrix3d = SquareMatrix; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 45917b6348f9..2a23d1d283b4 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -73,7 +73,6 @@ class Ekf final : public EstimatorInterface public: typedef matrix::Vector VectorState; typedef matrix::SquareMatrix SquareMatrixState; - typedef matrix::SquareMatrix Matrix2f; Ekf() {