Webrtc Intro - Kalman Filter of Remb

//  请参考草案定义看UpdateKalman的实现

void OveruseDetector::UpdateKalman(int64_t t_delta,
                                   double ts_delta,
                                   uint32_t frame_size,
                                   uint32_t prev_frame_size) {
  const double min_frame_period = UpdateMinFramePeriod(ts_delta);
  const double drift = CurrentDrift();
  // Compensate for drift

  // We define the inter-arrival time, t(i) - t(i-1), as the difference in
  // arrival time of two packets or two groups of packets.
  // Correspondingly, the inter-departure time, T(i) - T(i-1), is defined
  // as the difference in departure-time of two packets or two groups of
  // packets.
  // i.e., t(i) - t(i-1) is difference in arrival time of two packets;
  // and T(i) - T(i-1) is difference in rtp timestamp of two packets,

  // d(i) = t(i) - t(i-1) - (T(i) - T(i-1)) =d(i) = dL(i)/C(i) + m(i) + v(i)

  const double t_ts_delta = t_delta - ts_delta / drift;  // d(i)

  double fs_delta = static_cast<double>(frame_size) - prev_frame_size;  // dL(i)

  // Update the Kalman filter

  // process_noise_ is diag(Q(i))

  // followings are calculating E(i - 1) + Q(i)

  // covariance Q(i) = E{u_bar(i) * u_bar(i)^T}
  // Q(i) is RECOMMENDED as a diagonal matrix with main diagonal elements
  // as:
  // diag(Q(i)) = [10^-13 10^-3]^T

  // Notes: only calcluate diag element, statement two variables of this WGN

  // is mutual independance, so theire coviance of off diag is zero... ???

  const double scale_factor =  min_frame_period / (1000.0 / 30.0);
  E_[0][0] += process_noise_[0] * scale_factor;
  E_[1][1] += process_noise_[1] * scale_factor;

  if ((hypothesis_ == kBwOverusing && offset_ < prev_offset_) ||
      (hypothesis_ == kBwUnderusing && offset_ > prev_offset_)) {
    E_[1][1] += 10 * process_noise_[1] * scale_factor;
  }

  const double h[2] = {fs_delta, 1.0};  // h_bar; h_bar(i) = [dL(i) 1]^T

  // h_bar(i)^T * (E(i-1) + Q(i)) * h_bar(i)

  // Eh[2] = (E(i-1) + Q(i)) * h_bar(i)

  const double Eh[2] = {E_[0][0]*h[0] + E_[0][1]*h[1],
                        E_[1][0]*h[0] + E_[1][1]*h[1]};

  // z(i) = z(i) = d(i) - h_bar(i)^T * theta_hat(i-1)
  // h_bar(i) = [dL(i) 1]^T = [h[0], 1]^T
  // theta_hat(i-1) = [slope_(i-1), offset_(i-1)]^T,
  // slope_(i-1) = 1/C(i-1), offset_(i-1) = m(i-1)
 
  const double residual = t_ts_delta - slope_*h[0] - offset_;      // z(i)

  const bool stable_state =
      (BWE_MIN(num_of_deltas_, 60) * fabs(offset_) < threshold_);
  // We try to filter out very late frames. For instance periodic key
  // frames doesn't fit the Gaussian model well.

  // Since our assumption that v(i) should be zero
  // mean WGN is less accurate in some cases, we have introduced an
  // additional outlier filter around the updates of var_v_hat. If z(i) >
  // 3*sqrt(var_v_hat) the filter is updated with 3*sqrt(var_v_hat) rather

  // than z(i). For instance v(i) will not be white in situations where
  // packets are sent at a higher rate than the channel capacity, in which
 // case they will be queued behind each other.


  if (fabs(residual) < 3 * sqrt(var_noise_)) {
    UpdateNoiseEstimate(residual, min_frame_period, stable_state);
  } else {
    UpdateNoiseEstimate(3 * sqrt(var_noise_), min_frame_period, stable_state);
  }

//                   ( E(i-1) + Q(i) ) * h_bar(i)
// k_bar(i) = ------------------------------------------------------
//                 var_v_hat(i) + h_bar(i)^T * (E(i-1) + Q(i)) * h_bar(i)
    
// E(i) = (I - k_bar(i) * h_bar(i)^T) * (E(i-1) + Q(i))
// where I is the 2-by-2 identity matrix.

// denom = var_v_hat(i) + h_bar(i)^T * (E(i-1) + Q(i)) * h_bar(i),

// and h[0]*Eh[0] + h[1]*Eh[1] = h_bar(i)^T * (E(i-1) + Q(i)) * h_bar(i)

// Eh[2] =  (E(i-1) + Q(i)) * h_bar(i)

  const double denom = var_noise_ + h[0]*Eh[0] + h[1]*Eh[1];

  const double K[2] = {Eh[0] / denom,
                       Eh[1] / denom}; // K[2] = k_bar

 // IKh[2][2] = (I - k_bar(i) * h_bar(i)^T)

  const double IKh[2][2] = {{1.0 - K[0]*h[0], -K[0]*h[1]},
                            {-K[1]*h[0], 1.0 - K[1]*h[1]}};

  // So far, E_(i) = E_(i - 1) + Q(i)
  const double e00 = E_[0][0];
  const double e01 = E_[0][1];

  // Update state

  // what followings is update E(i) in light of following formula

  // E(i) = (I - k_bar(i) * h_bar(i)^T) * (E(i-1) + Q(i))
  E_[0][0] = e00 * IKh[0][0] + E_[1][0] * IKh[0][1];
  E_[0][1] = e01 * IKh[0][0] + E_[1][1] * IKh[0][1];
  E_[1][0] = e00 * IKh[1][0] + E_[1][0] * IKh[1][1];
  E_[1][1] = e01 * IKh[1][0] + E_[1][1] * IKh[1][1];

  // Covariance matrix, must be positive semi-definite
  assert(E_[0][0] + E_[1][1] >= 0 &&
         E_[0][0] * E_[1][1] - E_[0][1] * E_[1][0] >= 0 &&
         E_[0][0] >= 0);

  // theta_bar(i+1) = theta_bar(i) + u_bar(i)
  // theta_bar(i) = [1/C[i], m(i)] = [slope_, offset_]

  // where u_bar(i) is the state noise that we model as a stationary
  // process with Gaussian statistic with zero mean and covariance
  // Q(i) = E{u_bar(i) * u_bar(i)^T}
  // Q(i) is RECOMMENDED as a diagonal matrix with main diagonal elements
  // as:
  // diag(Q(i)) = [10^-13 10^-3]^T

  slope_ = slope_ + K[0] * residual;
  prev_offset_ = offset_;
  offset_ = offset_ + K[1] * residual;

  Detect(ts_delta);
}

//  请参考草案定义看UpdateNoiseEstimate 的实现

void OveruseDetector::UpdateNoiseEstimate(double residual, double ts_delta, bool stable_state)
{
  if (!stable_state) {
    return;
  }
  // Faster filter during startup to faster adapt to the jitter level
  // of the network alpha is tuned for 30 frames per second, but
  double alpha = 0.01;
  if (num_of_deltas_ > 10*30) {
    alpha = 0.002;
  }
  // Only update the noise estimate if we're not over-using
  // beta is a function of alpha and the time delta since
  // the previous update.

  // We define the inter-arrival time, t(i) - t(i-1), as the difference in
  // arrival time of two packets or two groups of packets.
  // Correspondingly, the inter-departure time, T(i) - T(i-1), is defined
  // as the difference in departure-time of two packets or two groups of
  // packets.
  // i.e., t(i) - t(i-1) is difference in arrival time of two packets;
  // and T(i) - T(i-1) is difference in rtp timestamp of two packets,

  // beta = (1-chi)^(30/(1000 * f_max))
  // where f_max = max {1/(T(j) - T(j-1))} for j in i-K+1,...,i is the
  // highest rate at which the last K packet groups have been received and
  // chi is a filter coefficient typically chosen as a number in the
 // interval [0.1, 0.001]. 

  const double beta = pow(1 - alpha, ts_delta * 30.0 / 1000.0);
  avg_noise_ = beta * avg_noise_  + (1 - beta) * residual;

 

  // var_v_hat(i) = max(beta * var_v_hat(i-1) + (1-beta) * z(i)^2, 1)
 
  var_noise_ = beta * var_noise_  + (1 - beta) * (avg_noise_ - residual) * (avg_noise_ - residual);
  if (var_noise_ < 1e-7) { var_noise_ = 1e-7; }
}