|
15 | 15 | */
|
16 | 16 | public class FastGaussCalculator
|
17 | 17 | {
|
18 |
| - private final Parameters fc; |
19 |
| - // we will utilize ring buffers whose current positions will be driven |
20 |
| - // by the currently processed index inside the input array -- we will use |
21 |
| - // bit masking to achieve fast the effect of the operation modulo, furthermore |
22 |
| - // it does not suffer from sign effect: -5 % 8 = -5 in Java; I need it be +3 |
23 |
| - // to make it work nicely for the ring buffer (which cannot have negative indices) |
24 |
| - // |
25 |
| - // remember current plus last two results for every yk-term (there's M of them), |
26 |
| - // so we need either 3*3 or 3*4=12 values => 4 bits = capacity for 16 values, |
27 |
| - // |
28 |
| - // actually, we will use 4 ring sub-buffers (each of length 4, 2 bits) for the |
29 |
| - // individual yks...: y1, y3, y5, y7 (the order in which they are touched during |
30 |
| - // the computation of the filter) |
31 |
| - // |
32 |
| - private final double[] yk0 = new double[ 4 ]; |
33 |
| - |
34 |
| - private final double[] yk1 = new double[ 4 ]; |
35 |
| - |
36 |
| - private final double[] yk2 = new double[ 4 ]; |
37 |
| - |
38 |
| - private final double[] yk3 = new double[ 4 ]; |
39 |
| - |
40 |
| - private int x; |
| 18 | + private double[] y_n = new double[ 4 ]; // stores y values for k=1,3,5,7 of the current step |
| 19 | + |
| 20 | + private double[] y_n_minus_1 = new double[ 4 ]; // stores the y_n value of the previous step |
| 21 | + |
| 22 | + private double[] y_n_minus_2 = new double[ 4 ]; // stores the y_n value of the step before the previous step |
| 23 | + |
| 24 | + private final double[] nk_2; |
| 25 | + |
| 26 | + private final double[] dk_1; |
| 27 | + |
| 28 | + private final int M; |
41 | 29 |
|
42 | 30 | public FastGaussCalculator( final Parameters fc )
|
43 | 31 | {
|
44 |
| - this.fc = fc; |
| 32 | + nk_2 = fc.nk_2; |
| 33 | + dk_1 = fc.dk_1; |
| 34 | + M = fc.M; |
45 | 35 | }
|
46 | 36 |
|
47 | 37 | public void initialize( final double boundaryValue )
|
48 | 38 | {
|
49 | 39 | //calculate yk that one would get on constant signal of 1.0
|
50 |
| - //(Vlado's invention by solving eq. (35) assuming yk_n = yk_n-1 = yk_n-2) |
51 |
| - final double y1_mN = 2.0 * fc.nk_2[ 0 ] / ( fc.dk_1[ 0 ] + 2.0 ); |
52 |
| - final double y3_mN = 2.0 * fc.nk_2[ 1 ] / ( fc.dk_1[ 1 ] + 2.0 ); |
53 |
| - final double y5_mN = 2.0 * fc.nk_2[ 2 ] / ( fc.dk_1[ 2 ] + 2.0 ); |
54 |
| - final double y7_mN = fc.M == 4 ? 2.0 * fc.nk_2[ 3 ] / ( fc.dk_1[ 3 ] + 2.0 ) : 0.0; |
55 |
| - |
56 |
| - //calculate yk that one would get on constant signal of array[0], |
57 |
| - //and use this value for yk[x-2] and yk[x-1] when x=-Nm1 |
58 |
| - x = -1; |
59 |
| - yk0[ x & 3 ] = yk0[ ( x - 1 ) & 3 ] = boundaryValue * y1_mN; |
60 |
| - yk1[ x & 3 ] = yk1[ ( x - 1 ) & 3 ] = boundaryValue * y3_mN; |
61 |
| - yk2[ x & 3 ] = yk2[ ( x - 1 ) & 3 ] = boundaryValue * y5_mN; |
62 |
| - yk3[ x & 3 ] = yk3[ ( x - 1 ) & 3 ] = boundaryValue * y7_mN; |
| 40 | + //(Vlado's invention by solving eq. (35) assuming y_n = y_n_minus_1 = y_n_minus_2) |
| 41 | + |
| 42 | + for ( int i = 0; i < M; i++ ) |
| 43 | + y_n[ i ] = y_n_minus_1[ i ] = boundaryValue * 2.0 * nk_2[ i ] / ( dk_1[ i ] + 2.0 ); |
63 | 44 | }
|
64 | 45 |
|
65 | 46 | public void update( final double tmp )
|
66 | 47 | {
|
67 |
| - ++x; |
68 |
| - yk0[ x & 3 ] = fc.nk_2[ 0 ] * tmp - fc.dk_1[ 0 ] * yk0[ ( x - 1 ) & 3 ] - yk0[ ( x - 2 ) & 3 ]; |
69 |
| - yk1[ x & 3 ] = fc.nk_2[ 1 ] * tmp - fc.dk_1[ 1 ] * yk1[ ( x - 1 ) & 3 ] - yk1[ ( x - 2 ) & 3 ]; |
70 |
| - yk2[ x & 3 ] = fc.nk_2[ 2 ] * tmp - fc.dk_1[ 2 ] * yk2[ ( x - 1 ) & 3 ] - yk2[ ( x - 2 ) & 3 ]; |
71 |
| - yk3[ x & 3 ] = fc.M == 4 ? fc.nk_2[ 3 ] * tmp - fc.dk_1[ 3 ] * yk3[ ( x - 1 ) & 3 ] - yk3[ ( x - 2 ) & 3 ] : 0; |
| 48 | + double[] t = y_n_minus_2; |
| 49 | + y_n_minus_2 = y_n_minus_1; |
| 50 | + y_n_minus_1 = y_n; |
| 51 | + y_n = t; |
| 52 | + y_n[ 0 ] = nk_2[ 0 ] * tmp - dk_1[ 0 ] * y_n_minus_1[ 0 ] - y_n_minus_2[ 0 ]; |
| 53 | + y_n[ 1 ] = nk_2[ 1 ] * tmp - dk_1[ 1 ] * y_n_minus_1[ 1 ] - y_n_minus_2[ 1 ]; |
| 54 | + y_n[ 2 ] = nk_2[ 2 ] * tmp - dk_1[ 2 ] * y_n_minus_1[ 2 ] - y_n_minus_2[ 2 ]; |
| 55 | + y_n[ 3 ] = nk_2[ 3 ] * tmp - dk_1[ 3 ] * y_n_minus_1[ 3 ] - y_n_minus_2[ 3 ]; |
72 | 56 | }
|
73 | 57 |
|
74 | 58 | public double getValue()
|
75 | 59 | {
|
76 |
| - return yk0[ x & 3 ] + yk1[ x & 3 ] + yk2[ x & 3 ] + yk3[ x & 3 ]; |
| 60 | + return y_n[ 0 ] + y_n[ 1 ] + y_n[ 2 ] + y_n[ 3 ]; |
77 | 61 | }
|
78 | 62 |
|
79 | 63 | /**
|
|
0 commit comments