Improve the algo to calculate good mean frequency

Summary: Calculate good mean frequency based on the points where offset was within the range (new filter config option, default to 100ns). It takes frequency into account only when current and previous offsets were in the range. Only in this case we can say that the last frequency was not breaking PHC, or it was stable enough to be used in corner cases.

Reviewed By: abulimov

Differential Revision: D52788332

fbshipit-source-id: 5b2038c74a661b0332dacbfb7fb2d47c8301881c

servo/pi.go

@@ -60,6 +60,7 @@ type PiServoFilterCfg struct {
 	maxFreqChange     int64   // The amount of ppb the oscillator can drift per 1s
 	maxSkipCount      int     // The amount of samples to skip via filter
 	maxOffsetInit     int64   // The initial value above which sample is treated as outlier
+	offsetRange       int64   // The range of values within which we consider the sample as valid
 	offsetStdevFactor float64 // Standard deviation factor for offset stddev calculations
 	freqStdevFactor   float64 // Standard deviation factor for frequency stddev calculations
 	ringSize          int     // The amount of samples we have to collect to activate filter
@@ -73,16 +74,19 @@ type PiServoFilterSample struct {
 
 // PiServoFilter is a filter state structure
 type PiServoFilter struct {
-	offsetStdev   int64
-	offsetSigmaSq int64
-	offsetMean    int64
-	freqStdev     float64
-	freqSigmaSq   float64
-	freqMean      float64
-	skippedCount  int
-	samples       *ring.Ring
-	samplesCount  int
-	cfg           *PiServoFilterCfg
+	offsetStdev        int64
+	offsetSigmaSq      int64
+	offsetMean         int64
+	lastOffset         int64
+	freqStdev          float64
+	freqSigmaSq        float64
+	freqMean           float64
+	skippedCount       int
+	offsetSamples      *ring.Ring
+	offsetSamplesCount int
+	freqSamples        *ring.Ring
+	freqSamplesCount   int
+	cfg                *PiServoFilterCfg
 }
 
 // PiServo is an integral servo
@@ -252,7 +256,7 @@ func (f *PiServoFilter) isSpike(offset int64, lastCorrection time.Time) filterSt
 	if f.skippedCount >= f.cfg.maxSkipCount {
 		return filterReset
 	}
-	if f.samplesCount != f.cfg.ringSize {
+	if f.offsetSamplesCount != f.cfg.ringSize {
 		return filterNoSpike
 	}
 	maxOffsetLocked := int64(f.cfg.offsetStdevFactor * float64(f.offsetStdev))
@@ -267,43 +271,78 @@ func (f *PiServoFilter) isSpike(offset int64, lastCorrection time.Time) filterSt
 	return filterNoSpike
 }
 
+func inRange(value, min, max int64) bool {
+	if value >= min && value <= max {
+		return true
+	}
+	return false
+}
+
 // Sample to add a sample to filter and recalculate value
 func (f *PiServoFilter) Sample(s *PiServoFilterSample) {
-	f.samples.Value = s
-	f.samples = f.samples.Next()
-	if f.samplesCount != f.cfg.ringSize {
-		f.samplesCount++
+	f.offsetSamples.Value = s
+	f.offsetSamples = f.offsetSamples.Next()
+	if f.offsetSamplesCount != f.cfg.ringSize {
+		f.offsetSamplesCount++
 	}
 	var offsetSigmaSq, offsetMean int64
-	var freqSigmaSq, freqMean float64
-	f.samples.Do(func(val any) {
+	var freqSigmaSq float64
+	f.offsetSamples.Do(func(val any) {
 		if val == nil {
 			return
 		}
 		v := val.(*PiServoFilterSample)
 		offsetSigmaSq += v.offset * v.offset
 		offsetMean += v.offset
 		freqSigmaSq += v.freq * v.freq
-		freqMean += v.freq
 	})
-	f.offsetMean = offsetMean / int64(f.samplesCount)
-	f.offsetStdev = int64(math.Sqrt(float64(offsetSigmaSq) / float64(f.samplesCount)))
-
-	f.freqMean = freqMean / float64(f.samplesCount)
-	f.freqStdev = math.Sqrt(freqSigmaSq / float64(f.samplesCount))
+	f.offsetMean = offsetMean / int64(f.offsetSamplesCount)
+	f.offsetStdev = int64(math.Sqrt(float64(offsetSigmaSq) / float64(f.offsetSamplesCount)))
+	f.freqStdev = math.Sqrt(freqSigmaSq / float64(f.offsetSamplesCount))
+
+	/*
+	 * Mean frequency is heavily affected by the values used to compensate for offsets in case of
+	 * recovering after holdover state. If we have to go to holdover again while recovering from
+	 * previous holdover, we may apply bad frequency which will cause PHC going off pretty fast.
+	 * Let's calculate mean frequency only when we are sure that PHC is running more or less stable.
+	 */
+	if inRange(f.lastOffset, -f.cfg.offsetRange, f.cfg.offsetRange) && inRange(s.offset, -f.cfg.offsetRange, f.cfg.offsetRange) {
+		var freqMean float64
+		f.freqSamples.Value = s
+		f.freqSamples = f.freqSamples.Next()
+		if f.freqSamplesCount != f.cfg.ringSize {
+			f.freqSamplesCount++
+		}
+		f.freqSamples.Do(func(val any) {
+			if val == nil {
+				return
+			}
+			v := val.(*PiServoFilterSample)
+			freqMean += v.freq
+		})
+		f.freqMean = freqMean / float64(f.freqSamplesCount)
+	}
+	f.lastOffset = s.offset
 }
 
 // Reset - cleanup and restart filter
 func (f *PiServoFilter) Reset() {
-	f.samples = ring.New(f.cfg.ringSize)
+	f.offsetSamples = ring.New(f.cfg.ringSize)
+	f.freqSamples = ring.New(f.cfg.ringSize)
 	f.offsetStdev = 0
 	f.offsetSigmaSq = 0
 	f.offsetMean = 0
 	f.freqStdev = 0.0
 	f.freqSigmaSq = 0.0
 	f.freqMean = 0.0
 	f.skippedCount = 0
-	f.samplesCount = 0
+	f.offsetSamplesCount = 0
+	f.freqSamplesCount = 0
+}
+
+// HasMeanFreq to check if filter has enough samples to calculate mean frequency
+func (f *PiServoFilter) HasMeanFreq() bool {
+	return f.freqSamplesCount != 0
 }
 
 // MeanFreq to return best calculated frequency
@@ -313,7 +352,7 @@ func (f *PiServoFilter) MeanFreq() float64 {
 
 // MeanFreq to return best calculated frequency from filter
 func (s *PiServo) MeanFreq() float64 {
-	if s.filter != nil {
+	if s.filter != nil && s.filter.HasMeanFreq() {
 		return s.filter.MeanFreq()
 	}
 	return s.lastFreq
@@ -362,6 +401,7 @@ func DefaultPiServoFilterCfg() *PiServoFilterCfg {
 		maxFreqChange:     40,
 		maxSkipCount:      15,
 		maxOffsetInit:     500000,
+		offsetRange:       100, // the range of the offset values that are considered "normal"
 		offsetStdevFactor: 3.0,
 		freqStdevFactor:   3.0,
 		ringSize:          30,

servo/pi_test.go

@@ -90,6 +90,7 @@ func TestPiServoFilterSample(t *testing.T) {
 	piFilterCfg := DefaultPiServoFilterCfg()
 	piFilterCfg.ringSize = 3
 	piFilterCfg.maxSkipCount = 2
+	piFilterCfg.offsetRange = 100000
 	f := NewPiServoFilter(pi, piFilterCfg)
 
 	require.InEpsilon(t, -111288.406372, pi.lastFreq, 0.00001)
@@ -176,3 +177,52 @@ func TestPiServoSetFreq(t *testing.T) {
 	require.InEpsilon(t, 11111.0025, pi.lastFreq, 0.00001)
 	require.InEpsilon(t, 11111.0025, pi.drift, 0.00001)
 }
+
+func TestPiServoFilterMeanFreq(t *testing.T) {
+	pi := NewPiServo(DefaultServoConfig(), DefaultPiServoCfg(), -111288.406372)
+	pi.SyncInterval(1)
+	piFilterCfg := DefaultPiServoFilterCfg()
+	piFilterCfg.ringSize = 3
+	piFilterCfg.maxSkipCount = 2
+	piFilterCfg.offsetRange = 1000
+	f := NewPiServoFilter(pi, piFilterCfg)
+
+	require.InEpsilon(t, -111288.406372, pi.lastFreq, 0.00001)
+	require.InEpsilon(t, -111288.406372, pi.drift, 0.00001)
+
+	freq, state := pi.Sample(1191, 1674148530671467104)
+	require.InEpsilon(t, -111288.406372, freq, 0.00001)
+	require.Equal(t, StateInit, state)
+
+	freq, state = pi.Sample(225, 1674148531671518924)
+	require.InEpsilon(t, -112254.463816, freq, 0.00001)
+	require.Equal(t, StateLocked, state)
+
+	freq, state = pi.Sample(-170, 1674148532671555647)
+	require.InEpsilon(t, -112424.463816, freq, 0.00001)
+	require.Equal(t, StateLocked, state)
+
+	freq, state = pi.Sample(68, 1674148533671484215)
+	require.InEpsilon(t, -112237.463816, freq, 0.00001)
+	require.Equal(t, StateLocked, state)
+	require.Equal(t, 0, pi.filter.skippedCount)
+
+	freq, state = pi.Sample(919000, 1674148534671684215)
+	require.InEpsilon(t, -112305.463816, freq, 0.00001)
+	require.InEpsilon(t, f.freqMean, freq, 0.00001)
+	require.Equal(t, StateFilter, state)
+	require.Equal(t, 1, f.skippedCount)
+
+	freq = pi.MeanFreq()
+	require.InEpsilon(t, -112305.463816, freq, 0.00001)
+
+	freq, state = pi.Sample(1921000, 1674148535771674067)
+	require.InEpsilon(t, -112305.463816, freq, 0.00001)
+	require.Equal(t, StateFilter, state)
+	require.Equal(t, 2, f.skippedCount)
+
+	freq, state = pi.Sample(1921000, 1674148535771674067)
+	require.InEpsilon(t, -112305.463816, freq, 0.00001)
+	require.Equal(t, f.freqMean, 0.0)
+	require.Equal(t, StateInit, state)
+}