From 39a84268220ea2b7904937ceadfd0944a8ff9fc7 Mon Sep 17 00:00:00 2001 From: cmayer Date: Tue, 6 Nov 2018 17:34:48 +0100 Subject: [PATCH] use more of numpy; improved doppler estimation --- examples/test_188-110A.grc | 2 +- examples/test_188-110C.grc | 126 ++++++++++++++++++++-- lib/adaptive_dfe_impl.cc | 54 +++++----- lib/adaptive_dfe_impl.h | 2 + python/physical_layer/CMakeLists.txt | 1 + python/physical_layer/MIL_STD_188_110A.py | 110 +++++++++---------- python/physical_layer/MIL_STD_188_110C.py | 117 ++++++++++---------- python/physical_layer/STANAG_4285.py | 38 ++----- python/physical_layer/common.py | 29 +++++ 9 files changed, 302 insertions(+), 177 deletions(-) create mode 100644 python/physical_layer/common.py diff --git a/examples/test_188-110A.grc b/examples/test_188-110A.grc index 876d805..be4d45b 100644 --- a/examples/test_188-110A.grc +++ b/examples/test_188-110A.grc @@ -191,7 +191,7 @@ value - 0.001 + 0.005 _enabled diff --git a/examples/test_188-110C.grc b/examples/test_188-110C.grc index e6ac025..4bf8892 100644 --- a/examples/test_188-110C.grc +++ b/examples/test_188-110C.grc @@ -467,6 +467,57 @@ 1 + + blocks_file_sink + + append + False + + + alias + + + + comment + + + + affinity + + + + _enabled + True + + + file + /Users/chm/Software/gr-digitalhf/examples/soft_dec.bin + + + _coordinate + (693, 677) + + + _rotation + 0 + + + id + blocks_file_sink_0 + + + type + float + + + unbuffered + True + + + vlen + 1 + + blocks_float_to_complex @@ -561,6 +612,53 @@ 1 + + blocks_pdu_to_tagged_stream + + alias + + + + comment + + + + affinity + + + + _enabled + True + + + _coordinate + (458, 581) + + + _rotation + 0 + + + id + blocks_pdu_to_tagged_stream_0 + + + type + float + + + tag + packet_len + + + maxoutbuf + 0 + + + minoutbuf + 0 + + blocks_tag_debug @@ -766,7 +864,7 @@ alpha - 0.0005 + 0.0001 mode @@ -2022,11 +2120,11 @@ size - 1024 + 768 srate - samp_rate/sps/2 + samp_rate/sps/3 stemplot @@ -2046,7 +2144,7 @@ tr_mode - qtgui.TRIG_MODE_AUTO + qtgui.TRIG_MODE_TAG tr_slope @@ -2054,11 +2152,11 @@ tr_tag - "soft_dec" + "packet_len" type - msg_float + float update_time @@ -2322,6 +2420,18 @@ 0 0 + + blocks_pdu_to_tagged_stream_0 + blocks_file_sink_0 + 0 + 0 + + + blocks_pdu_to_tagged_stream_0 + qtgui_time_sink_x_1 + 0 + 0 + blocks_throttle_0 fir_filter_xxx_0 @@ -2348,9 +2458,9 @@ digitalhf_adaptive_dfe_0 - qtgui_time_sink_x_1 + blocks_pdu_to_tagged_stream_0 soft_dec - in + pdus fir_filter_xxx_0 diff --git a/lib/adaptive_dfe_impl.cc b/lib/adaptive_dfe_impl.cc index 4673800..a1f761d 100644 --- a/lib/adaptive_dfe_impl.cc +++ b/lib/adaptive_dfe_impl.cc @@ -93,6 +93,7 @@ adaptive_dfe_impl::adaptive_dfe_impl(int sps, // samples per symbol , _nW(nW) , _mu(mu) , _alpha(alpha) + , _use_symbol_taps(true) , _py_module_name(python_module_name) , _physicalLayer() , _taps_samples(nullptr) @@ -183,6 +184,7 @@ adaptive_dfe_impl::general_work(int noutput_items, std::fill_n(_hist_symbols, 2*_nW, gr_complex(0)); std::fill_n(_taps_samples, _nB+_nF+1, gr_complex(0)); std::fill_n(_taps_symbols, _nW, gr_complex(0)); + _use_symbol_taps = true; _samples.clear(); _phase = -phase_est; _taps_samples[_nB+1] = 0.01; @@ -279,8 +281,10 @@ adaptive_dfe_impl::general_work(int noutput_items, } // publish soft decisions if (!_vec_soft_decisions.empty()) { + std::cout << "soft_dec " << _vec_soft_decisions.size() << "\n"; unsigned int const bits_per_symbol = _constellations[_constellation_index]->bits_per_symbol(); _msg_metadata = pmt::dict_add(_msg_metadata, pmt::mp("bits_per_symbol"), pmt::from_long(bits_per_symbol)); + _msg_metadata = pmt::dict_add(_msg_metadata, pmt::mp("packet_len"), pmt::mp(_vec_soft_decisions.size())); message_port_pub(_msg_port_name, pmt::cons(_msg_metadata, pmt::init_f32vector(_vec_soft_decisions.size(), _vec_soft_decisions))); @@ -382,16 +386,21 @@ gr_complex adaptive_dfe_impl::filter() { _taps_samples, _nB+_nF+1); gr_complex dot_symbols=0; - for (int l=0; l<_nW; ++l) { - assert(_hist_symbol_index+l < 2*_nW); - dot_symbols += _hist_symbols[_hist_symbol_index+l]*_taps_symbols[l]; + gr::digital::constellation_sptr constell = _constellations[_constellation_index]; + bool const update_taps = constell->bits_per_symbol() <= 3; + if (constell->bits_per_symbol() > 3) + _use_symbol_taps = false; + if (_use_symbol_taps) { + for (int l=0; l<_nW; ++l) { + assert(_hist_symbol_index+l < 2*_nW); + dot_symbols += _hist_symbols[_hist_symbol_index+l]*_taps_symbols[l]; + } + filter_output += dot_symbols; } - filter_output += dot_symbols; gr_complex known_symbol = _symbols[_symbol_counter]; bool const is_known = std::abs(known_symbol) > 1e-5; if (not is_known) { // not known gr_complex const descrambled_filter_output = std::conj(_scramble[_symbol_counter]) * filter_output; - gr::digital::constellation_sptr constell = _constellations[_constellation_index]; unsigned int jc = constell->decision_maker(&descrambled_filter_output); gr_complex descrambled_symbol = 0; constell->map_to_points(jc, &descrambled_symbol); @@ -403,29 +412,24 @@ gr_complex adaptive_dfe_impl::filter() { _npwr[_constellation_index] = (1-alpha)*_npwr[_constellation_index] + alpha*err; std::vector const soft_dec = constell->calc_soft_dec(descrambled_filter_output, _npwr[_constellation_index]); std::copy(soft_dec.begin(), soft_dec.end(), std::back_inserter >(_vec_soft_decisions)); - // std::cout << "soft_dec " << _npwr[_constellation_index] << " : "; - // for (int k=0; k= self._num_frames_per_block-2: idx_d1d2 = self._frame_counter - self._num_frames_per_block + 2; a['symb'][n_unknown :n_unknown+ 8] *= n_psk(2, WALSH[self._d1d2[idx_d1d2]][:]) a['symb'][n_unknown+8:n_unknown+16] *= n_psk(2, WALSH[self._d1d2[idx_d1d2]][:]) - a['scramble'] = n_psk(8, np.array([self._scr_data.next() for _ in range(self._frame_len)])) - a['symb'] *= a['scramble'] self._frame_counter += 1 return [a, self._mode['ci'],False,True] @@ -154,8 +152,7 @@ class PhysicalLayer(object): [1] ... doppler estimate (rad/symbol) if available""" print('-------------------- get_doppler --------------------', self._frame_counter,len(symbols),len(iq_samples)) - success = False - doppler = 0 + success,doppler = False,0 if self._frame_counter == -1: ## -- preamble ---- success,doppler = self.get_doppler_from_preamble(symbols, iq_samples) if len(symbols) != 0: @@ -174,35 +171,37 @@ class PhysicalLayer(object): def get_doppler_from_preamble(self, symbols, iq_samples): """quality check and doppler estimation for preamble""" - success = True - doppler = 0 + success,doppler = True,0 if len(iq_samples) != 0: - zp = np.conj(self.get_preamble_z(self._sps))[9*self._sps:] - cc = np.array([np.sum(iq_samples[i*self._sps:(3*32+i-9)*self._sps]*zp) - for i in range(4*32)]) - acc = np.abs(cc) - for i in range(0,len(cc),32): - print('i=%3d: '%i,end='') - for j in range(32): - print('%3.0f ' % acc[i+j], end='') - print() - imax = np.argmax(np.abs(cc[0:2*32])) - pks = cc[(imax,imax+3*16,imax+3*16+1,imax+3*32),] - apks = np.abs(pks) - print('imax=', imax, 'apks=',apks) - success = np.mean(apks[(0,3),]) > 2*np.mean(apks[(1,2),]) - doppler = np.diff(np.unwrap(np.angle(pks[(0,3),])))[0]/(3*32) if success else 0 + sps = self._sps + zp = np.array([z for z in PhysicalLayer.get_preamble()['symb'] + for _ in range(sps)], dtype=np.complex64) + ## find starting point + cc = np.correlate(iq_samples, zp[0:3*32*sps]) + imax = np.argmax(np.abs(cc[0:2*32*sps])) + pks = cc[(imax, imax+3*32*sps),] + tpks = cc[imax+3*16*sps:imax+5*16*sps] + print('imax=', imax, 'apks=',np.abs(pks), + np.mean(np.abs(pks)), np.mean(np.abs(tpks)), np.abs(tpks)) + success = np.mean(np.abs(pks)) > 2*np.mean(np.abs(tpks)) + doppler = np.diff(np.unwrap(np.angle(pks)))[0]/(3*32) if success else 0 + if success: + idx = np.arange(32*sps) + pks = [np.correlate(iq_samples[imax+i*32*sps+idx], + zp[i*32*sps+idx])[0] + for i in range(9)] + doppler = freq_est(pks)/32 print('success=', success, 'doppler=', doppler) return success,doppler def decode_preamble(self, symbols): - data = [FROM_WALSH[walsh_to_num + data = [FROM_WALSH[np.packbits (np.real (np.sum - (symbols[i:i+32].reshape((4,8)),0))<0)] + (symbols[i:i+32].reshape((4,8)),0))<0)[0]] for i in range(0,15*32,32)] print('data=',data) - self._pre_counter = sum((np.array(data[11:14])&3)*(1<<2*np.arange(3)[::-1])) + self._pre_counter = sum([(x&3)*(1<<2*y) for (x,y) in zip(data[11:14][::-1], range(3))]) self._d1d2 = data[9:11] self._mode = MODE[data[9]][data[10]] self._block_len = 11520 if self._mode['interleaver'][0] == 'L' else 1440 @@ -213,27 +212,17 @@ class PhysicalLayer(object): @staticmethod def get_preamble(): """preamble symbols + scrambler""" - a=np.zeros(15*32, dtype=[('symb', np.complex64), - ('scramble', np.complex64)]) - a['symb'] = PRE_SCRAMBLE*PRE_SYMBOLS - a['scramble'] = PRE_SCRAMBLE - return a + return np.array(zip(PRE_SCRAMBLE*PRE_SYMBOLS, + PRE_SCRAMBLE), + dtype=[('symb', np.complex64), + ('scramble', np.complex64)]) @staticmethod def get_preamble_z(sps): """preamble symbols for preamble correlation""" a = PhysicalLayer.get_preamble() return np.array([z for z in a['symb'][0:32*3] - for i in range(sps)]) - - @staticmethod - def make_psk(n, gray_code): - """generates n-PSK constellation data""" - c = np.zeros(n, dtype=[('points', np.complex64), - ('symbols', np.uint8)]) - c['points'] = n_psk(n,np.arange(n)) - c['symbols'] = gray_code - return c + for _ in range(sps)]) if __name__ == '__main__': def gen_data_scramble(): @@ -245,13 +234,13 @@ if __name__ == '__main__': a = np.zeros(160, dtype=np.uint8) s = 0xBAD for i in range(160): - for j in range(8): s = advance(s) + for _ in range(8): s = advance(s) a[i] = s&7; return a p=PhysicalLayer(5) - z1=np.array([x for x in PRE_SYMBOLS for i in range(5)]) - z2=np.array([x for x in PRE_SCRAMBLE for i in range(5)]) + z1=np.array([x for x in PRE_SYMBOLS for _ in range(5)]) + z2=np.array([x for x in PRE_SCRAMBLE for _ in range(5)]) z=z1*z2 for i in range(3): @@ -264,6 +253,5 @@ if __name__ == '__main__': print(gen_data_scramble()) s=ScrambleData() - print(type(s)) print([s.next() for _ in range(160)]) print([s.next() for _ in range(160)]) diff --git a/python/physical_layer/MIL_STD_188_110C.py b/python/physical_layer/MIL_STD_188_110C.py index 217cf6e..84f1af0 100644 --- a/python/physical_layer/MIL_STD_188_110C.py +++ b/python/physical_layer/MIL_STD_188_110C.py @@ -2,13 +2,9 @@ from __future__ import print_function import numpy as np - -def n_psk(n,x): - return np.complex64(np.exp(2j*np.pi*x/n)) +from common import * ## ---- constellations ----------------------------------------------------------- -CONST_DTYPE=np.dtype([('points', np.complex64), - ('symbols', np.uint8)]) BPSK=np.array(zip(np.exp(2j*np.pi*np.arange(2)/2), [0,1]), CONST_DTYPE) QPSK=np.array(zip(np.exp(2j*np.pi*np.arange(4)/4), [0,1,3,2]), CONST_DTYPE) PSK8=np.array(zip(np.exp(2j*np.pi*np.arange(8)/8), [0,1,3,2,7,6,4,5]), CONST_DTYPE) @@ -47,6 +43,10 @@ QAM64=np.array( -0.360142-0.932897j, -0.353057-0.588429j, -0.353057-0.117686j, -0.353057-0.353057j], range(64)), CONST_DTYPE) +## for test +QAM64 = QAM64[(7,3,24,56,35,39,60,28),] +QAM64['symbols'] = [1, 0, 2, 6, 4, 5, 7, 3] + ## ---- constellation indices --------------------------------------------------- MODE_BPSK = 0 MODE_QPSK = 1 @@ -62,20 +62,18 @@ class ScrambleData(object): self.reset() def reset(self): - self._state = 1 + self._state = np.array([0,0,0,0,0,0,0,0,1], dtype=np.bool) + self._taps = np.array([0,0,0,0,1,0,0,0,1], dtype=np.bool) def next(self, num_bits): - r = self._state & ((1<>1)&511 - if lsb: - self._state ^= 0x10B - return self._state + self._state = np.concatenate(([np.sum(self._state&self._taps)&1], + self._state[0:-1])) ## ---- preamble definitions --------------------------------------------------- ## 184 = 8*23 @@ -132,10 +130,13 @@ class PhysicalLayer(object): ## --- preamble frame ---- if self._frame_counter == -1: return [self._preamble,MODE_BPSK,True,False] - ## ----- data frame ------ + ## ----- (re)inserted preamble ------ if self._frame_counter == 0: self.a = self.make_reinserted_preamble() - return [self.a, MODE_QPSK,False,True] + return [self.a, MODE_QPSK,False,False] + if self._frame_counter >= 1: + self.a = self.make_data_frame() + return [self.a, MODE_64QAM,False,True] def get_doppler(self, symbols, iq_samples): """returns a tuple @@ -145,50 +146,45 @@ class PhysicalLayer(object): self._frame_counter,len(symbols),len(iq_samples)) #if len(symbols)!=0: # print('symb=', symbols) - success = False - doppler = 0 + success,doppler = False,0 if self._frame_counter == -1: ## -- preamble ---- success,doppler = self.get_doppler_from_preamble(symbols, iq_samples) if len(symbols) != 0: for s in symbols: print(s) self._frame_counter = 0 + elif self._frame_counter >= 0 and self._frame_counter <5: ## -- reinserted preamble ---- + for s in symbols: + print(s) + self._frame_counter += 1 + success = True else: ## ------------------------ data frame ---- - if len(symbols) != 0: - for s in symbols: - print(s) + for s in symbols: + print(s) success = False self._frame_counter = -1 return success,doppler def get_doppler_from_preamble(self, symbols, iq_samples): """quality check and doppler estimation for preamble""" - success = True - doppler = 0 - shift=9 + success,doppler = True,0 if len(iq_samples) != 0: - zp = np.conj(self.get_preamble_z(self._sps)[shift*self._sps:]) - cc = np.array([np.sum(iq_samples[i:i+23*self._sps] * - zp[0:23*self._sps]) - for i in range(23*3*self._sps)]) - acc = np.abs(cc) - for i in range(0,len(cc),23*self._sps): - print('i=%3d: '%i,end='') - for j in range(23*self._sps): - print('%3.0f ' % acc[i+j], end='') - print() - - imax = np.argmax(np.abs(cc[0:3*23*self._sps])) - print(imax) - pks = np.array([np.sum(iq_samples[(imax+23*i*self._sps): - (imax+23*i*self._sps+23*self._sps)] * - zp[(23*i*self._sps): - (23*i*self._sps+23*self._sps)]) - for i in range(1,5)]) - print('doppler apks', np.abs(pks)) - print('doppler ppks', np.angle(pks), np.diff(np.unwrap(np.angle(pks)))/23) - doppler = np.mean(np.diff(np.unwrap(np.angle(pks))))/23 - success = True + sps = self._sps + idx = np.arange(23*sps) + zp = self.get_preamble_z(self._sps) + cc = np.correlate(iq_samples, zp[idx]) + imax = np.argmax(np.abs(cc[0:23*sps])) + pks = [np.correlate(iq_samples[imax+i*23*sps+idx], + zp[i*23*sps+idx])[0] + for i in range(7)] + success = np.mean(np.abs(pks)) > 2*np.mean(np.abs(cc[imax+11*sps+range(-sps,sps)])) + print('test:',imax, np.mean(np.abs(pks)), np.mean(np.abs(cc[imax+11*sps+range(-sps,sps)]))) + if success: + print('doppler apks', np.abs(pks)) + print('doppler ppks', np.angle(pks), + np.diff(np.unwrap(np.angle(pks)))/23, + np.mean(np.diff(np.unwrap(np.angle(pks)))/23)) + doppler = freq_est(pks[1:])/23; print('success=', success, 'doppler=', doppler) return success,doppler @@ -199,6 +195,13 @@ class PhysicalLayer(object): ('scramble', np.complex64)]) a['symb'][32:32+3*13] = 0 ## D0,D1,D2 return a + def make_data_frame(self): + a=np.zeros(256+31, dtype=[('symb', np.complex64), + ('scramble', np.complex64)]) + a['scramble'] = 1 + a['symb'][256:] = MINI_PROBE_MINUS + a['scramble'][256:] = MINI_PROBE_MINUS + return a @staticmethod def get_preamble(): @@ -210,7 +213,7 @@ class PhysicalLayer(object): @staticmethod def get_preamble_z(sps): """preamble symbols for preamble correlation""" - return np.array([z for z in PREAMBLE for i in range(sps)]) + return np.array([z for z in PREAMBLE for _ in range(sps)]) if __name__ == '__main__': print(PREAMBLE) @@ -224,12 +227,16 @@ if __name__ == '__main__': print([s.next(1) for _ in range(511)]) print([s.next(1) for _ in range(511)] == [s.next(1) for _ in range(511)]) - print(QAM64) - print(QAM32) - print(QAM16) - print(PSK8) - print(QPSK) - print(BPSK) - print(MINI_PROBE_PLUS) - print(MINI_PROBE_MINUS) - print(MINI_PROBE_PLUS*MINI_PROBE_MINUS) + #print(QAM64) + #print(QAM32) + #print(QAM16) + #print(PSK8) + #print(QPSK) + #print(BPSK) + #print(MINI_PROBE_PLUS) + #print(MINI_PROBE_MINUS) + #print(MINI_PROBE_PLUS*MINI_PROBE_MINUS) + #for i in range(len(QAM64)): + # print(QAM64['points'][i]) + + print([s.next(6) for _ in range(256)]) diff --git a/python/physical_layer/STANAG_4285.py b/python/physical_layer/STANAG_4285.py index 0b151ad..881e2de 100644 --- a/python/physical_layer/STANAG_4285.py +++ b/python/physical_layer/STANAG_4285.py @@ -57,13 +57,15 @@ class PhysicalLayer(object): success = True doppler = 0 if len(iq_samples) != 0: - zp = [x for x in self._preamble['symb'][9:40] for i in range(self._sps)] - cc = np.array([np.sum(iq_samples[ i*5:(31+i)*5]*zp) for i in range(49)]) - imax = np.argmax(np.abs(cc[0:18])) - pks = cc[(imax,imax+15,imax+16,imax+31),] - apks = np.abs(pks) - success = np.mean(apks[(0,3),]) > 2*np.mean(apks[(1,2),]) - doppler = np.diff(np.unwrap(np.angle(pks[(0,3),])))[0]/31 if success else 0 + sps = self._sps + zp = np.array([x for x in self._preamble['symb'][9:40] + for _ in range(sps)], dtype=np.complex64) + cc = np.correlate(iq_samples, zp) + imax = np.argmax(np.abs(cc[0:18*sps])) + pks = cc[(imax,imax+31*sps),] + tpks = cc[imax+15*sps:imax+16*sps] + success = np.mean(np.abs(pks)) > 2*np.mean(np.abs(tpks)) + doppler = np.diff(np.unwrap(np.angle(pks)))[0]/31 if success else 0 if len(symbols) != 0: idx = range(30,80) if self._is_first_frame else range(80) z = symbols[idx]*np.conj(self._preamble['symb'][idx]) @@ -102,7 +104,7 @@ class PhysicalLayer(object): p = np.zeros(176, dtype=np.uint8) for i in range(176): p[i] = np.sum(state[-3:]*[4,2,1]) - for j in range(3): + for _ in range(3): state = np.concatenate(([np.sum(state&taps)&1], state[0:-1])) a=np.zeros(176, dtype=[('symb',np.complex64), ('scramble', np.complex64)]) ## 8PSK modulation @@ -116,24 +118,6 @@ class PhysicalLayer(object): def make_psk(n, gray_code): """generates n-PSK constellation data""" c = np.zeros(n, dtype=[('points', np.complex64), ('symbols', np.uint8)]) - c['points'] = np.exp(2*np.pi*1j*np.array(range(n))/n) + c['points'] = np.exp(2*np.pi*1j*np.arange(n)/n) c['symbols'] = gray_code return c - - ## for now not used (doppler estimation after adaptive filtering does not work) - @staticmethod - def data_aided_frequency_estimation(x,c): - """Data-Aided Frequency Estimation for Burst Digital Transmission, - Umberto Mengali and M. Morelli, IEEE TRANSACTIONS ON COMMUNICATIONS, - VOL. 45, NO. 1, JANUARY 1997""" - z = x*np.conj(c) ## eq (2) - L0 = len(z) - N = L0//2 - R = np.zeros(N, dtype=np.complex64) - for i in range(N): - R[i] = 1.0/(L0-i)*np.sum(z[i:]*np.conj(z[0:L0-i])) ## eq (3) - m = np.array(range(N), dtype=np.float) - w = 3*((L0-m)*(L0-m+1)-N*(L0-N))/(N*(4*N*N - 6*N*L0 + 3*L0*L0-1)) ## eq (9) - mod_2pi = lambda x : np.mod(x-np.pi, 2*np.pi) - np.pi - fd = np.sum(w[1:] * mod_2pi(np.diff(np.angle(R)))) ## eq (8) - return fd diff --git a/python/physical_layer/common.py b/python/physical_layer/common.py new file mode 100644 index 0000000..67a406f --- /dev/null +++ b/python/physical_layer/common.py @@ -0,0 +1,29 @@ +## -*- python -*- + +import numpy as np + +CONST_DTYPE=np.dtype([('points', np.complex64), + ('symbols', np.uint8)]) + +def n_psk(n,x): + """n-ary PSK constellation""" + return np.complex64(np.exp(2j*np.pi*x/n)) + +def freq_est(z): + """Data-Aided Frequency Estimation for Burst Digital Transmission, + Umberto Mengali and M. Morelli, IEEE TRANSACTIONS ON COMMUNICATIONS, + VOL. 45, NO. 1, JANUARY 1997""" + L0 = len(z) + N = L0//2 + R = np.zeros(N+1, dtype=np.complex64) + for i in range(N+1): + R[i] = 1.0/(L0-i)*np.sum(z[i:]*np.conj(z[0:L0-i])) ## eq (3) + m = np.arange(N+1, dtype=np.float32) + w = 3*((L0-m)*(L0-m+1)-N*(L0-N))/(N*(4*N*N - 6*N*L0 + 3*L0*L0-1)) ## eq (9) + mod_2pi = lambda x : np.mod(x-np.pi, 2*np.pi) - np.pi + return np.sum(w[1:] * mod_2pi(np.diff(np.angle(R)))) ## eq (8) + +if __name__ == '__main__': + idx=np.arange(3) + z=np.exp(1j*idx*0.056+1j) + print(freq_est(z)/0.056)