chirptest/chirp.c

   1 /********************************************************************
   2  *                                                                  *
   3  * THIS FILE IS PART OF THE OggGhost SOFTWARE CODEC SOURCE CODE.    *
   4  * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
   5  * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
   6  * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
   7  *                                                                  *
   8  * THE OggGhost SOURCE CODE IS (C) COPYRIGHT 2007-2011              *
   9  * by the Xiph.Org Foundation http://www.xiph.org/                  *
  10  *                                                                  *
  11  ********************************************************************
  12
  13  function: research-grade chirp estimation code
  14  last mod: $Id$
  15
  16  Provides a set of chirp estimation algorithm variants for comparison
  17  and characterization.  This is canned code meant for testing and
  18  exploration of sinusoidal estimation.  It's designed to be held still
  19  and used for reference and comparison against later improvement.
  20  Please, don't optimize this code.  Test optimizations elsewhere in
  21  code to be compared to this code.  Roll new technique improvements
  22  into this reference set only after they have been completed.
  23
  24  ********************************************************************/
  25
  26 #define _GNU_SOURCE
  27 #include <math.h>
  28 #include "chirp.h"
  29 #include "scales.h"
  30 #include <stdio.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33
  34 static int cmp_descending_A(const void *p1, const void *p2){
  35   chirp *A = (chirp *)p1;
  36   chirp *B = (chirp *)p2;
  37
  38   return (A->A<B->A) - (B->A<A->A);
  39 }
  40 static int cmp_ascending_W(const void *p1, const void *p2){
  41   chirp *A = (chirp *)p1;
  42   chirp *B = (chirp *)p2;
  43
  44   return (B->W<A->W) - (A->W<B->W);
  45 }
  46
  47 /* The stimators project a residue vecotr onto basis functions; the
  48    results of the accumulation map directly to the parameters we're
  49    interested in (A, P, W, dA, dW, ddA) and vice versa.  The mapping
  50    equations are broken out below. */
  51
  52 static float toA(float Ai, float Bi){
  53   return hypotf(Ai,Bi);
  54 }
  55 static float toP(float Ai, float Bi){
  56   return -atan2f(Bi, Ai);
  57 }
  58 static float toW(float Ai, float Bi, float Ci, float Di){
  59   return (Ai*Ai || Bi*Bi) ? (Ci*Bi - Di*Ai)/(Ai*Ai + Bi*Bi) : 0;
  60 }
  61 static float todA(float Ai, float Bi, float Ci, float Di){
  62   return (Ai*Ai || Bi*Bi) ? (Ci*Ai + Di*Bi)/hypotf(Ai,Bi) : 0;
  63 }
  64 static float todW(float Ai, float Bi, float Ei, float Fi){
  65   return (Ai*Ai || Bi*Bi) ? (Ei*Bi - Fi*Ai)/(Ai*Ai + Bi*Bi) : 0;
  66 }
  67 static float toddA(float Ai, float Bi, float Ei, float Fi){
  68   return (Ai*Ai || Bi*Bi) ? (Ei*Ai + Fi*Bi)/hypotf(Ai,Bi) : 0;
  69 }
  70
  71 static float toAi(float A, float P){
  72   return A * cosf(P);
  73 }
  74 static float toBi(float A, float P){
  75   return A * -sinf(P);
  76 }
  77
  78 static float WtoCi(float A, float P, float W){
  79   return -W*A*sinf(P);
  80 }
  81
  82 static float WtoDi(float A, float P, float W){
  83   return -W*A*cosf(P);
  84 }
  85
  86 static float dWtoEi(float A, float P, float dW){
  87   return -A*dW*sinf(P);
  88 }
  89 static float dWtoFi(float A, float P, float dW){
  90   return -A*dW*cosf(P);
  91 }
  92
  93 static float dAtoCi(float P, float dA){
  94   return dA*cosf(P);
  95 }
  96 static float dAtoDi(float P, float dA){
  97   return -dA*sinf(P);
  98 }
  99 static float ddAtoEi(float P, float ddA){
 100   return ddA*cosf(P);
 101 }
 102 static float ddAtoFi(float P, float ddA){
 103   return -ddA*sinf(P);
 104 }
 105
 106 /* fully nonlinear estimation iterator; it restarts the basis
 107    functions (W and dW) and error vector for each chirp after each new
 108    fit. Reconstruction (and thus error) are exact calculations. */
 109 static int full_nonlinear_iterate(const float *x,
 110                                   const float *window,
 111                                   int len,
 112                                   chirp *cc,
 113                                   int n,
 114
 115                                   float fit_limit,
 116                                   int iter_limit,
 117                                   int fit_gs,
 118
 119                                   int fitW,
 120                                   int fitdA,
 121                                   int fitdW,
 122                                   int fitddA,
 123                                   float fit_W_alpha,
 124                                   float fit_dW_alpha,
 125
 126                                   int symm_norm,
 127                                   float E0,
 128                                   float E1,
 129                                   float E2,
 130
 131                                   int bound_edges){
 132   int i,j;
 133   int flag=1;
 134   float r[len];
 135   float y[len];
 136
 137   /* Build a starting reconstruction based on the passed-in initial
 138      chirp estimates */
 139   memset(y,0,sizeof(*y)*len);
 140   for(i=0;i<n;i++){
 141     for(j=0;j<len;j++){
 142       double jj = j-len*.5+.5;
 143       float a = cc[i].A + (cc[i].dA + cc[i].ddA*jj)*jj;
 144       y[j] += a*cos((cc[i].W + cc[i].dW*jj)*jj + cc[i].P);
 145     }
 146   }
 147
 148   /* outer fit iteration */
 149   while(flag && iter_limit>0){
 150     flag=0;
 151
 152     /* precompute the portion of the projection/fit estimate shared by
 153        the zero, first and second order fits.  Subtracts the current
 154        best fits from the input signal and windows the result. */
 155     for(j=0;j<len;j++)
 156       r[j]=(x[j]-y[j])*window[j];
 157     memset(y,0,sizeof(*y)*len);
 158
 159     /* Sort chirps by descending amplitude */
 160     qsort(cc, n, sizeof(*cc), cmp_descending_A);
 161
 162     for(i=0;i<n;i++){
 163       chirp *c = cc+i;
 164       float aP=0, bP=0;
 165       float cP=0, dP=0;
 166       float eP=0, fP=0;
 167       float cP2=0, dP2=0;
 168       float eP2=0, fP2=0;
 169       float eP3=0, fP3=0;
 170       float aE=0, bE=0;
 171       float cE=0, dE=0;
 172       float eE=0, fE=0;
 173       float aC = cos(c->P);
 174       float aS = sin(c->P);
 175       float move;
 176
 177       if(c->A==-1) continue;
 178
 179       for(j=0;j<len;j++){
 180
 181         /* no part of the nonlinear algorithm requires double
 182            precision, however the largest source of noise will be from
 183            a naive computation of the instantaneous basis or
 184            reconstruction chirp phase.  Because dW is usually very
 185            small compared to W (especially c->W*jj), (W + dW*jj)*jj
 186            quickly becomes noticably short of significant bits.
 187
 188            We can either calculate everything mod 2PI, use a double
 189            for just this calculation (and passing the result to
 190            sincos) or decide we don't care.  In the production code we
 191            will probably not care as most of the depth in the
 192            estimator isn't needed.  Here we'll take the easiest route
 193            to have a deep reference and just use doubles. */
 194
 195         double jj = j-len*.5+.5;
 196         float jj2 = jj*jj;
 197         double co,si;
 198         float c2,s2;
 199         float yy=r[j];
 200
 201         sincos((c->W + c->dW*jj)*jj,&si,&co);
 202
 203         si*=window[j];
 204         co*=window[j];
 205
 206         /* add the current estimate back to the residue vector */
 207         r[j] += (aC*co-aS*si) * (c->A + (c->dA + c->ddA*jj)*jj);
 208
 209         c2 = co*co*jj;
 210         s2 = si*si*jj;
 211
 212         /* zero order projection */
 213         aP += co*yy;
 214         bP += si*yy;
 215         /* first order projection */
 216         cP += co*yy*jj;
 217         dP += si*yy*jj;
 218         /* second order projection */
 219         eP += co*yy*jj2;
 220         fP += si*yy*jj2;
 221
 222         if(fit_gs){
 223           /* subtract zero order estimate from first */
 224           cP2 += c2;
 225           dP2 += s2;
 226           /* subtract zero order estimate from second */
 227           eP2 += c2*jj;
 228           fP2 += s2*jj;
 229           /* subtract first order estimate from second */
 230           eP3 += c2*jj2;
 231           fP3 += s2*jj2;
 232         }
 233
 234         if(!symm_norm){
 235           /* accumulate per-basis energy for normalization */
 236           aE += co*co;
 237           bE += si*si;
 238           cE += c2*jj;
 239           dE += s2*jj;
 240           eE += c2*jj*jj2;
 241           fE += s2*jj*jj2;
 242         }
 243       }
 244
 245       /* normalize, complete projections */
 246       if(symm_norm){
 247         aP *= E0;
 248         bP *= E0;
 249         cP = (cP-aP*cP2)*E1;
 250         dP = (dP-bP*dP2)*E1;
 251         eP = (eP-aP*eP2-cP*eP3)*E2;
 252         fP = (fP-bP*fP2-dP*fP3)*E2;
 253       }else{
 254         aP = (aE>1e-20f?aP/aE:0.f);
 255         bP = (bE>1e-20f?bP/bE:0.f);
 256         cP = (cE>1e-20f?(cP-aP*cP2)/cE:0.f);
 257         dP = (dE>1e-20f?(dP-bP*dP2)/dE:0.f);
 258         eP = (eE>1e-20f?(eP-aP*eP2-cP*eP3)/eE:0.f);
 259         fP = (fE>1e-20f?(fP-bP*fP2-dP*fP3)/fE:0.f);
 260       }
 261
 262       if(!fitW && !fitdA)
 263         cP=dP=0;
 264       if(!fitdW && !fitddA)
 265         eP=fP=0;
 266
 267       move = aP*aP + bP*bP;
 268
 269       /* we're fitting to the remaining error; add the fit to date
 270          back in to relate our newest incremental results to the
 271          global fit so far.  Note that this does not include W (or dW
 272          if it is also recentered), as they're already 'subtracted'
 273          when the bases are recentered each iteration */
 274       aP += toAi(c->A, c->P);
 275       bP += toBi(c->A, c->P);
 276       cP += dAtoCi(c->P,c->dA);
 277       dP += dAtoDi(c->P,c->dA);
 278       eP += ddAtoEi(c->P,c->ddA);
 279       fP += ddAtoFi(c->P,c->ddA);
 280
 281       /* guard overflow */
 282       if((aP*aP + bP*bP)>1e10 ||
 283          (cP*cP + dP*dP)>1e10 ||
 284          (eP*eP + fP*fP)>1e10){
 285         /* mark this chirp inactive */
 286         c->A=-1;
 287         continue;
 288       }
 289
 290       {
 291         chirp old = *c;
 292         float cc=0,dd=0;
 293         float ee=0,ff=0;
 294
 295         /* save new estimate */
 296         c->A = toA(aP,bP);
 297         c->P = toP(aP,bP);
 298         c->W += fit_W_alpha*(fitW ? toW(aP,bP,cP,dP) : 0);
 299         c->dA = (fitdA ? todA(aP,bP,cP,dP) : 0);
 300         c->dW += fit_dW_alpha*(fitdW ? todW(aP,bP,eP,fP) : 0);
 301         c->ddA = (fitddA ? toddA(aP,bP,eP,fP) : 0);
 302
 303         /* base convergence on basis projection movement this
 304            iteration, but only consider the contribution of terms we fit. */
 305
 306         if(fitW){
 307           float W = c->W - old.W;
 308           cc += WtoCi(c->A,c->P,W);
 309           dd += WtoDi(c->A,c->P,W);
 310         }
 311         if(fitdA){
 312           float dA = c->dA - old.dA;
 313           cc += dAtoCi(c->P,dA);
 314           dd += dAtoDi(c->P,dA);
 315         }
 316         if(fitdW){
 317           float dW = c->dW - old.dW;
 318           ee += dWtoEi(c->A,c->P,dW);
 319           ff += dWtoFi(c->A,c->P,dW);
 320         }
 321         if(fitddA){
 322           float ddA = c->ddA - old.ddA;
 323           ee += ddAtoEi(c->P,ddA);
 324           ff += ddAtoFi(c->P,ddA);
 325         }
 326         move = move + cc*cc + dd*dd + ee*ee + ff*ff;
 327         if(fit_limit>0 && move>fit_limit*fit_limit)flag=1;
 328         if(fit_limit<0 && move>1e-14)flag=1;
 329       }
 330
 331       if(bound_edges){
 332         /* do not allow negative or trans-Nyquist center frequencies */
 333         if(c->W<0){
 334           c->W = 0; /* clamp frequency to 0 (DC) */
 335           c->dW = 0; /* if W is 0, the chirp rate must also be 0 to
 336                                  avoid negative frequencies */
 337         }
 338         if(c->W>M_PI){
 339           c->W = M_PI; /* clamp frequency to Nyquist */
 340           c->dW = 0; /* if W is at Nyquist, the chirp rate must
 341                            also be 0 to avoid trans-Nyquist
 342                            frequencies */
 343         }
 344         /* Just like with the center frequency, don't allow the
 345            chirp rate (dW) parameter to cross over to a negative
 346            instantaneous frequency */
 347         if(c->W+c->dW*len < 0)
 348           c->dW = -c->W/len;
 349         if(c->W-c->dW*len < 0)
 350           c->dW = c->W/len;
 351         /* ...or exceed Nyquist */
 352         if(c->W + c->dW*len > M_PI)
 353           c->dW = M_PI/len - c->W/len;
 354         if(c->W - c->dW*len > M_PI)
 355           c->dW = c->W/len - M_PI/len;
 356       }
 357
 358       /* update the reconstruction/residue vectors with new fit */
 359       {
 360         for(j=0;j<len;j++){
 361           double jj = j-len*.5+.5;
 362           float a = c->A + (c->dA + c->ddA*jj)*jj;
 363           float v = a*cos(c->P + (c->W + c->dW*jj)*jj);
 364           r[j] -= v*window[j];
 365           y[j] += v;
 366         }
 367       }
 368     }
 369     if(flag)iter_limit--;
 370   }
 371   return iter_limit;
 372 }
 373
 374
 375 /* partial-nonlinear estimation iterator; it restarts the basis
 376    functions (W only).  Reconstruction and error are approximated from
 377    basis (as done in the original paper). Basis function is not
 378    chirped, regardless of initial dW estimate */
 379 static int partial_nonlinear_iterate(const float *x,
 380                                      const float *window,
 381                                      int len,
 382                                      chirp *ch,
 383                                      int n,
 384
 385                                      float fit_limit,
 386                                      int   iter_limit,
 387                                      int   fit_gs,
 388
 389                                      int fitW,
 390                                      int fitdA,
 391                                      int fitdW,
 392                                      int fitddA,
 393                                      float fit_W_alpha,
 394
 395                                      int symm_norm,
 396                                      float E0,
 397                                      float E1,
 398                                      float E2){
 399   int i,j;
 400   float y[len];
 401   int flag=1;
 402
 403   float *cos_table[n];
 404   float *sin_table[n];
 405   float *tcos_table[n];
 406   float *tsin_table[n];
 407   float *ttcos_table[n];
 408   float *ttsin_table[n];
 409   float cosE[n], sinE[n];
 410   float tcosE[n], tsinE[n];
 411   float ttcosE[n], ttsinE[n];
 412   float tcosC2[n], tsinC2[n];
 413   float ttcosC2[n], ttsinC2[n];
 414   float ttcosC3[n], ttsinC3[n];
 415
 416   float ai[n], bi[n];
 417   float ci[n], di[n];
 418   float ei[n], fi[n];
 419
 420   for (i=0;i<n;i++){
 421     cos_table[i]=calloc(len,sizeof(**cos_table));
 422     sin_table[i]=calloc(len,sizeof(**sin_table));
 423     tcos_table[i]=calloc(len,sizeof(**tcos_table));
 424     tsin_table[i]=calloc(len,sizeof(**tsin_table));
 425     ttcos_table[i]=calloc(len,sizeof(**ttcos_table));
 426     ttsin_table[i]=calloc(len,sizeof(**ttsin_table));
 427   }
 428
 429   /* outer fit iteration */
 430   while(flag && iter_limit>0){
 431     flag=0;
 432     memset(y,0,sizeof(y));
 433
 434     /* Sort chirps by descending amplitude */
 435     qsort(ch, n, sizeof(*ch), cmp_descending_A);
 436
 437     /* Calculate new basis functions */
 438     for (i=0;i<n;i++){
 439       float tmpa=0;
 440       float tmpb=0;
 441       float tmpc=0;
 442       float tmpd=0;
 443       float tmpe=0;
 444       float tmpf=0;
 445
 446       float tmpc2=0;
 447       float tmpd2=0;
 448       float tmpe3=0;
 449       float tmpf3=0;
 450
 451       if(ch[i].A==-1) continue;
 452
 453       /* chirp param -> basis acc */
 454       ai[i] = toAi(ch[i].A, ch[i].P);
 455       bi[i] = toBi(ch[i].A, ch[i].P);
 456       ci[i] = dAtoCi(ch[i].P, ch[i].dA);
 457       di[i] = dAtoDi(ch[i].P, ch[i].dA);
 458       ei[i] = ddAtoEi(ch[i].P, ch[i].ddA) + dWtoEi(ch[i].A, ch[i].P, ch[i].dW);
 459       fi[i] = ddAtoFi(ch[i].P, ch[i].ddA) + dWtoFi(ch[i].A, ch[i].P, ch[i].dW);
 460
 461       for (j=0;j<len;j++){
 462         double jj = j-len/2.+.5;
 463         double c,s;
 464         sincos(ch[i].W*jj,&s,&c);
 465
 466         /* save basis funcs */
 467         /* calc reconstruction vector */
 468         y[j] +=
 469           ai[i]*(cos_table[i][j] = c) +
 470           bi[i]*(sin_table[i][j] = s) +
 471           ci[i]*(tcos_table[i][j] = jj*c) +
 472           di[i]*(tsin_table[i][j] = jj*s) +
 473           ei[i]*(ttcos_table[i][j] = jj*jj*c) +
 474           fi[i]*(ttsin_table[i][j] = jj*jj*s);
 475
 476         /* The modulation terms */
 477         tmpc += tcos_table[i][j]*tcos_table[i][j]*window[j]*window[j];
 478         tmpd += tsin_table[i][j]*tsin_table[i][j]*window[j]*window[j];
 479
 480         if(!symm_norm){
 481           /* The sinusoidal terms */
 482           tmpa += cos_table[i][j]*cos_table[i][j]*window[j]*window[j];
 483           tmpb += sin_table[i][j]*sin_table[i][j]*window[j]*window[j];
 484
 485           /* The second order modulations */
 486           tmpe += ttcos_table[i][j]*ttcos_table[i][j]*window[j]*window[j];
 487           tmpf += ttsin_table[i][j]*ttsin_table[i][j]*window[j]*window[j];
 488         }
 489
 490         /* gs fit terms */
 491         if(fit_gs){
 492           tmpc2 += cos_table[i][j]*tcos_table[i][j]*window[j]*window[j];
 493           tmpd2 += sin_table[i][j]*tsin_table[i][j]*window[j]*window[j];
 494           tmpe3 += tcos_table[i][j]*ttcos_table[i][j]*window[j]*window[j];
 495           tmpf3 += tsin_table[i][j]*ttsin_table[i][j]*window[j]*window[j];
 496         }
 497       }
 498
 499       /* Set basis normalizations */
 500       if(symm_norm){
 501         cosE[i] = sinE[i] = E0;
 502         tcosE[i] = tsinE[i] = E1;
 503         ttcosE[i] = ttsinE[i] = E2;
 504       }else{
 505         cosE[i] = (tmpa>1e-20f ? 1.f/tmpa : 0);
 506         sinE[i] = (tmpb>1e-20f ? 1.f/tmpb : 0);
 507         tcosE[i] = (tmpc>1e-20f ? 1.f/tmpc : 0);
 508         tsinE[i] = (tmpd>1e-20f ? 1.f/tmpd : 0);
 509         ttcosE[i] = (tmpe>1e-20f ? 1.f/tmpe : 0);
 510         ttsinE[i] = (tmpf>1e-20f ? 1.f/tmpf : 0);
 511       }
 512
 513       if(fit_gs){
 514         /* set gs fit terms */
 515         tcosC2[i] = tmpc2;
 516         tsinC2[i] = tmpd2;
 517         ttcosC2[i] = tmpc;
 518         ttsinC2[i] = tmpd;
 519         ttcosC3[i] = tmpe3;
 520         ttsinC3[i] = tmpf3;
 521       }
 522     }
 523
 524     for (i=0;i<n;i++){
 525
 526       float tmpa=0, tmpb=0;
 527       float tmpc=0, tmpd=0;
 528       float tmpe=0, tmpf=0;
 529
 530       /* (Sort of) project the residual on the four basis functions */
 531       for (j=0;j<len;j++){
 532         float r = (x[j]-y[j])*window[j]*window[j];
 533         tmpa += r*cos_table[i][j];
 534         tmpb += r*sin_table[i][j];
 535         tmpc += r*tcos_table[i][j];
 536         tmpd += r*tsin_table[i][j];
 537         tmpe += r*ttcos_table[i][j];
 538         tmpf += r*ttsin_table[i][j];
 539       }
 540
 541       tmpa*=cosE[i];
 542       tmpb*=sinE[i];
 543
 544       if(fit_gs){
 545         tmpc -= tmpa*tcosC2[i];
 546         tmpd -= tmpb*tsinC2[i];
 547       }
 548
 549       tmpc*=tcosE[i];
 550       tmpd*=tsinE[i];
 551
 552       if(fit_gs){
 553         tmpe -= tmpa*ttcosC2[i] + tmpc*ttcosC3[i];
 554         tmpf -= tmpb*ttsinC2[i] + tmpd*ttsinC3[i];
 555       }
 556
 557       tmpe*=ttcosE[i];
 558       tmpf*=ttsinE[i];
 559
 560       if(!fitW && !fitdA)
 561         tmpc=tmpd=0;
 562       if(!fitdW && !fitddA)
 563         tmpe=tmpf=0;
 564
 565       /* Update the signal approximation for the next iteration */
 566       for (j=0;j<len;j++){
 567         y[j] +=
 568           tmpa*cos_table[i][j]+
 569           tmpb*sin_table[i][j]+
 570           tmpc*tcos_table[i][j]+
 571           tmpd*tsin_table[i][j]+
 572           tmpe*ttcos_table[i][j]+
 573           tmpf*ttsin_table[i][j];
 574       }
 575
 576       ai[i] += tmpa;
 577       bi[i] += tmpb;
 578       ci[i] += tmpc;
 579       di[i] += tmpd;
 580       ei[i] += tmpe;
 581       fi[i] += tmpf;
 582
 583       /* guard overflow */
 584       if((ai[i]*ai[i] + bi[i]*bi[i])>1e10 ||
 585          (ci[i]*ci[i] + di[i]*di[i])>1e10 ||
 586          (ei[i]*ei[i] + fi[i]*fi[i])>1e10){
 587         for (j=0;j<len;j++){
 588           y[j] +=
 589             ai[i]*cos_table[i][j]+
 590             bi[i]*sin_table[i][j]+
 591             ci[i]*tcos_table[i][j]+
 592             di[i]*tsin_table[i][j]+
 593             ei[i]*ttcos_table[i][j]+
 594             fi[i]*ttsin_table[i][j];
 595         }
 596         ch[i].A=-1;
 597         continue;
 598       }
 599
 600       /* save new estimate */
 601       ch[i].A = toA(ai[i],bi[i]);
 602       ch[i].P = toP(ai[i],bi[i]);
 603       ch[i].W += fit_W_alpha*(fitW ? toW(ai[i],bi[i],ci[i],di[i]) : 0);
 604       ch[i].dA = (fitdA ? todA(ai[i],bi[i],ci[i],di[i]) : 0);
 605       ch[i].dW = (fitdW ? todW(ai[i],bi[i],ei[i],fi[i]) : 0);
 606       ch[i].ddA = (fitddA ? toddA(ai[i],bi[i],ei[i],fi[i]) : 0);
 607
 608       /* base convergence on basis projection movement this
 609          iteration, but only consider the contribution of terms we fit. */
 610       {
 611         float move;
 612         float cc=0,dd=0;
 613         float ee=0,ff=0;
 614         if(fitW){
 615           float W = toW(ai[i],bi[i],ci[i],di[i]);
 616           cc += WtoCi(ch[i].A,ch[i].P,W);
 617           dd += WtoDi(ch[i].A,ch[i].P,W);
 618         }
 619         if(fitdA){
 620           float dA = todA(ai[i],bi[i],tmpc,tmpd);
 621           cc += dAtoCi(ch[i].P,dA);
 622           dd += dAtoDi(ch[i].P,dA);
 623         }
 624         if(fitdW){
 625           float dW = todW(ai[i],bi[i],tmpe,tmpf);
 626           ee += dWtoEi(ch[i].A,ch[i].P,dW);
 627           ff += dWtoFi(ch[i].A,ch[i].P,dW);
 628         }
 629         if(fitddA){
 630           float ddA = toddA(ai[i],bi[i],tmpe,tmpf);
 631           ee += ddAtoEi(ch[i].P,ddA);
 632           ff += ddAtoFi(ch[i].P,ddA);
 633         }
 634         move = tmpa*tmpa + tmpb*tmpb + cc*cc + dd*dd + ee*ee + ff*ff;
 635         if(fit_limit>0 && move>fit_limit*fit_limit)flag=1;
 636         if(fit_limit<0 && move>1e-14)flag=1;
 637       }
 638     }
 639     if(flag)iter_limit--;
 640   }
 641
 642   for(i=0;i<n;i++){
 643     free(cos_table[i]);
 644     free(sin_table[i]);
 645     free(tcos_table[i]);
 646     free(tsin_table[i]);
 647     free(ttcos_table[i]);
 648     free(ttsin_table[i]);
 649   }
 650
 651   return iter_limit;
 652 }
 653
 654 /* linear estimation iterator; sets fixed basis functions for each
 655    chirp according to the passed in initial estimates. This code
 656    follows the paper as exactly as possible (reconstruction is
 657    estimated from basis, basis is not chirped regardless of initial
 658    estimate, etc). */
 659 static int linear_iterate(const float *x,
 660                           const float *window,
 661                           int len,
 662                           chirp *ch,
 663                           int n,
 664
 665                           float fit_limit,
 666                           int iter_limit,
 667                           int fit_gs,
 668
 669                           int fitW,
 670                           int fitdA,
 671                           int fitdW,
 672                           int fitddA,
 673                           int symm_norm,
 674                           float E0,
 675                           float E1,
 676                           float E2){
 677
 678   float *cos_table[n];
 679   float *sin_table[n];
 680   float *tcos_table[n];
 681   float *tsin_table[n];
 682   float *ttcos_table[n];
 683   float *ttsin_table[n];
 684   float cosE[n], sinE[n];
 685   float tcosE[n], tsinE[n];
 686   float ttcosE[n], ttsinE[n];
 687   float tcosC2[n], tsinC2[n];
 688   float ttcosC2[n], ttsinC2[n];
 689   float ttcosC3[n], ttsinC3[n];
 690
 691   float ai[n], bi[n];
 692   float ci[n], di[n];
 693   float ei[n], fi[n];
 694   int i,j;
 695   int flag=1;
 696   float y[len];
 697
 698   memset(y,0,sizeof(y));
 699
 700   for (i=0;i<n;i++){
 701     float tmpa=0;
 702     float tmpb=0;
 703     float tmpc=0;
 704     float tmpd=0;
 705     float tmpe=0;
 706     float tmpf=0;
 707
 708     float tmpc2=0;
 709     float tmpd2=0;
 710     float tmpe3=0;
 711     float tmpf3=0;
 712
 713     if(ch[i].A==-1)continue;
 714
 715     /* seed the basis accumulators from our passed in estimated parameters */
 716     /* Don't add in W; this is already included by the basis */
 717     ai[i] = toAi(ch[i].A, ch[i].P);
 718     bi[i] = toBi(ch[i].A, ch[i].P);
 719     ci[i] = dAtoCi(ch[i].P,ch[i].dA);
 720     di[i] = dAtoDi(ch[i].P,ch[i].dA);
 721     ei[i] = ddAtoEi(ch[i].P, ch[i].ddA) + dWtoEi(ch[i].A, ch[i].P, ch[i].dW);
 722     fi[i] = ddAtoFi(ch[i].P, ch[i].ddA) + dWtoFi(ch[i].A, ch[i].P, ch[i].dW);
 723
 724     /* Build a table for the basis functions at each frequency */
 725     cos_table[i]=calloc(len,sizeof(**cos_table));
 726     sin_table[i]=calloc(len,sizeof(**sin_table));
 727     tcos_table[i]=calloc(len,sizeof(**tcos_table));
 728     tsin_table[i]=calloc(len,sizeof(**tsin_table));
 729     ttcos_table[i]=calloc(len,sizeof(**ttcos_table));
 730     ttsin_table[i]=calloc(len,sizeof(**ttsin_table));
 731
 732     for (j=0;j<len;j++){
 733       double jj = j-len/2.+.5;
 734       double c,s;
 735       sincos(ch[i].W*jj,&s,&c);
 736
 737       /* save basis funcs */
 738       /* initial reconstruction */
 739       y[j] +=
 740         ai[i]*(cos_table[i][j] = c) +
 741         bi[i]*(sin_table[i][j] = s) +
 742         ci[i]*(tcos_table[i][j] = jj*c) +
 743         di[i]*(tsin_table[i][j] = jj*s) +
 744         ei[i]*(ttcos_table[i][j] = jj*jj*c) +
 745         fi[i]*(ttsin_table[i][j] = jj*jj*s);
 746
 747       /* The sinusoidal terms */
 748       tmpa += cos_table[i][j]*cos_table[i][j]*window[j]*window[j];
 749       tmpb += sin_table[i][j]*sin_table[i][j]*window[j]*window[j];
 750
 751       /* The modulation terms */
 752       tmpc += tcos_table[i][j]*tcos_table[i][j]*window[j]*window[j];
 753       tmpd += tsin_table[i][j]*tsin_table[i][j]*window[j]*window[j];
 754
 755       /* The second order modulations */
 756       tmpe += ttcos_table[i][j]*ttcos_table[i][j]*window[j]*window[j];
 757       tmpf += ttsin_table[i][j]*ttsin_table[i][j]*window[j]*window[j];
 758
 759       /* gs fit terms */
 760       tmpc2 += cos_table[i][j]*tcos_table[i][j]*window[j]*window[j];
 761       tmpd2 += sin_table[i][j]*tsin_table[i][j]*window[j]*window[j];
 762       tmpe3 += tcos_table[i][j]*ttcos_table[i][j]*window[j]*window[j];
 763       tmpf3 += tsin_table[i][j]*ttsin_table[i][j]*window[j]*window[j];
 764
 765     }
 766
 767     /* Set basis normalizations */
 768     if(symm_norm){
 769       cosE[i] = sinE[i] = E0;
 770       tcosE[i] = tsinE[i] = E1;
 771       ttcosE[i] = ttsinE[i] = E2;
 772     }else{
 773       cosE[i] = (tmpa>1e-20f ? 1.f/tmpa : 0);
 774       sinE[i] = (tmpb>1e-20f ? 1.f/tmpb : 0);
 775       tcosE[i] = (tmpc>1e-20f ? 1.f/tmpc : 0);
 776       tsinE[i] = (tmpd>1e-20f ? 1.f/tmpd : 0);
 777       ttcosE[i] = (tmpe>1e-20f ? 1.f/tmpe : 0);
 778       ttsinE[i] = (tmpf>1e-20f ? 1.f/tmpf : 0);
 779     }
 780
 781     /* set gs fit terms */
 782     tcosC2[i] = tmpc2;
 783     tsinC2[i] = tmpd2;
 784     ttcosC2[i] = tmpc;
 785     ttsinC2[i] = tmpd;
 786     ttcosC3[i] = tmpe3;
 787     ttsinC3[i] = tmpf3;
 788
 789   }
 790
 791   while(flag && iter_limit>0){
 792     flag=0;
 793
 794     for (i=0;i<n;i++){
 795
 796       float tmpa=0, tmpb=0;
 797       float tmpc=0, tmpd=0;
 798       float tmpe=0, tmpf=0;
 799
 800       /* (Sort of) project the residual on the four basis functions */
 801       for (j=0;j<len;j++){
 802         float r = (x[j]-y[j])*window[j]*window[j];
 803         tmpa += r*cos_table[i][j];
 804         tmpb += r*sin_table[i][j];
 805         tmpc += r*tcos_table[i][j];
 806         tmpd += r*tsin_table[i][j];
 807         tmpe += r*ttcos_table[i][j];
 808         tmpf += r*ttsin_table[i][j];
 809       }
 810
 811       tmpa*=cosE[i];
 812       tmpb*=sinE[i];
 813
 814       if(fit_gs){
 815         tmpc -= tmpa*tcosC2[i];
 816         tmpd -= tmpb*tsinC2[i];
 817       }
 818
 819       tmpc*=tcosE[i];
 820       tmpd*=tsinE[i];
 821
 822       if(fit_gs){
 823         tmpe -= tmpa*ttcosC2[i] + tmpc*ttcosC3[i];
 824         tmpf -= tmpb*ttsinC2[i] + tmpd*ttsinC3[i];
 825       }
 826
 827       tmpe*=ttcosE[i];
 828       tmpf*=ttsinE[i];
 829
 830       if(!fitW && !fitdA)
 831         tmpc=tmpd=0;
 832       if(!fitdW && !fitddA)
 833         tmpe=tmpf=0;
 834
 835       /* Update the signal approximation for the next iteration */
 836       for (j=0;j<len;j++){
 837         y[j] +=
 838           tmpa*cos_table[i][j]+
 839           tmpb*sin_table[i][j]+
 840           tmpc*tcos_table[i][j]+
 841           tmpd*tsin_table[i][j]+
 842           tmpe*ttcos_table[i][j]+
 843           tmpf*ttsin_table[i][j];
 844       }
 845
 846       ai[i] += tmpa;
 847       bi[i] += tmpb;
 848       ci[i] += tmpc;
 849       di[i] += tmpd;
 850       ei[i] += tmpe;
 851       fi[i] += tmpf;
 852
 853       /* guard overflow */
 854       if((ai[i]*ai[i] + bi[i]*bi[i])>1e10 ||
 855          (ci[i]*ci[i] + di[i]*di[i])>1e10 ||
 856          (ei[i]*ei[i] + fi[i]*fi[i])>1e10){
 857         for (j=0;j<len;j++){
 858           y[j] +=
 859             ai[i]*cos_table[i][j]+
 860             bi[i]*sin_table[i][j]+
 861             ci[i]*tcos_table[i][j]+
 862             di[i]*tsin_table[i][j]+
 863             ei[i]*ttcos_table[i][j]+
 864             fi[i]*ttsin_table[i][j];
 865         }
 866         ch[i].A=-1;
 867         continue;
 868       }
 869
 870       /* base convergence on basis projection movement this
 871          iteration */
 872       {
 873         float move = tmpa*tmpa + tmpb*tmpb +tmpc*tmpc + tmpd*tmpd + tmpe*tmpe + tmpf*tmpf;
 874
 875         if(fit_limit>0 && move>fit_limit*fit_limit)flag=1;
 876         if(fit_limit<0 && move>1e-14)flag=1;
 877       }
 878
 879
 880     }
 881     if(flag)iter_limit--;
 882   }
 883
 884   for(i=0;i<n;i++){
 885     if(ch[i].A!=-1){
 886       ch[i].A = toA(ai[i],bi[i]);
 887       ch[i].P = toP(ai[i],bi[i]);
 888       ch[i].W += (fitW ? toW(ai[i],bi[i],ci[i],di[i]) : 0);
 889       ch[i].dA = (fitdA ? todA(ai[i],bi[i],ci[i],di[i]) : 0);
 890       ch[i].dW += (fitdW ? todW(ai[i],bi[i],ei[i],fi[i]) : 0);
 891       ch[i].ddA = (fitddA ? toddA(ai[i],bi[i],ei[i],fi[i]) : 0);
 892     }else{
 893       memset(ch+i,0,sizeof(ch[i]));
 894       ch[i].A=-1;
 895     }
 896
 897     free(cos_table[i]);
 898     free(sin_table[i]);
 899     free(tcos_table[i]);
 900     free(tsin_table[i]);
 901     free(ttcos_table[i]);
 902     free(ttsin_table[i]);
 903   }
 904
 905   return iter_limit;
 906 }
 907
 908 /* Performs an iterative chirp estimation using the passed
 909    in chirp paramters as an initial estimate.
 910
 911    x: input signal (unwindowed)
 912    y: output reconstruction (unwindowed)
 913    window: window to apply to input and bases during fitting process
 914    len: length of x,y,r and window vectors
 915    c: chirp initial estimation inputs, chirp fit outputs (sorted in frequency order)
 916    n: number of chirps
 917    iter_limit: maximum allowable number of iterations
 918    fit_limit: desired fit quality limit
 919
 920    fitW : flag indicating that fitting should include the W parameter.
 921    fitdA : flag indicating that fitting should include the dA parameter.
 922    fitdW : flag indicating that fitting should include the dW parameter.
 923    fitddA : flag indicating that fitting should include the ddA parameter.
 924    symm_norm
 925    int fit_gs
 926    int bound_zero
 927
 928    Input estimates affect convergence region and speed and fit
 929    quality; precise effects depend on the fit estimation algorithm
 930    selected.  Note that non-zero initial values for dA, dW or ddA are
 931    ignored when fitdA, fitdW, fitddA are unset.  An initial value for
 932    W is always used even when W is left unfitted.
 933
 934    Fitting terminates when no fit parameter changes by more than
 935    fit_limit in an iteration or when the fit loop reaches the
 936    iteration limit.  The fit parameters as checked are scaled over the
 937    length of the block.
 938
 939 */
 940
 941 int estimate_chirps(const float *x,
 942                     const float *window,
 943                     int len,
 944                     chirp *c,
 945                     int n,
 946
 947                     float fit_limit,
 948                     int iter_limit,
 949                     int fit_gs,
 950
 951                     int fitW,
 952                     int fitdA,
 953                     int fitdW,
 954                     int fitddA,
 955                     int nonlinear,
 956                     float fit_W_alpha,
 957                     float fit_dW_alpha,
 958                     int symm_norm,
 959                     int bound_zero
 960                     ){
 961
 962   int i;
 963   float E0=0, E1=0, E2=0;
 964
 965   if(symm_norm){
 966     for(i=0;i<len;i++){
 967       float jj = i-len*.5+.5;
 968       float w2 = window[i]*window[i];
 969       float w2j2 = w2*jj*jj;
 970       float w2j4 = w2j2*jj*jj;
 971       E0 += w2;
 972       E1 += w2j2;
 973       E2 += w2j4;
 974     }
 975     E0=2/E0;
 976     E1=2/E1;
 977     E2=2/E2;
 978   }
 979
 980   /* zero out initial estimates for params we'll not fit */
 981   for(i=0;i<n;i++){
 982     if(!fitdA) c[i].dA=0.;
 983     if(!fitdW) c[i].dW=0.;
 984     if(!fitddA) c[i].ddA=0.;
 985   }
 986
 987   switch(nonlinear){
 988   case 0:
 989     iter_limit = linear_iterate(x,window,len,c,n,
 990                                 fit_limit,iter_limit,fit_gs,
 991                                 fitW,fitdA,fitdW,fitddA,
 992                                 symm_norm,E0,E1,E2);
 993     break;
 994   case 1:
 995     iter_limit = partial_nonlinear_iterate(x,window,len,c,n,
 996                                            fit_limit,iter_limit,fit_gs,
 997                                            fitW,fitdA,fitdW,fitddA,
 998                                            fit_W_alpha,
 999                                            symm_norm,E0,E1,E2);
1000     break;
1001   case 2:
1002     iter_limit = full_nonlinear_iterate(x,window,len,c,n,
1003                                         fit_limit,iter_limit,fit_gs,
1004                                         fitW,fitdA,fitdW,fitddA,
1005                                         fit_W_alpha,fit_dW_alpha,
1006                                         symm_norm,E0,E1,E2,
1007                                         bound_zero);
1008     break;
1009   }
1010
1011   /* Sort by ascending frequency */
1012   qsort(c, n, sizeof(*c), cmp_ascending_W);
1013
1014   return iter_limit;
1015 }
1016
1017 /* advances/extrapolates the passed in chirps so that the params are
1018    centered forward in time by len samples. */
1019
1020 void advance_chirps(chirp *c, int n, int len){
1021   int i;
1022   for(i=0;i<n;i++){
1023     c[i].A += c[i].dA*len;
1024     c[i].P += (c[i].W+c[i].dW*len)*len;
1025     c[i].W += c[i].dW*len*2;
1026   }
1027 }
1028
1029 /* dead, but leave it around; does a slow, brute force DCFT */
1030 void slow_dcft_row(float *x, float *y, int k, int n){
1031   int i,j;
1032   for(i=0;i<n/2+1;i++){ /* frequency loop */
1033     float real=0.,imag=0.;
1034     for(j=0;j<n;j++){ /* multiply loop */
1035       float s,c;
1036       float jj = (j-n/2+.5)/n;
1037       sincosf(2.*M_PI*(i+k*jj)*jj,&s,&c);
1038       real += c*x[j];
1039       imag += s*x[j];
1040     }
1041     y[i] = hypot(real,imag);
1042   }
1043 }
1044