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)