1%% Blocking Matrix LMS Filter
  2%
  3% Apply an LMS filter to the blocking matrix of a GSC beamformer.  
  4% output is subtracted from the delayed original track, and the error
  5% signal is the rerouted output signal, causing a decrease in target
  6% signal power.  
  7%
  8% *Syntax*
  9% |[z, bmWall, mcAdapt, snrAll] = bmlms(x, b, mu, order, beta, phi, 
 10%      psi, bPrev, bmWinit, bmWForce, snrThresh, snrRate, snrInit)|
 11%
 12% * Inputs*  
 13% (Supply [] if not needed)
 14% *|bmWinit|-Matrix of tap weights for the first iteration. To
 15%     avoid forcing this initial value, supply []
 16% *|bmWForce|-Matrix of tap weights to force the filters into using,
 17%     rather than adapting. useful for recreating a previous
 18%     run with the same filter behavior. 
 19% *|snrThresh|-When not empty, computes an estimate of the SNR at each
 20%     iteration as the ratio of the FBF output power to the BM output
 21%     power on each track.  If this ratio is greater than |snrThresh|
 22%     the BM taps are updated; otherwise the MC taps are updated (see
 23%     outputs)
 24% *|snrRate| - Estimate SNR every snrRate samples
 25% *|snrInit| - Assume this vector of SNR values at the start
 26
 27% *Outputs*
 28% *|z|-Matrix of Blocking Matrix output, ready to the (MC)
 29%
 30% *|bmWall|-Matrix of all tap weights. These are useful for debugging and 
 31%   for recreating the same filtering effects with different inputs,
 32%   and the last set can be used as input to a subsequent period if time windowing.
 33%
 34% *|mcAdapt|-Binary matrix instructing the corresponding filters in
 35%     the MC to adapt.  One column is written each snrRate samples.
 36% *|snrAll| - Matrix of estimated SNR's in dB at every iteration
 37%
 38% *Todo*
 39% * Play with limiting (use .1 threshold on norm(bWin))
 40% * Plot the SNR to see what a good snrThresh would be
 41
 42
 43%% Function Declaration
 44function [z,bmw,mcAdapt, snrAll] = bmlms(x, b, mu, order, ...
 45    beta, phi, psi, bp, bw1, bmF, snrThresh, snrRate, snrInit)
 46
 47%% Argument Error Checking
 48narginchk(13, 13);
 49if ~isreal(x) || length(size(x)) ~= 2 || ~all(all(isfinite(x)))
 50    error('x must be a real matrix');
 51elseif ~isvector(b) || ~isreal(b) || ~all(isfinite(b))
 52    error('b must be a real vector');
 53elseif ~isvector(bp) || ~isreal(bp) || ...
 54    ~all(all(isfinite(bp)))
 55    error('bp must be a real vector');
 56elseif isempty(mu) || ~isscalar(mu) || ~isreal(mu) || ~isfinite(mu)
 57    error('mu must be a real scalar');
 58elseif isempty(order) || ~isscalar(order) || ~isreal(order) || ...
 59        ~isfinite(order) || order < 0 || ...
 60    abs(mod(order,floor(order))) > 0
 61    error('order must be a positive integer');
 62elseif isempty(beta) || ~isscalar(beta) || ~isreal(beta) || ...
 63        ~isfinite(beta) || beta < 0 || beta > 1
 64    error('beta must be a real scalar in [0,1]');
 65elseif ~isempty(bw1) && (length(size(bw1)) ~= 2 || ...
 66        ~isreal(bw1) || ~all(all(isfinite(bw1))))
 67    error('bw1 must be a real matrix');
 68elseif ~isempty(bmF) && ...
 69    (~isreal(bmF) || length(size(bmF)) ~= 3 || ...
 70     size(bmF,1) ~= order || size(bmF,2) ~= size(x,2) || ...
 71     ~all(all(all(isfinite(bmF)))))
 72    error('bmF must be a real cubic matrix');
 73elseif ~isempty(snrThresh) && (~isscalar(snrThresh) || ...
 74        ~isreal(snrThresh) || ~isfinite(snrThresh))
 75    error('snrThresh must be a real scalar')
 76elseif ~isempty(snrRate) && ...
 77    (~isscalar(snrRate) || ~isreal(snrRate) || ...
 78     ~isfinite(snrRate) || snrRate < 0 || ...
 79     abs(mod(snrRate,floor(snrRate))) > 0)
 80    error('snrRate must be a positive integer')
 81elseif xor(isempty(snrThresh), isempty(snrRate))
 82    error(['snrThresh and snrRate must both be specified or' ...
 83        ' both empty'])
 84elseif ~isempty(snrInit) && (~isvector(snrInit) || ~isreal(snrInit)) 
 85    error('snrInit must be a real vector');
 86elseif ~isempty(phi) && (length(size(phi))~=2 || ~isreal(phi) || ...
 87        ~all(all(isfinite(phi))))
 88    error('phi must be a real matrix')
 89elseif ~isempty(psi) && (length(size(psi))~=2 || ~isreal(psi) || ...
 90        ~all(all(isfinite(psi))))
 91    error('psi must be a real matrix')   
 92end
 93
 94%% Adaptive Filtering
 95% If phi and psi weren't specified, we get
 96% the normalized LMS adaptation over each column
 97%
 98%{w}i(n+1) = beta {w}_i(n)+mu*frac{z_i(n)}{{\bf b}^T(n){\bf b}(n)}{\bf b}(n),
 99% beta in [0, 1] , mu in [0, 2]
100% where the algorithm is "leaky" if beta < 1.
101%
102% If phi and psi are specified, we have a coefficient-constrainted
103% adaptive filter (CCAF), This method gives more
104% direct control over the power limit of the filter:
105%
106% w_k(n+1)=phi_k, w_k(n+1) > \phi_k
107%
108% w_k(n+1)=psi_k, w_k(n+1) < \psi_k
109%
110% using matrices for better performance. See the m-file ccafbounds.m 
111%for a way to calculate the bounds phi and psi.
112
113% Each column of w holds the taps for an adaptive filter on a column of 
114% the input microphone tracks x. bmWall saves a 3-D matrix of all values of w.
115% Use bmWinit for initial value of w if supplied; zeros otherwise.
116
117%% inital values of BMLMS
118[N, nmic] = size(x);  % length of tracks, number of mics
119if nargin == 3,  bp = zeros(order, 1);  end 
120bmw = zeros(order, nmic, length(b));
121% initial targbm
122if ~isempty(bw1)
123   w=bw1; else w= zeros(order, nmic);
124end
125z=zeros(length(b), nmic);
126% initial AMC
127snrAll = zeros(N, nmic);
128mcAdapt = zeros(length(b),nmic);
129snr=zeros(1, nmic);
130% amcInds=gt(snr,snrThresh);
131if ~isempty(snrThresh)
132    % Use snrInit if supplied; otherwise set to infinity so that we
133    % always adapt over the first power window.
134    if isempty(snrInit)
135        snr = Inf*ones(1, size(z,2)); 
136    else
137        snr = snrInit; 
138    end
139    % Matrix of columns to adapt in MC
140    mcAdapt = zeros(size(z)); 
141    % Initial value of amcInds
142    amcInds=gt(snr,snrThresh);
143end
144
145% initial optwbm
146
147
148% ord--one for each track of input x
149for n = 1:length(b)     
150% calculate targbm output and ready to opt
151[bWin,z,w1]=targbm(bp,b,x,n,order,w,z);
152
153%check bmWF
154if ~isempty(bmF), w= bmF(:,:,n); continue, end
155
156% If enough samples have passed and we're doing AMC
157% thresholding, find the SNR for each track.
158[snr,amcInds,mcAdapt]=AMC(b,z,n,snrThresh,snrRate,snr,amcInds,mcAdapt);
159% save SNR values for debug output
160snrAll(n,:) = snr;    
161%Perform LMS tap update under threshold
162%figure out if bmWForce supplied
163[w]=optwbm(bWin,n,w1,z,beta,mu,snrThresh,amcInds);
164% CCAF (Coefficient Constrained) Tap Constraints
165[w]=CCAF(phi,psi,order,w,snrThresh,amcInds);
166% Save the taps used on this iteration
167bmw(:,:,n)=w;
168
169end  % for n = 1:length(b)
170% bmw=w;
171end  % function bmlms