impact/filter.go at master · ozym/impact · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
package impact

import (
	"math"
)

type HighPass struct {
	a, b float64 // filter coeffs
	x, y float64 // previous input & output
}

func NewHighPass(gain float64, q float64) *HighPass {
	f := new(HighPass)

	f.a = (1.0 + q) / (2.0 * gain)
	f.b = q

	f.x = 0.0
	f.y = math.NaN()

	return f
}

func (f *HighPass) Reset() {
	f.y = math.NaN()
}

func (f *HighPass) Set(y float64) {
	f.y = y
}

func (f *HighPass) Sample(x float64) float64 {
	var y float64

	if math.IsNaN(f.y) {
		f.x = x
		f.y = 0.0
	}

	y = (f.a*(x-f.x) + f.b*f.y)

	f.x = x
	f.y = y

	return y
}

type Integrator struct {
	a, b float64 // filter coeffs
	x, y float64 // previous input & output
}

func NewIntegrator(gain float64, dt float64, q float64) *Integrator {
	f := new(Integrator)

	f.a = (1.0 + q) * dt / (4.0 * gain)
	f.b = q

	f.x = 0.0
	f.y = math.NaN()

	return f
}

func (f *Integrator) Reset() {
	f.y = math.NaN()
}

func (f *Integrator) Set(y float64) {
	f.y = y
}

func (f *Integrator) Sample(x float64) float64 {
	var y float64

	if math.IsNaN(f.y) {
		f.x = x
		f.y = 0.0
	}

	y = f.a*(x+f.x) + f.b*f.y

	f.x = x
	f.y = y

	return y
}