-
Notifications
You must be signed in to change notification settings - Fork 14
/
biquad.hh
142 lines (134 loc) · 3.11 KB
/
biquad.hh
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
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
/*
Digital biquad filter
Copyright 2019 Ahmet Inan <[email protected]>
*/
#pragma once
#include "const.hh"
#include "decibel.hh"
#include "unit_circle.hh"
namespace DSP {
template <typename TYPE, typename VALUE>
class Biquad
{
TYPE x1, x2, y1, y2;
VALUE b0a0, b1a0, b2a0, a1a0, a2a0;
public:
constexpr Biquad() : x1(0), x2(0), y1(0), y2(0),
b0a0(1), b1a0(0), b2a0(0), a1a0(0), a2a0(0)
{
}
void lowpass(int n, int N, VALUE Q = Const<VALUE>::InvSqrtTwo())
{
VALUE alpha = UnitCircle<VALUE>::sin(n, N) / (VALUE(2) * Q),
cn = UnitCircle<VALUE>::cos(n, N),
b0 = (VALUE(1) - cn) / VALUE(2),
b1 = VALUE(1) - cn,
b2 = (VALUE(1) - cn) / VALUE(2),
a0 = VALUE(1) + alpha,
a1 = -VALUE(2) * cn,
a2 = VALUE(1) - alpha;
b0a0 = b0 / a0;
b1a0 = b1 / a0;
b2a0 = b2 / a0;
a1a0 = a1 / a0;
a2a0 = a2 / a0;
}
void highpass(int n, int N, VALUE Q = Const<VALUE>::InvSqrtTwo())
{
VALUE alpha = UnitCircle<VALUE>::sin(n, N) / (VALUE(2) * Q),
cn = UnitCircle<VALUE>::cos(n, N),
b0 = (VALUE(1) + cn) / VALUE(2),
b1 = -(VALUE(1) + cn),
b2 = (VALUE(1) + cn) / VALUE(2),
a0 = VALUE(1) + alpha,
a1 = -VALUE(2) * cn,
a2 = VALUE(1) - alpha;
b0a0 = b0 / a0;
b1a0 = b1 / a0;
b2a0 = b2 / a0;
a1a0 = a1 / a0;
a2a0 = a2 / a0;
}
void notch(int n, int N, VALUE QdB)
{
VALUE alpha = UnitCircle<VALUE>::sin(n, N) / (VALUE(2) * idecibel(QdB)),
cn = UnitCircle<VALUE>::cos(n, N),
b0 = VALUE(1),
b1 = -VALUE(2) * cn,
b2 = VALUE(1),
a0 = VALUE(1) + alpha,
a1 = -VALUE(2) * cn,
a2 = VALUE(1) - alpha;
b0a0 = b0 / a0;
b1a0 = b1 / a0;
b2a0 = b2 / a0;
a1a0 = a1 / a0;
a2a0 = a2 / a0;
}
void bandpass(int n, int N, VALUE QdB)
{
VALUE alpha = UnitCircle<VALUE>::sin(n, N) / (VALUE(2) * idecibel(QdB)),
cn = UnitCircle<VALUE>::cos(n, N),
b0 = alpha,
b1 = VALUE(0),
b2 = -alpha,
a0 = VALUE(1) + alpha,
a1 = -VALUE(2) * cn,
a2 = VALUE(1) - alpha;
b0a0 = b0 / a0;
b1a0 = b1 / a0;
b2a0 = b2 / a0;
a1a0 = a1 / a0;
a2a0 = a2 / a0;
}
void allpass(int n, int N, VALUE QdB)
{
VALUE alpha = UnitCircle<VALUE>::sin(n, N) / (VALUE(2) * idecibel(QdB)),
cn = UnitCircle<VALUE>::cos(n, N),
b0 = VALUE(1) - alpha,
b1 = -VALUE(2) * cn,
b2 = VALUE(1) + alpha,
a0 = VALUE(1) + alpha,
a1 = -VALUE(2) * cn,
a2 = VALUE(1) - alpha;
b0a0 = b0 / a0;
b1a0 = b1 / a0;
b2a0 = b2 / a0;
a1a0 = a1 / a0;
a2a0 = a2 / a0;
}
TYPE operator()(TYPE x0)
{
TYPE y0 = b0a0*x0 + b1a0*x1 + b2a0*x2
- a1a0*y1 - a2a0*y2;
x2 = x1; x1 = x0;
y2 = y1; y1 = y0;
return y0;
}
};
template <typename TYPE, typename VALUE, int NUM = 1>
class BiquadCascade
{
static const int ORDER = 2 * NUM;
Biquad<TYPE, VALUE> cascade[NUM];
public:
void lowpass(int n, int N)
{
for (int i = 0; i < NUM; ++i)
cascade[i].lowpass(n, N, VALUE(1) / (VALUE(2) *
UnitCircle<VALUE>::cos(2*i+1, 4*ORDER)));
}
void highpass(int n, int N)
{
for (int i = 0; i < NUM; ++i)
cascade[i].highpass(n, N, VALUE(1) / (VALUE(2) *
UnitCircle<VALUE>::cos(2*i+1, 4*ORDER)));
}
TYPE operator()(TYPE input)
{
for (int i = 0; i < NUM; ++i)
input = cascade[i](input);
return input;
}
};
}