[pulseaudio-discuss] [PATCH v6 20/25] loopback: Add latency prediction and Kalman filter

[Georg Chini]

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