ssa

1
package savgol
2

3
import (
4
	"fmt"
5
	"math"
6

7
	"github.com/RB-PRO/ssa/pkg/oss"
8
	"github.com/mjibson/go-dsp/fft"
9
	"github.com/pkg/errors"
10
	"gonum.org/v1/gonum/blas"
11
	"gonum.org/v1/gonum/floats"
12
	"gonum.org/v1/gonum/lapack/lapack64"
13
	"gonum.org/v1/gonum/mat"
14
)
15

16
// SavGolFilter implements Savitzky-Golay filter (https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.savgol_filter.html)
17
// based on: https://github.com/scipy/scipy/blob/v1.3.0rc1/scipy/signal/_savitzky_golay.py#L227
18
func SavGolFilter(x []float64, window_length int, polyorder int, deriv int /*=0*/, delta float64 /*=1.0*/) []float64 {
19
	// computing filter coefficients
20
	// the outputs of this step seem to be numerically same as the Python code ones
21
	coeffs := SavGolCoeffs(window_length, polyorder, deriv, delta, true)
22
	// convolving the original signal with the filter coefficients
23
	// note: the outputs of this step are not completely numerically same as the Python code ones (because the latter uses different convolution function)
24

25
	if len(x) < window_length {
26
		return nil
27
	}
28

29
	convolutionOutput := Convolve1d(x, coeffs)
30

31
	result := make([]float64, 0)
32
	for i := 0; i < len(convolutionOutput); i++ {
33
		result = append(result, real(convolutionOutput[i]))
34
	}
35
	return result
36
}
37

38
func Convolve1d(x []float64, coeffs []float64) []complex128 {
39
	xFFt := fft.FFTReal(x)
40
	coefftsFFt := fft.FFTReal(coeffs)
41

42
	if len(xFFt) != len(coefftsFFt) {
43
		for i := len(coefftsFFt); i < len(xFFt); i++ {
44
			coefftsFFt = append(coefftsFFt, 0+0i)
45
		}
46
	}
47
	matrixProduct := make([]complex128, 0)
48
	for i := 0; i < len(xFFt); i++ {
49
		matrixProduct = append(matrixProduct, xFFt[i]*coefftsFFt[i])
50
	}
51
	return fft.IFFT(matrixProduct)
52
}
53

54
// Computes Savitzky-Golay filter coefficients.
55
func SavGolCoeffs(window_length int, polyorder int, deriv int, delta float64, useInConv bool) []float64 {
56
	if polyorder >= window_length {
57
		panic("polyorder must be less than window_length.")
58
	}
59
	fmt.Println("window_length", window_length)
60
	if window_length%2 == 0 {
61
		panic("window_length must be odd.")
62
	}
63
	pos := window_length / 2
64
	if pos < 0 {
65
		panic("pos must be nonnegative.")
66
	}
67

68
	// Form the design matrix `A`. The columns of `A` are powers of the integers
69
	// from -pos to window_length - pos - 1.  The powers (i.e. rows) range
70
	// from 0 to polyorder.
71
	aRowTemplate := Arange(-pos, window_length-pos)
72
	if useInConv {
73
		// Reverse so that result can be used in a convolution.
74
		floats.Reverse(aRowTemplate)
75
	}
76
	a := oss.MakeMatrix(polyorder+1, len(aRowTemplate), func(i, j int) float64 {
77
		return math.Pow(aRowTemplate[j], float64(i))
78
	})
79

80
	// `b` determines which order derivative is returned.
81
	// The coefficient assigned to b[deriv] scales the result to take into
82
	// account the order of the derivative and the sample spacing.
83
	b := oss.MakeMatrix(polyorder+1, 1, func(i, j int) float64 {
84
		if i != deriv {
85
			return 0
86
		}
87
		return float64(factorial(deriv)) / math.Pow(delta, float64(deriv))
88
	})
89

90
	// finding the least-squares solution of A*x = b
91
	coeff := LstSq(a, b)
92
	if _, cols := coeff.Dims(); cols != 1 {
93
		panic(errors.Errorf("SHOULD NOT HAPPEN: LstSq result contains %d columns instead of 1", cols))
94
	}
95
	return coeff.RawMatrix().Data
96
}
97

98
// LstSq computes least-squares solution to equation A*x = b, i.e. computes a vector x such that the 2-norm “|b - A x|“ is minimized.
99
func LstSq(a, b *mat.Dense) *mat.Dense {
100
	// m is a number of columns in `a`, n is a number of rows in `a`
101
	m, n := a.Dims()
102
	if m == 0 || n == 0 {
103
		panic("zero-sized problem is not supported (confuses LAPACK)")
104
	}
105

106
	// nhrs (why is it called so?) is a number of rows in `b`
107
	m2, nhrs := b.Dims()
108
	if m2 != m {
109
		panic(errors.Errorf("shape mismatch: a and b should have the same number of rows: %d != %d", m, m2))
110
	}
111

112
	// LAPACK uses `b` as an output parameter as well - and therefore wants it to be resized from (m, nhrs) to (max(m,n), nhrs)
113
	// here we copy `b` anyway (even if it doesn't need to be resized) - to avoid overwriting the user-supplied `b`
114
	b = oss.MakeMatrix(max2(m, n), nhrs, func(i, j int) float64 {
115
		if i < m {
116
			return b.At(i, j)
117
		}
118
		return 0
119
	})
120

121
	// LAPACK function for computing least-squares solutions to linear equations
122
	gels := func(work []float64, lwork int) bool {
123
		return lapack64.Gels(blas.NoTrans, a.RawMatrix(), b.RawMatrix(), work, lwork)
124
	}
125

126
	// retrieving the size of work space needed (this is how LAPACK interfaces are designed:
127
	// if we call the function with lwork=-1, it returns the work size needed in work[0])
128
	work := make([]float64, 1)
129
	gels(work, -1)
130
	lwork := int(math.Ceil(work[0]))
131

132
	// solving the equation itself
133
	result := gels(make([]float64, lwork), lwork)
134
	if !result {
135
		panic(errors.Errorf("gels: computation didn't converge: A='%+v', b='%+v'", a, b))
136
	}
137

138
	// dgels writes a solution into b
139
	return b
140
}
141

142
// Arange implements `np.arange` - i.e. returns a list of integers (start, ..., stop - 1) in the form of []float64
143
func Arange(start int, stop int) []float64 {
144
	return Linspace(float64(start), float64(stop-1), stop-start)
145
}
146

147
// Zeroes returns an array of zeroes of specified size.
148
// It's encouraged to use it instead of just make() in case the further code relies on the fact that the array contains zeroes.
149
func Zeroes(size int) []float64 {
150
	return make([]float64, size)
151
}
152

153
// Ones return an array of ones of specified size.
154
func Ones(size int) []float64 {
155
	result := make([]float64, size)
156
	for i := range result {
157
		result[i] = 1
158
	}
159
	return result
160
}
161

162
// Linspace implements `np.linspace` - i.e. splits the interval [start, end] into `num - 1` equal intervals and returns `num` split points.
163
func Linspace(start, end float64, num int) []float64 {
164
	if num < 0 {
165
		panic(errors.Errorf("number of samples, %d, must be non-negative.", num))
166
	}
167
	result := make([]float64, num)
168
	step := (end - start) / float64(num-1)
169
	for i := range result {
170
		result[i] = start + float64(i)*step
171
	}
172
	return result
173
}
174

175
// maximum of two integers
176
func max2(a, b int) int {
177
	if a > b {
178
		return a
179
	}
180
	return b
181
}
182

183
// computes `n!`
184
func factorial(n int) int {
185
	result := 1
186
	for i := 1; i <= n; i++ {
187
		result *= i
188
	}
189
	return result
190
}
191

192
// *** my realyze
193

194
func savitzky_goley(y []float64, f, k int) []float64 {
195
	// Функция, реализующая сглаживание с помощью фильтра Савицкого-Голея.
196
	// f - окно сглаживания, желательно, чтобы оно было нечётным числом.
197
	// k - степень полинома, оно должно быть меньше чем f
198

199
	x := make([]int, len(y))
200
	for ind := range x {
201
		x[ind] = ind
202
	}
203
	n := len(x)
204
	f = int(math.Floor(float64(f)))
205
	f = min2(f, n)
206
	hf := (f - 1) / 2
207

208
	//v := dinamicMatrix_float64(f, k+1)
209
	var v mat.Dense = *(mat.NewDense(f, k+1, nil))
210

211
	t := make([]int, hf*2+1)
212
	for ind := range t {
213
		t[ind] = -hf + ind
214
	}
215

216
	for i := 0; i < f; i++ {
217
		for j := 0; j <= k; j++ {
218
			v.Set(i, j, math.Pow(float64(t[i]), float64(j)))
219
		}
220
	}
221

222
	q, _ := QRDec(v)
223

224
	ymid := filt(q, hf, y)
225

226
	ybegin := yBegin(q, hf, f, y)
227
	yend := yEnd(q, hf, f, y)
228
	for i := 0; i < len(ybegin); i++ {
229
		ymid[i] = ybegin[i]
230
	}
231

232
	// fmt.Println(len(ymid)-len(yend)-1, len(ymid), "len(yend)", len(yend))
233

234
	for i := len(ymid) - len(yend); i < len(ymid); i++ {
235
		//fmt.Println(i - (len(ymid) - len(yend)))
236
		//fmt.Println(ybegin[i-(len(ymid)-len(yend))])
237
		ymid[i] = ybegin[i-(len(ymid)-len(yend))]
238
	}
239

240
	return ymid
241
}
242

243
func yBegin(q mat.Dense, hf, f int, y []float64) []float64 {
244
	//ybegin = q(1:hf,:)*q'*y(1:f);
245
	_, col := q.Dims()
246

247
	sliseQ := q.Slice(0, hf, 0, col)
248
	var matr mat.Dense
249
	matr.Mul(sliseQ, q.T())
250

251
	var matVecDense mat.VecDense
252
	matVecDense.MulVec(mat.Matrix(&matr), mat.Vector(mat.NewVecDense(f, y[:f])))
253

254
	return oss.VecDense_in_float64(matVecDense)
255
}
256
func yEnd(q mat.Dense, hf, f int, y []float64) []float64 {
257
	//yend   = q((hf+2):end,:)*q'*y(n-f+1:n);
258
	row, col := q.Dims()
259

260
	sliseQ := q.Slice(hf+1, row, 0, col)
261
	var matr mat.Dense
262
	matr.Mul(sliseQ, q.T())
263

264
	var matVecDense mat.VecDense
265
	matVecDense.MulVec(mat.Matrix(&matr), mat.Vector(mat.NewVecDense(f, y[len(y)-f:])))
266
	return oss.VecDense_in_float64(matVecDense)
267
}
268

269
func filt(q mat.Dense, hf int, x []float64) []float64 {
270
	// b=q*q(hf+1,:)'
271
	var b mat.VecDense
272
	elem := q.RowView(hf + 1)            // q(hf+1,:)'
273
	var matr mat.Matrix = mat.Matrix(&q) // q
274
	b.MulVec(matr, elem)                 // q*q(hf+1,:)'
275

276
	y := make([]float64, len(x))
277
	y[0] = b.AtVec(0) * x[0]
278

279
	var y_tmp, y_b1, y_j float64
280

281
	for i := 1; i < len(x); i++ {
282
		y_tmp = 0
283
		y_b1 = b.AtVec(1) * x[i]
284

285
		for j := 1; j < b.Len(); j++ {
286
			if i-j == 0 {
287
				y_j = b.AtVec(j) * x[i-j+1]
288
				y_tmp = y_tmp + y_j
289
				y[i] = y_b1 + y_tmp
290
				break
291
			} else {
292
				y_j = b.AtVec(j) * x[i-j+1]
293
				y_tmp = y_tmp + y_j
294
				if i-j >= 1 {
295
					y[i] = y_b1 + y_tmp
296
				}
297
			}
298
		}
299

300
	}
301

302
	return y
303
}
304

305
// QR Decomposition - Некорректный вывод
306
func QRDec(a mat.Dense) (mat.Dense, mat.Dense) {
307
	_, col := a.Dims()
308
	q := a
309
	var r mat.Dense = *mat.NewDense(col, col, nil)
310
	var matVecDense mat.VecDense
311
	for i := 0; i < col; i++ {
312
		for j := 0; j < i; j++ {
313
			r.Set(i, j, oss.MulVecToVec(q.ColView(j), q.ColView(i)))
314
			matVecDense.ScaleVec(r.At(j, i), q.ColView(j))
315
			matVecDense.SubVec(q.ColView(i), &matVecDense)
316
			q.SetCol(i, oss.VecDense_in_float64(matVecDense))
317
		}
318
		matVecDense = *mat.VecDenseCopyOf(q.ColView(i))
319
		r.Set(i, i, matVecDense.Norm(2.0))
320
		//r.Set(i, i, p_norm(q.ColView(i), 2.0))
321

322
		if r.At(i, i) == 0.0 {
323
			break
324
		}
325
		matVecDense.ScaleVec(1/r.At(i, i), q.ColView(i))
326
		q.SetCol(i, oss.VecDense_in_float64(matVecDense))
327
	}
328
	return q, r
329
}
330

331
func p_norm(arr mat.Vector, p float64) float64 {
332
	//func p_norm(arr []float64, p float64) float64 {
333
	// The general definition for the p-norm of a vector v that has N elements is
334
	// If p = 1, then the resulting 1-norm is the sum of the absolute values of the vector elements.
335
	// If p = 2, then the resulting 2-norm gives the vector magnitude or Euclidean length of the vector.
336
	// If p = Inf, then v = max(arr)
337
	// If p = -Inf, then v = min(arr)
338
	var sum float64
339
	if p == 1 {
340
		for ind := 0; ind < arr.Len(); ind++ {
341
			//for _, value := range arr {
342
			sum += arr.AtVec(ind)
343
		}
344
	} else if p == 2 {
345
		for ind := 0; ind < arr.Len(); ind++ {
346
			//for _, value := range arr {
347
			sum += math.Pow(arr.AtVec(ind), 2.0)
348
		}
349
		sum = math.Sqrt(sum)
350
	} else {
351
		for ind := 0; ind < arr.Len(); ind++ {
352
			//for _, value := range arr {
353
			sum += math.Pow(arr.AtVec(ind), p)
354
		}
355
		sum = math.Pow(sum, 1/p)
356
	}
357
	return sum
358
}
359

360
func multipleArray(a, b []float64) (float64, error) {
361
	if len(a) != len(b) {
362
		return 0.0, errors.New("Length vector is different")
363
	}
364
	var sum float64
365
	for i := 0; i < len(a); i++ {
366
		sum += a[i] * b[i]
367
	}
368
	return sum, nil
369
}
370

371
// Вычитание вектора из вектора
372
func subVectors(a, b []float64) ([]float64, error) {
373
	if len(a) != len(b) {
374
		return nil, errors.New("Length vector is different")
375
	}
376
	for i := 0; i < len(a); i++ {
377
		a[i] -= b[i]
378
	}
379
	return a, nil
380
}
381

382
// Умножение вектора на константу
383
func multipleConstArray(a []float64, multipleConstant float64) []float64 {
384
	for i := 0; i < len(a); i++ {
385
		a[i] *= multipleConstant
386
	}
387
	return a
388
}
389

390
// Умножение вектора на константу
391
func divisionConstArray(a []float64, multipleConstant float64) []float64 {
392
	for i := 0; i < len(a); i++ {
393
		a[i] /= multipleConstant
394
	}
395
	return a
396
}
397

398
// Создать матрицу с размерами row, col. int
399
func dinamicMatrix(row, col int) [][]int {
400
	matrix := make([][]int, row)
401
	for ind := range matrix {
402
		matrix[ind] = make([]int, col)
403
	}
404
	return matrix
405
}
406

407
// Создать матрицу с размерами row, col. float64
408
func dinamicMatrix_float64(row, col int) [][]float64 {
409
	matrix := make([][]float64, row)
410
	for ind := range matrix {
411
		matrix[ind] = make([]float64, col)
412
	}
413
	return matrix
414
}
415

416
// Минимальное число из двух
417
func min2(a, b int) int {
418
	if a > b {
419
		return b
420
	} else {
421
		return a
422
	}
423
}
424