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