[PATCH 4/4] alsa-{source, sink}: Add Kalman filter to smoother function

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

 



Like in module-loopback a Kalman filter is used to reduce noise. Since it
strongly depends on the validity of the underlying model, the average
square of the time factor derivative is used as a measure of convergence
of the time factor.

---
 src/modules/alsa/alsa-sink.c   | 36 +++++++++++++++++++++++++++++++++---
 src/modules/alsa/alsa-source.c | 30 +++++++++++++++++++++++++++++-
 2 files changed, 62 insertions(+), 4 deletions(-)

diff --git a/src/modules/alsa/alsa-sink.c b/src/modules/alsa/alsa-sink.c
index de51710..37544e0 100644
--- a/src/modules/alsa/alsa-sink.c
+++ b/src/modules/alsa/alsa-sink.c
@@ -148,6 +148,10 @@ struct userdata {
     double drift_filter;
     double drift_filter_1;
     double time_factor;
+    double time_factor_variance;
+
+    double kalman_variance;
+    double time_variance;
 
     bool first_delayed;
     bool usb_hack;
@@ -866,6 +870,9 @@ static void init_time_estimate(struct userdata *u) {
     u->first_delayed = false;
     u->usb_hack = false;
     u->time_offset = 0;
+    u->time_factor_variance = 10000.0;
+    u->kalman_variance = 10000000.0;
+    u->time_variance = 100000.0;
 
     /* Check if this is an USB device, see alsa-util.c
      * USB devices unfortunately need some special handling */
@@ -891,7 +898,7 @@ static void update_time_estimate(struct userdata *u) {
     double byte_count, iteration_time;
     int err;
     double time_delta_system, time_delta_card, drift, filter_constant, filter_constant_1;
-    double temp;
+    double temp, filtered_time_delta_card, expected_time_delta_card;
     pa_usec_t time_stamp;
     snd_pcm_status_t *status;
 
@@ -957,6 +964,19 @@ static void update_time_estimate(struct userdata *u) {
      * pa_bytes_to_usec() here because u->start_pos need not
      * be on a sample boundary */
     time_delta_card = byte_count / u->frame_size / u->sink->sample_spec.rate * PA_USEC_PER_SEC;
+    filtered_time_delta_card = time_delta_card;
+
+    /* Prediction of measurement */
+    expected_time_delta_card = time_delta_system * u->time_factor;
+
+    /* Filtered variance of card time measurements */
+    u->time_variance = 0.9 * u->time_variance + 0.1 * (time_delta_card - expected_time_delta_card) * (time_delta_card - expected_time_delta_card);
+
+    /* Kalman filter, will only be used when the time factor has converged good enough */
+    if (u->time_factor_variance < 100) {
+        filtered_time_delta_card = (time_delta_card * u->kalman_variance + expected_time_delta_card * u->time_variance) / (u->kalman_variance + u->time_variance);
+        u->kalman_variance = u->kalman_variance * u->time_variance / (u->kalman_variance + u->time_variance) + u->time_variance / 4 + 500;
+    }
 
     /* This is a horrible hack which is necessary because USB devices seem to fix up
      * the reported delay by some millisecondsconds shortly after startup. This is
@@ -967,13 +987,16 @@ static void update_time_estimate(struct userdata *u) {
      * avoid false triggers. When run as batch device, the threshold for the hack must
      * be lower than for timer based scheduling. */
     if (u->usb_hack && time_delta_system < 5 * PA_USEC_PER_SEC) {
-        if (abs(time_delta_system - time_delta_card / u->time_factor) > u->hack_threshold) {
+        if (abs(time_delta_system - filtered_time_delta_card / u->time_factor) > u->hack_threshold) {
             /* Recalculate initial conditions */
             temp = time_stamp - time_delta_card - u->start_time;
             u->start_time += temp;
             u->first_start_time += temp;
             u->time_offset = -temp;
 
+            /* Reset time factor variance */
+            u->time_factor_variance = 10000;
+
             pa_log_debug("USB Hack, start time corrected by %0.2f usec", temp);
             u->usb_hack = false;
             return;
@@ -984,8 +1007,11 @@ static void update_time_estimate(struct userdata *u) {
     filter_constant = iteration_time / (iteration_time + 318310.0);
     filter_constant_1 = iteration_time / (iteration_time + 2387324.0);
 
+    /* Temporarily save the current time factor */
+    temp = u->time_factor;
+
     /* Calculate geometric series */
-    drift = (u->drift_filter_1 + 1.0) * (1.5 - time_delta_card / time_delta_system);
+    drift = (u->drift_filter_1 + 1.0) * (1.5 - filtered_time_delta_card / time_delta_system);
 
     /* 2nd order lowpass */
     u->drift_filter = (1 - filter_constant) * u->drift_filter + filter_constant * drift;
@@ -994,6 +1020,10 @@ static void update_time_estimate(struct userdata *u) {
     /* Calculate time conversion factor, filter again */
     u->time_factor = (1 - filter_constant_1) * u->time_factor + filter_constant_1 * (u->drift_filter_1 + 3) / (u->drift_filter_1 + 1) / 2;
 
+    /* Filtered variance of time factor derivative, used as measure for the convergence of the time factor */
+    temp = (u->time_factor - temp) / iteration_time * 10000000000000;
+    u->time_factor_variance = (1 - filter_constant_1) * u->time_factor_variance + filter_constant_1 * temp * temp;
+
     /* Save current system time */
     u->last_time = time_stamp;
 }
diff --git a/src/modules/alsa/alsa-source.c b/src/modules/alsa/alsa-source.c
index c3dd75d..ce2e79a 100644
--- a/src/modules/alsa/alsa-source.c
+++ b/src/modules/alsa/alsa-source.c
@@ -133,6 +133,10 @@ struct userdata {
     double drift_filter;
     double drift_filter_1;
     double time_factor;
+    double time_factor_variance;
+
+    double kalman_variance;
+    double time_variance;
 
     bool first_delayed;
 
@@ -776,6 +780,9 @@ static void init_time_estimate(struct userdata *u) {
     u->time_factor = 1.0;
     u->start_pos = 0;
     u->first_delayed = true;
+    u->time_factor_variance = 10000.0;
+    u->kalman_variance = 10000000.0;
+    u->time_variance = 100000.0;
 
     /* Start times */
     u->start_time = pa_rtclock_now();
@@ -787,6 +794,7 @@ static void update_time_estimate(struct userdata *u) {
     snd_pcm_sframes_t delay = 0;
     double byte_count, iteration_time;
     double time_delta_system, time_delta_card, drift, filter_constant, filter_constant_1;
+    double temp, filtered_time_delta_card, expected_time_delta_card;
     pa_usec_t time_stamp;
     int err;
     snd_pcm_status_t *status;
@@ -842,13 +850,29 @@ static void update_time_estimate(struct userdata *u) {
      * pa_bytes_to_usec() here because u->start_pos need not
      * be on a sample boundary */
     time_delta_card = byte_count / u->frame_size / u->source->sample_spec.rate * PA_USEC_PER_SEC;
+    filtered_time_delta_card = time_delta_card;
+
+    /* Prediction of measurement */
+    expected_time_delta_card = time_delta_system * u->time_factor;
+
+    /* Filtered variance of card time measurements */
+    u->time_variance = 0.9 * u->time_variance + 0.1 * (time_delta_card - expected_time_delta_card) * (time_delta_card - expected_time_delta_card);
+
+    /* Kalman filter, it will only be used when the time factor has converged good enough */
+    if (u->time_factor_variance < 100) {
+        filtered_time_delta_card = (time_delta_card * u->kalman_variance + expected_time_delta_card * u->time_variance) / (u->kalman_variance + u->time_variance);
+        u->kalman_variance = u->kalman_variance * u->time_variance / (u->kalman_variance + u->time_variance) + u->time_variance / 4 + 500;
+    }
 
     /* Parameter for lowpass filter with 2s and 15s time constant */
     filter_constant = iteration_time / (iteration_time + 318310.0);
     filter_constant_1 = iteration_time / (iteration_time + 2387324.0);
 
+    /* Temporarily save current time conversion factor */
+    temp = u->time_factor;
+
     /* Calculate geometric series */
-    drift = (u->drift_filter_1 + 1.0) * (1.5 - time_delta_card / time_delta_system);
+    drift = (u->drift_filter_1 + 1.0) * (1.5 - filtered_time_delta_card / time_delta_system);
 
     /* 2nd order lowpass */
     u->drift_filter = (1 - filter_constant) * u->drift_filter + filter_constant * drift;
@@ -857,6 +881,10 @@ static void update_time_estimate(struct userdata *u) {
     /* Calculate time conversion factor, filter again */
     u->time_factor = (1 - filter_constant_1) * u->time_factor + filter_constant_1 * (u->drift_filter_1 + 3) / (u->drift_filter_1 + 1) / 2;
 
+    /* Filtered variance of time factor derivative, used as a measure for the convergence of the time factor */
+    temp = (u->time_factor - temp) / iteration_time * 10000000000000;
+    u->time_factor_variance = (1 - filter_constant_1) * u->time_factor_variance + filter_constant_1 * temp * temp;
+
     /* Save current system time */
     u->last_time = time_stamp;
 }
-- 
2.8.1



[Index of Archives]     [Linux Audio Users]     [AMD Graphics]     [Linux USB Devel]     [Linux Audio Users]     [Yosemite News]     [Linux Kernel]     [Linux SCSI]

  Powered by Linux