diff --git a/.classpath b/.classpath index 2217d1f1..512a74ba 100644 --- a/.classpath +++ b/.classpath @@ -1,7 +1,6 @@ - @@ -30,5 +29,6 @@ + diff --git a/.project b/.project index f65c4170..7accc076 100644 --- a/.project +++ b/.project @@ -5,6 +5,11 @@ + + org.eclipse.xtext.ui.shared.xtextBuilder + + + org.eclipse.jdt.core.javabuilder @@ -13,5 +18,6 @@ org.eclipse.jdt.core.javanature + org.eclipse.xtext.ui.shared.xtextNature diff --git a/images/Curve-fitted-background.png b/images/Curve-fitted-background.png new file mode 100644 index 00000000..20858058 Binary files /dev/null and b/images/Curve-fitted-background.png differ diff --git a/images/Curve-fitted-chart-background.png b/images/Curve-fitted-chart-background.png new file mode 100644 index 00000000..5172edca Binary files /dev/null and b/images/Curve-fitted-chart-background.png differ diff --git a/images/Curve-fitted-graph-gridlines.png b/images/Curve-fitted-graph-gridlines.png new file mode 100644 index 00000000..648c4f9a Binary files /dev/null and b/images/Curve-fitted-graph-gridlines.png differ diff --git a/src/dsp/filter/design/FilterDesignException.java b/src/dsp/filter/design/FilterDesignException.java new file mode 100644 index 00000000..ac2c5a75 --- /dev/null +++ b/src/dsp/filter/design/FilterDesignException.java @@ -0,0 +1,16 @@ +package dsp.filter.design; + +public class FilterDesignException extends Exception +{ + private static final long serialVersionUID = 1L; + + public FilterDesignException( String text ) + { + super( text ); + } + + public FilterDesignException( String text, Throwable throwable ) + { + super( text, throwable ); + } +} diff --git a/src/dsp/filter/design/FilterView.css b/src/dsp/filter/design/FilterView.css new file mode 100644 index 00000000..9c70eec8 --- /dev/null +++ b/src/dsp/filter/design/FilterView.css @@ -0,0 +1,39 @@ +/* ....Show License.... */ +.chart { + -fx-background-image: url("../../../../images/Curve-fitted-background.png"); + -fx-padding: 15 25 15 15; +} +.chart-plot-background { + -fx-background-color: red; + -fx-background-image: url("../../../../images/Curve-fitted-chart-background.png"), + url("../../../../images/Curve-fitted-graph-gridlines.png"); + -fx-background-size: cover, auto; + -fx-background-repeat: no-repeat, repeat; + -fx-background-position: 0% 0%, 0% 100%; + -fx-border-color: black black transparent transparent; +} +.chart-area-symbol { + -fx-background-color: white; +} +.chart-series-area-line { + -fx-stroke: white; + -fx-stroke-width: 2px; +} +.chart-series-area-fill { + -fx-fill: linear-gradient(to right, white, rgba(255,255,255,0)); + -fx-blend-mode: OVERLAY; +} +.axis { + -fx-tick-mark-visible: false; + -fx-minor-tick-visible: false; + -fx-tick-length: 3; + -fx-minor-tick-length: 0; + -fx-border-color: transparent; + -fx-tick-label-fill: white; +} +.axis:bottom { + -fx-border-color: black transparent transparent transparent; +} +.axis:left { + -fx-border-color: transparent black transparent transparent; +} \ No newline at end of file diff --git a/src/dsp/filter/design/FilterView.java b/src/dsp/filter/design/FilterView.java new file mode 100644 index 00000000..12191462 --- /dev/null +++ b/src/dsp/filter/design/FilterView.java @@ -0,0 +1,171 @@ +package dsp.filter.design; + +import org.jtransforms.fft.FloatFFT_1D; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import dsp.filter.FilterFactory; +import dsp.filter.Window.WindowType; +import dsp.filter.fir.FIRFilterSpecification; +import dsp.filter.fir.remez.RemezFIRFilterDesigner; +import gui.control.CurveFittedAreaChart; +import javafx.collections.ObservableList; +import javafx.scene.chart.NumberAxis; +import javafx.scene.chart.XYChart; +import javafx.scene.chart.XYChart.Series; +import javafx.scene.layout.BorderPane; +import javafx.scene.layout.HBox; +import javafx.scene.layout.Priority; + +public class FilterView extends BorderPane +{ + private final static Logger mLog = LoggerFactory.getLogger( FilterView.class ); + + private CurveFittedAreaChart mImpulseChart; + private NumberAxis mImpulseXAxis; + private NumberAxis mImpulseYAxis; + private CurveFittedAreaChart mDFTChart; + private NumberAxis mDFTXAxis; + private NumberAxis mDFTYAxis; + private static final int FFT_SIZE = 4096; + private FloatFFT_1D mFFT = new FloatFFT_1D( FFT_SIZE ); + + public FilterView( float[] taps ) + { + init(); + + if( taps != null ) + { + setImpulseTaps( taps ); + + float[] dft = new float[ FFT_SIZE ]; + System.arraycopy( taps, 0, dft, 0, taps.length ); + + mFFT.realForward( dft ); + + setDFTTaps( convertDFTToDecibel( dft, taps.length ) ); + } + } + + private void init() + { + HBox hbox = new HBox(); + + mImpulseXAxis = new NumberAxis( 0.0, 0.5, 1.0 ); + mImpulseYAxis = new NumberAxis( -0.2, 1.2, .1 ); + + mImpulseChart = new CurveFittedAreaChart( mImpulseXAxis, mImpulseYAxis ); + mImpulseChart.setLegendVisible( false ); + mImpulseChart.setHorizontalGridLinesVisible( false ); + mImpulseChart.setVerticalGridLinesVisible(false); + mImpulseChart.setAlternativeColumnFillVisible(false); + mImpulseChart.setAlternativeRowFillVisible(false); + + String css = FilterView.class.getResource( "FilterView.css" ).toExternalForm(); + mImpulseChart.getStylesheets().add( css ); + + hbox.getChildren().add( mImpulseChart ); + HBox.setHgrow( mImpulseChart, Priority.ALWAYS ); + + mDFTXAxis = new NumberAxis( 0.0, FFT_SIZE / 2, FFT_SIZE / 8 ); + mDFTYAxis = new NumberAxis( -200.0, 10.0, 10.0 ); + + mDFTChart = new CurveFittedAreaChart( mDFTXAxis, mDFTYAxis ); + mDFTChart.setLegendVisible( false ); + mDFTChart.setHorizontalGridLinesVisible( false ); + mDFTChart.setVerticalGridLinesVisible(false); + mDFTChart.setAlternativeColumnFillVisible(false); + mDFTChart.setAlternativeRowFillVisible(false); + mDFTChart.getStylesheets().add( css ); + + hbox.getChildren().add( mDFTChart ); + HBox.setHgrow( mDFTChart, Priority.ALWAYS ); + + setCenter( hbox ); + } + + public void setImpulseTaps( double[] taps ) + { + float[] convertedTaps = new float[ taps.length ]; + + for( int x = 0; x < taps.length; x++ ) + { + convertedTaps[ x ] = (float)taps[ x ]; + } + + setImpulseTaps( convertedTaps ); + } + + public void setImpulseTaps( float[] taps ) + { + double smallest = 0.0; + double largest = 0.0; + + final XYChart.Series series = new XYChart.Series<>(); + + for( int x = 0; x < taps.length; x++ ) + { + if( taps[ x ] < smallest ) + { + smallest = taps[ x ]; + } + + if( taps[ x ] > largest ) + { + largest = taps[ x ]; + } + + series.getData().add( new XYChart.Data( x, taps[ x ] ) ); + } + + mImpulseXAxis.setLowerBound( 0.0 ); + mImpulseXAxis.setUpperBound( taps.length - 1 ); + mImpulseXAxis.setTickUnit( 1.0 ); + + double padding = Math.abs( largest - smallest ) * .1; + + mImpulseYAxis.setLowerBound( smallest - padding ); + mImpulseYAxis.setUpperBound( largest + padding ); + mImpulseYAxis.setTickUnit( padding ); + + ObservableList> list = mImpulseChart.getData(); + list.add( series ); + } + + public void setDFTTaps( float[] taps ) + { + double smallest = 0.0; + + final XYChart.Series series = new XYChart.Series<>(); + + for( int x = 0; x < taps.length; x++ ) + { + if( taps[ x ] < smallest ) + { + smallest = taps[ x ]; + } + + series.getData().add( new XYChart.Data( x, taps[ x ] ) ); + } + + ObservableList> list = mDFTChart.getData(); + list.add( series ); + } + + public float[] convertDFTToDecibel( float[] dft, int tapLength ) + { + float[] decibels = new float[ dft.length / 2 ]; + + int index = 0; + + for( int x = 0; x < decibels.length; x ++ ) + { + index = x * 2; + + decibels[ x ] = 20.0f * (float)Math.log10( ( ( dft[ index ] * dft[ index ] ) + + ( dft[ index + 1 ] * dft[ index + 1 ] ) ) ); + } + + return decibels; + } +} diff --git a/src/dsp/filter/design/FilterViewer.java b/src/dsp/filter/design/FilterViewer.java new file mode 100644 index 00000000..7dac65b7 --- /dev/null +++ b/src/dsp/filter/design/FilterViewer.java @@ -0,0 +1,96 @@ +package dsp.filter.design; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import dsp.filter.fir.FIRFilterSpecification; +import dsp.filter.fir.remez.RemezFIRFilterDesigner; +import javafx.application.Application; +import javafx.scene.Scene; +import javafx.stage.Stage; + +public class FilterViewer extends Application +{ + private final static Logger mLog = LoggerFactory.getLogger( FilterViewer.class ); + + /** + * Developer tool to visualize filter designs + */ + public FilterViewer() + { + } + + @Override + public void start( Stage primaryStage ) throws Exception + { + Scene scene = new Scene( new FilterView( getFilter() ) ); + + primaryStage.setTitle( "Filter Viewer" ); + primaryStage.setScene( scene ); + primaryStage.show(); + } + + public static void main( String[] args ) + { + launch( args ); + } + + /** + * Provides the filter to visualize. Modify this method to visualize your filter design. + */ + private float[] getFilter() + { + FIRFilterSpecification specification = FIRFilterSpecification.lowPassBuilder() + .sampleRate( 10000 ) + .gridDensity( 16 ) + .passBandCutoff( 1300 ) + .passBandAmplitude( 1.0 ) + .passBandRipple( 0.01 ) + .stopBandStart( 2675 ) + .stopBandAmplitude( 0.0 ) + .stopBandRipple( 0.001 ) + .build(); + +// FIRFilterSpecification specification = FIRFilterSpecification.highPassBuilder() +// .sampleRate( 8000 ) +// .gridDensity( 16 ) +// .stopBandCutoff( 2000 ) +// .stopBandAmplitude( 0.0 ) +// .stopBandRipple( 0.003 ) +// .passBandStart( 3000 ) +// .passBandAmplitude( 1.0 ) +// .passBandRipple( 0.001 ) +// .build(); + +// FIRFilterSpecification specification = FIRFilterSpecification.bandPassBuilder() +// .sampleRate( 8000 ) +// .order( 164 ) +// .stopFrequency1( 1100 ) +// .passFrequencyBegin( 1200 ) +// .passFrequencyEnd( 2400 ) +// .stopFrequency2( 2500 ) +// .stopRipple( 0.01 ) +// .passRipple( 0.01 ) +// .build(); + + float[] taps = null; + +// taps = FilterFactory.getLowPass( 16000, 2400, 73, WindowType.HANNING ); + + try + { + RemezFIRFilterDesigner designer = new RemezFIRFilterDesigner( specification ); + + if( designer.isValid() ) + { + taps = designer.getImpulseResponse(); + } + } + catch( FilterDesignException fde ) + { + mLog.error( "Filter design error", fde ); + } + + return taps; + } +} diff --git a/src/dsp/filter/fir/FIRFilterSpecification.java b/src/dsp/filter/fir/FIRFilterSpecification.java new file mode 100644 index 00000000..b03e82ec --- /dev/null +++ b/src/dsp/filter/fir/FIRFilterSpecification.java @@ -0,0 +1,1131 @@ +package dsp.filter.fir; + +import java.text.DecimalFormat; +import java.util.ArrayList; +import java.util.Collection; +import java.util.List; + +import dsp.filter.fir.remez.FIRLinearPhaseFilterType; + +public class FIRFilterSpecification +{ + private static final DecimalFormat DECIMAL_FORMATTER = new DecimalFormat( "0.00000" ); + + private FIRLinearPhaseFilterType mRemezFilterType; + private int mOrder; + private int mGridDensity = 16; + private List mFrequencyBands = new ArrayList<>(); + + /** + * Constructs a Remez filter specification. Use one of the filter builders to construct a Remez + * filter specification. + * + * @param order of the filter + * @param frequencyBands containing the start and stop values for each frequency band normalized + * to the sample rate where each frequency value is in the range (0.0 <> 0.5) + * @param amplitudes of each frequency band + * @param weights of the band ripples normalized to the largest ripple value + */ + private FIRFilterSpecification( FIRLinearPhaseFilterType type, int order, int gridDensity ) + { + mRemezFilterType = type; + mOrder = order; + mGridDensity = gridDensity; + } + + public String toString() + { + StringBuilder sb = new StringBuilder(); + sb.append( "Type: " ); + sb.append( mRemezFilterType.name() ); + sb.append( " Order: " ); + sb.append( mOrder ); + sb.append( " Extrema: " ); + sb.append( getExtremaCount() ); + sb.append( " Grid Density: " ); + sb.append( mGridDensity ); + sb.append( " Grid Size: " ); + sb.append( getGridSize() ); + + for ( FrequencyBand band : mFrequencyBands ) + { + sb.append( "\n" ); + sb.append( band.toString() ); + } + + return sb.toString(); + } + + public void addFrequencyBand( FrequencyBand band ) + { + mFrequencyBands.add( band ); + + updateGridSize(); + } + + public void addFrequencyBands( Collection bands ) + { + mFrequencyBands.addAll( bands ); + + updateGridSize(); + } + + public List getFrequencyBands() + { + return mFrequencyBands; + } + + /** + * Returns the total bandwidth of the defined frequency bands in the specification. + * + * @return total bandwidth -- should be less than or equal to 0.5 + */ + public double getTotalBandwidth() + { + double bandwidth = 0.0; + + for ( FrequencyBand band : mFrequencyBands ) + { + bandwidth += band.getBandWidth(); + } + + return bandwidth; + } + + /** + * Creates a Low-Pass FIR filter specification builder that allows you to define the filter + * parameters and create a filter specification. + */ + public static LowPassBuilder lowPassBuilder() + { + return new LowPassBuilder(); + } + + /** + * Creates a High-Pass FIR filter specification builder that allows you to define the filter + * parameters and create a filter specification. + */ + public static HighPassBuilder highPassBuilder() + { + return new HighPassBuilder(); + } + + /** + * Creates a Band-Pass FIR filter specification builder that allows you to define the filter + * parameters and create a filter specification. + */ + public static BandPassBuilder bandPassBuilder() + { + return new BandPassBuilder(); + } + + public FIRLinearPhaseFilterType getFilterType() + { + return mRemezFilterType; + } + + public boolean isSymmetrical() + { + return mRemezFilterType == FIRLinearPhaseFilterType.TYPE_1_ODD_LENGTH_EVEN_ORDER_SYMMETRICAL || + mRemezFilterType == FIRLinearPhaseFilterType.TYPE_2_EVEN_LENGTH_ODD_ORDER_SYMMETRICAL; + } + + public boolean isAntiSymmetrical() + { + return mRemezFilterType == FIRLinearPhaseFilterType.TYPE_3_ODD_LENGTH_EVEN_ORDER_ANTI_SYMMETRICAL || + mRemezFilterType == FIRLinearPhaseFilterType.TYPE_4_EVEN_LENGTH_ODD_ORDER_ANTI_SYMMETRICAL; + } + + public int getOrder() + { + return mOrder; + } + + public boolean isEvenOrder() + { + return mOrder % 2 == 0; + } + + public boolean isOddOrder() + { + return mOrder % 2 == 1; + } + + public int getFilterLength() + { + return mOrder + 1; + } + + public int getBandCount() + { + return mFrequencyBands.size(); + } + + /** + * Grid density setting. Default value is 16. + */ + public int getGridDensity() + { + return mGridDensity; + } + + /** + * Sets the grid density setting. Default value is 16. + */ + public void setGridDensity( int density ) + { + mGridDensity = density; + + updateGridSize(); + } + + /** + * Maximum number of alternations/extrema (L + 2 or M + 2) + */ + public int getExtremaCount() + { + return getHalfFilterOrder() + 2; + } + + /** + * Half of the filter order. + * + * This value is referred to as L or M, depending on the source. + */ + public int getHalfFilterOrder() + { + switch ( mRemezFilterType ) + { + case TYPE_1_ODD_LENGTH_EVEN_ORDER_SYMMETRICAL: + return ( mOrder ) / 2; + case TYPE_2_EVEN_LENGTH_ODD_ORDER_SYMMETRICAL: + return ( mOrder - 1 ) / 2; + case TYPE_3_ODD_LENGTH_EVEN_ORDER_ANTI_SYMMETRICAL: + return ( mOrder - 2 ) / 2; + case TYPE_4_EVEN_LENGTH_ODD_ORDER_ANTI_SYMMETRICAL: + return ( mOrder - 1 ) / 2; + default: + return 0; + } + } + + /** + * Determines the max band ripple from across the frequency bands in this specification. + */ + public double getMaxBandAmplitude() + { + double maxRippleAmplitude = 0.0; + + for ( FrequencyBand band : mFrequencyBands ) + { + double bandRippleAmplitude = band.getRippleAmplitude(); + + if ( bandRippleAmplitude > maxRippleAmplitude ) + { + maxRippleAmplitude = bandRippleAmplitude; + } + } + + return maxRippleAmplitude; + } + + public int getNominalGridSize() + { + return ( getExtremaCount() - 1 ) * getGridDensity() + 1; + } + + public int getGridSize() + { + int gridSize = 0; + + for ( FrequencyBand band : mFrequencyBands ) + { + gridSize += band.getGridSize(); + } + + return gridSize; + } + + private void updateGridSize() + { + if ( !mFrequencyBands.isEmpty() ) + { + int gridSize = getNominalGridSize(); + + double totalBandwidth = getTotalBandwidth(); + + for ( FrequencyBand band : mFrequencyBands ) + { + band.setGridSize( gridSize, totalBandwidth ); + } + } + } + + // /** + // * Determines the grid size needed to represent the percentage of weighted + // frequency bands + // * over the entire frequency band. + // */ + // public int getGridSize() + // { + // int gridSize = 0; + // + // int maxGridSize = getMaxGridSize(); + // + // //Add the proportional index counts from each weighted band + // for( FrequencyBand band: mFrequencyBands ) + // { + // gridSize += band.getBandGridSize( maxGridSize ); + // } + // + // if( getSymmetry() == Symmetry.NEGATIVE ) + // { + // gridSize--; + // } + // + // return gridSize; + // } + + /** + * Dense frequency spacing for extrema count * grid density over a frequency range of 0.0 - 1.0 + */ + public double getGridFrequencyInterval() + { + return getTotalBandwidth() / (double) ( ( getGridSize() - mFrequencyBands.size() ) ); + } + + // /** + // * ''' FIR order estimator (lowpass, highpass, bandpass, mulitiband). + // * + // * (n, fo, ao, w) = remezord (f, a, dev) (n, fo, ao, w) = remezord (f, a, + // * dev, fs) + // * + // * (n, fo, ao, w) = remezord (f, a, dev) finds the approximate order, + // * normalized frequency band edges, frequency band amplitudes, and weights + // * that meet input specifications f, a, and dev, to use with the remez + // * command. + // * + // * f is a sequence of frequency band edges (between 0 and Fs/2, where Fs + // is + // * the sampling frequency), and a is a sequence specifying the desired + // * amplitude on the bands defined by f. The length of f is twice the + // length + // * of a, minus 2. The desired function is piecewise constant. + // * + // * dev is a sequence the same size as a that specifies the maximum + // allowable + // * deviation or ripples between the frequency response and the desired + // * amplitude of the output filter, for each band. + // * + // * Use remez with the resulting order n, frequency sequence fo, amplitude + // * response sequence ao, and weights w to design the filter b which + // * approximately meets the specifications given by remezord input + // parameters + // * f, a, and dev: + // * + // * b = remez (n, fo, ao, w) + // * + // * (n, fo, ao, w) = remezord (f, a, dev, Fs) specifies a sampling + // frequency + // * Fs. + // * + // * Fs defaults to 2 Hz, implying a Nyquist frequency of 1 Hz. You can + // * therefore specify band edges scaled to a particular applications + // sampling + // * frequency. + // * + // * In some cases remezord underestimates the order n. If the filter does + // not + // * meet the specifications, try a higher order such as n+1 or n+2. ''' + // * + // * @param fcuts + // * @param mags + // * @param devs + // * @param fsamp + // */ + // // TODO: rename to calculateFilterOrder() + // public static FIRFilterSpecification remezOrd( int[] fcutoffs, + // double[] mags, double[] devs, int fsamp ) + // { + // // def remezord (fcuts, mags, devs, fsamp = 2): + // + // double[] fcuts = new double[ fcutoffs.length ]; + // + // for ( int x = 0; x < fcuts.length; x++ ) + // { + // fcuts[ x ] = (double) fcutoffs[ x ] / (double) fsamp; + // } + // + // int nf = fcuts.length; + // int nm = mags.length; + // int nd = devs.length; + // int nbands = nm; + // + // if ( mags.length != devs.length ) + // { + // throw new IllegalArgumentException( + // "Length of magnitudes and deviations must be equal" ); + // } + // + // if ( fcuts.length != ( 2 * ( mags.length - 1 ) ) ) + // { + // throw new IllegalArgumentException( + // "Length of frequency band edges must be 2 * mags length - 2" ); + // } + // + // for ( int x = 0; x < mags.length; x++ ) + // { + // if ( mags[ x ] != 0 ) + // { + // devs[ x ] = devs[ x ] / mags[ x ]; + // } + // } + // + // // Separate the passband and stopband edges + // double[] f1 = new double[ fcuts.length / 2 ]; + // double[] f2 = new double[ fcuts.length / 2 ]; + // + // for ( int x = 0; x < fcuts.length / 2; x++ ) + // { + // f1[ x ] = fcuts[ 2 * x ]; + // f2[ x ] = fcuts[ 2 * x + 1 ]; + // } + // + // int n = 0; + // + // double min_delta = 2.0; + // + // for ( int x = 0; x < f1.length; x++ ) + // { + // if ( f2[ x ] - f1[ x ] < min_delta ) + // { + // n = x; + // min_delta = f2[ x ] - f1[ x ]; + // } + // } + // + // double l; + // + // if ( nbands == 2 ) + // { + // // lowpass or highpass case (use formula) + // l = lporder( f1[ n ], f2[ n ], devs[ 0 ], devs[ 1 ] ); + // } else + // { + // // bandpass or multipass case + // // try different lowpasses and take the worst one that + // // goes through the BP specs + // l = 0; + // + // for ( int x = 1; x < nbands - 1; x++ ) + // { + // double l1 = lporder( f1[ x - 1 ], f2[ x - 1 ], devs[ x ], + // devs[ x - 1 ] ); + // double l2 = lporder( f1[ x ], f2[ x ], devs[ x ], + // devs[ x + 1 ] ); + // l = Math.max( l1, l2 ); + // } + // } + // + // n = (int) ( Math.ceil( l ) - 1 ); // need order, not length for remez + // + // // // cook up remez compatible result + // double[] ff = new double[ fcuts.length + 2 ]; + // ff[ 0 ] = 0.0; + // ff[ ff.length - 1 ] = 1.0; + // + // System.arraycopy( fcuts, 0, ff, 1, fcuts.length ); + // + // for ( int x = 1; x < ff.length - 1; x++ ) + // { + // ff[ x ] *= 2; + // } + // + // double[] aa = new double[ mags.length * 2 ]; + // + // for ( int x = 0; x < mags.length; x++ ) + // { + // aa[ 2 * x ] = mags[ x ]; + // aa[ 2 * x + 1 ] = mags[ x ]; + // } + // + // double max_dev = 0; + // + // for ( double dev : devs ) + // { + // if ( dev > max_dev ) + // { + // max_dev = dev; + // } + // } + // + // double[] wts = new double[ devs.length ]; + // + // for ( int x = 0; x < devs.length; x++ ) + // { + // wts[ x ] = max_dev / devs[ x ]; + // } + // + // return null; + // // return new RemezFilterSpecification( RemezFilterType.BANDPASS, n, ff, + // // aa, wts, 16); + // } + + /** + * Low-pass FIR filter specification builder + */ + public static class LowPassBuilder + { + private int mSampleRate; + private int mOrder; + private int mGridDensity = 16; + private int mPassBandEndFrequency; + private int mStopBandStartFrequency; + private double mPassBandRipple; + private double mStopBandRipple; + private double mPassBandAmplitude = 1.0; + private double mStopBandAmplitude = 0.0; + + public LowPassBuilder() + { + } + + public LowPassBuilder sampleRate( int sampleRateHz ) + { + mSampleRate = sampleRateHz; + return this; + } + + /** + * Sets the filter order. If the order is not specified, it will be calculated from the + * other parameters. + */ + public LowPassBuilder order( int order ) + { + mOrder = order; + return this; + } + + public LowPassBuilder gridDensity( int density ) + { + mGridDensity = density; + return this; + } + + public LowPassBuilder passBandCutoff( int frequencyHz ) + { + mPassBandEndFrequency = frequencyHz; + return this; + } + + public LowPassBuilder stopBandStart( int frequencyHz ) + { + mStopBandStartFrequency = frequencyHz; + return this; + } + + public LowPassBuilder passBandRipple( double rippleDb ) + { + mPassBandRipple = rippleDb; + return this; + } + + public LowPassBuilder stopBandRipple( double rippleDb ) + { + mStopBandRipple = rippleDb; + return this; + } + + public LowPassBuilder passBandAmplitude( double amplitude ) + { + mPassBandAmplitude = amplitude; + return this; + } + + public LowPassBuilder stopBandAmplitude( double amplitude ) + { + mStopBandAmplitude = amplitude; + return this; + } + + public FIRFilterSpecification build() + { + if ( mOrder < 6 ) + { + mOrder = estimateFilterOrder( mSampleRate, mPassBandEndFrequency, + mStopBandStartFrequency, mPassBandRipple, mStopBandRipple ); + } + + FIRLinearPhaseFilterType type = ( mOrder % 2 == 0 + ? FIRLinearPhaseFilterType.TYPE_1_ODD_LENGTH_EVEN_ORDER_SYMMETRICAL + : FIRLinearPhaseFilterType.TYPE_2_EVEN_LENGTH_ODD_ORDER_SYMMETRICAL ); + + FIRFilterSpecification spec = new FIRFilterSpecification( type, mOrder, mGridDensity ); + + FrequencyBand passBand = new FrequencyBand( mSampleRate, 0, mPassBandEndFrequency, + mPassBandAmplitude, mPassBandRipple ); + spec.addFrequencyBand( passBand ); + + FrequencyBand stopBand = new FrequencyBand( mSampleRate, mStopBandStartFrequency, + mSampleRate / 2, mStopBandAmplitude, mStopBandRipple ); + spec.addFrequencyBand( stopBand ); + + return spec; + } + } + + /** + * Remez high-pass FIR filter specification builder + */ + public static class HighPassBuilder + { + private int mSampleRate; + private int mOrder = -1; + private int mGridDensity = 16; + private int mPassBandStartFrequency; + private int mStopBandEndFrequency; + private double mPassBandRipple; + private double mStopBandRipple; + private double mPassBandAmplitude = 1.0; + private double mStopBandAmplitude = 0.0; + + public HighPassBuilder() + { + } + + public HighPassBuilder sampleRate( int sampleRateHz ) + { + mSampleRate = sampleRateHz; + return this; + } + + /** + * Sets the filter order. If the order is not specified, it will be calculated from the + * other parameters. + */ + public HighPassBuilder order( int order ) + { + mOrder = order; + return this; + } + + public HighPassBuilder gridDensity( int density ) + { + mGridDensity = density; + return this; + } + + public HighPassBuilder passBandStart( int frequencyHz ) + { + mPassBandStartFrequency = frequencyHz; + return this; + } + + public HighPassBuilder stopBandCutoff( int frequencyHz ) + { + mStopBandEndFrequency = frequencyHz; + return this; + } + + public HighPassBuilder passBandRipple( double rippleDb ) + { + mPassBandRipple = rippleDb; + return this; + } + + public HighPassBuilder stopBandRipple( double rippleDb ) + { + mStopBandRipple = rippleDb; + return this; + } + + public HighPassBuilder passBandAmplitude( double amplitude ) + { + mPassBandAmplitude = amplitude; + return this; + } + + public HighPassBuilder stopBandAmplitude( double amplitude ) + { + mStopBandAmplitude = amplitude; + return this; + } + + public FIRFilterSpecification build() + { + if ( mOrder < 6 ) + { + mOrder = estimateFilterOrder( mSampleRate, mStopBandEndFrequency, + mPassBandStartFrequency, mPassBandRipple, mStopBandRipple ); + } + + // High pass is a Type 1 filter -- ensure order is even + if ( mOrder % 2 == 1 ) + { + mOrder++; + } + + FIRFilterSpecification spec = new FIRFilterSpecification( + FIRLinearPhaseFilterType.TYPE_1_ODD_LENGTH_EVEN_ORDER_SYMMETRICAL, mOrder, + mGridDensity ); + + FrequencyBand stopBand = new FrequencyBand( mSampleRate, 0, mStopBandEndFrequency, + mStopBandAmplitude, mStopBandRipple ); + spec.addFrequencyBand( stopBand ); + + FrequencyBand passBand = new FrequencyBand( mSampleRate, mPassBandStartFrequency, + mSampleRate / 2, mPassBandAmplitude, mPassBandRipple ); + spec.addFrequencyBand( passBand ); + + return spec; + } + } + + /** + * Band-pass or band-stop FIR filter specification builder + */ + public static class BandPassBuilder + { + private int mOrder; + private int mGridDensity = 16; + private int mSampleRate; + private int mStopFrequency1; + private int mPassFrequencyBegin; + private int mPassFrequencyEnd; + private int mStopFrequency2; + private double mStopRipple = 0.01; + private double mPassRipple = 0.01; + private double mStopAmplitude = 0.0; + private double mPassAmplitude = 1.0; + + public BandPassBuilder() + { + } + + /** + * Sets the filter order. If the order is not specified, it will be calculated from the + * other parameters. If you specify and odd-length order, it will be corrected (+1) to make + * it an even order number. + */ + public BandPassBuilder order( int order ) + { + mOrder = order; + return this; + } + + /** + * Sets the frequency grid density. Default is 16. + */ + public BandPassBuilder gridDensity( int density ) + { + mGridDensity = density; + return this; + } + + /** + * Sets the filter sample rate in hertz + */ + public BandPassBuilder sampleRate( int sampleRate ) + { + mSampleRate = sampleRate; + return this; + } + + /** + * Specifies the ending frequency of the first stop band + */ + public BandPassBuilder stopFrequency1( int stopFrequency ) + { + mStopFrequency1 = stopFrequency; + return this; + } + + /** + * Specifies the beginning frequency of the second stop band + */ + public BandPassBuilder stopFrequency2( int stopFrequency ) + { + mStopFrequency2 = stopFrequency; + return this; + } + + /** + * Specifies the pass band beginning frequency + */ + public BandPassBuilder passFrequencyBegin( int passFrequency ) + { + mPassFrequencyBegin = passFrequency; + return this; + } + + /** + * Specifies the pass band ending frequency + */ + public BandPassBuilder passFrequencyEnd( int passFrequency ) + { + mPassFrequencyEnd = passFrequency; + return this; + } + + /** + * Specifies the ripple in dB for both stop bands + */ + public BandPassBuilder stopRipple( double rippleDb ) + { + mStopRipple = rippleDb; + return this; + } + + /** + * Specifies the ripple in dB for the pass band + */ + public BandPassBuilder passRipple( double rippleDb ) + { + mPassRipple = rippleDb; + return this; + } + + /** + * Specifies the amplitude of the stop bands. Default is 0.0 + */ + public BandPassBuilder stopAmplitude( double amplitude ) + { + mStopAmplitude = amplitude; + return this; + } + + /** + * Specifies the amplitude of the pass band. Default is 1.0 + */ + public BandPassBuilder passAmplitude( double amplitude ) + { + mPassAmplitude = amplitude; + return this; + } + + /** + * Validates the frequency bands to test for overlap and completeness. + */ + private void validateFrequencyBands() + { + if ( mStopFrequency1 >= mPassFrequencyBegin ) + { + throw new IllegalArgumentException( "Stop band 1 ending frequency [" + + mStopFrequency1 + "] must be less than the pass band start frequency [" + + mPassFrequencyBegin + "]" ); + } + + if ( mPassFrequencyBegin >= mPassFrequencyEnd ) + { + throw new IllegalArgumentException( "Pass band begin frequency [" + + mPassFrequencyBegin + "] must be less than the pass band end frequency [" + + mPassFrequencyEnd + "]" ); + } + + if ( mPassFrequencyEnd >= mStopFrequency2 ) + { + throw new IllegalArgumentException( "Pass band end frequency [" + mPassFrequencyEnd + + "] must be less than stop band 2 beginning frequency [" + mStopFrequency2 + + "]" ); + } + + if ( mStopFrequency2 >= ( mSampleRate / 2 ) ) + { + throw new IllegalArgumentException( "Stop band 2 beginning frequency [" + + mStopFrequency2 + "] must be less than half of the sample rate [" + + ( mSampleRate / 2 ) + "]" ); + } + } + + public FIRFilterSpecification build() + { + validateFrequencyBands(); + + FIRLinearPhaseFilterType type = FIRLinearPhaseFilterType.TYPE_1_ODD_LENGTH_EVEN_ORDER_SYMMETRICAL; + + if ( mOrder < 10 ) + { + mOrder = estimateBandPassOrder( mSampleRate, mPassFrequencyBegin, mPassFrequencyEnd, + mPassRipple, mStopRipple ); + } + + // Ensure even order since we're designing a Type 1 filter + if ( mOrder % 2 == 1 ) + { + mOrder++; + } + + FIRFilterSpecification spec = new FIRFilterSpecification( type, mOrder, mGridDensity ); + + spec.addFrequencyBand( new FrequencyBand( mSampleRate, 0, mStopFrequency1, + mStopAmplitude, mStopRipple ) ); + spec.addFrequencyBand( new FrequencyBand( mSampleRate, mPassFrequencyBegin, + mPassFrequencyEnd, mPassAmplitude, mPassRipple ) ); + spec.addFrequencyBand( new FrequencyBand( mSampleRate, mStopFrequency2, + (int) ( mSampleRate / 2 ), mStopAmplitude, mStopRipple ) ); + + return spec; + } + } + + // /** + // * Converts the set of band amplitude values into a set of weights + // * normalized to the largest band amplitude value + // */ + // private static double[] getBandWeights( double... amplitudes ) + // { + // double maximum = 0.0; + // + // for ( double amplitude : amplitudes ) + // { + // if ( amplitude > maximum ) + // { + // maximum = amplitude; + // } + // } + // + // double[] weights = new double[ amplitudes.length ]; + // + // for ( int x = 0; x < amplitudes.length; x++ ) + // { + // weights[ x ] = maximum / amplitudes[ x ]; + // } + // + // return weights; + // } + + /** + * Calculates filter order from filter length + */ + private static int getFilterOrder( double length ) + { + return (int) ( Math.ceil( length ) - 1 ); + } + + /** + * Estimates filter length for a two-band (high or low pass) FIR filter. + * + * Note: this estimator doesn't work well if the transition frequency is near 0 or sampleRate/2 + * + * From Herrmann et al (1973), Practical design rules for optimum finite impulse response + * filters. Bell System Technical J., 52, 769-99 + * + * @param sampleRate in hertz + * @param frequency1 in hertz + * @param frequency2 in hertz + * @param passBandRipple pass band ripple in dB + * @param stopBandRipple stop band ripple in dB + * + * @return estimated filter length + */ + private static int estimateFilterOrder( int sampleRate, int frequency1, int frequency2, + double passBandRipple, double stopBandRipple ) + { + double df = Math.abs( frequency2 - frequency1 ) / (double) sampleRate; + + double ddp = Math + .log10( stopBandRipple <= passBandRipple ? passBandRipple : stopBandRipple ); + double dds = Math + .log10( stopBandRipple <= passBandRipple ? stopBandRipple : passBandRipple ); + + double a1 = 5.309e-3; + double a2 = 7.114e-2; + double a3 = -4.761e-1; + double a4 = -2.66e-3; + double a5 = -5.941e-1; + double a6 = -4.278e-1; + + double b1 = 11.01217; + double b2 = 0.5124401; + + double t1 = a1 * ddp * ddp; + double t2 = a2 * ddp; + double t3 = a4 * ddp * ddp; + double t4 = a5 * ddp; + + double dinf = ( ( t1 + t2 + a3 ) * dds ) + ( t3 + t4 + a6 ); + double ff = b1 + b2 * ( ddp - dds ); + double n = dinf / df - ff * df + 1.0; + + return (int) Math.ceil( n ); + } + + /** + * Estimates band pass filter length; + * + * @param passBandStart frequency normalized to sample rate + * @param passBandEnd frequency normalized to sample rate + * @param passBandRippleDb + * @param stopBandRippleDb + */ + public static int estimateBandPassOrder( int sampleRate, int passBandStart, int passBandEnd, + double passBandRippleDb, double stopBandRippleDb ) + { + double df = (double) Math.abs( passBandEnd - passBandStart ) / (double) sampleRate; + double ddp = (double) Math.log10( passBandRippleDb ); + double dds = (double) Math.log10( stopBandRippleDb ); + + double a1 = 0.01201; + double a2 = 0.09664; + double a3 = -0.51325; + double a4 = 0.00203; + double a5 = -0.57054; + double a6 = -0.44314; + + double t1 = a1 * ddp * ddp; + double t2 = a2 * ddp; + double t3 = a4 * ddp * ddp; + double t4 = a5 * ddp; + + double cinf = dds * ( t1 + t2 + a3 ) + t3 + t4 + a6; + double ginf = -14.6f * (double) Math.log10( passBandRippleDb / stopBandRippleDb ) - 16.9; + double n = cinf / df + ginf * df + 1.0; + + return (int) Math.ceil( n ); + } + + /** + * Frequency band + */ + public static class FrequencyBand + { + private int mGridSize; + private double mStart; + private double mEnd; + private double mAmplitude; + private double mRippleDB; + + /** + * Constructs a frequency band where the start and end frequencies are normalized to a + * sample rate of 1. + * + * @param start edge of the frequency band (0.0 - 1.0) + * @param end edge of the frequency band (0.0 - 1.0) + * @param amplitude of the frequency band on the unity scale (0.0 - 1.0) + * @param ripple of the frequency band specified as db ripple + */ + public FrequencyBand( double start, double end, double amplitude, double ripple ) + { + assert ( 0.0 <= mStart ); + assert ( mStart < mEnd ); + assert ( mEnd <= 1.0 ); + assert ( 0.0 <= amplitude && amplitude <= 1.0 ); + assert ( 0.0 <= ripple ); + + mStart = start; + mEnd = end; + mAmplitude = amplitude; + mRippleDB = ripple; + } + + /** + * Constructs a frequency band for the sample rate and start and end frequencies. + * + * @param sampleRate in hertz + * @param start edge of the frequency band in hertz + * @param end edge of the frequency band in hertz + * @param amplitude of the frequency band relative to unity (0.0 - 1.0) + * @param ripple of the frequency band specified as db ripple + */ + public FrequencyBand( int sampleRate, int start, int end, double amplitude, double ripple ) + { + this( (double) start / (double) sampleRate, (double) end / (double) sampleRate, + amplitude, ripple ); + } + + public int getGridSize() + { + return mGridSize; + } + + public void setGridSize( int totalGridSize, double totalBandwidth ) + { + mGridSize = (int) Math + .ceil( (double) totalGridSize * ( getBandWidth() / totalBandwidth ) ); + } + + public String toString() + { + StringBuilder sb = new StringBuilder(); + + sb.append( "Band Start: " ); + sb.append( mStart ); + sb.append( " End:" ); + sb.append( mEnd ); + sb.append( " Amplitude: " ); + sb.append( mAmplitude ); + sb.append( " Ripple dB: " ); + sb.append( mRippleDB ); + sb.append( " Ripple Amplitude: " ); + sb.append( DECIMAL_FORMATTER.format( getRippleAmplitude() ) ); + sb.append( " Grid Size: " ); + sb.append( mGridSize ); + + return sb.toString(); + } + + /** + * Start frequency edge for this band normalized to 1 Hz. + */ + public double getStart() + { + return mStart; + } + + /** + * End frequency edge for this band normalized to 1 Hz. + */ + public double getEnd() + { + return mEnd; + } + + /** + * Desired amplitude response for this frequency band + * + * @return amplitude (0.0 - 1.0) + */ + public double getAmplitude() + { + return mAmplitude; + } + + /** + * Frequency band ripple in decibels + */ + public double getRippleDB() + { + return mRippleDB; + } + + /** + * Frequency band ripple as an amplitude value + */ + public double getRippleAmplitude() + { + return ( Math.pow( 10.0, ( mRippleDB / 20 ) ) - 1 ) + / ( Math.pow( 10.0, ( mRippleDB / 20 ) ) + 1 ); + } + + /** + * Calculates the weighting of this frequency band's ripple relative to the max ripple for a + * filter specification. + * + * @param maxRippleAmplitude for the filter specification + * + * @return weight of this frequency band + */ + public double getWeight( double maxRippleAmplitude ) + { + return 1.0 / ( getRippleAmplitude() / maxRippleAmplitude ); + } + + /** + * Double-sided bandwidth of this frequency band + */ + public double getBandWidth() + { + return mEnd - mStart; + } + } +} diff --git a/src/dsp/filter/fir/remez/FIRLinearPhaseFilterType.java b/src/dsp/filter/fir/remez/FIRLinearPhaseFilterType.java new file mode 100644 index 00000000..aef00ec7 --- /dev/null +++ b/src/dsp/filter/fir/remez/FIRLinearPhaseFilterType.java @@ -0,0 +1,56 @@ +package dsp.filter.fir.remez; + +public enum FIRLinearPhaseFilterType +{ + /** + * TYPE 1 + * Length: ODD + * Order: EVEN + * Symmetry: SYMMETRICAL + * + * Valid for: LOW PASS, HIGH PASS + */ + TYPE_1_ODD_LENGTH_EVEN_ORDER_SYMMETRICAL, + + /** + * TYPE 2 + * Length: EVEN + * Order: ODD + * Symmetry: SYMMETRICAL + * Valid for: LOW PASS + * + * Notes: + * -Frequency response at FS/2 (Pi) must be 0 -- not suitable for High Pass. + */ + TYPE_2_EVEN_LENGTH_ODD_ORDER_SYMMETRICAL, + + /** + * TYPE 3 + * Length: ODD + * Order: EVEN + * Symmetry: ANTI-SYMMETRICAL + * Valid for: + * + * Notes: + * -Introduces a 90 degree phase shift such that h[N-n] = -h[n] for all n + * -Frequency response at 0 and FS/2 (Pi) must be 0 -- not suitable for Low or High Pass. + */ + TYPE_3_ODD_LENGTH_EVEN_ORDER_ANTI_SYMMETRICAL, + + /** + * TYPE 4 + * Length: EVEN + * Order: ODD + * Symmetry: ANTI-SYMMETRICAL + * Valid for: HIGH PASS + * + * Notes: + * -Introduces a 90 degree phase shift such that h[N-n] = -h[n] for all n + * -Frequency response at 0 must be zero -- not suitable for Low Pass + */ + TYPE_4_EVEN_LENGTH_ODD_ORDER_ANTI_SYMMETRICAL, + + BANDPASS, + DIFFERENTIATOR, + HILBERT; +} diff --git a/src/dsp/filter/fir/remez/Grid.java b/src/dsp/filter/fir/remez/Grid.java new file mode 100644 index 00000000..bca44b40 --- /dev/null +++ b/src/dsp/filter/fir/remez/Grid.java @@ -0,0 +1,112 @@ +package dsp.filter.fir.remez; + +import dsp.filter.fir.FIRFilterSpecification; +import dsp.filter.fir.FIRFilterSpecification.FrequencyBand; + +public class Grid +{ + public static final double TWO_PI = 2.0 * Math.PI; + + private double[] mFrequencyGrid; + private double[] mCosineOfFrequencyGrid; + private double[] mDesiredResponse; + private double[] mWeight; + + /** + * Creates a dense frequency grid from the filter specification for those frequency bands + * where the weighted value is non-zero. This grid is appropriate for use in the Remez + * exchange algorithm. + */ + public Grid( FIRFilterSpecification specification ) + { + create( specification ); + } + + private void create( FIRFilterSpecification specification ) + { + int gridSize = specification.getGridSize(); + + mFrequencyGrid = new double[ gridSize ]; + mCosineOfFrequencyGrid = new double[ gridSize ]; + mDesiredResponse = new double[ gridSize ]; + mWeight = new double[ gridSize ]; + + double gridFrequencyInterval = specification.getGridFrequencyInterval(); + + double bandZeroEdge = specification.getFrequencyBands().get( 0 ).getStart(); + + // For differentiator, hilbert, symmetry is odd and Grid[0] = max(delf, bands[0]) + double grid0 = specification.isAntiSymmetrical() && + ( gridFrequencyInterval > bandZeroEdge ) ? gridFrequencyInterval : bandZeroEdge; + + int j = 0; + + double maxRipple = specification.getMaxBandAmplitude(); + + for( int x = 0; x < specification.getFrequencyBands().size(); x++ ) + { + FrequencyBand band = specification.getFrequencyBands().get( x ); + + double lowFrequency = ( x == 0 ? grid0 : band.getStart() ); + + for( int i = 0; i < band.getGridSize(); i++ ) + { + mDesiredResponse[ j ] = band.getAmplitude(); + + //Set the weighting function equally across this band's dense grids + mWeight[ j ] = band.getWeight( maxRipple ); + + //Frequency grid is linear taper from start frequency to stop frequency in + //gridFrequencyInterval increments + mFrequencyGrid[ j ] = lowFrequency; + + mCosineOfFrequencyGrid[ j ] = Math.cos( TWO_PI * mFrequencyGrid[ j ] ); + + lowFrequency += gridFrequencyInterval; + + j++; + } + + int lastIndex = j - 1; + + mFrequencyGrid[ lastIndex ] = band.getEnd(); + } + } + + /** + * Size of the dense frequency grid + */ + public int getSize() + { + return mFrequencyGrid.length; + } + + /** + * The dense frequency grid + */ + public double[] getFrequencyGrid() + { + return mFrequencyGrid; + } + + public double[] getCosineFrequencyGrid() + { + return mCosineOfFrequencyGrid; + } + + /** + * Desired frequency response + */ + public double[] getDesiredResponse() + { + return mDesiredResponse; + } + + /** + * Weighting function + */ + public double[] getWeight() + { + return mWeight; + } +} diff --git a/src/dsp/filter/fir/remez/RemezFIRFilterDesigner.java b/src/dsp/filter/fir/remez/RemezFIRFilterDesigner.java new file mode 100644 index 00000000..e5ca196b --- /dev/null +++ b/src/dsp/filter/fir/remez/RemezFIRFilterDesigner.java @@ -0,0 +1,675 @@ +package dsp.filter.fir.remez; + +import java.text.DecimalFormat; +import java.util.ArrayList; +import java.util.Iterator; +import java.util.List; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import dsp.filter.design.FilterDesignException; +import dsp.filter.fir.FIRFilterSpecification; + +public class RemezFIRFilterDesigner +{ + private final static Logger mLog = LoggerFactory.getLogger( RemezFIRFilterDesigner.class ); + + private static final double CONVERGENCE_THRESHOLD = 0.0001; + public static final int MAXIMUM_ITERATION_COUNT = 40; + public static final double TWO_PI = 2.0 * Math.PI; + + private static final DecimalFormat DECIMAL_FORMATTER = new DecimalFormat( "0.00000000" ); + + private FIRFilterSpecification mSpecification; + private Grid mGrid; + private List mExtremalIndices; + + private double[] mD; + private double[] mGridErrors; + private double[] mGridFrequencyResponse; + private double[] mIdealFrequencyResponse; + + private double mDelta; + + private boolean mConverged; + + /** + * Linear FIR filter designer using the Parks-McClelland Remez exchange algorithm. + * + * Construct a filter specification using the builder methods in the FIRFilterSpecification + * class and construct an instance of this class with the specification. Check the isValid() + * method after construction to see if the filter design was successful. If successful, use the + * getFrequencyResponse() and getImpulseResponse() methods to access the filter taps as the + * specified length. + * + * If the filter cannot be designed from the specification the isValid() method will return + * false and any attempts to access the getFrequencyResponse() or getImpulseResponse() methods + * will throw a FilterDesignException. + * + * @param specification that defines a Type 1-4 linear phase FIR Filter + */ + public RemezFIRFilterDesigner( FIRFilterSpecification specification ) + { + mSpecification = specification; + + design(); + } + + /** + * Designs the filter according to the specification + */ + private void design() + { + mGrid = new Grid( mSpecification ); + mExtremalIndices = getInitialExtremalIndices(); + + int iterationCount = 0; + + try + { + do + { + calculateGridFrequencyResponse(); + calculateGridError(); + findExtremalIndices(); + checkConvergence(); + + iterationCount++; + } + while( !mConverged && iterationCount < MAXIMUM_ITERATION_COUNT ); + } + catch( FilterDesignException fde ) + { + mLog.error( "Filter design error - couldn't find extremal indices at count [" + + iterationCount + "] try changing filter order up/down from [" + + mSpecification.getOrder() + "]" ); + + mConverged = false; + } + + if( mConverged ) + { + //Update frequency response using the final set of extremal indices + calculateGridFrequencyResponse(); + } + } + + /** + * Indicates if the filter was successfully created from the filter specification. Check this + * method before accessing frequency response or filter response, otherwise a FilterDesignException + * is thrown. + * + * @return true if the filter was successfully designed. + */ + public boolean isValid() + { + return mConverged; + } + + /** + * Frequency response of the designed filter with double precision + * + * @return half ( length / 2 ) of filter frequency response sampled at 0.0 - PI radians + * @throws FilterDesignException if the specified filter cannot be designed + */ + public double[] getFrequencyResponse() throws FilterDesignException + { + if( !mConverged ) + { + throw new FilterDesignException( "Can't create filter from specification - failed " + + "to converge" ); + } + + //Resample the final polynomial at the desired filter length + double[] resampled = resample(); + + //Apply Parks/McClelland frequency response corrections according to the filter type + return correctFrequencyResponse( resampled ); + } + + /** + * Impulse response of the designed filter with floating point precision. + * + * @return filter impulse response + * @throws FilterDesignException if the specified filter cannot be designed + */ + public float[] getImpulseResponse() throws FilterDesignException + { + return convertToFloatArray( getImpulseResponseDoubles() ); + } + + /** + * Impulse response of the designed filter with double precision + */ + public double[] getImpulseResponseDoubles() throws FilterDesignException + { + double[] frequencyResponse = getFrequencyResponse(); + + int length = mSpecification.getFilterLength(); + + double[] impulseResponse = new double[ length ]; + + double M; + + switch( mSpecification.getFilterType() ) + { + case TYPE_1_ODD_LENGTH_EVEN_ORDER_SYMMETRICAL: + M = ( (double)length - 1.0 ) / 2.0; + + for( int n = 0; n < length; n++ ) + { + double accumulator = frequencyResponse[ 0 ]; + + double frequency = TWO_PI * ( n - M ) / length; + + for( int k = 1; k <= M; k++ ) + { + accumulator += 2.0 * frequencyResponse[ k ] * Math.cos( frequency * (double)k ); + } + + impulseResponse[ n ] = accumulator / (double)length; + } + break; + case TYPE_2_EVEN_LENGTH_ODD_ORDER_SYMMETRICAL: + double offset = (double)( length - 1 ) / 2.0; + + for( int n = 0; n < length; n++ ) + { + double accumulator = frequencyResponse[ 0 ]; + + double frequency = TWO_PI * ( (double)n - offset ) / (double)length; + + for( int k = 1; k < frequencyResponse.length; k++ ) + { + accumulator += 2.0 * frequencyResponse[ k ] * Math.cos( frequency * (double)k ); + } + + impulseResponse[ n ] = accumulator / (double)length; + } + break; + } + + return impulseResponse; + } + + /** + * Calculates the frequency response of this filter at the specified cosine of the frequency + * + * Implements Oppenheim/Schafer Discrete Time Signal Processing, 3e, 2016, equation 116a to + * determine the actual response of the specified frequency cosine using Lagrange + * interpolation of the polynomial formed at the extremal index set. + * + * @param cosineOfFrequency to calculate the frequency response + * + * @return actual frequency response for the specified frequency with the current set of + * extreaml indices + */ + private double getFrequencyResponse( double cosineOfFrequency ) + { + double numerator = 0.0; + double denominator = 0.0; + + for( int k = 0; k < mExtremalIndices.size() - 1; k++ ) + { + double cosineDelta = cosineOfFrequency - + mGrid.getCosineFrequencyGrid()[ mExtremalIndices.get( k ) ]; + + //If this frequency is or is close to one of the polynomial points, use c[k] for that point + if( Math.abs( cosineDelta ) < 1.0e-7 ) + { + numerator = mIdealFrequencyResponse[ k ]; + denominator = 1.0; + break; + } + else + { + double dkOverCosineDelta = mD[ k ] / cosineDelta; + + numerator += dkOverCosineDelta * mIdealFrequencyResponse[ k ]; + denominator += dkOverCosineDelta; + } + } + + return numerator / denominator; + } + + /** + * Calculated Delta value + * + * @return delta value + */ + public double getDelta() + { + return mDelta; + } + + /** + * Creates initial array of extremal frequency indices spaced at intervals of the grid frequency + * set for all frequencies where a frequency band weighting is specified. + */ + private List getInitialExtremalIndices() + { + int count = mSpecification.getExtremaCount(); + + int density = mSpecification.getGridDensity(); + + List extremalIndices = new ArrayList<>(); + + for( int i = 0; i < count; i++ ) + { + extremalIndices.add( i * density ); + } + + return extremalIndices; + } + + /** + * Calculates the actual frequency response of a dense frequency grid using the polynomial + * represented by the extremal indices. + */ + private void calculateGridFrequencyResponse() + { + double[] b = calculateB(); + + calculateDelta( b ); + + calculateC(); + + calculateD( b ); + + updateGridFrequencyResponse(); + } + + /** + * Updates the frequency response of frequencies specified in the dense frequency grid + */ + private void updateGridFrequencyResponse() + { + mGridFrequencyResponse = new double[ mGrid.getSize() ]; + + double[] gridFrequencyCosines = mGrid.getCosineFrequencyGrid(); + + for( int i = 0; i < mGridFrequencyResponse.length; i++ ) + { + mGridFrequencyResponse[ i ] = getFrequencyResponse( gridFrequencyCosines[ i ] ); + } + } + + /** + * Calculates the value of b across the set of extremal indices. + * + * Implements Oppenheim/Schafer Discrete Time Signal Processing, 3e, 2016, equation 115 + * + * @return the array of b values of length L+2 + */ + private double[] calculateB() + { + int length = mExtremalIndices.size(); + + double[] b = new double[ length ]; + + for( int k = 0; k < length; k++ ) + { + b[ k ] = 1.0; + + double xk = mGrid.getCosineFrequencyGrid()[ mExtremalIndices.get( k ) ]; + + for( int i = 0; i < length; i++ ) + { + if( i != k ) + { + double xi = mGrid.getCosineFrequencyGrid()[ mExtremalIndices.get( i ) ]; + + double denominator = xk - xi; + + if( Math.abs( denominator ) < 0.00001 ) + { + denominator = 0.00001; + } + + b[ k ] *= 1.0 / denominator; + } + } + } + + return b; + } + + /** + * Calculates the value of delta that represents the maximum ripple for the current set of + * extremal indices (L+2) + * + * Implements Oppenheim/Schafer Discrete Time Signal Processing, 3e, 2016, equation 114 + * + * @param b calculated for the grid from equation 115 + */ + private void calculateDelta( double[] b ) + { + double numerator = 0.0; + double denominator = 0.0; + + double sign = 1.0; + + for( int k = 0; k < b.length; k++ ) + { + if( k < mExtremalIndices.size() ) + { + int extremalIndex = mExtremalIndices.get( k ); + + numerator += ( b[ k ] * mGrid.getDesiredResponse()[ extremalIndex ] ); + denominator += b[ k ] * sign / mGrid.getWeight()[ extremalIndex ]; + sign = -sign; + } + else + { + mLog.error( "Something went wrong -- the length of b exceeds the set of extremal indices" ); + } + } + + mDelta = numerator / denominator; + } + + /** + * Calculates the ideal frequency response (C) of the current extremal index set. This reference + * is used to determine the level of error in the set of actual frequency responses calculated + * from this set of extremal indices to determine convergence. + * + * Implements Oppenheim/Schafer Discrete Time Signal Processing, 3e, 2016, equation 116b + */ + private void calculateC() + { + int length = mSpecification.getExtremaCount() - 1; + + mIdealFrequencyResponse = new double[ length ]; + + double sign = 1.0; + + for( int k = 0; k < length; k++ ) + { + if( k < mExtremalIndices.size() ) + { + int index = mExtremalIndices.get( k ); + + mIdealFrequencyResponse[ k ] = mGrid.getDesiredResponse()[ index ] - + ( sign * mDelta / mGrid.getWeight()[ index ] ); + + sign = -sign; + } + } + } + + /** + * Calculates the set of D values for the current extremal index set. + * + * Implements Oppenheim/Schafer Discrete Time Signal Processing, 3e, 2016, equation 116c + */ + private void calculateD( double[] b ) + { + int length = mExtremalIndices.size() - 1; + + mD = new double[ length ]; + + for( int k = 0; k < length; k++ ) + { + //Note: extremalCosines array is one index longer than d array, so we use length as + //a pointer to the final (L+2) extremalCosine index + mD[k] = b[k] * ( mGrid.getCosineFrequencyGrid()[ mExtremalIndices.get( k ) ] - + mGrid.getCosineFrequencyGrid()[ mExtremalIndices.get( length ) ] ); + } + } + + /** + * Calculates the weighted error between the specification desired frequency response and the + * actual frequency response of the current extremal index set across the frequencies in the + * dense frequency grid. + * + * Implements Oppenheim/Schafer Discrete Time Signal Processing, 3e, 2016, equation 112 + */ + public void calculateGridError() + { + int length = mGridFrequencyResponse.length; + + mGridErrors = new double[ length ]; + + for( int i = 0; i < length; i++ ) + { + mGridErrors[ i ] = mGrid.getWeight()[ i ] * + ( mGrid.getDesiredResponse()[ i ] - mGridFrequencyResponse[ i ] ); + } + } + + /** + * Identifies the indexes of the extremal error values using the Alternation theorem + * + * @throws FilterDesignException if the full set of extremal indices can't be identified + */ + private void findExtremalIndices() throws FilterDesignException + { + mExtremalIndices.clear(); + + //Check for extremum at error index 0 + if( ( ( mGridErrors[ 0 ] > 0.0 && mGridErrors[ 0 ] > mGridErrors[ 1 ] ) || + ( mGridErrors[ 0 ] < 0.0 && mGridErrors[ 0 ] < mGridErrors[ 1 ] ) ) && + isGTEDelta( mGridErrors[ 0 ] ) ) + { + mExtremalIndices.add( 0 ); + } + + //Check for extrema in middle error indices ) + for( int x = 1; x < mGridErrors.length - 1; x++ ) + { + if( ( ( mGridErrors[ x ] > 0.0 && ( mGridErrors[ x - 1 ] <= mGridErrors[ x ] && mGridErrors[ x ] > mGridErrors[ x + 1 ] ) ) || + ( mGridErrors[ x ] < 0.0 && ( mGridErrors[ x - 1 ] >= mGridErrors[ x ] && mGridErrors[ x ] < mGridErrors[ x + 1 ] ) ) ) && + isGTEDelta( mGridErrors[ x ] ) ) + { + mExtremalIndices.add( x ); + } + } + + //Check for extremum at final error index + int last = mGridErrors.length - 1; + + if( ( ( mGridErrors[ last ] > 0.0 && ( mGridErrors[ last ] > mGridErrors[ last - 1 ] ) ) || + ( mGridErrors[ last ] < 0.0 && ( mGridErrors[ last ] < mGridErrors[ last - 1 ] ) ) ) && + isGTEDelta( mGridErrors[ last ] ) ) + { + mExtremalIndices.add( last ); + } + + //Ensure we have the minimum of extremals before we continue + if( mExtremalIndices.size() < mSpecification.getExtremaCount() ) + { + throw new FilterDesignException( "Couldn't find the minimum extremal frequencies in " + + "error set" ); + } + + //Enforce alternation theory -- only one extremal for each transition about the zero axis + List indicesToRemove = new ArrayList<>(); + + Iterator it = mExtremalIndices.iterator(); + Integer current = it.next(); + Integer next; + + boolean positiveAxis = mGridErrors[ current ] > 0.0; + + while( it.hasNext() ) + { + next = it.next(); + + //Check for consecutive (redundant) indices on same side of zero axis - retain largest + if( !( positiveAxis ^ ( mGridErrors[ next ] > 0.0 ) ) ) + { + if( Math.abs( mGridErrors[ next ] ) <= Math.abs( mGridErrors[ current ] )) + { + //Remove next error index that is less than current index and on + it.remove(); + next = current; + } + else + { + //Since we can't remove the current index with the iterator, queue for later removal + indicesToRemove.add( current ); + } + } + else + { + positiveAxis = !positiveAxis; + } + + current = next; + } + + //Remove redundant extremal indices that couldn't be removed via the iterator + mExtremalIndices.removeAll( indicesToRemove ); + + //Truncate the list to one larger than needed by removing excess tailing indices + while( mExtremalIndices.size() > mSpecification.getExtremaCount() ) + { + mExtremalIndices.remove( mExtremalIndices.size() - 1 ); + } + + //If we have one too many indices, delete the smaller of the first or last + if( mExtremalIndices.size() > mSpecification.getExtremaCount() ) + { + int lastIndex = mExtremalIndices.size() - 1; + + if( Math.abs( mGridErrors[ mExtremalIndices.get( 0 )] ) > + Math.abs( mGridErrors[ mExtremalIndices.get( lastIndex )] ) ) + { + mExtremalIndices.remove( lastIndex ); + } + else + { + mExtremalIndices.remove( 0 ); + } + } + + //Detect if we have too few indices + if( mExtremalIndices.size() < mSpecification.getExtremaCount() ) + { + throw new FilterDesignException( "Couldn't find the minimum extremal frequencies in " + + "error set" ); + } + } + + /** + * Indicates if the absolute value of the argument is greater than or equal to the delta + * value with accuracy to 14 digits of precision. This avoids rounding errors at 15 digits or + * precision or greater. + * + * @param value to evaluate + * + * @return true if the absolute value is greater than or equal to the absolute delta value + */ + private boolean isGTEDelta( double value ) + { + return Math.abs( value ) - Math.abs( mDelta ) > -1.0e-5; + } + + /** + * Checks for convergence of the frequency response of the current set of extremal indices to + * the filter specification by comparing the maximum absolute error value against the delta + * value to determine if these two values are within the convergence threshold. + */ + public void checkConvergence() + { + double maximum = Math.abs( mGridErrors[ mExtremalIndices.get( 0 ) ] ); + + for( int i = 1; i < mExtremalIndices.size(); i++ ) + { + double current = Math.abs( mGridErrors[ mExtremalIndices.get( i ) ] ); + + if( current > maximum ) + { + maximum = current; + } + } + + double convergence = maximum - Math.abs( mDelta ); + + mConverged = convergence < CONVERGENCE_THRESHOLD; + } + + /** + * Coverts/casts the double array to a float array + */ + private static float[] convertToFloatArray( double[] samples ) + { + float[] converted = new float[ samples.length ]; + + for( int x = 0; x < samples.length; x++ ) + { + converted[ x ] = (float)samples[ x ]; + } + + return converted; + } + + /** + * Resamples the designed filter at half of the filter length at evenly spaced intervals of the + * frequency spectrum from 0 to PI radians. + * + * @return frequency response of the filter sampled the desired filter length and order + */ + private double[] resample() + { + int length = mSpecification.getFilterLength(); + + if( length % 2 == 0 ) + { + length--; + } + + double half = (double)length / 2.0; + + double[] resampled = new double[ (int)Math.ceil( half ) ] ; + + for( int x = 0; x < resampled.length; x++ ) + { + resampled[ x ] = getFrequencyResponse( Math.cos( Math.PI * (double)x / half ) ); + } + + return resampled; + } + + /** + * Applies Parks/McClelland filter type correction to the final frequency response array + * + * @param frequencyResponse of the resampled filter design from 0 <> ~ PI radians + * + * @return corrected frequency response + */ + private double[] correctFrequencyResponse( double[] frequencyResponse ) + { + double filterLength = mSpecification.getFilterLength(); + + switch( mSpecification.getFilterType() ) + { + case TYPE_1_ODD_LENGTH_EVEN_ORDER_SYMMETRICAL: + //No correction needed + break; + case TYPE_2_EVEN_LENGTH_ODD_ORDER_SYMMETRICAL: + //No correction needed + break; + case TYPE_3_ODD_LENGTH_EVEN_ORDER_ANTI_SYMMETRICAL: + for( int x = 0; x < frequencyResponse.length; x++ ) + { + double frequencyRadians = Math.PI * ( ( (double)x ) / filterLength ); + frequencyResponse[ x ] *= Math.sin( TWO_PI * frequencyRadians ); + } + break; + case TYPE_4_EVEN_LENGTH_ODD_ORDER_ANTI_SYMMETRICAL: + for( int x = 0; x < frequencyResponse.length; x++ ) + { + double frequencyRadians = Math.PI * ( ( (double)x ) / filterLength ); + frequencyResponse[ x ] *= Math.sin( Math.PI * frequencyRadians ); + } + break; + default: + break; + } + + return frequencyResponse; + } +} diff --git a/src/gui/control/CurveFittedAreaChart.java b/src/gui/control/CurveFittedAreaChart.java new file mode 100644 index 00000000..98ca9068 --- /dev/null +++ b/src/gui/control/CurveFittedAreaChart.java @@ -0,0 +1,207 @@ +package gui.control; + +import javafx.collections.ObservableList; +import javafx.geometry.Point2D; +import javafx.scene.Group; +import javafx.scene.chart.AreaChart; +import javafx.scene.chart.NumberAxis; +import javafx.scene.chart.XYChart; +import javafx.scene.shape.ClosePath; +import javafx.scene.shape.CubicCurveTo; +import javafx.scene.shape.LineTo; +import javafx.scene.shape.MoveTo; +import javafx.scene.shape.Path; +import javafx.scene.shape.PathElement; +import javafx.util.Pair; + +public class CurveFittedAreaChart extends AreaChart +{ + + public CurveFittedAreaChart( NumberAxis xAxis, NumberAxis yAxis ) + { + super( xAxis, yAxis ); + } + + @Override + protected void layoutPlotChildren() + { + super.layoutPlotChildren(); + for ( int seriesIndex = 0; seriesIndex < getDataSize(); seriesIndex++ ) + { + final XYChart.Series series = getData() + .get( seriesIndex ); + final Path seriesLine = (Path) ( (Group) series.getNode() ) + .getChildren().get( 1 ); + final Path fillPath = (Path) ( (Group) series.getNode() ) + .getChildren().get( 0 ); + smooth( seriesLine.getElements(), fillPath.getElements() ); + } + } + + private int getDataSize() + { + final ObservableList> data = getData(); + return ( data != null ) ? data.size() : 0; + } + + private static void smooth( ObservableList strokeElements, + ObservableList fillElements ) + { + // as we do not have direct access to the data, first recreate the list + // of all the data points we have + final Point2D[] dataPoints = new Point2D[ strokeElements.size() ]; + for ( int i = 0; i < strokeElements.size(); i++ ) + { + final PathElement element = strokeElements.get( i ); + if ( element instanceof MoveTo ) + { + final MoveTo move = (MoveTo) element; + dataPoints[ i ] = new Point2D( move.getX(), move.getY() ); + } else if ( element instanceof LineTo ) + { + final LineTo line = (LineTo) element; + final double x = line.getX(), y = line.getY(); + dataPoints[ i ] = new Point2D( x, y ); + } + } + // next we need to know the zero Y value + final double zeroY = ( (MoveTo) fillElements.get( 0 ) ).getY(); + // now clear and rebuild elements + strokeElements.clear(); + fillElements.clear(); + Pair result = calcCurveControlPoints( + dataPoints ); + Point2D[] firstControlPoints = result.getKey(); + Point2D[] secondControlPoints = result.getValue(); + // start both paths + strokeElements.add( + new MoveTo( dataPoints[ 0 ].getX(), dataPoints[ 0 ].getY() ) ); + fillElements.add( new MoveTo( dataPoints[ 0 ].getX(), zeroY ) ); + fillElements.add( + new LineTo( dataPoints[ 0 ].getX(), dataPoints[ 0 ].getY() ) ); + // add curves + for ( int i = 1; i < dataPoints.length; i++ ) + { + final int ci = i - 1; + strokeElements + .add( new CubicCurveTo( firstControlPoints[ ci ].getX(), + firstControlPoints[ ci ].getY(), + secondControlPoints[ ci ].getX(), + secondControlPoints[ ci ].getY(), + dataPoints[ i ].getX(), dataPoints[ i ].getY() ) ); + fillElements.add( new CubicCurveTo( firstControlPoints[ ci ].getX(), + firstControlPoints[ ci ].getY(), + secondControlPoints[ ci ].getX(), + secondControlPoints[ ci ].getY(), dataPoints[ i ].getX(), + dataPoints[ i ].getY() ) ); + } + // end the paths + fillElements.add( new LineTo( + dataPoints[ dataPoints.length - 1 ].getX(), zeroY ) ); + fillElements.add( new ClosePath() ); + } + + /** + * Calculate open-ended Bezier Spline Control Points. + */ + public static Pair calcCurveControlPoints( + Point2D[] dataPoints ) + { + Point2D[] firstControlPoints; + Point2D[] secondControlPoints; + int n = dataPoints.length - 1; + if ( n == 1 ) + { // Special case: Bezier curve should be a straight line. + firstControlPoints = new Point2D[ 1 ]; + // 3P1 = 2P0 + P3 + firstControlPoints[ 0 ] = new Point2D( + ( 2 * dataPoints[ 0 ].getX() + dataPoints[ 1 ].getX() ) / 3, + ( 2 * dataPoints[ 0 ].getY() + dataPoints[ 1 ].getY() ) + / 3 ); + + secondControlPoints = new Point2D[ 1 ]; + // P2 = 2P1 – P0 + secondControlPoints[ 0 ] = new Point2D( + 2 * firstControlPoints[ 0 ].getX() - dataPoints[ 0 ].getX(), + 2 * firstControlPoints[ 0 ].getY() + - dataPoints[ 0 ].getY() ); + return new Pair( firstControlPoints, + secondControlPoints ); + } + + // Calculate first Bezier control points + // Right hand side vector + double[] rhs = new double[ n ]; + + // Set right hand side X values + for ( int i = 1; i < n - 1; ++i ) + { + rhs[ i ] = 4 * dataPoints[ i ].getX() + + 2 * dataPoints[ i + 1 ].getX(); + } + rhs[ 0 ] = dataPoints[ 0 ].getX() + 2 * dataPoints[ 1 ].getX(); + rhs[ n - 1 ] = ( 8 * dataPoints[ n - 1 ].getX() + + dataPoints[ n ].getX() ) / 2.0; + // Get first control points X-values + double[] x = GetFirstControlPoints( rhs ); + + // Set right hand side Y values + for ( int i = 1; i < n - 1; ++i ) + { + rhs[ i ] = 4 * dataPoints[ i ].getY() + + 2 * dataPoints[ i + 1 ].getY(); + } + rhs[ 0 ] = dataPoints[ 0 ].getY() + 2 * dataPoints[ 1 ].getY(); + rhs[ n - 1 ] = ( 8 * dataPoints[ n - 1 ].getY() + + dataPoints[ n ].getY() ) / 2.0; + // Get first control points Y-values + double[] y = GetFirstControlPoints( rhs ); + + // Fill output arrays. + firstControlPoints = new Point2D[ n ]; + secondControlPoints = new Point2D[ n ]; + for ( int i = 0; i < n; ++i ) + { + // First control point + firstControlPoints[ i ] = new Point2D( x[ i ], y[ i ] ); + // Second control point + if ( i < n - 1 ) + { + secondControlPoints[ i ] = new Point2D( + 2 * dataPoints[ i + 1 ].getX() - x[ i + 1 ], + 2 * dataPoints[ i + 1 ].getY() - y[ i + 1 ] ); + } else + { + secondControlPoints[ i ] = new Point2D( + ( dataPoints[ n ].getX() + x[ n - 1 ] ) / 2, + ( dataPoints[ n ].getY() + y[ n - 1 ] ) / 2 ); + } + } + return new Pair( firstControlPoints, + secondControlPoints ); + } + + /** + * Solves a tridiagonal system for one of coordinates (x or y) of first + * Bezier control points. + */ + private static double[] GetFirstControlPoints( double[] rhs ) + { + int n = rhs.length; + double[] x = new double[ n ]; // Solution vector. + double[] tmp = new double[ n ]; // Temp workspace. + double b = 2.0; + x[ 0 ] = rhs[ 0 ] / b; + for ( int i = 1; i < n; i++ ) + {// Decomposition and forward substitution. + tmp[ i ] = 1 / b; + b = ( i < n - 1 ? 4.0 : 3.5 ) - tmp[ i ]; + x[ i ] = ( rhs[ i ] - x[ i - 1 ] ) / b; + } + for ( int i = 1; i < n; i++ ) + { + x[ n - i - 1 ] -= tmp[ n - i ] * x[ n - i ]; // Backsubstitution. + } + return x; + } +} \ No newline at end of file