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 |