% ******************************************************* % delay-matching between two signals (complex/real-valued) % Markus Nentwig, 2005-2012 % % signal and ref are two sampled signals, same number of points. % both are treated as cyclic (use zero-padding, if needed) % % * output: % => coeff: % complex scaling factor that scales 'ref' into 'signal' % => delay 'deltaN' samples (with subsample resolution) % applying deltaN delay to 'ref' turns it into 'signal' % => 'shiftedRef': 'ref' with 'coeff' and 'deltaN' applied % For example, norm(signal - shiftedRef) should be at a minimum. % % Properly used, the timing can be resolved with an accuracy of a small % fraction of the sample duration, the "PART I" algorithm may reach % nano-sample accuracy for noise-free signals. % % The most recent version can be found here: % http://www.dsprelated.com/showcode/207.php % % description of the least-squares based algorithm ('PART I'): % http://www.dsprelated.com/showarticle/26.php % % description of the iterative algorithm ('PART II'): % http://www.dsprelated.com/showcode/288.php % % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. % ******************************************************* % ######### "PART 1" ######## % The algorithm as advertised above. % Call the function from outside this script. function [coeff, shiftedRef, deltaN] = fitSignal_120825(signal, ref, opts) if nargin < 2 error('need two arguments'); elseif nargin == 2 opts = struct(); end def = struct(); % force use of an alternative iterative algorithm % less accurate at high signal-to-noise ratio, slower, but more robust % to noisy signals and heavy group delay variations def.forceIterativeAlgorithm = false; % standard ('PART I') algorithm % high accuracy % fast % fails for too noisy signals % fails for heavy group delay distortion def.enableCoarseCorr = true; % phase unwrapping % can be set (together with enableCoarseCorr = false) to % effectively use a conventional phase-unwrapping algorithm. % fails if the phase "tracking" is lost across regions of % low signal-to-noise ratio, for example % - when a DC term is removed from a measurement % - in a multi-channel signal % - under narrow-band interference % - due to frequency-selective fading in a radio channel def.enableUnwrap = false; % generate some plots (others remain disabled, edit the code) def.doPlot = false; % checks, whether the crosscorrelation peak with the % shifted and scaled reference signal is at zero, as it % should be. def.enableSanityCheck = false; % ******************************************************* % merge default options % ******************************************************* tmp = fieldnames(def); for ix = 1 : numel(tmp) if ~isfield(opts, tmp{ix}) opts.(tmp{ix}) = def.(tmp{ix}); end end % ******************************************************* % use PART II algorithm, if requested. % otherwise, it is a fallback solution only. % ******************************************************* if opts.forceIterativeAlgorithm [coeff, shiftedRef, deltaN] = mn_fitSignal_corrSearch(signal, ref); return; end signal = signal(:) .'; % force row vector ref = ref(:) .'; n = numel(signal); assert(n == numel(ref), 'length mismatch'); % ******************************************************* % Calculate the normalized frequency for each FFT bin % [-0.5..0.5[ % ******************************************************* binFreq=(mod(((0:n-1)+floor(n/2)), n)-floor(n/2))/n; % ******************************************************* % Convert signals to frequency domain % ******************************************************* sig_FD = fft(signal); ref_FD = fft(ref); % rotate each bin backwards with the phase of the reference u = sig_FD .* conj(ref_FD); negateCoeff = false; if opts.enableCoarseCorr % ******************************************************* % Coarse correction (+/- 1 sample) using crosscorrelation between % signal and reference... % ******************************************************* NyqBin = []; if mod(n, 2) == 0 % for an even-sized FFT, it is a matter of interpretation, whether the center % bin should be treated as a positive or a negative frequency. % Effectively, it describes a series such as [-1, 1, -1, 1, -1, ...] in the time % domain. % Interpolation between samples is not possible, as the direction of the rotating % phasor is not known. % In a real-valued signal, it is effectively "half positive, half negative frequency". % This definition allows ifft(fft([-1, 1, -1, 1, ...]) to return the original signal. % still, it cannot be evaluated between samples. % % Making a long story short: % Disregard it. u(n/2 + 1) = 0; end Xcor = ifft(u); % ******************************************************* % Each bin in Xcor corresponds to a delay in samples. % The bin with the highest absolute value corresponds to % the delay where maximum correlation occurs. % ******************************************************* ix = find(abs(Xcor) == max(abs(Xcor))); ix = ix(1); % in case there are several bitwise-identical peaks % ******************************************************* % A negative peak implies a sign change: signal matches -ref. % ******************************************************* if real(Xcor(ix)) > 0 % positive correlation peak else % negative correlation peak % This would result in a 180 degree baseline phase in the % LS solver, which creates ambiguity problems % Solution: Negate the signal (and all dependent expressions), % and undo by negating coeff at the end. signal = -signal; sig_FD = -sig_FD; u = -u; Xcor = -Xcor; negateCoeff = true; end % the location of the peak denotes the delay, with a resolution of % one sample. Delay 0 appears in bin 1 etc. integerDelay = ix - 1; if opts.doPlot figure(); hold on; v = abs(Xcor); plot(v, 'b'); h = plot(integerDelay+1, v(integerDelay+1), 'r+'); set(h, 'lineWidth', 3); legend('crosscorrelation', 'coarse (integer) delay estimate'); title('coarse delay estimation via crosscorrelation'); end else integerDelay = 0; end % ******************************************************* % un-delay the signal by the estimated coarse delay. % causality is not an issue, as the signal is cyclic (it % repeats continuously over time = -inf..inf) % ******************************************************* % exp(-2i pi f) is the Fourier transform of a unity delay % exp(2i pi f) is a negative delay. rotN = exp(2i*pi*integerDelay .* binFreq); if opts.doPlot tref = ifft(fft(ref) .* conj(rotN)); % conj => sign change of delay figure(); hold on; plot(real(ref), 'b'); plot(real(signal), 'k'); plot(real(tref), 'r'); legend('original reference', 'signal', 'reference, coarse aligned'); end if false && opts.doPlot figure(); hold on; plot(fftshift(binFreq), fftshift(20*log10(abs(fft(ref)) + eps)), 'k'); plot(fftshift(binFreq), fftshift(20*log10(abs(fft(signal)) + eps)), 'b'); legend('reference', 'signal'); xlabel('normalized frequency'); ylabel('dB'); title('power spectrum'); end % ******************************************************* % Find the exact delay using linear least mean squares fit % on the phase. % % u is the phase difference on any frequency bin. % Delay appears as linear increase in phase, but unwrapping % the phase (2 pi ambiguity) across gaps in the power spectrum % is not robust enough. % Therefore, use the coarse delay (with +/- 1/2 sample accuracy) to % rotate back the phase. This removes most of the 2 pi steps. % Note this is method is not completely "foolproof" either but % usually quite robust. % In case of failure, try oversampling (fft;zero pad; ifft) on % both signal and reference. % ******************************************************* angCorr = angle(u .* rotN); ang = angCorr; if opts.enableUnwrap ang = fftPhaseUnwrap(ang); end if false && opts.doPlot angRaw = angle(u); figure(); grid on; hold on; h = plot(angRaw, 'k'); set(h, 'lineWidth', 2); h = plot(angCorr, 'b'); set(h, 'lineWidth', 2); h = plot(ang, 'g'); set(h, 'lineWidth', 1); xlabel('FFT bin index'); ylabel('phase / radians'); title('phase'); legend('raw phase', 'phase corrected with coarse estimate', 'corrected and unwrapped'); end % ******************************************************* % For the least-squares fit, the phase is weighted according to % the product of amplitudes from signal and reference. % As intuitive explanation: any bin is disregarded, if either % reference signal or received signal contain too little energy, % as the phase would be meaningless and vary wildly as result of % added noise etc. % ******************************************************* weight = abs(u); % normalize for plotting (doesn't affect the result) weight = weight / max(weight); % Phase shift of a complex-valued scaling factor % It rotates all frequencies by the same amount. % Apply per-frequency weighting. constRotPhase = 1 .* weight; % Phase shift of a unit delay % Apply per-frequency weighting uDelayPhase = -2*pi*binFreq .* weight; % The observed phase difference between signal and reference % (corrected by the coarse estimate), and weighted. ang = ang .* weight; if false % discard low-value bins (optimization for long, highly oversampled signals) chkThr = abs(ang); thr = max(chkThr) * 1e-6; ixPow = find(chkThr > thr); constRotPhase = constRotPhase(ixPow); uDelayPhase = uDelayPhase(ixPow); ang = ang(ixPow); end % Least-squares equation system that attempts to "explain" the observed % phase in terms of % - a constant phase shift => r(1) % - a unity delay => r(2) % base .' * r = ang .' % least-squares solve for r: base = [constRotPhase; uDelayPhase]; r = base .' \ ang.'; %linear mean square solution if opts.doPlot figure(); hold on; h = plot(ang, 'k'); set(h, 'lineWidth', 2); plot(constRotPhase, 'g'); plot(uDelayPhase, 'b'); plot(base.' * r, 'r'); title('phase vectors in least-squares solver'); legend('signal, weighted', 'constant, weighted', 'unit delay, weighted', 'least-squares fit'); xlabel('FFT bin index'); ylabel('weighted phase'); end % The constant phase rotation r(1) is not used % It will be obtained later as the phase of 'coeff' % least-squares optimal number of unit delays fractionalDelay = r(2); if opts.enableCoarseCorr % assert(abs(fractionalDelay) <= 0.5*1.2345, 'mismatch between coarse- and fine estimation'); if abs(fractionalDelay) > 0.5*1.2345 disp('*** fitSignal: reverting to iterative algorithm ***'); [coeff, shiftedRef, deltaN] = mn_fitSignal_corrSearch(signal, ref); return; end end % ******************************************************* % Final delay estimate: Restore the coarse estimate that % had been removed earlier % (returned result) % ******************************************************* deltaN = integerDelay + fractionalDelay; % ******************************************************* % Convert numbers beyond the midpoint into a negative % number (optional) % ******************************************************* deltaN = mod(deltaN-n/2, n) + n/2; % ******************************************************* % Delay ref with the final delay estimate % ******************************************************* ref_FD = ref_FD .* exp(-2i*pi*deltaN .* binFreq); shiftedRef = ifft(ref_FD); % ******************************************************* % Crosscorrelation with the now time-aligned signal % The resulting coeff minimizes norm(signal - coeff * shiftedRef) % (returned result) % ******************************************************* coeff = sum(signal .* conj(shiftedRef)) / sum(shiftedRef .* conj(shiftedRef)); if negateCoeff % sign was changed earlier. Change it back. coeff = -coeff; end % ******************************************************* % apply the coefficient to the delayed reference signal. % (returned result) % ******************************************************* shiftedRef = shiftedRef * coeff; % ******************************************************* % Sanity check: % calculate the crosscorrelation for all delays. % Assuming we have done a proper job, the peak must be % at zero delay. Otherwise, it would mean that there is a % "better" delay on the tested one-sample grid. % ******************************************************* if opts.enableSanityCheck u = sig_FD .* conj(ref_FD); Xcor=abs(ifft(u)); if Xcor(1) ~= max(Xcor) figure(); plot(abs(Xcor)); title('sanity check failed'); error ('not locked to the correct bin'); end assert(real(Xcor(1)) >= 0, 'sign is wrong?!'); end % ******************************************************* % Report a delay beyond the midpoint as negative % ******************************************************* halfLen = floor(n / 2); deltaN = mod(deltaN + halfLen, n) - halfLen; end % p is a phase of a complex-valued FFT, with the "0 Hz bin" at index 1 % removes phase jumps in excess of pi in positive and negative direction function p = fftPhaseUnwrap(p) n = numel(p); lastPosIx = ceil(n/2); % last positive frequency % positive frequencies - search from zero in positive direction ixVec = 2:lastPosIx; % ixVecAdj points towards the adjacent bin, in direction % towards the origin (minus one bin for pos. frequencies) ixVecAdj = ixVec - 1; delta = p(ixVec) - p(ixVecAdj); delta = round(delta / 2 / pi) * 2 * pi; delta = cumsum(delta); p(ixVec) = p(ixVec) - delta; % negative frequencies % search from zero in negative direction % flip by reverse indexing, then use fast cumsum ixVec = n:-1:lastPosIx+1; % ixVecAdj points towards the adjacent bin, in direction % towards the origin (plus one bin for neg. frequencies) ixVecAdj = ixVec + 1; ixVecAdj(1) = 1; % n+1 wraps around to 1 delta = p(ixVec) - p(ixVecAdj); delta = round(delta / 2 / pi) * 2 * pi; delta = cumsum(delta); p(ixVec) = p(ixVec) - delta; end % **************************************************************** % ######### "PART 2" ######## % % The functions below implement a fallback solution, if the above % algorithm fails. This variant performs better at low signal-to- % noise ration. % The algorithm iteratively determines the crosscorrelation, % predicts the peak location between bins, time-shifts and repeats. % Documentation: % http://www.dsprelated.com/showcode/288.php % It is not as accurate and slower as the first algorithm, but should % always converge, even if "the" delay between two signals is not well- % defined because of group delay variations, multiple periods etc. % **************************************************************** % wrapper function to provide the same interface as the original % fitSignal function function [coeff, shiftedRef, deltaN] = mn_fitSignal_corrSearch(signal, ref) % **************************************************************** % estimate delay % **************************************************************** [deltaN, coeff] = iterDelayEst(signal, ref); % this subroutine uses a different definition coeff = 1 / coeff; deltaN = -deltaN; % **************************************************************** % correct it % **************************************************************** shiftedRef = cs_delay(ref, 1, deltaN); shiftedRef = shiftedRef * coeff; end % **************************************************************** % estimates delay and scaling factor % **************************************************************** function [delay_samples, coeff] = iterDelayEst(s1, s2) s1 = s1(:) .'; % force row vectors s2 = s2(:) .'; rflag = isreal(s1) && isreal(s2); n = numel(s1); halfN = floor(n/2); assert(numel(s2) == n, 'signals must have same length'); % **************************************************************** % constants % **************************************************************** % exit if uncertainty below threshold thr_samples = 1e-6; % exit after fixed number of iterations nIter = 30; % frequency domain representation of signals fd1 = fft(s1); fd2 = fft(s2); % first round: No delay was applied tau = []; fd2Tau = fd2; % delayed s2 in freq. domain % frequency corresponding to each FFT bin -0.5..0.5 f = (mod(((0:n-1)+floor(n/2)), n)-floor(n/2))/n; % uncertainty plot data e = []; % normalization factor nf = sqrt((fd1 * fd1') * (fd2 * fd2')) / n; % normalizes to 1 % search window: % known maximum and two surrounding points x1 = -1; x2 = -1; x3 = -1; y1 = -1; y2 = -1; y3 = -1; % **************************************************************** % iteration loop % **************************************************************** for count = 1:nIter % **************************************************************** % crosscorrelation with time-shifted signal % **************************************************************** xcorr = abs(ifft(fd2Tau .* conj(fd1)))/ nf; % **************************************************************** % detect peak % **************************************************************** if isempty(tau) % **************************************************************** % startup % initialize with three adjacent bins around peak % **************************************************************** ix = find(xcorr == max(xcorr)); ix = ix(1); % use any, if multiple bitwise identical peaks % indices of three bins around peak ixLow = mod(ix-1-1, n) + 1; % one below ixMid = ix; ixHigh = mod(ix-1+1, n) + 1; % one above % delay corresponding to the three bins tauLow = mod(ixLow -1 + halfN, n) - halfN; tauMid = mod(ixMid -1 + halfN, n) - halfN; tauHigh = mod(ixHigh -1 + halfN, n) - halfN; % crosscorrelation value for the three bins xcLow = xcorr(ixLow); xcMid = xcorr(ixMid); xcHigh = xcorr(ixHigh); x1 = tauLow; x2 = tauMid; x3 = tauHigh; y1 = xcLow; y2 = xcMid; y3 = xcHigh; else % **************************************************************** % only main peak at first bin is of interest % **************************************************************** tauMid = tau; xcMid = xcorr(1); if xcMid > y2 % **************************************************************** % improve midpoint % **************************************************************** if tauMid > x2 % midpoint becomes lower point x1 = x2; y1 = y2; else % midpoint becomes upper point x3 = x2; y3 = y2; end x2 = tauMid; y2 = xcMid; elseif tauMid < x2 % **************************************************************** % improve low point % **************************************************************** assert(tauMid >= x1); % bitwise identical is OK assert(tauMid > x1 || xcMid > y1); % expect improvement x1 = tauMid; y1 = xcMid; elseif tauMid > x2 % **************************************************************** % improve high point % **************************************************************** assert(tauMid <= x3); % bitwise identical is OK assert((tauMid < x3) || (xcMid > y3)); % expect improvement x3 = tauMid; y3 = xcMid; else assert(false, '?? evaluated for existing tau ??'); end end % **************************************************************** % calculate uncertainty (window width) % **************************************************************** eIter = abs(x3 - x1); e = [e, eIter]; if eIter < thr_samples % disp('threshold reached, exiting'); break; end if y1 == y2 || y2 == y3 % reached limit of numerical accuracy on one side usePoly = 0; else % **************************************************************** % fit 2nd order polynomial and find maximum % **************************************************************** num = (x2^2-x1^2)*y3+(x1^2-x3^2)*y2+(x3^2-x2^2)*y1; denom = (2*x2-2*x1)*y3+(2*x1-2*x3)*y2+(2*x3-2*x2)*y1; if denom ~= 0 tau = num / denom; % is the point within [x1, x3]? usePoly = ((tau > x1) && (tau < x3)); else usePoly = 0; end end if ~usePoly % revert to linear interpolation on the side with the % less-accurate outer sample % Note: There is no guarantee that the side with the more accurate % outer sample is the right one, as the samples aren't % placed on a regular grid! % Therefore, iterate to improve the "worse" side, which will % eventually become the "better side", and iteration converges. tauLow = (x1 + x2) / 2; tauHigh = (x2 + x3) / 2; if y1 < y3 o = [tauLow, tauHigh]; else o = [tauHigh, tauLow]; end % don't choose point that is identical to one that is already known tau = o(1); if tau == x1 || tau == x2 || tau == x3 tau = o(2); if tau == x1 || tau == x2 || tau == x3 break; end end end % **************************************************************** % advance 2nd signal according to location of maximum % phase shift in frequency domain - delay in time domain % **************************************************************** fd2Tau = fd2 .* exp(2i * pi * f * tau); end % for % **************************************************************** % plot the uncertainty (window size) over the number of iterations % **************************************************************** if false figure(); semilogy(e, '+-'); grid on; xlabel('iteration'); title('uncertainty in delay'); end % **************************************************************** % the delay estimate is the final location of the delay that % maximized crosscorrelation (center of window). % **************************************************************** delay_samples = x2; % **************************************************************** % Coefficient: Turn signal 1 into signal 2 % **************************************************************** coeff = fd2Tau * fd1' ./ (fd1 * fd1'); % **************************************************************** % chop roundoff error, if input signals are known to be % real-valued. % **************************************************************** if rflag coeff = real(coeff); end end % **************************************************************** % delay cyclic signal by phase shift % **************************************************************** function waveform = cs_delay(waveform, rate_Hz, delay_s) rflag = isreal(waveform); n = numel(waveform); cycLen_s = n / rate_Hz; nCyc = delay_s / cycLen_s(); f = 0:(n - 1); f = f + floor(n / 2); f = mod(f, n); f = f - floor(n / 2); phase = -2 * pi * f * nCyc; rot = exp(1i*phase); waveform = ifft(fft(waveform) .* rot); if rflag waveform = real(waveform); end end