roctave 0.0.1

data/lib/roctave/firpm.rb

@@ -0,0 +1,499 @@
+## Copyright (C) 2019 Théotime Bollengier <theotime.bollengier@gmail.com>
+##
+## This file is part of Roctave
+## 
+## Roctave is free software: you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+## 
+## Roctave is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+## GNU General Public License for more details.
+## 
+## You should have received a copy of the GNU General Public License
+## along with Roctave. If not, see <https://www.gnu.org/licenses/>.
+
+
+class Matrix
+	def puts
+		strings = Array.new(self.row_size){Array.new(self.column_size)}
+		(0 ... self.row_size).each do |i|
+			(0 ... self.column_size).each do |j|
+				strings[i][j] = self[i, j].round(4).to_s
+			end
+		end
+		maxstrlen = strings.flatten.collect{|s| s.length + 2}.max
+		Kernel.puts
+		(0 ... self.row_size).each do |i|
+			if i == 0 and i == self.row_size - 1 then
+				print ' ['
+			elsif i == 0 then
+				print ' ⎡'
+			elsif i == self.row_size - 1 then
+				print ' ⎣'
+			else
+				print ' ⎢'
+			end
+
+			(0 ... self.column_size).each do |j|
+				print strings[i][j].center(maxstrlen)
+			end
+
+			if i == 0 and i == self.row_size - 1 then
+				Kernel.puts ']'
+			elsif i == 0 then
+				Kernel.puts '⎤'
+			elsif i == self.row_size - 1 then
+				Kernel.puts '⎦'
+			else
+				Kernel.puts '⎥'
+			end
+		end
+		Kernel.puts
+	end
+end
+
+
+module Roctave
+	# @!group FIR filters
+
+	##
+	# Weighted least-squares method for FIR filter approximation by optimization.
+	# Minimizes the square of the energy of the error function.
+	# @param n [Integer] The filter order
+	# @param f [Array<Float, Array<Float>, Range>] Frequency band edges, onced flattened, must be of even length
+	# @param a [Array<Float, Array<Float>, Range>] Magnitude at frequency bands. Each element must be a scalar (for the whole band), or a range or a two element array (for the two band edges of the band). Length +a+ == length +f+ / 2
+	# @param w [Array<Float, Array<Float>, Range] Weight at frequency bands. Each element must be a scalar (for the whole band), or a range or a two element array (for the two band edges of the band). Length +w+ == length +f+ / 2
+	# @param type [:odd_symmetry, :even_symmetry] Specify the symmetry of the filter. nil and :even_symmetry are the same and default, for type 1 and 2 filters, :odd_symmetry is for type 3 and 4 filters.
+	# @param grid [Integer] The length of the grid over which the frequency response is evaluated. Defaults to 16, for an interpolation on 16*+n+.
+	# @return [Array<Float>] The filter coefficients B
+	def self.firpm (n, f, a, *args)
+		raise ArgumentError.new "Expecting no more than 6 arguments" if args.length > 3
+
+		n = [1, n.to_i].max
+
+		case f
+		when Array
+		when Range
+			f = [f]
+		else
+			raise ArgumentError.new "F must be an array or a range"
+		end
+		f.collect! do |e|
+			case e
+			when Range
+				[e.begin.to_f, e.end.to_f]
+			when Array
+				raise ArgumentError.new "Elements of F which are arrays must have exaclty two floats" unless (e.length == 2 and e.first.kind_of?(Float) and e.last.kind_of?(Float))
+				[e.first.to_f, e.last.to_f]
+			else
+				e.to_f
+			end
+		end
+		f = f.flatten
+		raise ArgumentError.new "At least one frequency band must be specified!" if f.empty?
+		raise ArgumentError.new "Once flattened, F must have an even number of frequency bands!" unless (f.length & 1) == 0
+		raise ArgumentError.new "F must have at least one band!" unless f.length >= 2
+		f.each do |e|
+			raise ArgumentError.new "Frequency band edges must be in the range [0, 1]" unless (e >= 0.0 and e <= 1.0)
+		end
+		(1...f.length).each do |i|
+			raise ArgumentError.new "Frequecy band edges must be strictly increasing" if f[i-1] >= f[i]
+		end
+
+		case a
+		when Array
+		when Range
+			[a]
+		when Numeric
+			[a]
+		else
+			raise ArgumentError.new "A must be an array or a range"
+		end
+		raise ArgumentError.new "Length of A must be half the length of F" unless a.length == f.length/2
+		a.collect! do |e|
+			case e
+			when Range
+				[e.begin.to_f.abs, e.end.to_f.abs]
+			when Array
+				raise ArgumentError.new "Elements of A which are arrays must have at least two floats" if e.length < 2
+				e.collect{|v| v.to_f}
+			else
+				[e.to_f.abs, e.to_f.abs]
+			end
+		end
+
+		w = nil
+		type = :even_symmetry
+		grid = 16
+
+		args.each.with_index do |arg, i|
+			case arg
+			when :even_symmetry
+				type = :even_symmetry
+			when :odd_symmetry
+				type = :odd_symmetry
+			when Numeric
+				grid = [1, arg.to_i].max
+				STDERR.puts "WARNING: recommended grid sampling factor are integers in the range [8, 16]." unless Range.new(8, 16).include?(grid)
+			when Array
+				raise ArgumentError.new "Length of W must be half the length of F" unless arg.length == f.length/2
+				w = arg.collect do |e|
+					case e
+					when Range
+						[e.begin.to_f.abs, e.end.to_f.abs]
+					when Array
+						raise ArgumentError.new "Elements of W which are arrays must have at least two floats" if e.length < 2
+						e.collect{|v| v.to_f}
+					else
+						[e.to_f.abs, e.to_f.abs]
+					end
+				end
+			else
+				raise ArgumentError.new "Argument #{i+4} must either be :odd_symmetry, :even_symmetry, an array of weight or the grid sampling (Integer)"
+			end
+		end
+
+		w = (f.length / 2).times.collect{[1.0, 1.0]} if w.nil?
+
+		bands = (0 ... a.length).collect do |i|
+			{
+				frequencies: Range.new(f[2*i], f[2*i+1]),
+				amplitudes:  a[i],
+				weights: w[i]
+			}
+		end
+		band_edges = bands.collect{|h| [h[:frequencies].begin, h[:frequencies].end]}.flatten.collect{|v| v*Math::PI}.uniq
+
+		## Create the sampling grid by interpolation ##
+		nb_sample_points = grid*n
+		constrained_frequency_amount = bands.inject(0.0){|m, h| m + h[:frequencies].end - h[:frequencies].begin}
+		# debug_amp_plot_args = []
+		# debug_weight_plot_args = []
+		bands.each do |band|
+			# debug_amp_plot_args << Roctave.linspace(band[:frequencies].begin, band[:frequencies].end, band[:amplitudes].length)
+			# debug_amp_plot_args << band[:amplitudes].collect{|e| e}
+			# debug_amp_plot_args << '-+b'
+			# debug_weight_plot_args << Roctave.linspace(band[:frequencies].begin, band[:frequencies].end, band[:weights].length)
+			# debug_weight_plot_args << band[:weights].collect{|e| e}
+			# debug_weight_plot_args << '-+b'
+
+			fi = Roctave.linspace(band[:frequencies].begin, band[:frequencies].end, [2, ((band[:frequencies].end - band[:frequencies].begin) * nb_sample_points / constrained_frequency_amount).ceil].max)
+			ai = Roctave.interp1(Roctave.linspace(band[:frequencies].begin, band[:frequencies].end, band[:amplitudes].length), band[:amplitudes], fi)
+			wi = Roctave.interp1(Roctave.linspace(band[:frequencies].begin, band[:frequencies].end, band[:weights].length), band[:weights], fi)
+
+			# debug_amp_plot_args << fi.collect{|e| e}
+			# debug_amp_plot_args << ai.collect{|e| e}
+			# debug_amp_plot_args << '-xr'
+			# debug_weight_plot_args << fi.collect{|e| e}
+			# debug_weight_plot_args << wi.collect{|e| e}
+			# debug_weight_plot_args << '-xr'
+
+			band[:frequencies] = fi.collect{|v| v*Math::PI}
+			band[:amplitudes]  = ai
+			band[:weights]     = wi
+		end
+		nb_sample_points = bands.inject(0.0){|m, h| m + h[:frequencies].length}
+
+
+		## Filter type ##
+		if type == :even_symmetry then
+			if (n & 1) == 0 then
+				filter_type = 1
+				beta = 0.0
+				q = -> w { 1.0 }
+				l = n / 2
+				coefs_from_p = -> p {
+					(-l .. l).collect{|m| m.abs}.collect do |m|
+						if m == 0 then
+							p[m, 0]
+						else
+							p[m, 0] / 2.0
+						end
+					end
+				}
+			else
+				filter_type = 2
+				beta = 0.0
+				q = -> w { Math.cos(w/2.0) }
+				l = (n - 1) / 2
+				coefs_from_p = -> p {
+					id = (1..l+1).to_a
+					(id.reverse + id).collect{ |m|
+						if m == 1 then
+							p[0, 0] + 0.5*p[1, 0]
+						elsif m == l+1 then
+							0.5*p[l, 0]
+						else
+							0.5*(p[m-1, 0] + p[m, 0])
+						end
+					}.collect{|v| v/2.0}
+				}
+			end
+		else
+			if (n & 1) == 0 then
+				filter_type = 3
+				beta = Math::PI/2
+				q = -> w { Math.sin(w) }
+				l = n / 2 - 1
+				coefs_from_p = -> p {
+					id = (1..l+1).to_a
+					(id.reverse + [0] + id).collect{ |m|
+						if m == 0 then
+							0.0
+						elsif m == 1 then
+							p[0, 0] - 0.5*p[2, 0]
+						elsif m >= l then
+							0.5*p[m-1, 0]
+						else
+							0.5*(p[m-1,0] - p[m+1, 0])
+						end
+					}.collect.with_index{ |v, i|
+						if i < l + 1 then
+							v/2.0
+						else
+							-v/2.0
+						end
+					}
+				}
+			else
+				filter_type = 4
+				beta = Math::PI/2
+				q = -> w { Math.sin(w/2.0) }
+				l = (n - 1) / 2
+				coefs_from_p = -> p {
+					id = (1..l+1).to_a
+					(id.reverse + id).collect{ |m|
+						if m == 1 then
+							p[0, 0] - 0.5*p[1, 0]
+						elsif m == l+1 then
+							0.5*p[l, 0]
+						else
+							0.5*(p[m-1, 0] - p[m, 0])
+						end
+					}.collect.with_index{ |v, i|
+						if i < l + 1 then
+							v/2.0
+						else
+							-v/2.0
+						end
+					}
+				}
+			end
+		end
+		# puts "Filter Type #{case filter_type; when 1 then 'I'; when 2 then 'II'; when 3 then 'III'; else 'IV' end}"
+
+
+		## Create the matrices ##
+		frequencies = bands.inject([]){|m, h| m + h[:frequencies]}
+		amplitudes  = bands.inject([]){|m, h| m + h[:amplitudes]}
+		weights     = bands.inject([]){|m, h| m + h[:weights]}
+
+		band_edges_faw = []
+		frequencies.each.with_index do |v, i|
+			if band_edges.include?(v) then
+				band_edges_faw << {f: v, a: amplitudes[i], w: weights[i]}
+			end
+		end
+
+		#		## (i) Initialize an estimate for the extreme frequencies w0, w1, ..., wL+1 by selecting (L+2) 
+		#		## equally spaced frequencies at the bands specified for the desired filter.
+		#
+		#		nb_test_points = l+2
+		#		extreme_frequencies = :wq
+		#		constrained_frequency_amount = bands.inject(0.0){|m, h| m + h[:frequencies].end - h[:frequencies].begin}
+		#		bands.each do |band|
+		#			fi = Roctave.linspace(band[:frequencies].begin, band[:frequencies].end, [2, ((band[:frequencies].end - band[:frequencies].begin) * nb_sample_points / constrained_frequency_amount).ceil].max)
+		#			ai = Roctave.interp1(Roctave.linspace(band[:frequencies].begin, band[:frequencies].end, band[:amplitudes].length), band[:amplitudes], fi)
+		#			wi = Roctave.interp1(Roctave.linspace(band[:frequencies].begin, band[:frequencies].end, band[:weights].length), band[:weights], fi)
+		#
+		#			band[:frequencies] = fi.collect{|v| v*Math::PI}
+		#			band[:amplitudes]  = ai
+		#			band[:weights]     = wi
+		#		end
+		#		nb_sample_points = bands.inject(0.0){|m, h| m + h[:frequencies].length}
+		#		####
+
+
+		mat_Wq2 = Matrix.build(nb_sample_points) do |i, j|
+			if i == j then
+				(weights[i] * q.call(frequencies[i]))**2
+			else
+				0.0
+			end
+		end
+
+		mat_dq = Matrix.build(nb_sample_points, 1) do |i|
+			qw = q.call(frequencies[i])
+			qw = 0.000000001 if qw == 0.0
+			amplitudes[i] / qw
+		end
+
+		mat_U = Matrix.build(nb_sample_points, l + 1) do |i, j|
+			Math.cos(j * frequencies[i])
+		end
+
+		mat_UT_times_mat_Wq2 = mat_U.transpose * mat_Wq2
+		p = (mat_UT_times_mat_Wq2 * mat_U).inverse * (mat_UT_times_mat_Wq2 * mat_dq)
+
+		p.puts
+
+		b = coefs_from_p.call(p)
+
+		###
+		# Matrix.column_vector(b).puts
+		#
+		# if filter_type == 1 or filter_type == 3 then
+		# 	Roctave.stem((-n/2..n/2).to_a, b, :filled, grid: true)
+		# else
+		# 	Roctave.stem((n+1).times.collect{|i| i - (n+1)/2.0 + 0.5}, b, :filled, grid: true)
+		# end
+		#
+		# r2 = Roctave.dft(b, 2048)
+		# h2 = r2[0...1024].abs
+		# p2 = r2[0...1024].arg
+		# p2 = Roctave.unwrap(p2)
+		# w2 = Roctave.linspace(0, 1, 1024)
+		# debug_amp_plot_args << w2
+		# debug_amp_plot_args << h2
+		# debug_amp_plot_args << '-g;Response;'
+		# 
+		# #Roctave.plot(*debug_weight_plot_args, title: 'Target weights', xlim: (0 .. 1), grid: true)
+		# Roctave.plot(*debug_amp_plot_args, title: 'Target filter magnitude response', xlim: (0 .. 1), grid: true)
+		# Roctave.plot(w2, h2.collect{|v| 10*Math.log10(v)}, xlabel: 'Normalized frequency', ylabel: 'Magnitude response (dB)', grid: true, title: 'Magnitude')
+		# Roctave.plot(w2, p2, grid: true, title: 'Phase')
+		###
+
+		# Evaluate the response
+		p_w = -> fre {
+			(0..l).inject(0.0) do |m, i|
+				m + p[i,0]*Math.cos(fre*i)
+			end
+		}
+		alpha = n / 2.0
+		# freq = Roctave.linspace(0, Math::PI, 1024)
+		# resp = freq.collect{|v| Complex.polar(1.0, -alpha*v - beta)*q.call(v)*p_w.call(v)}
+		# Roctave.plot(freq.collect{|v| v/Math::PI}, resp.abs, title: 'Evaluated response', grid: true)
+
+		# Evaluate the error
+		error = frequencies.length.times.collect do |i|
+			weights[i]*(amplitudes[i] - q.call(frequencies[i])*p_w.call(frequencies[i]))
+		end
+		#error.collect!{|v| v.abs}
+	
+		i = 0
+		loop do
+			maximum_freq = []
+			maximum_err  = []
+			maximum_wei  = []
+			maximum_amp  = []
+			(1...frequencies.length - 1).each do |i|
+				if (error[i-1] < error[i] and error[i+1] < error[i]) or (error[i-1] > error[i] and error[i+1] > error[i]) then
+					maximum_freq << frequencies[i]
+					maximum_err << error[i]
+					maximum_wei << weights[i]
+					maximum_amp << amplitudes[i]
+				end
+			end
+
+			#Roctave.plot(frequencies, error, ';Error;', maximum_freq, maximum_err, 'o;mamimums;', title: 'Evaluated error', grid: true, xlim: [0, Math::PI])
+
+			maximum_freq_and_error = (0...maximum_freq.length).collect{|i|
+				{f: maximum_freq[i], e: maximum_err[i], a: maximum_amp[i], w: maximum_wei[i]}
+			}.reject{|h| band_edges.include?(h[:f])}.sort_by{|h| h[:e]}.reverse
+			l_plus_two_extremal = (band_edges_faw + maximum_freq_and_error[(0...(l+2 - band_edges_faw.length))]).sort_by{|h| h[:f]}
+			raise "Couldn't get #{l+2} extremals!" if (l_plus_two_extremal.length != l+2 or l_plus_two_extremal.include?(nil))
+			l_plus_two_extremal_frequencies = l_plus_two_extremal.collect{|h| h[:f]}
+			l_plus_two_extremal_amplitudes  = l_plus_two_extremal.collect{|h| h[:a]}
+			l_plus_two_extremal_weights     = l_plus_two_extremal.collect{|h| h[:w]}
+
+			###
+
+			ak = (0 ... l+2).collect do |i|
+				(0 ... l+2).inject(1.0) do |m, j|
+					if j == i then
+						m
+					else
+						m / (Math.cos(l_plus_two_extremal_frequencies[i]) - Math.cos(l_plus_two_extremal_frequencies[j]))
+					end
+				end
+			end
+			deltatruc_numerator = (0 ... l+2).inject(0.0) do |m, i|
+				m + ak[i] * l_plus_two_extremal_amplitudes[i] / q.call(l_plus_two_extremal_frequencies[i])
+			end
+			deltatruc_denominator = (0 ... l+2).inject(0.0) do |m, i|
+				m + (-1)**i * ak[i] / l_plus_two_extremal_weights[i] * q.call(l_plus_two_extremal_frequencies[i])
+			end
+			deltatruc = deltatruc_numerator / deltatruc_denominator
+			puts "d_num = #{deltatruc_numerator}"
+			puts "d_den = #{deltatruc_denominator}"
+			puts "d = #{deltatruc}"
+
+
+
+			## Interpolate p(w) ##
+
+			resp_interpolate = frequencies.collect.with_index do |freq, i|
+				k = l_plus_two_extremal_frequencies.index(freq)
+				if k then
+					amplitudes[i] - (((-1)**k) * deltatruc / weights[i])
+				else
+					beta_k = ak.collect.with_index{|v, k| v*(Math.cos(l_plus_two_extremal_frequencies[k]) - Math.cos(l_plus_two_extremal_frequencies[l+1]))}
+					betai_sur_machin = (0..l).collect do |j|
+						beta_k[j] / (Math.cos(freq) - Math.cos(l_plus_two_extremal_frequencies[j]))
+					end
+					den = betai_sur_machin.inject(&:+)
+					num = (0..l).inject(0.0) do |m, j|
+						m + betai_sur_machin[j] * ((l_plus_two_extremal_amplitudes[j] / q.call(l_plus_two_extremal_frequencies[j])) - (((-1)**j) * deltatruc / (l_plus_two_extremal_weights[j] * q.call(l_plus_two_extremal_frequencies[j]))))
+					end
+					q.call(freq) * (num / den)
+				end
+			end
+			######################
+
+			i += 1
+
+			debug_plot_args = []
+			(0 ... band_edges_faw.length/2).each do |i|
+				debug_plot_args << [band_edges_faw[i*2][:f], band_edges_faw[i*2+1][:f]]
+				debug_plot_args << [band_edges_faw[i*2][:a], band_edges_faw[i*2+1][:a]]
+				debug_plot_args << "-xb#{(i == 0) ? ';Target;' : ''}"
+
+				debug_plot_args << [band_edges_faw[i*2][:f], band_edges_faw[i*2+1][:f]]
+				debug_plot_args << [band_edges_faw[i*2][:a] + deltatruc, band_edges_faw[i*2+1][:a] + deltatruc]
+				debug_plot_args << "-k#{(i == 0) ? ';Delta;' : ''}"
+				debug_plot_args << [band_edges_faw[i*2][:f], band_edges_faw[i*2+1][:f]]
+				debug_plot_args << [band_edges_faw[i*2][:a] - deltatruc, band_edges_faw[i*2+1][:a] - deltatruc]
+				debug_plot_args << '-k'
+			end
+			debug_plot_args << Roctave.linspace(0, Math::PI, 1024)
+			debug_plot_args << debug_plot_args[-1].collect{|v| q.call(v)*p_w.call(v)}
+			debug_plot_args << '-g;First response;'
+
+			debug_plot_args << frequencies
+			debug_plot_args << resp_interpolate
+			debug_plot_args << '-r;New response;'
+
+			debug_plot_args << l_plus_two_extremal_frequencies
+			debug_plot_args << Roctave.interp1(frequencies, resp_interpolate, l_plus_two_extremal_frequencies)
+			debug_plot_args << 'pr;Extremals;'
+			Roctave.plot(*debug_plot_args, grid: true, xlim: [0, Math::PI], title: "Iteration #{i}")
+
+			error = (0 ... frequencies.length).collect do |i|
+				weights[i]*(amplitudes[i] - resp_interpolate[i])
+			end
+
+			max_error = error.collect{|r| r.abs - deltatruc.abs}.max
+			puts "Maximum |error - delta| = #{max_error}"
+			break if max_error < 0.001 or i > 10
+		end
+
+		raise "Failed to converge" if i > 10
+		###
+
+		b
+	end
+end
+