Mercurial > hg > ltpda

comparison src/ltpda_dft/ltpda_dft.c @ 0:f0afece42f48

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Import.
author Daniele Nicolodi <nicolodi@science.unitn.it>
date Wed, 23 Nov 2011 19:22:13 +0100
parents
children
comparison
equal deleted inserted replaced
-1:000000000000 0:f0afece42f48
1 /*
2 * Mex file that implements the core DFT part of the LPSD algorithm.
3 *
4 * M Hewitson 15-01-08
5 *
6 * $Id: ltpda_dft.c,v 1.16 2009/08/18 10:53:27 hewitson Exp $
7 */
8
9
10 #include <math.h>
11 #include <stdio.h>
12 #include <stdlib.h>
13 #include <string.h>
14 #include <mex.h>
15
16 #include "ltpda_dft.h"
17 #include "version.h"
18 #include "../c_sources/polyreg.c"
19
20 #define DEBUG 0
21
22
23 /*
24 * Rounding function
25 *
26 */
27 int myround(double x) {
28 return ((int) (floor(x + 0.5)));
29 }
30
31
32 /*
33 * Matlab mex file to make a dft at a single frequency
34 *
35 * Inputs
36 * - data
37 * - seg length
38 * - DFT coefficients
39 * - overlap (%)
40 * - order of detrending
41 *
42 * function [P, navs] = ltpda_dft(x, seglen, DFTcoeffs, olap, order);
43 *
44 */
45 void mexFunction( int nlhs, mxArray *plhs[],
46 int nrhs, const mxArray *prhs[]) {
47 /* Parse inputs */
48 if( (nrhs == 0) && (nlhs == 0) ) {
49 print_usage(VERSION);
50 return;
51 }
52 else if ( (nrhs == 5) && (nlhs == 3) ) /* let's go */ {
53 double Pr, Vr;
54 double *xdata, *Cr, *Ci;
55 double olap;
56 long int nData;
57 long int nSegs;
58 long int segLen;
59 int order;
60 double *ptr;
61
62 if( !mxIsComplex(prhs[2]) )
63 mexErrMsgTxt("DFT coefficients must be complex.\n");
64
65 /* Extract inputs */
66 xdata = mxGetPr(prhs[0]); /* Pointer to data */
67 nData = mxGetNumberOfElements(prhs[0]); /* Number of data points */
68 segLen = (int)mxGetScalar(prhs[1]); /* Segment length */
69 Cr = mxGetPr(prhs[2]); /* Real part of DFT coefficients */
70 Ci = mxGetPi(prhs[2]); /* Imag part of DFT coefficients */
71 olap = mxGetScalar(prhs[3]); /* Overlap percentage */
72 order = (int)mxGetScalar(prhs[4]); /* Order of detrending */
73
74 if (order > 10 || order < -1)
75 mexErrMsgTxt("Detrending order must be between -1 and 10");
76
77 /*mexPrintf("Input data: %d samples\n", nData);*/
78 /*mexPrintf("Segment length: %d\n", segLen);*/
79 /*mexPrintf("Overlap: %f\n", olap);*/
80
81 /* Compute DFT */
82 dft(&Pr, &Vr, &nSegs, xdata, nData, segLen, Cr, Ci, olap, order);
83
84 /* Set output matrices */
85 plhs[0] = mxCreateDoubleMatrix(1, 1, mxCOMPLEX);
86 plhs[1] = mxCreateDoubleMatrix(1, 1, mxCOMPLEX);
87 plhs[2] = mxCreateDoubleMatrix(1, 1, mxCOMPLEX);
88
89 /* Get pointers to output matrices */
90 ptr = mxGetPr(plhs[0]);
91 ptr[0] = Pr;
92 ptr = mxGetPr(plhs[1]);
93 ptr[0] = Vr;
94 ptr = mxGetPr(plhs[2]);
95 ptr[0] = nSegs;
96
97 }
98 else if ( (nrhs == 6) && (nlhs == 5) ) /* let's go */ {
99 double Mr, Mi, XX, YY, M2;
100 double *xdata, *ydata, *Cr, *Ci;
101 double olap;
102 long int nData, nxData, nyData;
103 long int nSegs;
104 long int segLen;
105 int order;
106 double *ptr;
107
108 if( !mxIsComplex(prhs[3]) )
109 mexErrMsgTxt("DFT coefficients must be complex.\n");
110
111 /* Extract inputs */
112 xdata = mxGetPr(prhs[0]); /* Pointer to data */
113 ydata = mxGetPr(prhs[1]); /* Pointer to data */
114 nxData = mxGetNumberOfElements(prhs[0]); /* Number of data points */
115 nyData = mxGetNumberOfElements(prhs[0]); /* Number of data points */
116 segLen = (int)mxGetScalar(prhs[2]); /* Segment length */
117 Cr = mxGetPr(prhs[3]); /* Real part of DFT coefficients */
118 Ci = mxGetPi(prhs[3]); /* Imag part of DFT coefficients */
119 olap = mxGetScalar(prhs[4]); /* Overlap percentage */
120 order = (int)mxGetScalar(prhs[5]); /* Order of detrending */
121
122 if (order > 10 || order < -1)
123 mexErrMsgTxt("Detrending order must be between -1 and 10");
124
125 if (nxData != nyData)
126 mexErrMsgTxt("The two input data vector should be the same length.");
127
128 nData = nxData;
129
130 /*mexPrintf("Input data: %d samples\n", nData);*/
131 /*mexPrintf("Segment length: %d\n", segLen);*/
132 /*mexPrintf("Overlap: %f\n", olap);*/
133
134 /* Compute DFT */
135 xdft(&Mr, &Mi, &XX, &YY, &M2, &nSegs, xdata, ydata, nData, segLen, Cr, Ci, olap, order);
136
137 /* Set output matrices */
138 plhs[0] = mxCreateDoubleMatrix(1, 1, mxCOMPLEX);
139 plhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);
140 plhs[2] = mxCreateDoubleMatrix(1, 1, mxREAL);
141 plhs[3] = mxCreateDoubleMatrix(1, 1, mxREAL);
142 plhs[4] = mxCreateDoubleMatrix(1, 1, mxREAL);
143
144 /* Get pointers to output matrices */
145 ptr = mxGetPr(plhs[0]);
146 ptr[0] = Mr;
147 ptr = mxGetPi(plhs[0]);
148 ptr[0] = Mi;
149 ptr = mxGetPr(plhs[1]);
150 ptr[0] = XX;
151 ptr = mxGetPr(plhs[2]);
152 ptr[0] = YY;
153 ptr = mxGetPr(plhs[3]);
154 ptr[0] = M2;
155 ptr = mxGetPr(plhs[4]);
156 ptr[0] = nSegs;
157
158
159 }
160 else /* we have an error */ {
161 print_usage(VERSION);
162 mexErrMsgTxt("### incorrect usage");
163 }
164 }
165
166 /*
167 * Output usage to MATLAB terminal
168 *
169 */
170 void print_usage(char *version) {
171 mexPrintf("ltpda_dft version %s\n", version);
172 mexPrintf(" usage: function [P, navs] = ltpda_dft(x, seglen, DFTcoeffs, olap, order); \n");
173 }
174
175 /*
176 * Short routine to compute the DFT at a single frequency
177 *
178 */
179 void dft(double *Pr, double *Vr, long int *Navs,
180 double *xdata, long int nData, long int segLen, double *Cr, double *Ci, double olap, int order) {
181 long int istart;
182 double shift, start;
183 double *px, *cr, *ci;
184 double rxsum, ixsum;
185 double Xr, Mr, M2, Qr;
186 double p, *x, *a;
187 long int jj, ii;
188
189
190 /* Compute the number of averages we want here */
191 double ovfact = 1. / (1. - olap / 100.);
192 double davg = ((double) ((nData - segLen)) * ovfact) / segLen + 1;
193 long int navg = myround(davg);
194
195 /* Compute steps between segments */
196 if (navg == 1)
197 shift = 1;
198 else
199 shift = (double) (nData - segLen) / (double) (navg - 1);
200
201 if (shift < 1)
202 shift = 1;
203
204 /* mexPrintf("Seglen: %d\t | Shift: %f\t | navs: %d\n", segLen, shift, navg);*/
205
206 /* allocate vectors */
207 x = (double*)mxCalloc(segLen, sizeof(double)); /* detrending output */
208 a = (double*)mxCalloc(order+1, sizeof(double)); /* detrending coefficients */
209
210 /* Loop over segments */
211 start = 0.0;
212 Xr = 0.0;
213 Qr = 0.0;
214 Mr = 0.0;
215 M2 = 0.0;
216
217 for (ii = 0; ii < navg; ii++) {
218 /* compute start index */
219 istart = myround(start);
220 start += shift;
221
222 /* pointer to start of this segment */
223 px = &(xdata[istart]);
224
225 /* pointer to DFT coeffs */
226 cr = &(Cr[0]);
227 ci = &(Ci[0]);
228
229 /* Detrend segment */
230 switch (order) {
231 case -1:
232 /* no detrending */
233 memcpy(x, px, segLen*sizeof(double));
234 break;
235 case 0:
236 /* mean removal */
237 polyreg0(px, segLen, x, a);
238 break;
239 case 1:
240 /* linear detrending */
241 polyreg1(px, segLen, x, a);
242 break;
243 case 2:
244 /* 2nd order detrending */
245 polyreg2(px, segLen, x, a);
246 break;
247 case 3:
248 /* 3rd order detrending */
249 polyreg3(px, segLen, x, a);
250 break;
251 case 4:
252 /* 4th order detrending */
253 polyreg4(px, segLen, x, a);
254 break;
255 case 5:
256 /* 5th order detrending */
257 polyreg5(px, segLen, x, a);
258 break;
259 case 6:
260 /* 6th order detrending */
261 polyreg6(px, segLen, x, a);
262 break;
263 case 7:
264 /* 7th order detrending */
265 polyreg7(px, segLen, x, a);
266 break;
267 case 8:
268 /* 8th order detrending */
269 polyreg8(px, segLen, x, a);
270 break;
271 case 9:
272 /* 9th order detrending */
273 polyreg9(px, segLen, x, a);
274 break;
275 case 10:
276 /* 10th order detrending */
277 polyreg10(px, segLen, x, a);
278 break;
279 }
280
281 /* Go over all samples in this segment */
282 rxsum = ixsum = 0.0;
283 for (jj=0; jj<segLen; jj++) {
284 p = x[jj];
285 rxsum += (*cr) * p; /* cos term */
286 ixsum += (*ci) * p; /* sin term */
287
288 /* increment pointers */
289 cr++;
290 ci++;
291 }
292 /*mexPrintf(" xsum=(%g +i %g), ysum=(%g + i%g)\n", rxsum, ixsum, rysum, iysum);*/
293
294 /* Average the cross-power
295 * Rsum += rxsum*rxsum + ixsum*ixsum;
296 */
297
298 /* Welford's algorithm to update mean and variance */
299 if (ii == 0) {
300 Mr = (rxsum*rxsum + ixsum*ixsum);
301 } else {
302 Xr = (rxsum*rxsum + ixsum*ixsum);
303 Qr = Xr - Mr;
304 Mr += Qr/ii;
305 M2 += Qr * (Xr - Mr);
306 }
307
308 }
309
310 /* mexPrintf(" start: %f \t istart: %d | %d \n", start, istart, nData-istart);*/
311 /*mexPrintf(" Rsum=%g, MR=%g \n", Rsum,MR); */
312
313 /* clean up */
314 mxFree(x);
315 mxFree(a);
316
317 /* Outputs */
318 *Pr = Mr;
319 if(navg == 1){
320 *Vr = Mr*Mr;
321 } else {
322 *Vr = M2/(navg-1);
323 }
324 *Navs = navg;
325 }
326
327 /*
328 * Short routine to compute the cross-DFT at a single frequency
329 *
330 */
331 void xdft(double *Pxyr, double *Pxyi, double *Pxx, double *Pyy, double *Vr, long int *Navs,
332 double *xdata, double *ydata, long int nData, long int segLen, double *Cr, double *Ci, double olap, int order) {
333 long int istart;
334 double shift, start;
335 double *px, *py, *cr, *ci;
336 double rxsum, ixsum, rysum, iysum;
337 double XYr, XYi, QXYr, QXYi, QXYrn, QXYin;
338 double MXYr, MXYi, MXY2;
339 double XX, YY, QXX, QYY;
340 double MXX, MYY, MXX2, MYY2;
341 double p, *x, *y, *a;
342 double ct, st;
343 long int jj, ii;
344
345 /* Compute the number of averages we want here */
346 double ovfact = 1. / (1. - olap / 100.);
347 double davg = ((double) ((nData - segLen)) * ovfact) / segLen + 1;
348 long int navg = myround( davg );
349
350 /* Compute steps between segments */
351 if (navg == 1)
352 shift = 1;
353 else
354 shift = (double) (nData - segLen) / (double) (navg - 1);
355
356 if (shift < 1)
357 shift = 1;
358
359 /* mexPrintf("Seglen: %d\t | Shift: %f\t | navs: %d\n", segLen, shift, navg); */
360
361 /* allocate vectors */
362 y = (double*)mxCalloc(segLen, sizeof(double)); /* detrending output */
363 x = (double*)mxCalloc(segLen, sizeof(double)); /* detrending output */
364 a = (double*)mxCalloc(order+1, sizeof(double)); /* detrending coefficients */
365
366 /* Loop over segments */
367 start = 0.0;
368 MXYr = 0.0;
369 MXYi = 0.0;
370 MXY2 = 0.0;
371 MXX = 0.0;
372 MYY = 0.0;
373 MXX2 = 0.0;
374 MYY2 = 0.0;
375
376 for (ii = 0; ii < navg; ii++) {
377 /* compute start index */
378 istart = myround(start);
379 start += shift;
380
381 /* pointer to start of this segment */
382 px = &(xdata[istart]);
383 py = &(ydata[istart]);
384
385 /* pointer to DFT coeffs */
386 cr = &(Cr[0]);
387 ci = &(Ci[0]);
388
389 /* Detrend segment */
390 switch (order) {
391 case -1:
392 /* no detrending */
393 memcpy(x, px, segLen*sizeof(double));
394 memcpy(y, py, segLen*sizeof(double));
395 break;
396 case 0:
397 /* mean removal */
398 polyreg0(px, segLen, x, a);
399 polyreg0(py, segLen, y, a);
400 break;
401 case 1:
402 /* linear detrending */
403 polyreg1(px, segLen, x, a);
404 polyreg1(py, segLen, y, a);
405 break;
406 case 2:
407 /* 2nd order detrending */
408 polyreg2(px, segLen, x, a);
409 polyreg2(py, segLen, y, a);
410 break;
411 case 3:
412 /* 3rd order detrending */
413 polyreg3(px, segLen, x, a);
414 polyreg3(py, segLen, y, a);
415 break;
416 case 4:
417 /* 4th order detrending */
418 polyreg4(px, segLen, x, a);
419 polyreg4(py, segLen, y, a);
420 break;
421 case 5:
422 /* 5th order detrending */
423 polyreg5(px, segLen, x, a);
424 polyreg5(py, segLen, y, a);
425 break;
426 case 6:
427 /* 6th order detrending */
428 polyreg6(px, segLen, x, a);
429 polyreg6(py, segLen, y, a);
430 break;
431 case 7:
432 /* 7th order detrending */
433 polyreg7(px, segLen, x, a);
434 polyreg7(py, segLen, y, a);
435 break;
436 case 8:
437 /* 8th order detrending */
438 polyreg8(px, segLen, x, a);
439 polyreg8(py, segLen, y, a);
440 break;
441 case 9:
442 /* 9th order detrending */
443 polyreg9(px, segLen, x, a);
444 polyreg9(py, segLen, y, a);
445 break;
446 case 10:
447 /* 10th order detrending */
448 polyreg10(px, segLen, x, a);
449 polyreg10(py, segLen, y, a);
450 break;
451 }
452
453 /* Go over all samples in this segment */
454 rxsum = ixsum = 0.0;
455 rysum = iysum = 0.0;
456 for (jj=0; jj<segLen; jj++) {
457 ct = (*cr);
458 st = (*ci);
459 p = x[jj];
460 rxsum += ct * p; /* cos term */
461 ixsum += st * p; /* sin term */
462 p = y[jj];
463 rysum += ct * p; /* cos term */
464 iysum += st * p; /* sin term */
465
466 /* increment pointers */
467 cr++;
468 ci++;
469 }
470 /*mexPrintf(" xsum=(%g +i %g), ysum=(%g + i%g)\n", rxsum, ixsum, rysum, iysum);*/
471
472 /* Average XX and YY power
473 * XXsum += rxsum*rxsum + ixsum*ixsum;
474 * YYsum += rysum*rysum + iysum*iysum;
475 * XYRsum += rysum*rxsum + iysum*ixsum;
476 * XYIsum += iysum*rxsum - rysum*ixsum; */
477
478 /* Welford's algorithm to update mean and variance for cross-power */
479 if (ii == 0) {
480 /* for XY */
481 MXYr = rysum*rxsum + iysum*ixsum;
482 MXYi = iysum*rxsum - rysum*ixsum;
483 /* for XX */
484 MXX = rxsum*rxsum + ixsum*ixsum;
485 /* for YY */
486 MYY = rysum*rysum + iysum*iysum;
487 } else {
488 /* for XY cross - power */
489 XYr = rysum*rxsum + iysum*ixsum;
490 XYi = iysum*rxsum - rysum*ixsum;
491 QXYr = XYr - MXYr;
492 QXYi = XYi - MXYi;
493 MXYr += QXYr/ii;
494 MXYi += QXYi/ii;
495 /* new Qs, using new mean */
496 QXYrn = XYr - MXYr;
497 QXYin = XYi - MXYi;
498 /* taking abs to get real variance */
499 MXY2 += sqrt(pow(QXYr * QXYrn - QXYi * QXYin, 2) + pow(QXYr * QXYin + QXYi * QXYrn, 2));
500 /* for XX */
501 XX = rxsum*rxsum + ixsum*ixsum;
502 QXX = XX - MXX;
503 MXX += QXX/ii;
504 MXX2 += QXX*(XX - MXX);
505 /* for YY */
506 YY = rysum*rysum + iysum*iysum;
507 QYY = YY - MYY;
508 MYY += QYY/ii;
509 MYY2 += QYY*(YY - MYY);
510 }
511
512 }
513
514 /* mexPrintf(" start: %f \t istart: %d | %d \n", start, istart, nData-istart);*/
515 /*mexPrintf(" Rsum=%g, Isum=%g\n", Rsum, Isum);*/
516
517 /* clean up */
518 mxFree(y);
519 mxFree(x);
520 mxFree(a);
521
522 /* Outputs */
523 *Pxyr = MXYr;
524 *Pxyi = MXYi;
525 if(navg == 1){
526 *Vr = MXYr*MXYr; /* set to mean^2 here, but at the end returns Inf in the ao.dy field */
527 } else {
528 *Vr = MXY2/(navg-1);
529 }
530 *Pxx = MXX;
531 *Pyy = MYY; /* we don't return variance for XX and YY */
532 *Navs = navg;
533 }
534
535 /*
536 * Fast linear detrending routine
537 *
538 */
539 void remove_linear_drift(double *segm, double *data, int nfft) {
540 int i;
541 long double sx, sy, stt, sty, a, b, xm;
542
543 sx = (long double) nfft *(long double) (nfft - 1) / 2.0L;
544 xm = (long double) (nfft - 1) / 2.0L;
545 stt = ((long double) nfft * (long double) nfft * (long double) nfft - (long double) nfft) / 12.0L;
546
547 sy=sty = 0;
548 for (i = 0; i < nfft; i++) {
549 sy += data[i];
550 sty += (i-xm) * data[i];
551 }
552 b = sty / stt;
553 a = (sy - sx * b) / nfft;
554 for (i = 0; i < nfft; i++) {
555 segm[i] = data[i] - (a + b * i);
556 }
557 }
558
559