Compare commits
1 Commits
Author | SHA1 | Date |
---|---|---|
J. David Lee | e6340ab5d3 |
|
@ -0,0 +1,66 @@
|
||||||
|
package extractfreq
|
||||||
|
|
||||||
|
import (
|
||||||
|
"math"
|
||||||
|
|
||||||
|
"gonum.org/v1/gonum/optimize"
|
||||||
|
)
|
||||||
|
|
||||||
|
func harmonicModel(A, B, n float64) float64 {
|
||||||
|
return A * n * math.Sqrt(B*n*n+1)
|
||||||
|
}
|
||||||
|
|
||||||
|
// Fit spectrum peaks to function:
|
||||||
|
//
|
||||||
|
// f(n) = A*n * sqrt(1+B*n**2)
|
||||||
|
func fitSpectrumPeaks(
|
||||||
|
f1 float64, // Approximate fundamental.
|
||||||
|
ns []float64,
|
||||||
|
freqs []float64, // Peak frequencies.
|
||||||
|
amps []float64, // Peak amplitudes.
|
||||||
|
) (
|
||||||
|
A float64,
|
||||||
|
B float64,
|
||||||
|
err error,
|
||||||
|
) {
|
||||||
|
// Error function weights each harmonic error by its amplitude.
|
||||||
|
errFunc := func(x []float64) float64 {
|
||||||
|
A := math.Abs(x[0])
|
||||||
|
B := math.Abs(x[1])
|
||||||
|
|
||||||
|
sum := float64(0)
|
||||||
|
|
||||||
|
for i, fPeak := range freqs {
|
||||||
|
n := ns[i]
|
||||||
|
e := fPeak - harmonicModel(A, B, n)
|
||||||
|
e *= e
|
||||||
|
e *= amps[i]
|
||||||
|
sum += e
|
||||||
|
}
|
||||||
|
|
||||||
|
return sum
|
||||||
|
}
|
||||||
|
|
||||||
|
// Initial guess.
|
||||||
|
x := []float64{f1, 0}
|
||||||
|
|
||||||
|
problem := optimize.Problem{
|
||||||
|
Func: errFunc,
|
||||||
|
}
|
||||||
|
|
||||||
|
result, err := optimize.Minimize(problem, x, nil, nil)
|
||||||
|
|
||||||
|
return result.X[0], result.X[1], err
|
||||||
|
}
|
||||||
|
|
||||||
|
func ApproxHarmonicNumber(fFun, fPeak float64) float64 {
|
||||||
|
if fPeak < fFun {
|
||||||
|
switch math.Round(fFun / fPeak) {
|
||||||
|
case 2:
|
||||||
|
return 0.5
|
||||||
|
case 4:
|
||||||
|
return 0.25
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return math.Round(fPeak / fFun)
|
||||||
|
}
|
|
@ -0,0 +1 @@
|
||||||
|
package extractfreq
|
|
@ -0,0 +1,118 @@
|
||||||
|
package extractfreq
|
||||||
|
|
||||||
|
import "gonum.org/v1/gonum/floats"
|
||||||
|
|
||||||
|
type mmItem struct {
|
||||||
|
Val float64
|
||||||
|
MaxVal float64
|
||||||
|
}
|
||||||
|
|
||||||
|
type moveMax struct {
|
||||||
|
pushStack []mmItem
|
||||||
|
popStack []mmItem
|
||||||
|
}
|
||||||
|
|
||||||
|
func newMoveMax() *moveMax {
|
||||||
|
return &moveMax{
|
||||||
|
pushStack: []mmItem{},
|
||||||
|
popStack: []mmItem{},
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
func (mm *moveMax) Max() float64 {
|
||||||
|
pushTop := len(mm.pushStack) - 1
|
||||||
|
popTop := len(mm.popStack) - 1
|
||||||
|
|
||||||
|
if pushTop == -1 {
|
||||||
|
item := mm.popStack[popTop]
|
||||||
|
return item.MaxVal
|
||||||
|
} else if popTop == -1 {
|
||||||
|
item := mm.pushStack[pushTop]
|
||||||
|
return item.MaxVal
|
||||||
|
}
|
||||||
|
|
||||||
|
item1 := mm.pushStack[pushTop]
|
||||||
|
item2 := mm.popStack[popTop]
|
||||||
|
if item1.MaxVal > item2.MaxVal {
|
||||||
|
return item1.MaxVal
|
||||||
|
} else {
|
||||||
|
return item2.MaxVal
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
func (mm *moveMax) push(stack []mmItem, val float64) []mmItem {
|
||||||
|
item := mmItem{
|
||||||
|
Val: val,
|
||||||
|
MaxVal: val,
|
||||||
|
}
|
||||||
|
|
||||||
|
// Ensure max is set correctly.
|
||||||
|
pushTop := len(stack) - 1
|
||||||
|
if pushTop > -1 {
|
||||||
|
top := stack[pushTop]
|
||||||
|
if top.MaxVal > item.MaxVal {
|
||||||
|
item.MaxVal = top.MaxVal
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Push.
|
||||||
|
return append(stack, item)
|
||||||
|
}
|
||||||
|
|
||||||
|
func (mm *moveMax) Push(val float64) {
|
||||||
|
mm.pushStack = mm.push(mm.pushStack, val)
|
||||||
|
}
|
||||||
|
|
||||||
|
func (mm *moveMax) Pop() {
|
||||||
|
if len(mm.popStack) == 0 {
|
||||||
|
// Move push stack onto pop stack.
|
||||||
|
for len(mm.pushStack) > 0 {
|
||||||
|
item := mm.pushStack[len(mm.pushStack)-1]
|
||||||
|
mm.pushStack = mm.pushStack[:len(mm.pushStack)-1]
|
||||||
|
mm.popStack = mm.push(mm.popStack, item.Val)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
mm.popStack = mm.popStack[:len(mm.popStack)-1]
|
||||||
|
}
|
||||||
|
|
||||||
|
func getPeaks(nHalfWin int, data []float64) (inds []int, amps []float64) {
|
||||||
|
mm := newMoveMax()
|
||||||
|
nWin := 2*nHalfWin + 1
|
||||||
|
|
||||||
|
if len(data) < nWin {
|
||||||
|
nWin = len(data)
|
||||||
|
}
|
||||||
|
|
||||||
|
// Push nWin samples.
|
||||||
|
for i := range data[:nWin] {
|
||||||
|
mm.Push(data[i])
|
||||||
|
}
|
||||||
|
|
||||||
|
for idxPush := nWin + 1; idxPush < len(data); idxPush++ {
|
||||||
|
mm.Pop()
|
||||||
|
mm.Push(data[idxPush])
|
||||||
|
idx := idxPush - nHalfWin
|
||||||
|
max := mm.Max()
|
||||||
|
if data[idx] == max && data[idx+1] != max {
|
||||||
|
inds = append(inds, idx)
|
||||||
|
amps = append(amps, data[idx])
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return
|
||||||
|
}
|
||||||
|
|
||||||
|
func sortPeaks(inds []int, amps []float64) ([]int, []float64) {
|
||||||
|
sort := make([]int, len(amps))
|
||||||
|
out := make([]int, len(amps))
|
||||||
|
floats.Argsort(amps, sort)
|
||||||
|
for i, idx := range sort {
|
||||||
|
out[i] = inds[idx]
|
||||||
|
}
|
||||||
|
|
||||||
|
for i := 0; i < len(inds)/2; i++ {
|
||||||
|
out[i], out[len(out)-1-i] = out[len(out)-1-i], out[i]
|
||||||
|
amps[i], amps[len(amps)-1-i] = amps[len(amps)-1-i], amps[i]
|
||||||
|
}
|
||||||
|
return out, amps
|
||||||
|
}
|
|
@ -7,6 +7,7 @@ import (
|
||||||
"math/cmplx"
|
"math/cmplx"
|
||||||
|
|
||||||
"git.crumpington.com/public/jlaudio/lib/flac"
|
"git.crumpington.com/public/jlaudio/lib/flac"
|
||||||
|
"git.crumpington.com/public/jlaudio/lib/util"
|
||||||
"gonum.org/v1/gonum/fourier"
|
"gonum.org/v1/gonum/fourier"
|
||||||
"gonum.org/v1/plot"
|
"gonum.org/v1/plot"
|
||||||
"gonum.org/v1/plot/plotter"
|
"gonum.org/v1/plot/plotter"
|
||||||
|
@ -48,54 +49,169 @@ func PlotFFT(path string, nSamples int, fFundamental float64) {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Trim to frequencies of interest.
|
// Trim to frequencies of interest.
|
||||||
|
freq0 := fFundamental / 8.0
|
||||||
|
freq1 := 21 * fFundamental
|
||||||
|
if freq1 > 20000 {
|
||||||
|
freq1 = 20000
|
||||||
|
}
|
||||||
|
|
||||||
deltaF := freqs[1] - freqs[0]
|
deltaF := freqs[1] - freqs[0]
|
||||||
idx0 := int(20 / deltaF)
|
idx0 := int(freq0 / deltaF)
|
||||||
idx1 := int(16000 / deltaF)
|
idx1 := int(freq1 / deltaF)
|
||||||
freqs = freqs[idx0:idx1]
|
freqs = freqs[idx0:idx1]
|
||||||
amps = amps[idx0:idx1]
|
amps = amps[idx0:idx1]
|
||||||
|
|
||||||
// Multiply by A-weight function.
|
// Multiply by A-weight function, then normalize.
|
||||||
|
ampMax := float64(0)
|
||||||
for i := range amps {
|
for i := range amps {
|
||||||
amps[i] *= AWeight(freqs[i])
|
amps[i] *= AWeight(freqs[i])
|
||||||
|
if amps[i] > ampMax {
|
||||||
|
ampMax = amps[i]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for i := range amps {
|
||||||
|
if amps[i] > 0 {
|
||||||
|
amps[i] /= ampMax
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Find peaks.
|
// Find peaks.
|
||||||
dFreq := freqs[1] - freqs[0]
|
halfWinSize := int(math.Round((fFundamental / deltaF) / 3))
|
||||||
halfWinSize := int(math.Round((fFundamental / dFreq) / 3))
|
|
||||||
peakInds, peakAmps := getPeaks(halfWinSize, amps)
|
peakInds, peakAmps := getPeaks(halfWinSize, amps)
|
||||||
peakInds, peakAmps = sortPeaks(peakInds, peakAmps)
|
peakFreqs := make([]float64, len(peakInds))
|
||||||
|
for i, idx := range peakInds {
|
||||||
|
peakFreqs[i] = freqs[idx]
|
||||||
|
}
|
||||||
|
|
||||||
|
// First, determine harmonic numbers of each peak.
|
||||||
|
ns := make([]float64, len(peakFreqs))
|
||||||
|
for i, f := range peakFreqs {
|
||||||
|
ns[i] = ApproxHarmonicNumber(fFundamental, f)
|
||||||
|
}
|
||||||
|
|
||||||
|
// --------------------------------------------------------------------------
|
||||||
|
|
||||||
|
// First fit.
|
||||||
|
|
||||||
|
log.Printf("First fit...")
|
||||||
|
A, B, err := fitSpectrumPeaks(fFundamental, ns, peakFreqs, peakAmps)
|
||||||
|
if err != nil {
|
||||||
|
panic(err)
|
||||||
|
}
|
||||||
|
|
||||||
|
log.Printf("A: %.4f B: %0.4f", A, B)
|
||||||
|
|
||||||
p, err := plot.New()
|
p, err := plot.New()
|
||||||
if err != nil {
|
if err != nil {
|
||||||
panic(err)
|
panic(err)
|
||||||
}
|
}
|
||||||
|
|
||||||
fMin := float64(20)
|
ptsModel := make(plotter.XYs, len(ns))
|
||||||
fMax := float64(16000)
|
ptsData := make(plotter.XYs, len(ns))
|
||||||
|
for i, n := range ns {
|
||||||
|
ptsModel[i].X = n
|
||||||
|
ptsModel[i].Y = harmonicModel(A, B, n)
|
||||||
|
ptsData[i].X = n
|
||||||
|
ptsData[i].Y = peakFreqs[i]
|
||||||
|
}
|
||||||
|
|
||||||
|
if err = plotutil.AddScatters(p, "model", ptsModel, "data", ptsData); err != nil {
|
||||||
|
panic(err)
|
||||||
|
}
|
||||||
|
|
||||||
|
p.Add(plotter.NewGrid())
|
||||||
|
|
||||||
|
log.Printf("Saving...")
|
||||||
|
if err := p.Save(16*vg.Inch, 8*vg.Inch, "fit1.png"); err != nil {
|
||||||
|
panic(err)
|
||||||
|
}
|
||||||
|
|
||||||
|
// Remove peaks w/ larger errors:
|
||||||
|
// TODO: Method?
|
||||||
|
iOut := 0
|
||||||
|
for i := range ns {
|
||||||
|
f := peakFreqs[i]
|
||||||
|
fModel := harmonicModel(A, B, ns[i])
|
||||||
|
deltaF := math.Abs(f - fModel)
|
||||||
|
maxDeltaF := 20 * util.CentDeltaFreq(fModel)
|
||||||
|
if deltaF < maxDeltaF {
|
||||||
|
ns[iOut] = ns[i]
|
||||||
|
peakInds[iOut] = peakInds[i]
|
||||||
|
peakFreqs[iOut] = peakFreqs[i]
|
||||||
|
peakAmps[iOut] = peakAmps[i]
|
||||||
|
iOut++
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
ns = ns[:iOut]
|
||||||
|
peakInds = peakInds[:iOut]
|
||||||
|
peakFreqs = peakFreqs[:iOut]
|
||||||
|
peakAmps = peakAmps[:iOut]
|
||||||
|
|
||||||
|
// Second fit.
|
||||||
|
|
||||||
|
A, B, err = fitSpectrumPeaks(A, ns, peakFreqs, peakAmps)
|
||||||
|
if err != nil {
|
||||||
|
panic(err)
|
||||||
|
}
|
||||||
|
|
||||||
|
log.Printf("A: %.4f B: %0.4f", A, B)
|
||||||
|
|
||||||
|
p, err = plot.New()
|
||||||
|
if err != nil {
|
||||||
|
panic(err)
|
||||||
|
}
|
||||||
|
|
||||||
|
ptsModel = make(plotter.XYs, len(ns))
|
||||||
|
ptsData = make(plotter.XYs, len(ns))
|
||||||
|
for i, n := range ns {
|
||||||
|
ptsModel[i].X = n
|
||||||
|
ptsModel[i].Y = harmonicModel(A, B, n)
|
||||||
|
ptsData[i].X = n
|
||||||
|
ptsData[i].Y = peakFreqs[i]
|
||||||
|
}
|
||||||
|
|
||||||
|
if err = plotutil.AddScatters(p, "model", ptsModel, "data", ptsData); err != nil {
|
||||||
|
panic(err)
|
||||||
|
}
|
||||||
|
|
||||||
|
p.Add(plotter.NewGrid())
|
||||||
|
|
||||||
|
log.Printf("Saving...")
|
||||||
|
if err := p.Save(16*vg.Inch, 8*vg.Inch, "fit2.png"); err != nil {
|
||||||
|
panic(err)
|
||||||
|
}
|
||||||
|
|
||||||
|
// --------------------------------------------------------------------------
|
||||||
|
|
||||||
|
p, err = plot.New()
|
||||||
|
if err != nil {
|
||||||
|
panic(err)
|
||||||
|
}
|
||||||
|
|
||||||
log.Printf("Making points...")
|
log.Printf("Making points...")
|
||||||
pts := make(plotter.XYs, 0, len(amps))
|
pts := make(plotter.XYs, 0, len(amps))
|
||||||
for i := range amps {
|
for i := range amps {
|
||||||
if freqs[i] > fMin && freqs[i] < fMax {
|
pts = append(pts, plotter.XY{
|
||||||
pts = append(pts, plotter.XY{
|
X: freqs[i],
|
||||||
X: freqs[i],
|
Y: math.Log(amps[i]),
|
||||||
Y: amps[i],
|
})
|
||||||
})
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
pts2 := make(plotter.XYs, 0, len(amps))
|
pts2 := make(plotter.XYs, 0, len(amps))
|
||||||
for i := range peakInds {
|
for i := range peakInds {
|
||||||
if freqs[peakInds[i]] > fMin && freqs[peakInds[i]] < fMax {
|
pts2 = append(pts2, plotter.XY{
|
||||||
pts2 = append(pts2, plotter.XY{
|
X: freqs[peakInds[i]],
|
||||||
X: freqs[peakInds[i]],
|
Y: math.Log(peakAmps[i]),
|
||||||
Y: peakAmps[i],
|
})
|
||||||
})
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
log.Printf("Plotting...")
|
log.Printf("Plotting...")
|
||||||
if err = plotutil.AddScatters(p, "fft", pts, "max", pts2); err != nil {
|
if err = plotutil.AddLines(p, "fft", pts); err != nil {
|
||||||
|
panic(err)
|
||||||
|
}
|
||||||
|
if err = plotutil.AddScatters(p, "max", pts2); err != nil {
|
||||||
panic(err)
|
panic(err)
|
||||||
}
|
}
|
||||||
p.Add(plotter.NewGrid())
|
p.Add(plotter.NewGrid())
|
||||||
|
@ -106,7 +222,7 @@ func PlotFFT(path string, nSamples int, fFundamental float64) {
|
||||||
}
|
}
|
||||||
|
|
||||||
for i, idx := range peakInds {
|
for i, idx := range peakInds {
|
||||||
//amp := peakAmps[i]
|
n := ns[i]
|
||||||
|
|
||||||
// Make window.
|
// Make window.
|
||||||
ampsWin := amps[idx-halfWinSize : idx+halfWinSize]
|
ampsWin := amps[idx-halfWinSize : idx+halfWinSize]
|
||||||
|
@ -121,6 +237,15 @@ func PlotFFT(path string, nSamples int, fFundamental float64) {
|
||||||
})
|
})
|
||||||
}
|
}
|
||||||
|
|
||||||
|
pts2 := plotter.XYs{
|
||||||
|
{
|
||||||
|
X: harmonicModel(A, B, n),
|
||||||
|
Y: peakAmps[i],
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
log.Printf("%f, %f", peakFreqs[i], harmonicModel(A, B, n))
|
||||||
|
|
||||||
log.Printf("Plotting...")
|
log.Printf("Plotting...")
|
||||||
p, err := plot.New()
|
p, err := plot.New()
|
||||||
if err != nil {
|
if err != nil {
|
||||||
|
@ -130,12 +255,12 @@ func PlotFFT(path string, nSamples int, fFundamental float64) {
|
||||||
if err = plotutil.AddLines(p, "fft", pts); err != nil {
|
if err = plotutil.AddLines(p, "fft", pts); err != nil {
|
||||||
panic(err)
|
panic(err)
|
||||||
}
|
}
|
||||||
|
if err = plotutil.AddScatters(p, "model", pts2); err != nil {
|
||||||
//p.Y.Scale = plot.LogScale{}
|
panic(err)
|
||||||
//p.X.Scale = plot.LogScale{}
|
}
|
||||||
|
|
||||||
log.Printf("Saving...")
|
log.Printf("Saving...")
|
||||||
filename := fmt.Sprintf("plot-%03d.png", i)
|
filename := fmt.Sprintf("plot-%03f.png", n)
|
||||||
if err := p.Save(8*vg.Inch, 8*vg.Inch, filename); err != nil {
|
if err := p.Save(8*vg.Inch, 8*vg.Inch, filename); err != nil {
|
||||||
panic(err)
|
panic(err)
|
||||||
}
|
}
|
||||||
|
|
|
@ -3,5 +3,5 @@ package extractfreq
|
||||||
import "testing"
|
import "testing"
|
||||||
|
|
||||||
func TestPlotFFT(t *testing.T) {
|
func TestPlotFFT(t *testing.T) {
|
||||||
PlotFFT("test_files/440.flac", 96000, 440)
|
PlotFFT("test_files/440.flac", 48000, 440)
|
||||||
}
|
}
|
||||||
|
|
Reference in New Issue