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