A Kalman filter is added to further reduce noise. The Kalman filter needs a
latency prediction as input, so estimate the next expected latency as well.
Details regarding the filter can again be found in "rate_estimator.odt"
---
src/modules/module-loopback.c | 50 ++++++++++++++++++++++++++++++++++++++++++-
1 file changed, 49 insertions(+), 1 deletion(-)
diff --git a/src/modules/module-loopback.c b/src/modules/module-loopback.c
index 2893710..2189a48 100644
--- a/src/modules/module-loopback.c
+++ b/src/modules/module-loopback.c
@@ -112,8 +112,14 @@ struct userdata {
/* State variables of the latency controller,
* last values */
pa_usec_t extra_latency;
+ pa_usec_t next_latency_with_drift;
+ pa_usec_t next_latency_at_optimum_rate_with_drift;
int32_t last_latency_difference;
+ /* Variables for Kalman filter */
+ double latency_variance;
+ double kalman_variance;
+
/* Filter varables used for adaptive re-sampling */
double drift_filter;
double drift_compensation_rate;
@@ -261,6 +267,7 @@ static void adjust_rates(struct userdata *u) {
int32_t latency_difference;
pa_usec_t current_buffer_latency, snapshot_delay, current_source_sink_latency, current_latency, latency_at_optimum_rate;
pa_usec_t final_latency;
+ double filtered_latency, current_latency_error, latency_correction, base_rate_with_drift;
pa_assert(u);
pa_assert_ctl_context();
@@ -321,6 +328,23 @@ static void adjust_rates(struct userdata *u) {
final_latency = PA_MAX(u->latency, u->minimum_latency + u->extra_latency);
latency_difference = (int32_t)((int64_t)current_latency - final_latency);
+ /* Do not filter or calculate error if source or sink changed or if there was an underrun */
+ if (u->source_sink_changed || u->underrun_occured) {
+ /* Initial conditions are very unsure, so use a high variance */
+ u->kalman_variance = 10000000;
+ filtered_latency = latency_at_optimum_rate;
+ u->next_latency_at_optimum_rate_with_drift = latency_at_optimum_rate;
+ u->next_latency_with_drift = current_latency;
+
+ } else {
+ /* Low pass filtered latency variance */
+ current_latency_error = (double)abs((int32_t)(latency_at_optimum_rate - u->next_latency_at_optimum_rate_with_drift));
+ u->latency_variance = (1.0 - FILTER_PARAMETER) * u->latency_variance + FILTER_PARAMETER * current_latency_error * current_latency_error;
+ /* Kalman filter */
+ filtered_latency = (latency_at_optimum_rate * u->kalman_variance + u->next_latency_at_optimum_rate_with_drift * u->latency_variance) / (u->kalman_variance + u->latency_variance);
+ u->kalman_variance = u->kalman_variance * u->latency_variance / (u->kalman_variance + u->latency_variance) + u->latency_variance / 4 + 200;
+ }
+
pa_log_debug("Loopback overall latency is %0.2f ms + %0.2f ms + %0.2f ms = %0.2f ms",
(double) u->latency_snapshot.sink_latency / PA_USEC_PER_MSEC,
(double) current_buffer_latency / PA_USEC_PER_MSEC,
@@ -330,7 +354,7 @@ static void adjust_rates(struct userdata *u) {
pa_log_debug("Loopback latency at base rate is %0.2f ms", (double)latency_at_optimum_rate / PA_USEC_PER_MSEC);
/* Calculate new rate */
- new_rate = rate_controller(u, base_rate, old_rate, (int)(latency_at_optimum_rate - final_latency), latency_difference);
+ new_rate = rate_controller(u, base_rate, old_rate, (int)(filtered_latency - final_latency), latency_difference);
/* Save current latency difference at new rate for next cycle and reset flags */
u->last_latency_difference = current_source_sink_latency + current_buffer_latency * old_rate / new_rate - final_latency;
@@ -338,6 +362,30 @@ static void adjust_rates(struct userdata *u) {
u->source_sink_changed = false;
u->underrun_occured = false;
+ /* Predicton of next latency */
+
+ /* Evaluate optimum rate */
+ base_rate_with_drift = u->drift_compensation_rate + base_rate;
+
+ /* Latency correction on next iteration */
+ latency_correction = (base_rate_with_drift - new_rate) * (int64_t)u->real_adjust_time / new_rate;
+
+ if ((int)new_rate != (int)base_rate_with_drift || new_rate != old_rate) {
+ /* While we are correcting, the next latency is determined by the current value and the difference
+ * between the new sampling rate and the base rate*/
+ u->next_latency_with_drift = current_latency + latency_correction + ((double)old_rate / new_rate - 1) * current_buffer_latency;
+ u->next_latency_at_optimum_rate_with_drift = filtered_latency + latency_correction * new_rate / base_rate_with_drift;
+
+ } else {
+ /* We are in steady state, now only the fractional drift should matter.
+ * To makes sure that we do not drift away due to errors in the fractional
+ * drift, use a running average of the measured and predicted values */
+ u->next_latency_with_drift = (filtered_latency + u->next_latency_with_drift) / 2.0 + (1.0 - (double)(int)base_rate_with_drift / base_rate_with_drift) * (int64_t)u->real_adjust_time;
+
+ /* We are at the optimum rate, so nothing to correct */
+ u->next_latency_at_optimum_rate_with_drift = u->next_latency_with_drift;
+ }
+
/* Set rate */
pa_sink_input_set_rate(u->sink_input, new_rate);
pa_log_debug("[%s] Updated sampling rate to %lu Hz.", u->sink_input->sink->name, (unsigned long) new_rate);
--
2.8.1
