16f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin/* 26f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved. 36f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * 46f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * Use of this source code is governed by a BSD-style license 56f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * that can be found in the LICENSE file in the root of the source 66f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * tree. An additional intellectual property rights grant can be found 76f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * in the file PATENTS. All contributing project authors may 86f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * be found in the AUTHORS file in the root of the source tree. 96f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin */ 106f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 116f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin#include "pitch_estimator.h" 126f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 136f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin#include <math.h> 146f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin#include <memory.h> 156f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin#ifdef WEBRTC_ANDROID 166f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin#include <stdlib.h> 176f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin#endif 186f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 196f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkinstatic const double kInterpolWin[8] = {-0.00067556028640, 0.02184247643159, -0.12203175715679, 0.60086484101160, 206f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 0.60086484101160, -0.12203175715679, 0.02184247643159, -0.00067556028640}; 216f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 226f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin/* interpolation filter */ 236f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin__inline static void IntrepolFilter(double *data_ptr, double *intrp) 246f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin{ 256f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *intrp = kInterpolWin[0] * data_ptr[-3]; 266f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *intrp += kInterpolWin[1] * data_ptr[-2]; 276f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *intrp += kInterpolWin[2] * data_ptr[-1]; 286f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *intrp += kInterpolWin[3] * data_ptr[0]; 296f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *intrp += kInterpolWin[4] * data_ptr[1]; 306f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *intrp += kInterpolWin[5] * data_ptr[2]; 316f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *intrp += kInterpolWin[6] * data_ptr[3]; 326f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *intrp += kInterpolWin[7] * data_ptr[4]; 336f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin} 346f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 356f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 366f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin/* 2D parabolic interpolation */ 376f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin/* probably some 0.5 factors can be eliminated, and the square-roots can be removed from the Cholesky fact. */ 386f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin__inline static void Intrpol2D(double T[3][3], double *x, double *y, double *peak_val) 396f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin{ 406f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double c, b[2], A[2][2]; 416f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double t1, t2, d; 426f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double delta1, delta2; 436f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 446f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 456f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin // double T[3][3] = {{-1.25, -.25,-.25}, {-.25, .75, .75}, {-.25, .75, .75}}; 466f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin // should result in: delta1 = 0.5; delta2 = 0.0; peak_val = 1.0 476f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 486f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin c = T[1][1]; 496f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin b[0] = 0.5 * (T[1][2] + T[2][1] - T[0][1] - T[1][0]); 506f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin b[1] = 0.5 * (T[1][0] + T[2][1] - T[0][1] - T[1][2]); 516f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin A[0][1] = -0.5 * (T[0][1] + T[2][1] - T[1][0] - T[1][2]); 526f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin t1 = 0.5 * (T[0][0] + T[2][2]) - c; 536f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin t2 = 0.5 * (T[2][0] + T[0][2]) - c; 546f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin d = (T[0][1] + T[1][2] + T[1][0] + T[2][1]) - 4.0 * c - t1 - t2; 556f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin A[0][0] = -t1 - 0.5 * d; 566f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin A[1][1] = -t2 - 0.5 * d; 576f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 586f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* deal with singularities or ill-conditioned cases */ 596f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if ( (A[0][0] < 1e-7) || ((A[0][0] * A[1][1] - A[0][1] * A[0][1]) < 1e-7) ) { 606f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *peak_val = T[1][1]; 616f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin return; 626f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 636f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 646f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* Cholesky decomposition: replace A by upper-triangular factor */ 656f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin A[0][0] = sqrt(A[0][0]); 666f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin A[0][1] = A[0][1] / A[0][0]; 676f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin A[1][1] = sqrt(A[1][1] - A[0][1] * A[0][1]); 686f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 696f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* compute [x; y] = -0.5 * inv(A) * b */ 706f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin t1 = b[0] / A[0][0]; 716f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin t2 = (b[1] - t1 * A[0][1]) / A[1][1]; 726f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin delta2 = t2 / A[1][1]; 736f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin delta1 = 0.5 * (t1 - delta2 * A[0][1]) / A[0][0]; 746f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin delta2 *= 0.5; 756f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 766f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* limit norm */ 776f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin t1 = delta1 * delta1 + delta2 * delta2; 786f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (t1 > 1.0) { 796f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin delta1 /= t1; 806f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin delta2 /= t1; 816f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 826f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 836f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *peak_val = 0.5 * (b[0] * delta1 + b[1] * delta2) + c; 846f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 856f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *x += delta1; 866f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *y += delta2; 876f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin} 886f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 896f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 906f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkinstatic void PCorr(const double *in, double *outcorr) 916f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin{ 926f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double sum, ysum, prod; 936f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin const double *x, *inptr; 946f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin int k, n; 956f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 966f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin //ysum = 1e-6; /* use this with float (i.s.o. double)! */ 976f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ysum = 1e-13; 986f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin sum = 0.0; 996f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin x = in + PITCH_MAX_LAG/2 + 2; 1006f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (n = 0; n < PITCH_CORR_LEN2; n++) { 1016f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ysum += in[n] * in[n]; 1026f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin sum += x[n] * in[n]; 1036f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 1046f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1056f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin outcorr += PITCH_LAG_SPAN2 - 1; /* index of last element in array */ 1066f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *outcorr = sum / sqrt(ysum); 1076f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1086f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 1; k < PITCH_LAG_SPAN2; k++) { 1096f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ysum -= in[k-1] * in[k-1]; 1106f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ysum += in[PITCH_CORR_LEN2 + k - 1] * in[PITCH_CORR_LEN2 + k - 1]; 1116f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin sum = 0.0; 1126f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin inptr = &in[k]; 1136f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin prod = x[0] * inptr[0]; 1146f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (n = 1; n < PITCH_CORR_LEN2; n++) { 1156f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin sum += prod; 1166f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin prod = x[n] * inptr[n]; 1176f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 1186f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin sum += prod; 1196f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin outcorr--; 1206f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin *outcorr = sum / sqrt(ysum); 1216f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 1226f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin} 1236f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1246f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1256f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkinvoid WebRtcIsac_InitializePitch(const double *in, 1266f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin const double old_lag, 1276f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin const double old_gain, 1286f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin PitchAnalysisStruct *State, 1296f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double *lags) 1306f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin{ 1316f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double buf_dec[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2+2]; 1326f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double ratio, log_lag, gain_bias; 1336f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double bias; 1346f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double corrvec1[PITCH_LAG_SPAN2]; 1356f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double corrvec2[PITCH_LAG_SPAN2]; 1366f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin int m, k; 1376f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin // Allocating 10 extra entries at the begining of the CorrSurf 1386f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double corrSurfBuff[10 + (2*PITCH_BW+3)*(PITCH_LAG_SPAN2+4)]; 1396f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double* CorrSurf[2*PITCH_BW+3]; 1406f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double *CorrSurfPtr1, *CorrSurfPtr2; 1416f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double LagWin[3] = {0.2, 0.5, 0.98}; 1426f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin int ind1, ind2, peaks_ind, peak, max_ind; 1436f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin int peaks[PITCH_MAX_NUM_PEAKS]; 1446f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double adj, gain_tmp; 1456f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double corr, corr_max; 1466f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double intrp_a, intrp_b, intrp_c, intrp_d; 1476f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double peak_vals[PITCH_MAX_NUM_PEAKS]; 1486f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double lags1[PITCH_MAX_NUM_PEAKS]; 1496f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double lags2[PITCH_MAX_NUM_PEAKS]; 1506f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double T[3][3]; 1516f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin int row; 1526f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1536f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for(k = 0; k < 2*PITCH_BW+3; k++) 1546f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin { 1556f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurf[k] = &corrSurfBuff[10 + k * (PITCH_LAG_SPAN2+4)]; 1566f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 1576f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* reset CorrSurf matrix */ 1586f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin memset(corrSurfBuff, 0, sizeof(double) * (10 + (2*PITCH_BW+3) * (PITCH_LAG_SPAN2+4))); 1596f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1606f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin //warnings -DH 1616f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin max_ind = 0; 1626f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin peak = 0; 1636f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1646f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* copy old values from state buffer */ 1656f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin memcpy(buf_dec, State->dec_buffer, sizeof(double) * (PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2)); 1666f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1676f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* decimation; put result after the old values */ 1686f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin WebRtcIsac_DecimateAllpass(in, State->decimator_state, PITCH_FRAME_LEN, 1696f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin &buf_dec[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2]); 1706f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1716f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* low-pass filtering */ 1726f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2; k < PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2+2; k++) 1736f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin buf_dec[k] += 0.75 * buf_dec[k-1] - 0.25 * buf_dec[k-2]; 1746f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1756f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* copy end part back into state buffer */ 1766f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin memcpy(State->dec_buffer, buf_dec+PITCH_FRAME_LEN/2, sizeof(double) * (PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2)); 1776f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1786f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* compute correlation for first and second half of the frame */ 1796f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin PCorr(buf_dec, corrvec1); 1806f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin PCorr(buf_dec + PITCH_CORR_STEP2, corrvec2); 1816f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1826f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* bias towards pitch lag of previous frame */ 1836f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin log_lag = log(0.5 * old_lag); 1846f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin gain_bias = 4.0 * old_gain * old_gain; 1856f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (gain_bias > 0.8) gain_bias = 0.8; 1866f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < PITCH_LAG_SPAN2; k++) 1876f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin { 1886f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ratio = log((double) (k + (PITCH_MIN_LAG/2-2))) - log_lag; 1896f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin bias = 1.0 + gain_bias * exp(-5.0 * ratio * ratio); 1906f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corrvec1[k] *= bias; 1916f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 1926f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 1936f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* taper correlation functions */ 1946f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < 3; k++) { 1956f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin gain_tmp = LagWin[k]; 1966f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corrvec1[k] *= gain_tmp; 1976f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corrvec2[k] *= gain_tmp; 1986f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corrvec1[PITCH_LAG_SPAN2-1-k] *= gain_tmp; 1996f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corrvec2[PITCH_LAG_SPAN2-1-k] *= gain_tmp; 2006f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2016f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 2026f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr_max = 0.0; 2036f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* fill middle row of correlation surface */ 2046f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ind1 = 0; 2056f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ind2 = 0; 2066f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr1 = &CorrSurf[PITCH_BW][2]; 2076f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < PITCH_LAG_SPAN2; k++) { 2086f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr = corrvec1[ind1++] + corrvec2[ind2++]; 2096f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr1[k] = corr; 2106f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (corr > corr_max) { 2116f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr_max = corr; /* update maximum */ 2126f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); 2136f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2146f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2156f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* fill first and last rows of correlation surface */ 2166f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ind1 = 0; 2176f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ind2 = PITCH_BW; 2186f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr1 = &CorrSurf[0][2]; 2196f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr2 = &CorrSurf[2*PITCH_BW][PITCH_BW+2]; 2206f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < PITCH_LAG_SPAN2-PITCH_BW; k++) { 2216f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ratio = ((double) (ind1 + 12)) / ((double) (ind2 + 12)); 2226f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin adj = 0.2 * ratio * (2.0 - ratio); /* adjustment factor; inverse parabola as a function of ratio */ 2236f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr = adj * (corrvec1[ind1] + corrvec2[ind2]); 2246f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr1[k] = corr; 2256f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (corr > corr_max) { 2266f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr_max = corr; /* update maximum */ 2276f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); 2286f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2296f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr = adj * (corrvec1[ind2++] + corrvec2[ind1++]); 2306f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr2[k] = corr; 2316f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (corr > corr_max) { 2326f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr_max = corr; /* update maximum */ 2336f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin max_ind = (int)(&CorrSurfPtr2[k] - &CorrSurf[0][0]); 2346f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2356f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2366f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* fill second and next to last rows of correlation surface */ 2376f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ind1 = 0; 2386f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ind2 = PITCH_BW-1; 2396f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr1 = &CorrSurf[1][2]; 2406f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr2 = &CorrSurf[2*PITCH_BW-1][PITCH_BW+1]; 2416f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < PITCH_LAG_SPAN2-PITCH_BW+1; k++) { 2426f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ratio = ((double) (ind1 + 12)) / ((double) (ind2 + 12)); 2436f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin adj = 0.9 * ratio * (2.0 - ratio); /* adjustment factor; inverse parabola as a function of ratio */ 2446f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr = adj * (corrvec1[ind1] + corrvec2[ind2]); 2456f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr1[k] = corr; 2466f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (corr > corr_max) { 2476f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr_max = corr; /* update maximum */ 2486f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); 2496f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2506f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr = adj * (corrvec1[ind2++] + corrvec2[ind1++]); 2516f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr2[k] = corr; 2526f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (corr > corr_max) { 2536f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr_max = corr; /* update maximum */ 2546f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin max_ind = (int)(&CorrSurfPtr2[k] - &CorrSurf[0][0]); 2556f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2566f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2576f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* fill remainder of correlation surface */ 2586f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (m = 2; m < PITCH_BW; m++) { 2596f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ind1 = 0; 2606f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ind2 = PITCH_BW - m; /* always larger than ind1 */ 2616f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr1 = &CorrSurf[m][2]; 2626f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr2 = &CorrSurf[2*PITCH_BW-m][PITCH_BW+2-m]; 2636f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < PITCH_LAG_SPAN2-PITCH_BW+m; k++) { 2646f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin ratio = ((double) (ind1 + 12)) / ((double) (ind2 + 12)); 2656f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin adj = ratio * (2.0 - ratio); /* adjustment factor; inverse parabola as a function of ratio */ 2666f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr = adj * (corrvec1[ind1] + corrvec2[ind2]); 2676f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr1[k] = corr; 2686f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (corr > corr_max) { 2696f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr_max = corr; /* update maximum */ 2706f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); 2716f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2726f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr = adj * (corrvec1[ind2++] + corrvec2[ind1++]); 2736f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr2[k] = corr; 2746f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (corr > corr_max) { 2756f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr_max = corr; /* update maximum */ 2766f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin max_ind = (int)(&CorrSurfPtr2[k] - &CorrSurf[0][0]); 2776f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2786f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2796f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2806f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 2816f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* threshold value to qualify as a peak */ 2826f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr_max *= 0.6; 2836f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 2846f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin peaks_ind = 0; 2856f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* find peaks */ 2866f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (m = 1; m < PITCH_BW+1; m++) { 2876f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (peaks_ind == PITCH_MAX_NUM_PEAKS) break; 2886f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr1 = &CorrSurf[m][2]; 2896f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 2; k < PITCH_LAG_SPAN2-PITCH_BW-2+m; k++) { 2906f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr = CorrSurfPtr1[k]; 2916f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (corr > corr_max) { 2926f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if ( (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+5)]) && (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+4)]) ) { 2936f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if ( (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+4)]) && (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+5)]) ) { 2946f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* found a peak; store index into matrix */ 2956f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin peaks[peaks_ind++] = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); 2966f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (peaks_ind == PITCH_MAX_NUM_PEAKS) break; 2976f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2986f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 2996f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 3006f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 3016f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 3026f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (m = PITCH_BW+1; m < 2*PITCH_BW; m++) { 3036f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (peaks_ind == PITCH_MAX_NUM_PEAKS) break; 3046f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr1 = &CorrSurf[m][2]; 3056f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 2+m-PITCH_BW; k < PITCH_LAG_SPAN2-2; k++) { 3066f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr = CorrSurfPtr1[k]; 3076f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (corr > corr_max) { 3086f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if ( (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+5)]) && (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+4)]) ) { 3096f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if ( (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+4)]) && (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+5)]) ) { 3106f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* found a peak; store index into matrix */ 3116f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin peaks[peaks_ind++] = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); 3126f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (peaks_ind == PITCH_MAX_NUM_PEAKS) break; 3136f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 3146f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 3156f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 3166f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 3176f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 3186f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 3196f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (peaks_ind > 0) { 3206f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* examine each peak */ 3216f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin CorrSurfPtr1 = &CorrSurf[0][0]; 3226f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < peaks_ind; k++) { 3236f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin peak = peaks[k]; 3246f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 3256f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* compute four interpolated values around current peak */ 3266f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin IntrepolFilter(&CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)], &intrp_a); 3276f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin IntrepolFilter(&CorrSurfPtr1[peak - 1 ], &intrp_b); 3286f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin IntrepolFilter(&CorrSurfPtr1[peak ], &intrp_c); 3296f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin IntrepolFilter(&CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)], &intrp_d); 3306f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 3316f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* determine maximum of the interpolated values */ 3326f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr = CorrSurfPtr1[peak]; 3336f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr_max = intrp_a; 3346f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (intrp_b > corr_max) corr_max = intrp_b; 3356f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (intrp_c > corr_max) corr_max = intrp_c; 3366f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (intrp_d > corr_max) corr_max = intrp_d; 3376f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 3386f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* determine where the peak sits and fill a 3x3 matrix around it */ 3396f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin row = peak / (PITCH_LAG_SPAN2+4); 3406f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags1[k] = (double) ((peak - row * (PITCH_LAG_SPAN2+4)) + PITCH_MIN_LAG/2 - 4); 3416f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags2[k] = (double) (lags1[k] + PITCH_BW - row); 3426f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if ( corr > corr_max ) { 3436f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[0][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)]; 3446f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[2][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)]; 3456f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][1] = corr; 3466f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[0][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)]; 3476f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[2][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)]; 3486f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][0] = intrp_a; 3496f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[0][1] = intrp_b; 3506f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[2][1] = intrp_c; 3516f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][2] = intrp_d; 3526f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } else { 3536f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (intrp_a == corr_max) { 3546f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags1[k] -= 0.5; 3556f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags2[k] += 0.5; 3566f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin IntrepolFilter(&CorrSurfPtr1[peak - 2*(PITCH_LAG_SPAN2+5)], &T[0][0]); 3576f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin IntrepolFilter(&CorrSurfPtr1[peak - (2*PITCH_LAG_SPAN2+9)], &T[2][0]); 3586f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][1] = intrp_a; 3596f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[0][2] = intrp_b; 3606f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[2][2] = intrp_c; 3616f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][0] = CorrSurfPtr1[peak - (2*PITCH_LAG_SPAN2+9)]; 3626f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[0][1] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)]; 3636f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[2][1] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)]; 3646f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][2] = corr; 3656f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } else if (intrp_b == corr_max) { 3666f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags1[k] -= 0.5; 3676f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags2[k] -= 0.5; 3686f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin IntrepolFilter(&CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+6)], &T[0][0]); 3696f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[2][0] = intrp_a; 3706f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][1] = intrp_b; 3716f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin IntrepolFilter(&CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+3)], &T[0][2]); 3726f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[2][2] = intrp_d; 3736f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)]; 3746f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[0][1] = CorrSurfPtr1[peak - 1]; 3756f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[2][1] = corr; 3766f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)]; 3776f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } else if (intrp_c == corr_max) { 3786f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags1[k] += 0.5; 3796f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags2[k] += 0.5; 3806f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[0][0] = intrp_a; 3816f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin IntrepolFilter(&CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)], &T[2][0]); 3826f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][1] = intrp_c; 3836f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[0][2] = intrp_d; 3846f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin IntrepolFilter(&CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)], &T[2][2]); 3856f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)]; 3866f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[0][1] = corr; 3876f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[2][1] = CorrSurfPtr1[peak + 1]; 3886f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)]; 3896f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } else { 3906f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags1[k] += 0.5; 3916f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags2[k] -= 0.5; 3926f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[0][0] = intrp_b; 3936f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[2][0] = intrp_c; 3946f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][1] = intrp_d; 3956f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin IntrepolFilter(&CorrSurfPtr1[peak + 2*(PITCH_LAG_SPAN2+4)], &T[0][2]); 3966f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin IntrepolFilter(&CorrSurfPtr1[peak + (2*PITCH_LAG_SPAN2+9)], &T[2][2]); 3976f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][0] = corr; 3986f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[0][1] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)]; 3996f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[2][1] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)]; 4006f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin T[1][2] = CorrSurfPtr1[peak + (2*PITCH_LAG_SPAN2+9)]; 4016f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 4026f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 4036f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4046f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* 2D parabolic interpolation gives more accurate lags and peak value */ 4056f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin Intrpol2D(T, &lags1[k], &lags2[k], &peak_vals[k]); 4066f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 4076f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4086f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* determine the highest peak, after applying a bias towards short lags */ 4096f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr_max = 0.0; 4106f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < peaks_ind; k++) { 4116f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr = peak_vals[k] * pow(PITCH_PEAK_DECAY, log(lags1[k] + lags2[k])); 4126f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (corr > corr_max) { 4136f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin corr_max = corr; 4146f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin peak = k; 4156f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 4166f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 4176f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4186f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags1[peak] *= 2.0; 4196f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags2[peak] *= 2.0; 4206f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4216f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (lags1[peak] < (double) PITCH_MIN_LAG) lags1[peak] = (double) PITCH_MIN_LAG; 4226f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (lags2[peak] < (double) PITCH_MIN_LAG) lags2[peak] = (double) PITCH_MIN_LAG; 4236f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (lags1[peak] > (double) PITCH_MAX_LAG) lags1[peak] = (double) PITCH_MAX_LAG; 4246f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (lags2[peak] > (double) PITCH_MAX_LAG) lags2[peak] = (double) PITCH_MAX_LAG; 4256f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4266f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* store lags of highest peak in output array */ 4276f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags[0] = lags1[peak]; 4286f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags[1] = lags1[peak]; 4296f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags[2] = lags2[peak]; 4306f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags[3] = lags2[peak]; 4316f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 4326f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin else 4336f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin { 4346f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin row = max_ind / (PITCH_LAG_SPAN2+4); 4356f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags1[0] = (double) ((max_ind - row * (PITCH_LAG_SPAN2+4)) + PITCH_MIN_LAG/2 - 4); 4366f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags2[0] = (double) (lags1[0] + PITCH_BW - row); 4376f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4386f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (lags1[0] < (double) PITCH_MIN_LAG) lags1[0] = (double) PITCH_MIN_LAG; 4396f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (lags2[0] < (double) PITCH_MIN_LAG) lags2[0] = (double) PITCH_MIN_LAG; 4406f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (lags1[0] > (double) PITCH_MAX_LAG) lags1[0] = (double) PITCH_MAX_LAG; 4416f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (lags2[0] > (double) PITCH_MAX_LAG) lags2[0] = (double) PITCH_MAX_LAG; 4426f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4436f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* store lags of highest peak in output array */ 4446f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags[0] = lags1[0]; 4456f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags[1] = lags1[0]; 4466f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags[2] = lags2[0]; 4476f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin lags[3] = lags2[0]; 4486f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 4496f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin} 4506f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4516f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4526f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4536f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin/* create weighting matrix by orthogonalizing a basis of polynomials of increasing order 4546f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * t = (0:4)'; 4556f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * A = [t.^0, t.^1, t.^2, t.^3, t.^4]; 4566f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * [Q, dummy] = qr(A); 4576f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * P.Weight = Q * diag([0, .1, .5, 1, 1]) * Q'; */ 4586f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkinstatic const double kWeight[5][5] = { 4596f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin { 0.29714285714286, -0.30857142857143, -0.05714285714286, 0.05142857142857, 0.01714285714286}, 4606f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin {-0.30857142857143, 0.67428571428571, -0.27142857142857, -0.14571428571429, 0.05142857142857}, 4616f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin {-0.05714285714286, -0.27142857142857, 0.65714285714286, -0.27142857142857, -0.05714285714286}, 4626f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin { 0.05142857142857, -0.14571428571429, -0.27142857142857, 0.67428571428571, -0.30857142857143}, 4636f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin { 0.01714285714286, 0.05142857142857, -0.05714285714286, -0.30857142857143, 0.29714285714286} 4646f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin}; 4656f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4666f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4676f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkinvoid WebRtcIsac_PitchAnalysis(const double *in, /* PITCH_FRAME_LEN samples */ 4686f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double *out, /* PITCH_FRAME_LEN+QLOOKAHEAD samples */ 4696f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin PitchAnalysisStruct *State, 4706f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double *lags, 4716f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double *gains) 4726f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin{ 4736f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double HPin[PITCH_FRAME_LEN]; 4746f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double Weighted[PITCH_FRAME_LEN]; 4756f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double Whitened[PITCH_FRAME_LEN + QLOOKAHEAD]; 4766f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double inbuf[PITCH_FRAME_LEN + QLOOKAHEAD]; 4776f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double out_G[PITCH_FRAME_LEN + QLOOKAHEAD]; // could be removed by using out instead 4786f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double out_dG[4][PITCH_FRAME_LEN + QLOOKAHEAD]; 4796f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double old_lag, old_gain; 4806f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double nrg_wht, tmp; 4816f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double Wnrg, Wfluct, Wgain; 4826f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double H[4][4]; 4836f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double grad[4]; 4846f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin double dG[4]; 4856f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin int k, m, n, iter; 4866f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4876f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* high pass filtering using second order pole-zero filter */ 4886f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin WebRtcIsac_Highpass(in, HPin, State->hp_state, PITCH_FRAME_LEN); 4896f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4906f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* copy from state into buffer */ 4916f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin memcpy(Whitened, State->whitened_buf, sizeof(double) * QLOOKAHEAD); 4926f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4936f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* compute weighted and whitened signals */ 4946f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin WebRtcIsac_WeightingFilter(HPin, &Weighted[0], &Whitened[QLOOKAHEAD], &(State->Wghtstr)); 4956f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4966f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* copy from buffer into state */ 4976f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin memcpy(State->whitened_buf, Whitened+PITCH_FRAME_LEN, sizeof(double) * QLOOKAHEAD); 4986f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 4996f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin old_lag = State->PFstr_wght.oldlagp[0]; 5006f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin old_gain = State->PFstr_wght.oldgainp[0]; 5016f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5026f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* inital pitch estimate */ 5036f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin WebRtcIsac_InitializePitch(Weighted, old_lag, old_gain, State, lags); 5046f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5056f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5066f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* Iterative optimization of lags - to be done */ 5076f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5086f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* compute energy of whitened signal */ 5096f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin nrg_wht = 0.0; 5106f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < PITCH_FRAME_LEN + QLOOKAHEAD; k++) 5116f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin nrg_wht += Whitened[k] * Whitened[k]; 5126f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5136f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5146f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* Iterative optimization of gains */ 5156f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5166f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* set weights for energy, gain fluctiation, and spectral gain penalty functions */ 5176f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin Wnrg = 1.0 / nrg_wht; 5186f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin Wgain = 0.005; 5196f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin Wfluct = 3.0; 5206f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5216f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* set initial gains */ 5226f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < 4; k++) 5236f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin gains[k] = PITCH_MAX_GAIN_06; 5246f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5256f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* two iterations should be enough */ 5266f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (iter = 0; iter < 2; iter++) { 5276f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* compute Jacobian of pre-filter output towards gains */ 5286f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin WebRtcIsac_PitchfilterPre_gains(Whitened, out_G, out_dG, &(State->PFstr_wght), lags, gains); 5296f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5306f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* gradient and approximate Hessian (lower triangle) for minimizing the filter's output power */ 5316f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < 4; k++) { 5326f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin tmp = 0.0; 5336f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (n = 0; n < PITCH_FRAME_LEN + QLOOKAHEAD; n++) 5346f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin tmp += out_G[n] * out_dG[k][n]; 5356f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin grad[k] = tmp * Wnrg; 5366f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 5376f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < 4; k++) { 5386f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (m = 0; m <= k; m++) { 5396f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin tmp = 0.0; 5406f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (n = 0; n < PITCH_FRAME_LEN + QLOOKAHEAD; n++) 5416f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin tmp += out_dG[m][n] * out_dG[k][n]; 5426f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[k][m] = tmp * Wnrg; 5436f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 5446f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 5456f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5466f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* add gradient and Hessian (lower triangle) for dampening fast gain changes */ 5476f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < 4; k++) { 5486f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin tmp = kWeight[k+1][0] * old_gain; 5496f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (m = 0; m < 4; m++) 5506f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin tmp += kWeight[k+1][m+1] * gains[m]; 5516f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin grad[k] += tmp * Wfluct; 5526f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 5536f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < 4; k++) { 5546f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (m = 0; m <= k; m++) { 5556f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[k][m] += kWeight[k+1][m+1] * Wfluct; 5566f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 5576f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 5586f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5596f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* add gradient and Hessian for dampening gain */ 5606f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < 3; k++) { 5616f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin tmp = 1.0 / (1 - gains[k]); 5626f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin grad[k] += tmp * tmp * Wgain; 5636f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[k][k] += 2.0 * tmp * (tmp * tmp * Wgain); 5646f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 5656f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin tmp = 1.0 / (1 - gains[3]); 5666f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin grad[3] += 1.33 * (tmp * tmp * Wgain); 5676f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[3][3] += 2.66 * tmp * (tmp * tmp * Wgain); 5686f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5696f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5706f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* compute Cholesky factorization of Hessian 5716f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * by overwritting the upper triangle; scale factors on diagonal 5726f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin * (for non pc-platforms store the inverse of the diagonals seperately to minimize divisions) */ 5736f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[0][1] = H[1][0] / H[0][0]; 5746f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[0][2] = H[2][0] / H[0][0]; 5756f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[0][3] = H[3][0] / H[0][0]; 5766f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[1][1] -= H[0][0] * H[0][1] * H[0][1]; 5776f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[1][2] = (H[2][1] - H[0][1] * H[2][0]) / H[1][1]; 5786f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[1][3] = (H[3][1] - H[0][1] * H[3][0]) / H[1][1]; 5796f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[2][2] -= H[0][0] * H[0][2] * H[0][2] + H[1][1] * H[1][2] * H[1][2]; 5806f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[2][3] = (H[3][2] - H[0][2] * H[3][0] - H[1][2] * H[1][1] * H[1][3]) / H[2][2]; 5816f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin H[3][3] -= H[0][0] * H[0][3] * H[0][3] + H[1][1] * H[1][3] * H[1][3] + H[2][2] * H[2][3] * H[2][3]; 5826f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5836f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* Compute update as delta_gains = -inv(H) * grad */ 5846f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* copy and negate */ 5856f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < 4; k++) 5866f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin dG[k] = -grad[k]; 5876f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* back substitution */ 5886f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin dG[1] -= dG[0] * H[0][1]; 5896f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin dG[2] -= dG[0] * H[0][2] + dG[1] * H[1][2]; 5906f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin dG[3] -= dG[0] * H[0][3] + dG[1] * H[1][3] + dG[2] * H[2][3]; 5916f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* scale */ 5926f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < 4; k++) 5936f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin dG[k] /= H[k][k]; 5946f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* back substitution */ 5956f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin dG[2] -= dG[3] * H[2][3]; 5966f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin dG[1] -= dG[3] * H[1][3] + dG[2] * H[1][2]; 5976f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin dG[0] -= dG[3] * H[0][3] + dG[2] * H[0][2] + dG[1] * H[0][1]; 5986f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 5996f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* update gains and check range */ 6006f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < 4; k++) { 6016f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin gains[k] += dG[k]; 6026f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin if (gains[k] > PITCH_MAX_GAIN) 6036f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin gains[k] = PITCH_MAX_GAIN; 6046f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin else if (gains[k] < 0.0) 6056f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin gains[k] = 0.0; 6066f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 6076f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin } 6086f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 6096f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* update state for next frame */ 6106f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin WebRtcIsac_PitchfilterPre(Whitened, out, &(State->PFstr_wght), lags, gains); 6116f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 6126f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* concatenate previous input's end and current input */ 6136f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin memcpy(inbuf, State->inbuf, sizeof(double) * QLOOKAHEAD); 6146f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin memcpy(inbuf+QLOOKAHEAD, in, sizeof(double) * PITCH_FRAME_LEN); 6156f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 6166f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* lookahead pitch filtering for masking analysis */ 6176f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin WebRtcIsac_PitchfilterPre_la(inbuf, out, &(State->PFstr), lags, gains); 6186f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin 6196f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin /* store last part of input */ 6206f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin for (k = 0; k < QLOOKAHEAD; k++) 6216f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin State->inbuf[k] = inbuf[k + PITCH_FRAME_LEN]; 6226f12fff925188ced26e518cd2252aff3e93bb04eAlexander Gutkin} 623