From 39ae69690e30b614a8320c314a34b654b6c12256 Mon Sep 17 00:00:00 2001 From: Rasmus Brandt Date: Tue, 9 Aug 2022 14:40:05 +0200 Subject: [PATCH] Split out the jitter estimator's Kalman filter into its own class. The intention of this change is to separate the Kalman filter state (that prior to this change lived in JitterEstimator) from the other filter's state, making it easier to see how the different filters interact. This move does not include any interface, functional, or documentation changes. Those will follow in later changes. A very basic unit test is added, which will also be expanded later on. Bug: webrtc:14151 Change-Id: Ifb9b8ce2d9418ea52ccf64a77fd46d1ebba30779 Reviewed-on: https://webrtc-review.googlesource.com/c/src/+/264984 Commit-Queue: Rasmus Brandt Reviewed-by: Philip Eliasson Cr-Commit-Position: refs/heads/main@{#37721} --- modules/video_coding/timing/BUILD.gn | 18 +++ .../timing/frame_delay_delta_kalman_filter.cc | 132 ++++++++++++++++++ .../timing/frame_delay_delta_kalman_filter.h | 68 +++++++++ ...rame_delay_delta_kalman_filter_unittest.cc | 29 ++++ .../video_coding/timing/jitter_estimator.cc | 109 ++------------- .../video_coding/timing/jitter_estimator.h | 36 +---- 6 files changed, 261 insertions(+), 131 deletions(-) create mode 100644 modules/video_coding/timing/frame_delay_delta_kalman_filter.cc create mode 100644 modules/video_coding/timing/frame_delay_delta_kalman_filter.h create mode 100644 modules/video_coding/timing/frame_delay_delta_kalman_filter_unittest.cc diff --git a/modules/video_coding/timing/BUILD.gn b/modules/video_coding/timing/BUILD.gn index 17f716f254..857d27ce2d 100644 --- a/modules/video_coding/timing/BUILD.gn +++ b/modules/video_coding/timing/BUILD.gn @@ -30,12 +30,28 @@ rtc_library("inter_frame_delay") { absl_deps = [ "//third_party/abseil-cpp/absl/types:optional" ] } +rtc_library("frame_delay_delta_kalman_filter") { + sources = [ + "frame_delay_delta_kalman_filter.cc", + "frame_delay_delta_kalman_filter.h", + ] + deps = [ + "../../../api/units:data_size", + "../../../api/units:time_delta", + ] + visibility = [ + ":jitter_estimator", + ":timing_unittests", + ] +} + rtc_library("jitter_estimator") { sources = [ "jitter_estimator.cc", "jitter_estimator.h", ] deps = [ + ":frame_delay_delta_kalman_filter", ":rtt_filter", "../../../api:field_trials_view", "../../../api/units:data_size", @@ -86,12 +102,14 @@ rtc_library("timing_module") { rtc_library("timing_unittests") { testonly = true sources = [ + "frame_delay_delta_kalman_filter_unittest.cc", "inter_frame_delay_unittest.cc", "jitter_estimator_unittest.cc", "rtt_filter_unittest.cc", "timing_unittest.cc", ] deps = [ + ":frame_delay_delta_kalman_filter", ":inter_frame_delay", ":jitter_estimator", ":rtt_filter", diff --git a/modules/video_coding/timing/frame_delay_delta_kalman_filter.cc b/modules/video_coding/timing/frame_delay_delta_kalman_filter.cc new file mode 100644 index 0000000000..ae904342af --- /dev/null +++ b/modules/video_coding/timing/frame_delay_delta_kalman_filter.cc @@ -0,0 +1,132 @@ +/* + * Copyright (c) 2022 The WebRTC project authors. All Rights Reserved. + * + * Use of this source code is governed by a BSD-style license + * that can be found in the LICENSE file in the root of the source + * tree. An additional intellectual property rights grant can be found + * in the file PATENTS. All contributing project authors may + * be found in the AUTHORS file in the root of the source tree. + */ + +#include "modules/video_coding/timing/frame_delay_delta_kalman_filter.h" + +#include "api/units/data_size.h" +#include "api/units/time_delta.h" + +namespace webrtc { + +namespace { +constexpr double kThetaLow = 0.000001; +} + +FrameDelayDeltaKalmanFilter::FrameDelayDeltaKalmanFilter() { + Reset(); +} + +// Resets the JitterEstimate. +void FrameDelayDeltaKalmanFilter::Reset() { + theta_[0] = 1 / (512e3 / 8); + theta_[1] = 0; + + theta_cov_[0][0] = 1e-4; + theta_cov_[1][1] = 1e2; + theta_cov_[0][1] = theta_cov_[1][0] = 0; + q_cov_[0][0] = 2.5e-10; + q_cov_[1][1] = 1e-10; + q_cov_[0][1] = q_cov_[1][0] = 0; +} + +// Updates Kalman estimate of the channel. +// The caller is expected to sanity check the inputs. +void FrameDelayDeltaKalmanFilter::KalmanEstimateChannel( + TimeDelta frame_delay, + double delta_frame_size_bytes, + DataSize max_frame_size, + double var_noise) { + double Mh[2]; + double hMh_sigma; + double kalmanGain[2]; + double measureRes; + double t00, t01; + + // Kalman filtering + + // Prediction + // M = M + Q + theta_cov_[0][0] += q_cov_[0][0]; + theta_cov_[0][1] += q_cov_[0][1]; + theta_cov_[1][0] += q_cov_[1][0]; + theta_cov_[1][1] += q_cov_[1][1]; + + // Kalman gain + // K = M*h'/(sigma2n + h*M*h') = M*h'/(1 + h*M*h') + // h = [dFS 1] + // Mh = M*h' + // hMh_sigma = h*M*h' + R + Mh[0] = theta_cov_[0][0] * delta_frame_size_bytes + theta_cov_[0][1]; + Mh[1] = theta_cov_[1][0] * delta_frame_size_bytes + theta_cov_[1][1]; + // sigma weights measurements with a small deltaFS as noisy and + // measurements with large deltaFS as good + if (max_frame_size < DataSize::Bytes(1)) { + return; + } + double sigma = (300.0 * exp(-fabs(delta_frame_size_bytes) / + (1e0 * max_frame_size.bytes())) + + 1) * + sqrt(var_noise); + if (sigma < 1.0) { + sigma = 1.0; + } + hMh_sigma = delta_frame_size_bytes * Mh[0] + Mh[1] + sigma; + if ((hMh_sigma < 1e-9 && hMh_sigma >= 0) || + (hMh_sigma > -1e-9 && hMh_sigma <= 0)) { + RTC_DCHECK_NOTREACHED(); + return; + } + kalmanGain[0] = Mh[0] / hMh_sigma; + kalmanGain[1] = Mh[1] / hMh_sigma; + + // Correction + // theta = theta + K*(dT - h*theta) + measureRes = + frame_delay.ms() - (delta_frame_size_bytes * theta_[0] + theta_[1]); + theta_[0] += kalmanGain[0] * measureRes; + theta_[1] += kalmanGain[1] * measureRes; + + if (theta_[0] < kThetaLow) { + theta_[0] = kThetaLow; + } + + // M = (I - K*h)*M + t00 = theta_cov_[0][0]; + t01 = theta_cov_[0][1]; + theta_cov_[0][0] = (1 - kalmanGain[0] * delta_frame_size_bytes) * t00 - + kalmanGain[0] * theta_cov_[1][0]; + theta_cov_[0][1] = (1 - kalmanGain[0] * delta_frame_size_bytes) * t01 - + kalmanGain[0] * theta_cov_[1][1]; + theta_cov_[1][0] = theta_cov_[1][0] * (1 - kalmanGain[1]) - + kalmanGain[1] * delta_frame_size_bytes * t00; + theta_cov_[1][1] = theta_cov_[1][1] * (1 - kalmanGain[1]) - + kalmanGain[1] * delta_frame_size_bytes * t01; + + // Covariance matrix, must be positive semi-definite. + RTC_DCHECK(theta_cov_[0][0] + theta_cov_[1][1] >= 0 && + theta_cov_[0][0] * theta_cov_[1][1] - + theta_cov_[0][1] * theta_cov_[1][0] >= + 0 && + theta_cov_[0][0] >= 0); +} + +// Calculate difference in delay between a sample and the expected delay +// estimated by the Kalman filter +double FrameDelayDeltaKalmanFilter::DeviationFromExpectedDelay( + TimeDelta frame_delay, + double delta_frame_size_bytes) const { + return frame_delay.ms() - (theta_[0] * delta_frame_size_bytes + theta_[1]); +} + +double FrameDelayDeltaKalmanFilter::GetSlope() const { + return theta_[0]; +} + +} // namespace webrtc diff --git a/modules/video_coding/timing/frame_delay_delta_kalman_filter.h b/modules/video_coding/timing/frame_delay_delta_kalman_filter.h new file mode 100644 index 0000000000..15de7d5e60 --- /dev/null +++ b/modules/video_coding/timing/frame_delay_delta_kalman_filter.h @@ -0,0 +1,68 @@ +/* + * Copyright (c) 2022 The WebRTC project authors. All Rights Reserved. + * + * Use of this source code is governed by a BSD-style license + * that can be found in the LICENSE file in the root of the source + * tree. An additional intellectual property rights grant can be found + * in the file PATENTS. All contributing project authors may + * be found in the AUTHORS file in the root of the source tree. + */ + +#ifndef MODULES_VIDEO_CODING_TIMING_FRAME_DELAY_DELTA_KALMAN_FILTER_H_ +#define MODULES_VIDEO_CODING_TIMING_FRAME_DELAY_DELTA_KALMAN_FILTER_H_ + +#include "api/units/data_size.h" +#include "api/units/time_delta.h" + +namespace webrtc { + +class FrameDelayDeltaKalmanFilter { + public: + FrameDelayDeltaKalmanFilter(); + ~FrameDelayDeltaKalmanFilter() = default; + + // Resets the estimate to the initial state. + void Reset(); + + // Updates the Kalman filter for the line describing the frame size dependent + // jitter. + // + // Input: + // - frame_delay + // Delay-delta calculated by UTILDelayEstimate. + // - delta_frame_size_bytes + // Frame size delta, i.e. frame size at time T minus frame size + // at time T-1. (May be negative!) + // - max_frame_size + // Filtered version of the largest frame size received. + // - var_noise + // Variance of the estimated random jitter. + void KalmanEstimateChannel(TimeDelta frame_delay, + double delta_frame_size_bytes, + DataSize max_frame_size, + double var_noise); + + // Calculates the difference in delay between a sample and the expected delay + // estimated by the Kalman filter. + // + // Input: + // - frame_delay : Delay-delta calculated by UTILDelayEstimate. + // - delta_frame_size_bytes : Frame size delta, i.e. frame size at + // time T minus frame size at time T-1. + // + // Return value : The delay difference in ms. + double DeviationFromExpectedDelay(TimeDelta frame_delay, + double delta_frame_size_bytes) const; + + // Returns the estimated slope. + double GetSlope() const; + + private: + double theta_[2]; // Estimated line parameters (slope, offset) + double theta_cov_[2][2]; // Estimate covariance + double q_cov_[2][2]; // Process noise covariance +}; + +} // namespace webrtc + +#endif // MODULES_VIDEO_CODING_TIMING_FRAME_DELAY_DELTA_KALMAN_FILTER_H_ diff --git a/modules/video_coding/timing/frame_delay_delta_kalman_filter_unittest.cc b/modules/video_coding/timing/frame_delay_delta_kalman_filter_unittest.cc new file mode 100644 index 0000000000..3400ccf34c --- /dev/null +++ b/modules/video_coding/timing/frame_delay_delta_kalman_filter_unittest.cc @@ -0,0 +1,29 @@ +/* + * Copyright (c) 2022 The WebRTC project authors. All Rights Reserved. + * + * Use of this source code is governed by a BSD-style license + * that can be found in the LICENSE file in the root of the source + * tree. An additional intellectual property rights grant can be found + * in the file PATENTS. All contributing project authors may + * be found in the AUTHORS file in the root of the source tree. + */ + +#include "modules/video_coding/timing/frame_delay_delta_kalman_filter.h" + +#include "api/units/data_size.h" +#include "api/units/time_delta.h" +#include "test/gtest.h" + +namespace webrtc { +namespace { + +TEST(FrameDelayDeltaKalmanFilterTest, InitialBandwidthStateIs512kbps) { + FrameDelayDeltaKalmanFilter filter; + + // The slope corresponds to the estimated bandwidth, and the initial value + // is set in the implementation. + EXPECT_EQ(filter.GetSlope(), 1 / (512e3 / 8)); +} + +} // namespace +} // namespace webrtc diff --git a/modules/video_coding/timing/jitter_estimator.cc b/modules/video_coding/timing/jitter_estimator.cc index 0bb56bb15d..e5d13d9443 100644 --- a/modules/video_coding/timing/jitter_estimator.cc +++ b/modules/video_coding/timing/jitter_estimator.cc @@ -37,7 +37,6 @@ static constexpr double kDefaultMaxTimestampDeviationInSigmas = 3.5; constexpr double kPhi = 0.97; constexpr double kPsi = 0.9999; constexpr uint32_t kAlphaCountMax = 400; -constexpr double kThetaLow = 0.000001; constexpr uint32_t kNackLimit = 3; constexpr int32_t kNumStdDevDelayOutlier = 15; constexpr int32_t kNumStdDevFrameSizeOutlier = 3; @@ -60,16 +59,8 @@ JitterEstimator::~JitterEstimator() = default; // Resets the JitterEstimate. void JitterEstimator::Reset() { - theta_[0] = 1 / (512e3 / 8); - theta_[1] = 0; var_noise_ = 4.0; - theta_cov_[0][0] = 1e-4; - theta_cov_[1][1] = 1e2; - theta_cov_[0][1] = theta_cov_[1][0] = 0; - q_cov_[0][0] = 2.5e-10; - q_cov_[1][1] = 1e-10; - q_cov_[0][1] = q_cov_[1][0] = 0; avg_frame_size_ = kDefaultAvgAndMaxFrameSize; max_frame_size_ = kDefaultAvgAndMaxFrameSize; var_frame_size_ = 100; @@ -86,6 +77,8 @@ void JitterEstimator::Reset() { startup_count_ = 0; rtt_filter_.Reset(); fps_counter_.Reset(); + + kalman_filter_.Reset(); } // Updates the estimates with the new measurements. @@ -135,7 +128,8 @@ void JitterEstimator::UpdateEstimate(TimeDelta frame_delay, // outlier. Even if it is an extreme outlier from a delay point of view, if // the frame size also is large the deviation is probably due to an incorrect // line slope. - double deviation = DeviationFromExpectedDelay(frame_delay, delta_frame_bytes); + double deviation = + kalman_filter_.DeviationFromExpectedDelay(frame_delay, delta_frame_bytes); if (fabs(deviation) < kNumStdDevDelayOutlier * sqrt(var_noise_) || frame_size.bytes() > @@ -152,7 +146,8 @@ void JitterEstimator::UpdateEstimate(TimeDelta frame_delay, // frame. if (delta_frame_bytes > -0.25 * max_frame_size_.bytes()) { // Update the Kalman filter with the new data - KalmanEstimateChannel(frame_delay, delta_frame_bytes); + kalman_filter_.KalmanEstimateChannel(frame_delay, delta_frame_bytes, + max_frame_size_, var_noise_); } } else { int nStdDev = @@ -175,92 +170,6 @@ void JitterEstimator::FrameNacked() { latest_nack_ = clock_->CurrentTime(); } -// Updates Kalman estimate of the channel. -// The caller is expected to sanity check the inputs. -void JitterEstimator::KalmanEstimateChannel(TimeDelta frame_delay, - double delta_frame_size_bytes) { - double Mh[2]; - double hMh_sigma; - double kalmanGain[2]; - double measureRes; - double t00, t01; - - // Kalman filtering - - // Prediction - // M = M + Q - theta_cov_[0][0] += q_cov_[0][0]; - theta_cov_[0][1] += q_cov_[0][1]; - theta_cov_[1][0] += q_cov_[1][0]; - theta_cov_[1][1] += q_cov_[1][1]; - - // Kalman gain - // K = M*h'/(sigma2n + h*M*h') = M*h'/(1 + h*M*h') - // h = [dFS 1] - // Mh = M*h' - // hMh_sigma = h*M*h' + R - Mh[0] = theta_cov_[0][0] * delta_frame_size_bytes + theta_cov_[0][1]; - Mh[1] = theta_cov_[1][0] * delta_frame_size_bytes + theta_cov_[1][1]; - // sigma weights measurements with a small deltaFS as noisy and - // measurements with large deltaFS as good - if (max_frame_size_ < DataSize::Bytes(1)) { - return; - } - double sigma = (300.0 * exp(-fabs(delta_frame_size_bytes) / - (1e0 * max_frame_size_.bytes())) + - 1) * - sqrt(var_noise_); - if (sigma < 1.0) { - sigma = 1.0; - } - hMh_sigma = delta_frame_size_bytes * Mh[0] + Mh[1] + sigma; - if ((hMh_sigma < 1e-9 && hMh_sigma >= 0) || - (hMh_sigma > -1e-9 && hMh_sigma <= 0)) { - RTC_DCHECK_NOTREACHED(); - return; - } - kalmanGain[0] = Mh[0] / hMh_sigma; - kalmanGain[1] = Mh[1] / hMh_sigma; - - // Correction - // theta = theta + K*(dT - h*theta) - measureRes = - frame_delay.ms() - (delta_frame_size_bytes * theta_[0] + theta_[1]); - theta_[0] += kalmanGain[0] * measureRes; - theta_[1] += kalmanGain[1] * measureRes; - - if (theta_[0] < kThetaLow) { - theta_[0] = kThetaLow; - } - - // M = (I - K*h)*M - t00 = theta_cov_[0][0]; - t01 = theta_cov_[0][1]; - theta_cov_[0][0] = (1 - kalmanGain[0] * delta_frame_size_bytes) * t00 - - kalmanGain[0] * theta_cov_[1][0]; - theta_cov_[0][1] = (1 - kalmanGain[0] * delta_frame_size_bytes) * t01 - - kalmanGain[0] * theta_cov_[1][1]; - theta_cov_[1][0] = theta_cov_[1][0] * (1 - kalmanGain[1]) - - kalmanGain[1] * delta_frame_size_bytes * t00; - theta_cov_[1][1] = theta_cov_[1][1] * (1 - kalmanGain[1]) - - kalmanGain[1] * delta_frame_size_bytes * t01; - - // Covariance matrix, must be positive semi-definite. - RTC_DCHECK(theta_cov_[0][0] + theta_cov_[1][1] >= 0 && - theta_cov_[0][0] * theta_cov_[1][1] - - theta_cov_[0][1] * theta_cov_[1][0] >= - 0 && - theta_cov_[0][0] >= 0); -} - -// Calculate difference in delay between a sample and the expected delay -// estimated by the Kalman filter -double JitterEstimator::DeviationFromExpectedDelay( - TimeDelta frame_delay, - double delta_frame_size_bytes) const { - return frame_delay.ms() - (theta_[0] * delta_frame_size_bytes + theta_[1]); -} - // Estimates the random jitter by calculating the variance of the sample // distance from the line given by theta. void JitterEstimator::EstimateRandomJitter(double d_dT) { @@ -319,9 +228,9 @@ double JitterEstimator::NoiseThreshold() const { // Calculates the current jitter estimate from the filtered estimates. TimeDelta JitterEstimator::CalculateEstimate() { - double retMs = - theta_[0] * (max_frame_size_.bytes() - avg_frame_size_.bytes()) + - NoiseThreshold(); + double retMs = kalman_filter_.GetSlope() * + (max_frame_size_.bytes() - avg_frame_size_.bytes()) + + NoiseThreshold(); TimeDelta ret = TimeDelta::Millis(retMs); diff --git a/modules/video_coding/timing/jitter_estimator.h b/modules/video_coding/timing/jitter_estimator.h index 7d84601b87..8ed2b36900 100644 --- a/modules/video_coding/timing/jitter_estimator.h +++ b/modules/video_coding/timing/jitter_estimator.h @@ -17,6 +17,7 @@ #include "api/units/frequency.h" #include "api/units/time_delta.h" #include "api/units/timestamp.h" +#include "modules/video_coding/timing/frame_delay_delta_kalman_filter.h" #include "modules/video_coding/timing/rtt_filter.h" #include "rtc_base/rolling_accumulator.h" @@ -66,23 +67,8 @@ class JitterEstimator { // decoding delay estimate. static constexpr TimeDelta OPERATING_SYSTEM_JITTER = TimeDelta::Millis(10); - protected: - // These are protected for better testing possibilities. - double theta_[2]; // Estimated line parameters (slope, offset) - double var_noise_; // Variance of the time-deviation from the line - private: - // Updates the Kalman filter for the line describing the frame size dependent - // jitter. - // - // Input: - // - frame_delay - // Delay-delta calculated by UTILDelayEstimate. - // - delta_frame_size_bytes - // Frame size delta, i.e. frame size at time T minus frame size - // at time T-1. - void KalmanEstimateChannel(TimeDelta frame_delay, - double delta_frame_size_bytes); + double var_noise_; // Variance of the time-deviation from the line // Updates the random jitter estimate, i.e. the variance of the time // deviations from the line given by the Kalman filter. @@ -101,23 +87,11 @@ class JitterEstimator { // Post process the calculated estimate. void PostProcessEstimate(); - // Calculates the difference in delay between a sample and the expected delay - // estimated by the Kalman filter. - // - // Input: - // - frame_delay : Delay-delta calculated by UTILDelayEstimate. - // - delta_frame_size_bytes : Frame size delta, i.e. frame size at - // time - // T minus frame size at time T-1. - // - // Return value : The delay difference in ms. - double DeviationFromExpectedDelay(TimeDelta frame_delay, - double delta_frame_size_bytes) const; - Frequency GetFrameRate() const; - double theta_cov_[2][2]; // Estimate covariance - double q_cov_[2][2]; // Process noise covariance + // Filters the {frame_delay_delta, frame_size_delta} measurements through + // a linear Kalman filter. + FrameDelayDeltaKalmanFilter kalman_filter_; static constexpr DataSize kDefaultAvgAndMaxFrameSize = DataSize::Bytes(500); DataSize avg_frame_size_ = kDefaultAvgAndMaxFrameSize; // Average frame size