lsf.c 7.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283
  1. /******************************************************************
  2. iLBC Speech Coder ANSI-C Source Code
  3. lsf.c
  4. Copyright (C) The Internet Society (2004).
  5. All Rights Reserved.
  6. ******************************************************************/
  7. #include <string.h>
  8. #include <math.h>
  9. #include "iLBC_define.h"
  10. /*----------------------------------------------------------------*
  11. * conversion from lpc coefficients to lsf coefficients
  12. *---------------------------------------------------------------*/
  13. void a2lsf(
  14. float *freq,/* (o) lsf coefficients */
  15. float *a /* (i) lpc coefficients */
  16. ){
  17. float steps[LSF_NUMBER_OF_STEPS] =
  18. {(float)0.00635, (float)0.003175, (float)0.0015875,
  19. (float)0.00079375};
  20. float step;
  21. int step_idx;
  22. int lsp_index;
  23. float p[LPC_HALFORDER];
  24. float q[LPC_HALFORDER];
  25. float p_pre[LPC_HALFORDER];
  26. float q_pre[LPC_HALFORDER];
  27. float old_p, old_q, *old;
  28. float *pq_coef;
  29. float omega, old_omega;
  30. int i;
  31. float hlp, hlp1, hlp2, hlp3, hlp4, hlp5;
  32. for (i=0; i<LPC_HALFORDER; i++) {
  33. p[i] = (float)-1.0 * (a[i + 1] + a[LPC_FILTERORDER - i]);
  34. q[i] = a[LPC_FILTERORDER - i] - a[i + 1];
  35. }
  36. p_pre[0] = (float)-1.0 - p[0];
  37. p_pre[1] = - p_pre[0] - p[1];
  38. p_pre[2] = - p_pre[1] - p[2];
  39. p_pre[3] = - p_pre[2] - p[3];
  40. p_pre[4] = - p_pre[3] - p[4];
  41. p_pre[4] = p_pre[4] / 2;
  42. q_pre[0] = (float)1.0 - q[0];
  43. q_pre[1] = q_pre[0] - q[1];
  44. q_pre[2] = q_pre[1] - q[2];
  45. q_pre[3] = q_pre[2] - q[3];
  46. q_pre[4] = q_pre[3] - q[4];
  47. q_pre[4] = q_pre[4] / 2;
  48. omega = 0.0;
  49. old_omega = 0.0;
  50. old_p = FLOAT_MAX;
  51. old_q = FLOAT_MAX;
  52. /* Here we loop through lsp_index to find all the
  53. LPC_FILTERORDER roots for omega. */
  54. for (lsp_index = 0; lsp_index<LPC_FILTERORDER; lsp_index++) {
  55. /* Depending on lsp_index being even or odd, we
  56. alternatively solve the roots for the two LSP equations. */
  57. if ((lsp_index & 0x1) == 0) {
  58. pq_coef = p_pre;
  59. old = &old_p;
  60. } else {
  61. pq_coef = q_pre;
  62. old = &old_q;
  63. }
  64. /* Start with low resolution grid */
  65. for (step_idx = 0, step = steps[step_idx];
  66. step_idx < LSF_NUMBER_OF_STEPS;){
  67. /* cos(10piw) + pq(0)cos(8piw) + pq(1)cos(6piw) +
  68. pq(2)cos(4piw) + pq(3)cod(2piw) + pq(4) */
  69. hlp = (float)cos(omega * TWO_PI);
  70. hlp1 = (float)2.0 * hlp + pq_coef[0];
  71. hlp2 = (float)2.0 * hlp * hlp1 - (float)1.0 +
  72. pq_coef[1];
  73. hlp3 = (float)2.0 * hlp * hlp2 - hlp1 + pq_coef[2];
  74. hlp4 = (float)2.0 * hlp * hlp3 - hlp2 + pq_coef[3];
  75. hlp5 = hlp * hlp4 - hlp3 + pq_coef[4];
  76. if (((hlp5 * (*old)) <= 0.0) || (omega >= 0.5)){
  77. if (step_idx == (LSF_NUMBER_OF_STEPS - 1)){
  78. if (fabs(hlp5) >= fabs(*old)) {
  79. freq[lsp_index] = omega - step;
  80. } else {
  81. freq[lsp_index] = omega;
  82. }
  83. if ((*old) >= 0.0){
  84. *old = (float)-1.0 * FLOAT_MAX;
  85. } else {
  86. *old = FLOAT_MAX;
  87. }
  88. omega = old_omega;
  89. step_idx = 0;
  90. step_idx = LSF_NUMBER_OF_STEPS;
  91. } else {
  92. if (step_idx == 0) {
  93. old_omega = omega;
  94. }
  95. step_idx++;
  96. omega -= steps[step_idx];
  97. /* Go back one grid step */
  98. step = steps[step_idx];
  99. }
  100. } else {
  101. /* increment omega until they are of different sign,
  102. and we know there is at least one root between omega
  103. and old_omega */
  104. *old = hlp5;
  105. omega += step;
  106. }
  107. }
  108. }
  109. for (i = 0; i<LPC_FILTERORDER; i++) {
  110. freq[i] = freq[i] * TWO_PI;
  111. }
  112. }
  113. /*----------------------------------------------------------------*
  114. * conversion from lsf coefficients to lpc coefficients
  115. *---------------------------------------------------------------*/
  116. void lsf2a(
  117. float *a_coef, /* (o) lpc coefficients */
  118. float *freq /* (i) lsf coefficients */
  119. ){
  120. int i, j;
  121. float hlp;
  122. float p[LPC_HALFORDER], q[LPC_HALFORDER];
  123. float a[LPC_HALFORDER + 1], a1[LPC_HALFORDER],
  124. a2[LPC_HALFORDER];
  125. float b[LPC_HALFORDER + 1], b1[LPC_HALFORDER],
  126. b2[LPC_HALFORDER];
  127. for (i=0; i<LPC_FILTERORDER; i++) {
  128. freq[i] = freq[i] * PI2;
  129. }
  130. /* Check input for ill-conditioned cases. This part is not
  131. found in the TIA standard. It involves the following 2 IF
  132. blocks. If "freq" is judged ill-conditioned, then we first
  133. modify freq[0] and freq[LPC_HALFORDER-1] (normally
  134. LPC_HALFORDER = 10 for LPC applications), then we adjust
  135. the other "freq" values slightly */
  136. if ((freq[0] <= 0.0) || (freq[LPC_FILTERORDER - 1] >= 0.5)){
  137. if (freq[0] <= 0.0) {
  138. freq[0] = (float)0.022;
  139. }
  140. if (freq[LPC_FILTERORDER - 1] >= 0.5) {
  141. freq[LPC_FILTERORDER - 1] = (float)0.499;
  142. }
  143. hlp = (freq[LPC_FILTERORDER - 1] - freq[0]) /
  144. (float) (LPC_FILTERORDER - 1);
  145. for (i=1; i<LPC_FILTERORDER; i++) {
  146. freq[i] = freq[i - 1] + hlp;
  147. }
  148. }
  149. memset(a1, 0, LPC_HALFORDER*sizeof(float));
  150. memset(a2, 0, LPC_HALFORDER*sizeof(float));
  151. memset(b1, 0, LPC_HALFORDER*sizeof(float));
  152. memset(b2, 0, LPC_HALFORDER*sizeof(float));
  153. memset(a, 0, (LPC_HALFORDER+1)*sizeof(float));
  154. memset(b, 0, (LPC_HALFORDER+1)*sizeof(float));
  155. /* p[i] and q[i] compute cos(2*pi*omega_{2j}) and
  156. cos(2*pi*omega_{2j-1} in eqs. 4.2.2.2-1 and 4.2.2.2-2.
  157. Note that for this code p[i] specifies the coefficients
  158. used in .Q_A(z) while q[i] specifies the coefficients used
  159. in .P_A(z) */
  160. for (i=0; i<LPC_HALFORDER; i++) {
  161. p[i] = (float)cos(TWO_PI * freq[2 * i]);
  162. q[i] = (float)cos(TWO_PI * freq[2 * i + 1]);
  163. }
  164. a[0] = 0.25;
  165. b[0] = 0.25;
  166. for (i= 0; i<LPC_HALFORDER; i++) {
  167. a[i + 1] = a[i] - 2 * p[i] * a1[i] + a2[i];
  168. b[i + 1] = b[i] - 2 * q[i] * b1[i] + b2[i];
  169. a2[i] = a1[i];
  170. a1[i] = a[i];
  171. b2[i] = b1[i];
  172. b1[i] = b[i];
  173. }
  174. for (j=0; j<LPC_FILTERORDER; j++) {
  175. if (j == 0) {
  176. a[0] = 0.25;
  177. b[0] = -0.25;
  178. } else {
  179. a[0] = b[0] = 0.0;
  180. }
  181. for (i=0; i<LPC_HALFORDER; i++) {
  182. a[i + 1] = a[i] - 2 * p[i] * a1[i] + a2[i];
  183. b[i + 1] = b[i] - 2 * q[i] * b1[i] + b2[i];
  184. a2[i] = a1[i];
  185. a1[i] = a[i];
  186. b2[i] = b1[i];
  187. b1[i] = b[i];
  188. }
  189. a_coef[j + 1] = 2 * (a[LPC_HALFORDER] + b[LPC_HALFORDER]);
  190. }
  191. a_coef[0] = 1.0;
  192. }