battery: add internal resistance estimation

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

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

src/lib/battery/battery.cpp

@@ -46,17 +46,17 @@
 #include <px4_platform_common/defines.h>
 
 using namespace time_literals;
+using namespace matrix;
 
 Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source) :
 	ModuleParams(parent),
 	_index(index < 1 || index > 9 ? 1 : index),
 	_source(source)
 {
 	const float expected_filter_dt = static_cast<float>(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 &timestamp)
 {
 	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 &timestamp)
 
 	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<float, 2, 1>(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);

src/lib/battery/battery.h

@@ -58,7 +58,6 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/flight_phase_estimation.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_thrust_setpoint.h>
 
 /**
  * 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_s> _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)};
 	uORB::PublicationMulti<battery_status_s> _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<float> _voltage_filter_v;
+	AlphaFilter<float> _ocv_filter_v;
+	AlphaFilter<float> _cell_voltage_filter_v;
 	float _current_a{-1};
-	AlphaFilter<float> _current_filter_a;
 	AlphaFilter<float>
 	_current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight.
-	AlphaFilter<float> _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
 };

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:

src/lib/matrix/matrix/SquareMatrix.hpp

@@ -618,6 +618,7 @@ SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> &A)
 	return L_inv.T() * L_inv;
 }
 
+using Matrix2f = SquareMatrix<float, 2>;
 using Matrix3f = SquareMatrix<float, 3>;
 using Matrix3d = SquareMatrix<double, 3>;
 

src/modules/ekf2/EKF/ekf.h

@@ -73,7 +73,6 @@ class Ekf final : public EstimatorInterface
 public:
 	typedef matrix::Vector<float, State::size> VectorState;
 	typedef matrix::SquareMatrix<float, State::size> SquareMatrixState;
-	typedef matrix::SquareMatrix<float, 2> Matrix2f;
 
 	Ekf()
 	{