| OLD | NEW |
| 1 /* | 1 /* |
| 2 * Copyright (C) 2010 Google Inc. All rights reserved. | 2 * Copyright (C) 2010 Google Inc. All rights reserved. |
| 3 * | 3 * |
| 4 * Redistribution and use in source and binary forms, with or without | 4 * Redistribution and use in source and binary forms, with or without |
| 5 * modification, are permitted provided that the following conditions | 5 * modification, are permitted provided that the following conditions |
| 6 * are met: | 6 * are met: |
| 7 * | 7 * |
| 8 * 1. Redistributions of source code must retain the above copyright | 8 * 1. Redistributions of source code must retain the above copyright |
| 9 * notice, this list of conditions and the following disclaimer. | 9 * notice, this list of conditions and the following disclaimer. |
| 10 * 2. Redistributions in binary form must reproduce the above copyright | 10 * 2. Redistributions in binary form must reproduce the above copyright |
| (...skipping 567 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 578 std::complex<double> numerator = b0 + (b1 + b2 * z) * z; | 578 std::complex<double> numerator = b0 + (b1 + b2 * z) * z; |
| 579 std::complex<double> denominator = | 579 std::complex<double> denominator = |
| 580 std::complex<double>(1, 0) + (a1 + a2 * z) * z; | 580 std::complex<double>(1, 0) + (a1 + a2 * z) * z; |
| 581 std::complex<double> response = numerator / denominator; | 581 std::complex<double> response = numerator / denominator; |
| 582 mag_response[k] = static_cast<float>(abs(response)); | 582 mag_response[k] = static_cast<float>(abs(response)); |
| 583 phase_response[k] = | 583 phase_response[k] = |
| 584 static_cast<float>(atan2(imag(response), real(response))); | 584 static_cast<float>(atan2(imag(response), real(response))); |
| 585 } | 585 } |
| 586 } | 586 } |
| 587 | 587 |
| 588 static double RepeatedRootResponse(double n, | |
| 589 double c1, | |
| 590 double c2, | |
| 591 double r, | |
| 592 double log_eps) { | |
| 593 // The response is h(n) = r^(n-2)*[c1*(n+1)*r^2+c2]. We're looking | |
| 594 // for n such that |h(n)| = eps. Equivalently, we want a root | |
| 595 // of the equation log(|h(n)|) - log(eps) = 0 or | |
| 596 // | |
| 597 // (n-2)*log(r) + log(|c1*(n+1)*r^2+c2|) - log(eps) | |
| 598 // | |
| 599 // This helps with finding a nuemrical solution because this | |
| 600 // approximately linearizes the response for large n. | |
| 601 | |
| 602 return (n - 2) * log(r) + log(fabs(c1 * (n + 1) * r * r + c2)) - log_eps; | |
| 603 } | |
| 604 | |
| 605 // Regula Falsi root finder, Illinois variant | |
| 606 // (https://en.wikipedia.org/wiki/False_position_method#The_Illinois_algorithm). | |
| 607 // | |
| 608 // This finds a root of the repeated root response where the root is | |
| 609 // assumed to lie between |low| and |high|. The response is given by | |
| 610 // |c1|, |c2|, and |r| as determined by |RepeatedRootResponse|. | |
| 611 // |log_eps| is the log the the maximum allowed amplitude in the | |
| 612 // response. | |
| 613 static double RootFinder(double low, | |
| 614 double high, | |
| 615 double log_eps, | |
| 616 double c1, | |
| 617 double c2, | |
| 618 double r) { | |
| 619 // Desired accuray of the root (in frames). This doesn't need to be | |
| 620 // super-accurate, so half frame is good enough, and should be less | |
| 621 // than 1 because the algorithm may prematurely terminate. | |
| 622 const double kAccuracyThreshold = 0.5; | |
| 623 // Max number of iterations to do. If we haven't converged by now, | |
| 624 // just return whatever we've found. | |
| 625 const int kMaxIterations = 10; | |
| 626 | |
| 627 int side = 0; | |
| 628 double root = 0; | |
| 629 double f_low = RepeatedRootResponse(low, c1, c2, r, log_eps); | |
| 630 double f_high = RepeatedRootResponse(high, c1, c2, r, log_eps); | |
| 631 | |
| 632 // The function values must be finite and have opposite signs! | |
| 633 DCHECK(std::isfinite(f_low)); | |
| 634 DCHECK(std::isfinite(f_high)); | |
| 635 DCHECK_LE(f_low * f_high, 0); | |
| 636 | |
| 637 int iteration; | |
| 638 for (iteration = 0; iteration < kMaxIterations; ++iteration) { | |
| 639 root = (f_low * high - f_high * low) / (f_low - f_high); | |
| 640 if (fabs(high - low) < kAccuracyThreshold * fabs(high + low)) | |
| 641 break; | |
| 642 double fr = RepeatedRootResponse(root, c1, c2, r, log_eps); | |
| 643 | |
| 644 DCHECK(std::isfinite(fr)); | |
| 645 | |
| 646 if (fr * f_high > 0) { | |
| 647 // fr and f_high have same sign. Copy root to f_high | |
| 648 high = root; | |
| 649 f_high = fr; | |
| 650 side = -1; | |
| 651 } else if (f_low * fr > 0) { | |
| 652 // fr and f_low have same sign. Copy root to f_low | |
| 653 low = root; | |
| 654 f_low = fr; | |
| 655 if (side == 1) | |
| 656 f_high /= 2; | |
| 657 side = 1; | |
| 658 } else { | |
| 659 // f_low * fr looks like zero, so assume we've converged. | |
| 660 break; | |
| 661 } | |
| 662 } | |
| 663 | |
| 664 // Want to know if the max number of iterations is ever exceeded so | |
| 665 // we can understand why that happened. | |
| 666 DCHECK_LT(iteration, kMaxIterations); | |
| 667 | |
| 668 return root; | |
| 669 } | |
| 670 | |
| 671 double Biquad::TailFrame(int coef_index, double max_frame) { | |
| 672 // The Biquad filter is given by | |
| 673 // | |
| 674 // H(z) = (b0 + b1/z + b2/z^2)/(1 + a1/z + a2/z^2). | |
| 675 // | |
| 676 // To compute the tail time, compute the impulse response, h(n), of | |
| 677 // H(z), which we can do analytically. From this impulse response, | |
| 678 // find the value n0 where |h(n)| <= eps for n >= n0. | |
| 679 // | |
| 680 // Assume first that the two poles of H(z) are not repeated, say r1 | |
| 681 // and r2. Then, we can compute a partial fraction expansion of | |
| 682 // H(z): | |
| 683 // | |
| 684 // H(z) = (b0+b1/z+b2/z^2)/[(1-r1/z)*(1-r2/z)] | |
| 685 // = b0 + C2/(z-r2) - C1/(z-r1) | |
| 686 // | |
| 687 // where | |
| 688 // C2 = (b0*r2^2+b1*r2+b2)/(r2-r1) | |
| 689 // C1 = (b0*r1^2+b1*r1+b2)/(r2-r1) | |
| 690 // | |
| 691 // Expand H(z) then this in powers of 1/z gives: | |
| 692 // | |
| 693 // H(z) = b0 -(C2/r2+C1/r1) + sum(C2*r2^(i-1)/z^i + C1*r1^(i-1)/z^i) | |
| 694 // | |
| 695 // Thus, for n > 1 (we don't care about small n), | |
| 696 // | |
| 697 // h(n) = C2*r2^(n-1) + C1*r1^(n-1) | |
| 698 // | |
| 699 // We need to find n0 such that |h(n)| < eps for n > n0. | |
| 700 // | |
| 701 // Case 1: r1 and r2 are real and distinct, with |r1|>=|r2|. | |
| 702 // | |
| 703 // Then | |
| 704 // | |
| 705 // h(n) = C1*r1^(n-1)*(1 + C2/C1*(r2/r1)^(n-1)) | |
| 706 // | |
| 707 // so | |
| 708 // | |
| 709 // |h(n)| = |C1|*|r|^(n-1)*|1+C2/C1*(r2/r1)^(n-1)| | |
| 710 // <= |C1|*|r|^(n-1)*[1 + |C2/C1|*|r2/r1|^(n-1)] | |
| 711 // <= |C1|*|r|^(n-1)*[1 + |C2/C1|] | |
| 712 // | |
| 713 // by using the triangle inequality and the fact that |r2|<=|r1|. | |
| 714 // And we want |h(n)|<=eps which is true if | |
| 715 // | |
| 716 // |C1|*|r|^(n-1)*[1 + |C2/C1|] <= eps | |
| 717 // | |
| 718 // or | |
| 719 // | |
| 720 // n >= 1 + log(eps/C)/log(|r1|) | |
| 721 // | |
| 722 // where C = |C1|*[1+|C2/C1|] = |C1| + |C2|. | |
| 723 // | |
| 724 // Case 2: r1 and r2 are complex | |
| 725 // | |
| 726 // Thne we can write r1=r*exp(i*p) and r2=r*exp(-i*p). So, | |
| 727 // | |
| 728 // |h(n)| = |C2*r^(n-1)*exp(-i*p*(n-1)) + C1*r^(n-1)*exp(i*p*(n-1))| | |
| 729 // = |C1|*r^(n-1)*|1 + C2/C1*exp(-i*p*(n-1))/exp(i*n*(n-1))| | |
| 730 // <= |C1|*r^(n-1)*[1 + |C2/C1|] | |
| 731 // | |
| 732 // Again, this is easily solved to give | |
| 733 // | |
| 734 // n >= 1 + log(eps/C)/log(r) | |
| 735 // | |
| 736 // where C = |C1|*[1+|C2/C1|] = |C1| + |C2|. | |
| 737 // | |
| 738 // Case 3: Repeated roots, r1=r2=r. | |
| 739 // | |
| 740 // In this case, | |
| 741 // | |
| 742 // H(z) = (b0+b1/z+b2/z^2)/[(1-r/z)^2 | |
| 743 // | |
| 744 // Expanding this in powers of 1/z gives: | |
| 745 // | |
| 746 // H(z) = C1*sum((i+1)*r^i/z^i) - C2 * sum(r^(i-2)/z^i) + b2/r^2 | |
| 747 // = b2/r^2 + sum([C1*(i+1)*r^i + C2*r^(i-2)]/z^i) | |
| 748 // where | |
| 749 // C1 = (b0*r^2+b1*r+b2)/r^2 | |
| 750 // C2 = b1*r+2*b2 | |
| 751 // | |
| 752 // Thus, the impulse response is | |
| 753 // | |
| 754 // h(n) = C1*(n+1)*r^n + C2*r^(n-2) | |
| 755 // = r^(n-2)*[C1*(n+1)*r^2+C2] | |
| 756 // | |
| 757 // So | |
| 758 // | |
| 759 // |h(n)| = |r|^(n-2)*|C1*(n+1)*r^2+C2| | |
| 760 // | |
| 761 // To find n such that |h(n)| < eps, we need a numerical method in | |
| 762 // general, so there's no real reason to simplify this or use other | |
| 763 // approximations. Just solve |h(n)|=eps directly. | |
| 764 // | |
| 765 // Thus, for an set of filter coefficients, we can compute the tail | |
| 766 // time. | |
| 767 // | |
| 768 | |
| 769 // If the maximum amplitude of the impulse response is less than | |
| 770 // this, we assume that we've reached the tail of the response. | |
| 771 // Currently, this means that the impulse is less than 1 bit of a | |
| 772 // 16-bit PCM value. | |
| 773 const double kMaxTailAmplitude = 1 / 32768.0; | |
| 774 | |
| 775 // Find the roots of 1+a1/z+a2/z^2 = 0. Or equivalently, | |
| 776 // z^2+a1*z+a2 = 0. From the quadratic formula the roots are | |
| 777 // (-a1+/-sqrt(a1^2-4*a2))/2. | |
| 778 | |
| 779 double a1 = a1_[coef_index]; | |
| 780 double a2 = a2_[coef_index]; | |
| 781 double b0 = b0_[coef_index]; | |
| 782 double b1 = b1_[coef_index]; | |
| 783 double b2 = b2_[coef_index]; | |
| 784 | |
| 785 double tail_frame = 0; | |
| 786 double discrim = a1 * a1 - 4 * a2; | |
| 787 | |
| 788 if (discrim > 0) { | |
| 789 // Compute the real roots so that r1 has the largest magnitude. | |
| 790 double r1; | |
| 791 double r2; | |
| 792 if (a1 < 0) { | |
| 793 r1 = (-a1 + sqrt(discrim)) / 2; | |
| 794 } else { | |
| 795 r1 = (-a1 - sqrt(discrim)) / 2; | |
| 796 } | |
| 797 r2 = a2 / r1; | |
| 798 | |
| 799 double c1 = (b0 * r1 * r1 + b1 * r1 + b2) / (r2 - r1); | |
| 800 double c2 = (b0 * r2 * r2 + b1 * r2 + b2) / (r2 - r1); | |
| 801 | |
| 802 DCHECK(std::isfinite(r1)); | |
| 803 DCHECK(std::isfinite(r2)); | |
| 804 DCHECK(std::isfinite(c1)); | |
| 805 DCHECK(std::isfinite(c2)); | |
| 806 | |
| 807 // It's possible for kMaxTailAmplitude to be greater than c1 + c2. | |
| 808 // This may produce a negative tail frame. Just clamp the tail | |
| 809 // frame to 0. | |
| 810 tail_frame = clampTo( | |
| 811 1 + log(kMaxTailAmplitude / (fabs(c1) + fabs(c2))) / log(r1), 0); | |
| 812 | |
| 813 DCHECK(std::isfinite(tail_frame)); | |
| 814 } else if (discrim < 0) { | |
| 815 // Two complex roots. | |
| 816 // One root is -a1/2 + i*sqrt(-discrim)/2. | |
| 817 double x = -a1 / 2; | |
| 818 double y = sqrt(-discrim) / 2; | |
| 819 std::complex<double> r1(x, y); | |
| 820 std::complex<double> r2(x, -y); | |
| 821 double r = hypot(x, y); | |
| 822 | |
| 823 DCHECK(std::isfinite(r)); | |
| 824 | |
| 825 // It's possible for r to be 1. (LPF with Q very large can cause this.) | |
| 826 if (r == 1) { | |
| 827 tail_frame = max_frame; | |
| 828 } else { | |
| 829 double c1 = abs((b0 * r1 * r1 + b1 * r1 + b2) / (r2 - r1)); | |
| 830 double c2 = abs((b0 * r2 * r2 + b1 * r2 + b2) / (r2 - r1)); | |
| 831 | |
| 832 DCHECK(std::isfinite(c1)); | |
| 833 DCHECK(std::isfinite(c2)); | |
| 834 | |
| 835 tail_frame = 1 + log(kMaxTailAmplitude / (c1 + c2)) / log(r); | |
| 836 DCHECK(std::isfinite(tail_frame)); | |
| 837 } | |
| 838 } else { | |
| 839 // Repeated roots. This should be pretty rare because all the | |
| 840 // coefficients need to be just the right values to get a | |
| 841 // discriminant of exactly zero. | |
| 842 double r = -a1 / 2; | |
| 843 | |
| 844 if (r == 0) { | |
| 845 // Double pole at 0. This just delays the signal by 2 frames, | |
| 846 // so set the tail frame to 2. | |
| 847 tail_frame = 2; | |
| 848 } else { | |
| 849 double c1 = (b0 * r * r + b1 * r + b2) / (r * r); | |
| 850 double c2 = b1 * r + 2 * b2; | |
| 851 | |
| 852 DCHECK(std::isfinite(c1)); | |
| 853 DCHECK(std::isfinite(c2)); | |
| 854 | |
| 855 // It can happen that c1=c2=0. This basically means that H(z) = | |
| 856 // constant, which is the limiting case for several of the | |
| 857 // biquad filters. | |
| 858 if (c1 == 0 && c2 == 0) { | |
| 859 tail_frame = 0; | |
| 860 } else { | |
| 861 // The function c*(n+1)*r^n is not monotonic, but it's easy to | |
| 862 // find the max point since the derivative is | |
| 863 // c*r^n*(1+(n+1)*log(r)). This has a root at | |
| 864 // -(1+log(r))/log(r). so we can start our search from that | |
| 865 // point to max_frames. | |
| 866 | |
| 867 double low = clampTo(-(1 + log(r)) / log(r), 1.0, | |
| 868 static_cast<double>(max_frame - 1)); | |
| 869 double high = max_frame; | |
| 870 | |
| 871 DCHECK(std::isfinite(low)); | |
| 872 DCHECK(std::isfinite(high)); | |
| 873 | |
| 874 tail_frame = RootFinder(low, high, log(kMaxTailAmplitude), c1, c2, r); | |
| 875 } | |
| 876 } | |
| 877 } | |
| 878 | |
| 879 return tail_frame; | |
| 880 } | |
| 881 | |
| 882 } // namespace blink | 588 } // namespace blink |
| OLD | NEW |