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