1import cmsisdsp as dsp
2import numpy as np
3from numpy.testing import assert_allclose
4from scipy.stats import entropy,tstd, tvar
5from scipy.special import logsumexp
6from scipy.linalg import cholesky,ldl,solve_triangular
7from scipy import signal
8
9def imToReal1D(a):
10    ar=np.zeros(np.array(a.shape) * 2)
11    ar[0::2]=a.real
12    ar[1::2]=a.imag
13    return(ar)
14
15def realToIm1D(ar):
16    return(ar[0::2] + 1j * ar[1::2])
17
18print("Max and AbsMax")
19a=np.array([1.,-3.,4.,0.,-10.,8.])
20
21i=dsp.arm_absmax_no_idx_f32(a)
22print(i)
23assert i==10.0
24
25i=dsp.arm_absmax_no_idx_f64(a)
26print(i)
27assert i==10.0
28
29r,i=dsp.arm_absmax_f64(a)
30assert i==4
31assert r==10.0
32
33r,i=dsp.arm_max_f64(a)
34assert i==5
35assert r==8.0
36
37i=dsp.arm_max_no_idx_f32(a)
38print(i)
39assert i==8
40
41i=dsp.arm_max_no_idx_f64(a)
42print(i)
43assert i==8
44
45print("Min and AbsMin")
46a=np.array([1.,-3.,4.,0.5,-10.,8.])
47
48i=dsp.arm_absmin_no_idx_f32(a)
49print(i)
50assert i==0.5
51
52i=dsp.arm_absmin_no_idx_f64(a)
53print(i)
54assert i==0.5
55
56r,i=dsp.arm_absmin_f64(a)
57assert i==3
58assert r==0.5
59
60r,i=dsp.arm_min_f64(a)
61assert i==4
62assert r==-10
63
64i=dsp.arm_min_no_idx_f32(a)
65print(i)
66assert i==-10
67
68i=dsp.arm_min_no_idx_f64(a)
69print(i)
70assert i==-10
71
72print("Barycenter")
73
74a=np.zeros((12,3))
75w=np.array([[2.0] * 12])[0]
76w[0]=3
77w[11]=3
78a[0] =[0., 0., -0.951057]
79a[1] =[0., 0., 0.951057]
80a[2] =[-0.850651, 0., -0.425325]
81a[3] =[0.850651, 0., 0.425325]
82a[4] =[0.688191, -0.5, -0.425325]
83a[5] =[0.688191, 0.5, -0.425325]
84a[6] =[-0.688191, -0.5, 0.425325]
85a[7] =[-0.688191, 0.5, 0.425325]
86a[8] =[-0.262866, -0.809017, -0.425325]
87a[9] =[-0.262866, 0.809017, -0.425325]
88a[10]=[0.262866, -0.809017, 0.425325]
89a[11]=[0.262866, 0.809017, 0.425325]
90
91
92scaled= np.dot(a.T , w)
93print(scaled)
94ref=scaled/np.sum(w)
95#print(ref)
96
97points = np.array(a).reshape(12*3)
98weights = w.reshape(12)
99
100#print(points)
101#print(weights)
102
103result=dsp.arm_barycenter_f32(points,weights,12,3)
104
105
106assert_allclose(ref,result,rtol=1e-6,atol=1e-6)
107
108print("Weighted sum")
109
110nb=10
111s = np.random.randn(nb)
112w = np.random.randn(nb)
113
114ref=np.dot(s,w)/np.sum(w)
115print(ref)
116
117res=dsp.arm_weighted_sum_f32(s,w)
118print(res)
119
120assert_allclose(ref,res,2e-5)
121
122print("Entropy")
123s = np.abs(np.random.randn(nb))
124s = s / np.sum(s)
125
126ref=entropy(s)
127print(ref)
128res=dsp.arm_entropy_f32(s)
129print(res)
130assert_allclose(ref,res,1e-6)
131
132res=dsp.arm_entropy_f64(s)
133print(res)
134assert_allclose(ref,res,1e-10)
135
136print("Kullback-Leibler")
137sa = np.abs(np.random.randn(nb))
138sa = sa / np.sum(sa)
139
140sb = np.abs(np.random.randn(nb))
141sb = sb / np.sum(sb)
142
143ref=entropy(sa,sb)
144print(ref)
145res=dsp.arm_kullback_leibler_f32(sa,sb)
146print(res)
147assert_allclose(ref,res,1e-6)
148
149res=dsp.arm_kullback_leibler_f64(sa,sb)
150print(res)
151assert_allclose(ref,res,1e-10)
152
153print("Logsumexp")
154s = np.abs(np.random.randn(nb))
155s = s / np.sum(s)
156
157ref=logsumexp(s)
158print(ref)
159res=dsp.arm_logsumexp_f32(s)
160print(res)
161assert_allclose(ref,res,1e-6)
162
163print("Logsumexp dot prod")
164sa = np.abs(np.random.randn(nb))
165sa = sa / np.sum(sa)
166
167sb = np.abs(np.random.randn(nb))
168sb = sb / np.sum(sb)
169
170d = 0.001
171# It is a proba so must be in [0,1]
172# But restricted to ]d,1] so that the log exists
173sa = (1-d)*sa + d
174sb = (1-d)*sb + d
175
176ref=np.log(np.dot(sa,sb))
177print(ref)
178
179sa = np.log(sa)
180sb = np.log(sb)
181
182res=dsp.arm_logsumexp_dot_prod_f32(sa,sb)
183print(res)
184assert_allclose(ref,res,3e-6)
185
186print("vexp")
187sa = np.random.randn(nb)
188
189ref = np.exp(sa)
190print(ref)
191
192res=dsp.arm_vexp_f32(sa)
193print(res)
194assert_allclose(ref,res,1e-6)
195
196res=dsp.arm_vexp_f64(sa)
197print(res)
198assert_allclose(ref,res,1e-10)
199
200
201print("vlog")
202sa = np.abs(np.random.randn(nb)) + 0.001
203
204ref = np.log(sa)
205print(ref)
206
207res=dsp.arm_vlog_f32(sa)
208print(res)
209assert_allclose(ref,res,2e-5,1e-5)
210
211res=dsp.arm_vlog_f64(sa)
212print(res)
213assert_allclose(ref,res,2e-9,1e-9)
214
215print("Cholesky")
216
217a=np.array([[4,12,-16],[12,37,-43],[-16,-43,98]])
218ref=cholesky(a,lower=True)
219print(ref)
220
221status,res=dsp.arm_mat_cholesky_f32(a)
222print(res)
223assert_allclose(ref,res,1e-6,1e-6)
224
225status,res=dsp.arm_mat_cholesky_f64(a)
226print(res)
227assert_allclose(ref,res,1e-10,1e-10)
228
229print("LDLT")
230
231def swaprow(m,k,j):
232    tmp = np.copy(m[j,:])
233    m[j,:] = np.copy(m[k,:])
234    m[k,:] = tmp
235    return(m)
236
237# F32 test
238status,resl,resd,resperm=dsp.arm_mat_ldlt_f32(a)
239n=3
240p=np.identity(n)
241for k in range(0,n):
242    p = swaprow(p,k,resperm[k])
243
244res=resl.dot(resd).dot(resl.T)
245
246permutedSrc=p.dot(a).dot(p.T)
247print(res)
248print(permutedSrc)
249
250assert_allclose(permutedSrc,res,1e-5,1e-5)
251
252# F64 test
253print("LDLT F64")
254status,resl,resd,resperm=dsp.arm_mat_ldlt_f64(a)
255n=3
256p=np.identity(n)
257for k in range(0,n):
258    p = swaprow(p,k,resperm[k])
259
260res=resl.dot(resd).dot(resl.T)
261
262permutedSrc=p.dot(a).dot(p.T)
263print(res)
264print(permutedSrc)
265
266assert_allclose(permutedSrc,res,1e-9,1e-9)
267
268print("Solve lower triangular")
269a = np.array([[3, 0, 0, 0], [2, 1, 0, 0], [1, 0, 1, 0], [1, 1, 1, 1]])
270b = np.array([[4,2,4,2],[8,4,8,4]]).T
271x = solve_triangular(a, b,lower=True)
272print(a)
273print(b)
274print(x)
275
276b = np.array([[4,2,4,2],[8,4,8,4]]).T
277status,res=dsp.arm_mat_solve_lower_triangular_f32(a,b)
278print(res)
279assert_allclose(x,res,1e-5,1e-5)
280
281
282b = np.array([[4,2,4,2],[8,4,8,4]]).T
283status,res=dsp.arm_mat_solve_lower_triangular_f64(a,b)
284print(res)
285assert_allclose(x,res,1e-9,1e-9)
286
287
288print("Solve upper triangular")
289a = np.array([[3, 0, 0, 0], [2, 1, 0, 0], [1, 0, 1, 0], [1, 1, 1, 1]])
290b = np.array([[4,2,4,2],[8,4,8,4]]).T
291x = solve_triangular(a.T, b,lower=False)
292print(a.T)
293print(b)
294print(x)
295
296b = np.array([[4,2,4,2],[8,4,8,4]]).T
297status,res=dsp.arm_mat_solve_upper_triangular_f32(a.T,b)
298print(res)
299assert_allclose(x,res,1e-5,1e-5)
300
301
302b = np.array([[4,2,4,2],[8,4,8,4]]).T
303status,res=dsp.arm_mat_solve_upper_triangular_f64(a.T,b)
304print(res)
305assert_allclose(x,res,1e-9,1e-9)
306
307
308print("Mat mult f64")
309a = np.array([[3, 0, 0, 0], [2, 1, 0, 0], [1, 0, 1, 0], [1, 1, 1, 1]])
310b = np.array([[4,2,4,2],[8,4,8,4]]).T
311
312ref =a.dot(b)
313print(ref)
314
315status,res = dsp.arm_mat_mult_f64(a,b)
316print(res)
317assert_allclose(ref,res,1e-10,1e-10)
318
319print("mat sub f64")
320a = np.array([[3, 0, 0, 0], [2, 1, 0, 0], [1, 0, 1, 0], [1, 1, 1, 1]])
321b = a.T
322
323ref = a - b
324print(ref)
325
326status,res = dsp.arm_mat_sub_f64(a,b)
327print(res)
328assert_allclose(ref,res,1e-10,1e-10)
329
330print("abs f64")
331s = np.random.randn(nb)
332ref = np.abs(s)
333res=dsp.arm_abs_f64(s)
334print(ref)
335print(res)
336assert_allclose(ref,res,1e-10,1e-10)
337
338print("add f64")
339sa = np.random.randn(nb)
340sb = np.random.randn(nb)
341ref = sa + sb
342res=dsp.arm_add_f64(sa,sb)
343print(ref)
344print(res)
345assert_allclose(ref,res,1e-10,1e-10)
346
347print("sub f64")
348sa = np.random.randn(nb)
349sb = np.random.randn(nb)
350ref = sa - sb
351res=dsp.arm_sub_f64(sa,sb)
352print(ref)
353print(res)
354assert_allclose(ref,res,1e-10,1e-10)
355
356print("dot prod  f64")
357sa = np.random.randn(nb)
358sb = np.random.randn(nb)
359ref = sa.dot(sb)
360res=dsp.arm_dot_prod_f64(sa,sb)
361print(ref)
362print(res)
363assert_allclose(ref,res,1e-10,1e-10)
364
365print("mult f64")
366sa = np.random.randn(nb)
367sb = np.random.randn(nb)
368ref = sa * sb
369res=dsp.arm_mult_f64(sa,sb)
370print(ref)
371print(res)
372assert_allclose(ref,res,1e-10,1e-10)
373
374print("negate f64")
375sa = np.random.randn(nb)
376ref = -sa
377res=dsp.arm_negate_f64(sa)
378print(ref)
379print(res)
380assert_allclose(ref,res,1e-10,1e-10)
381
382print("offset f64")
383sa = np.random.randn(nb)
384ref = sa + 0.1
385res=dsp.arm_offset_f64(sa,0.1)
386print(ref)
387print(res)
388assert_allclose(ref,res,1e-10,1e-10)
389
390print("scale f64")
391sa = np.random.randn(nb)
392ref = sa * 0.1
393res=dsp.arm_scale_f64(sa,0.1)
394print(ref)
395print(res)
396assert_allclose(ref,res,1e-10,1e-10)
397
398print("mean f64")
399sa = np.random.randn(nb)
400ref = np.mean(sa)
401res=dsp.arm_mean_f64(sa)
402print(ref)
403print(res)
404assert_allclose(ref,res,1e-10,1e-10)
405
406print("power f64")
407sa = np.random.randn(nb)
408ref = np.sum(sa * sa)
409res=dsp.arm_power_f64(sa)
410print(ref)
411print(res)
412assert_allclose(ref,res,1e-10,1e-10)
413
414print("std f64")
415sa = np.random.randn(nb)
416ref = tstd(sa)
417res=dsp.arm_std_f64(sa)
418print(ref)
419print(res)
420assert_allclose(ref,res,1e-10,1e-10)
421
422print("variance f64")
423sa = np.random.randn(nb)
424ref = tvar(sa)
425res=dsp.arm_var_f64(sa)
426print(ref)
427print(res)
428assert_allclose(ref,res,1e-10,1e-10)
429
430print("fill f64")
431nb=20
432ref = np.ones(nb)*4.0
433res = dsp.arm_fill_f64(4.0,nb)
434assert_allclose(ref,res,1e-10,1e-10)
435
436print("copy f64")
437nb=20
438sa = np.random.randn(nb)
439ref = sa
440res = dsp.arm_copy_f64(sa)
441assert_allclose(ref,res,1e-10,1e-10)
442
443print("arm_div_int64_to_int32")
444den=0x7FFF00000000
445num=0x10000
446ref=den//num
447
448res=dsp.arm_div_int64_to_int32(den,num)
449print(ref)
450print(res)
451
452print("fir f64")
453
454firf64 = dsp.arm_fir_instance_f64()
455dsp.arm_fir_init_f64(firf64,3,[1.,2,3],[0,0,0,0,0,0,0])
456filtered_x = signal.lfilter([3,2,1.], 1.0, [1,2,3,4,5,1,2,3,4,5])
457print(filtered_x)
458ra=dsp.arm_fir_f64(firf64,[1,2,3,4,5])
459rb=dsp.arm_fir_f64(firf64,[1,2,3,4,5])
460assert ((filtered_x == np.hstack([ra,rb])).all)
461
462print("arm_cmplx_mag")
463sa = np.random.randn(nb)
464ca = realToIm1D(sa)
465ref = np.abs(ca)
466print(ref)
467res=dsp.arm_cmplx_mag_f32(sa)
468print(res)
469assert_allclose(ref,res,1e-6,1e-6)
470res=dsp.arm_cmplx_mag_f64(sa)
471print(res)
472assert_allclose(ref,res,1e-10,1e-10)
473
474print("arm_cmplx_mag_squared")
475sa = np.random.randn(nb)
476ca = realToIm1D(sa)
477ref = np.abs(ca) * np.abs(ca)
478print(ref)
479res=dsp.arm_cmplx_mag_squared_f32(sa)
480print(res)
481assert_allclose(ref,res,1e-6,1e-6)
482res=dsp.arm_cmplx_mag_squared_f64(sa)
483print(res)
484assert_allclose(ref,res,1e-10,1e-10)
485
486print("cmplx mult")
487sa = np.random.randn(nb)
488ca = realToIm1D(sa)
489sb = np.random.randn(nb)
490cb = realToIm1D(sb)
491ref = imToReal1D(ca * cb)
492print(ref)
493res = dsp.arm_cmplx_mult_cmplx_f32(sa,sb)
494print(res)
495assert_allclose(ref,res,1e-6,1e-6)
496
497res = dsp.arm_cmplx_mult_cmplx_f64(sa,sb)
498print(res)
499assert_allclose(ref,res,1e-10,1e-10)
500