1 /* 2 * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved. 3 * 4 * Use of this source code is governed by a BSD-style license 5 * that can be found in the LICENSE file in the root of the source 6 * tree. An additional intellectual property rights grant can be found 7 * in the file PATENTS. All contributing project authors may 8 * be found in the AUTHORS file in the root of the source tree. 9 */ 10 11 /* 12 * lattice.c 13 * 14 * contains the normalized lattice filter routines (MA and AR) for iSAC codec 15 * 16 */ 17 #include "settings.h" 18 #include "codec.h" 19 20 #include <math.h> 21 #include <memory.h> 22 #ifdef WEBRTC_ANDROID 23 #include <stdlib.h> 24 #endif 25 26 /* filter the signal using normalized lattice filter */ 27 /* MA filter */ 28 void WebRtcIsac_NormLatticeFilterMa(int orderCoef, 29 float *stateF, 30 float *stateG, 31 float *lat_in, 32 double *filtcoeflo, 33 double *lat_out) 34 { 35 int n,k,i,u,temp1; 36 int ord_1 = orderCoef+1; 37 float sth[MAX_AR_MODEL_ORDER]; 38 float cth[MAX_AR_MODEL_ORDER]; 39 float inv_cth[MAX_AR_MODEL_ORDER]; 40 double a[MAX_AR_MODEL_ORDER+1]; 41 float f[MAX_AR_MODEL_ORDER+1][HALF_SUBFRAMELEN], g[MAX_AR_MODEL_ORDER+1][HALF_SUBFRAMELEN]; 42 float gain1; 43 44 for (u=0;u<SUBFRAMES;u++) 45 { 46 /* set the Direct Form coefficients */ 47 temp1 = u*ord_1; 48 a[0] = 1; 49 memcpy(a+1, filtcoeflo+temp1+1, sizeof(double) * (ord_1-1)); 50 51 /* compute lattice filter coefficients */ 52 WebRtcIsac_Dir2Lat(a,orderCoef,sth,cth); 53 54 /* compute the gain */ 55 gain1 = (float)filtcoeflo[temp1]; 56 for (k=0;k<orderCoef;k++) 57 { 58 gain1 *= cth[k]; 59 inv_cth[k] = 1/cth[k]; 60 } 61 62 /* normalized lattice filter */ 63 /*****************************/ 64 65 /* initial conditions */ 66 for (i=0;i<HALF_SUBFRAMELEN;i++) 67 { 68 f[0][i] = lat_in[i + u * HALF_SUBFRAMELEN]; 69 g[0][i] = lat_in[i + u * HALF_SUBFRAMELEN]; 70 } 71 72 /* get the state of f&g for the first input, for all orders */ 73 for (i=1;i<ord_1;i++) 74 { 75 f[i][0] = inv_cth[i-1]*(f[i-1][0] + sth[i-1]*stateG[i-1]); 76 g[i][0] = cth[i-1]*stateG[i-1] + sth[i-1]* f[i][0]; 77 } 78 79 /* filtering */ 80 for(k=0;k<orderCoef;k++) 81 { 82 for(n=0;n<(HALF_SUBFRAMELEN-1);n++) 83 { 84 f[k+1][n+1] = inv_cth[k]*(f[k][n+1] + sth[k]*g[k][n]); 85 g[k+1][n+1] = cth[k]*g[k][n] + sth[k]* f[k+1][n+1]; 86 } 87 } 88 89 for(n=0;n<HALF_SUBFRAMELEN;n++) 90 { 91 lat_out[n + u * HALF_SUBFRAMELEN] = gain1 * f[orderCoef][n]; 92 } 93 94 /* save the states */ 95 for (i=0;i<ord_1;i++) 96 { 97 stateF[i] = f[i][HALF_SUBFRAMELEN-1]; 98 stateG[i] = g[i][HALF_SUBFRAMELEN-1]; 99 } 100 /* process next frame */ 101 } 102 103 return; 104 } 105 106 107 /*///////////////////AR filter ///////////////////////////////*/ 108 /* filter the signal using normalized lattice filter */ 109 void WebRtcIsac_NormLatticeFilterAr(int orderCoef, 110 float *stateF, 111 float *stateG, 112 double *lat_in, 113 double *lo_filt_coef, 114 float *lat_out) 115 { 116 int n,k,i,u,temp1; 117 int ord_1 = orderCoef+1; 118 float sth[MAX_AR_MODEL_ORDER]; 119 float cth[MAX_AR_MODEL_ORDER]; 120 double a[MAX_AR_MODEL_ORDER+1]; 121 float ARf[MAX_AR_MODEL_ORDER+1][HALF_SUBFRAMELEN], ARg[MAX_AR_MODEL_ORDER+1][HALF_SUBFRAMELEN]; 122 float gain1,inv_gain1; 123 124 for (u=0;u<SUBFRAMES;u++) 125 { 126 /* set the denominator and numerator of the Direct Form */ 127 temp1 = u*ord_1; 128 a[0] = 1; 129 130 memcpy(a+1, lo_filt_coef+temp1+1, sizeof(double) * (ord_1-1)); 131 132 WebRtcIsac_Dir2Lat(a,orderCoef,sth,cth); 133 134 gain1 = (float)lo_filt_coef[temp1]; 135 for (k=0;k<orderCoef;k++) 136 { 137 gain1 = cth[k]*gain1; 138 } 139 140 /* initial conditions */ 141 inv_gain1 = 1/gain1; 142 for (i=0;i<HALF_SUBFRAMELEN;i++) 143 { 144 ARf[orderCoef][i] = (float)lat_in[i + u * HALF_SUBFRAMELEN]*inv_gain1; 145 } 146 147 148 for (i=orderCoef-1;i>=0;i--) //get the state of f&g for the first input, for all orders 149 { 150 ARf[i][0] = cth[i]*ARf[i+1][0] - sth[i]*stateG[i]; 151 ARg[i+1][0] = sth[i]*ARf[i+1][0] + cth[i]* stateG[i]; 152 } 153 ARg[0][0] = ARf[0][0]; 154 155 for(n=0;n<(HALF_SUBFRAMELEN-1);n++) 156 { 157 for(k=orderCoef-1;k>=0;k--) 158 { 159 ARf[k][n+1] = cth[k]*ARf[k+1][n+1] - sth[k]*ARg[k][n]; 160 ARg[k+1][n+1] = sth[k]*ARf[k+1][n+1] + cth[k]* ARg[k][n]; 161 } 162 ARg[0][n+1] = ARf[0][n+1]; 163 } 164 165 memcpy(lat_out+u * HALF_SUBFRAMELEN, &(ARf[0][0]), sizeof(float) * HALF_SUBFRAMELEN); 166 167 /* cannot use memcpy in the following */ 168 for (i=0;i<ord_1;i++) 169 { 170 stateF[i] = ARf[i][HALF_SUBFRAMELEN-1]; 171 stateG[i] = ARg[i][HALF_SUBFRAMELEN-1]; 172 } 173 174 } 175 176 return; 177 } 178 179 180 /* compute the reflection coefficients using the step-down procedure*/ 181 /* converts the direct form parameters to lattice form.*/ 182 /* a and b are vectors which contain the direct form coefficients, 183 according to 184 A(z) = a(1) + a(2)*z + a(3)*z^2 + ... + a(M+1)*z^M 185 B(z) = b(1) + b(2)*z + b(3)*z^2 + ... + b(M+1)*z^M 186 */ 187 188 void WebRtcIsac_Dir2Lat(double *a, 189 int orderCoef, 190 float *sth, 191 float *cth) 192 { 193 int m, k; 194 float tmp[MAX_AR_MODEL_ORDER]; 195 float tmp_inv, cth2; 196 197 sth[orderCoef-1] = (float)a[orderCoef]; 198 cth2 = 1.0f - sth[orderCoef-1] * sth[orderCoef-1]; 199 cth[orderCoef-1] = (float)sqrt(cth2); 200 for (m=orderCoef-1; m>0; m--) 201 { 202 tmp_inv = 1.0f / cth2; 203 for (k=1; k<=m; k++) 204 { 205 tmp[k] = ((float)a[k] - sth[m] * (float)a[m-k+1]) * tmp_inv; 206 } 207 208 for (k=1; k<m; k++) 209 { 210 a[k] = tmp[k]; 211 } 212 213 sth[m-1] = tmp[m]; 214 cth2 = 1 - sth[m-1] * sth[m-1]; 215 cth[m-1] = (float)sqrt(cth2); 216 } 217 } 218
