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 */
WebRtcIsac_NormLatticeFilterMa(int orderCoef,float * stateF,float * stateG,float * lat_in,double * filtcoeflo,double * lat_out)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 */
WebRtcIsac_NormLatticeFilterAr(int orderCoef,float * stateF,float * stateG,double * lat_in,double * lo_filt_coef,float * lat_out)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
WebRtcIsac_Dir2Lat(double * a,int orderCoef,float * sth,float * cth)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