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