From 5bf620aae84030a1f41d0ade81fcd061640c4143 Mon Sep 17 00:00:00 2001 From: Tommi Paakki Date: Thu, 14 Jul 2016 17:48:50 +0300 Subject: [PATCH 1/3] Add script for computing controlled-root parameters --- peregrine/include/controlled_root.py | 110 +++++++++++++++++++++++++++ 1 file changed, 110 insertions(+) create mode 100644 peregrine/include/controlled_root.py diff --git a/peregrine/include/controlled_root.py b/peregrine/include/controlled_root.py new file mode 100644 index 0000000..412678c --- /dev/null +++ b/peregrine/include/controlled_root.py @@ -0,0 +1,110 @@ +# -*- coding: utf-8 -*- +""" +Created on Thu Jul 14 14:29:34 2016 + +@author: tpaakki + +This script generates controlled-root loop parameters, based on +"Stephens, S. A., and J. C. Thomas, "Controlled-Root Formulation for +Digital Phase-Locked Loops," IEEE Trans. on Aerospace and Electronics + Systems, January 1995" + +""" +from math import factorial, exp +import cmath + +def controlled_root(N, T, BW): + # Input Parameters + # N [-] Loop Order + # T [s] Integration Time + # BW [Hz] Loop Bandwidth + # Output Parameters + # K [-] Loop constants + + K = [] + tol = 1.e-6 # Error tolerance + goal = BW*T # This is the BLT we want to solve + + # Few precomputed factorial parameters + if N > 1: + fac1 = factorial(N)/(factorial(1)*factorial(N-1)) + if N > 2: + fac2 = factorial(N)/(factorial(2)*factorial(N-2)) + fac3 = factorial(N-1)/(factorial(1)*factorial(N-1-1)) + fac4 = factorial(N-2)/(factorial(1)*factorial(N-2-1)) + + beta = 0.5 + step = 0.25 + done = True + ii = 1 + if N == 1: + while done: + z1 = exp(-beta) + K1 = 1.-z1 + blt = K1/(2.*(2.-K1)) + err = goal-blt + if abs(err) <= tol: + K = [K1] + done = False + if err > 0.: + beta = beta + step + step = step / 2. + if err < 0.: + beta = beta - step + step = step / 2. + if ii > 30: + 'Error - did not converge' + done = False + ii = ii + 1; + if N == 2: + while done: + z1 = cmath.exp(-beta*(1.+1.j)) + z2 = cmath.exp(-beta*(1.-1.j)) + K1 = 1.-z1*z2 + K1 = K1.real + K2 = fac1-K1-z1-z2 + K2 = K2.real + blt = (2.*K1*K1+2.*K2+K1*K2)/(2.*K1*(4.-2*K1-K2)) + err = goal-blt + if abs(err) <= tol: + K = K1, K2 + done = False + if err > 0.: + beta = beta + step + step = step / 2. + if err < 0.: + beta = beta - step + step = step / 2. + if ii > 30: + 'Error - did not converge' + done = False + ii = ii + 1; + if N == 3: + while done: + z1 = exp(-beta) + z2 = cmath.exp(-beta*(1.+1.j)) + z3 = cmath.exp(-beta*(1.-1.j)) + K1 = 1-z1*z2*z3 + K1 = K1.real + summ = z1*z2+z1*z3+z2*z3; + K2 = (fac2-fac3*K1-summ)/fac4 + K2 = K2.real + K3 = fac1-K1-K2-z1-z2-z3 + K3 = K3.real + blt = (4.*K1*K1*K2-4.*K1*K3+4.*K2*K2+2.*K1*K2*K2+4.*K1*K1*K3+4.*K2*K3+3*K1*K2*K3+K3*K3+K1*K3*K3)/(2.*(K1*K2-K3+K1*K3)*(8.-4.*K1-2.*K2-K3)) + err = goal-blt + if abs(err) <= tol: + K = K1, K2, K3 + done = False + if err > 0.: + beta = beta + step + step = step / 2. + if err < 0.: + beta = beta - step + step = step / 2. + if ii > 30: + 'Error - did not converge' + done = False + ii = ii + 1; + + return K From b1b368056a601ac4673b1b30bac35193b7273ac4 Mon Sep 17 00:00:00 2001 From: Tommi Paakki Date: Thu, 14 Jul 2016 18:17:55 +0300 Subject: [PATCH 2/3] Added comments --- peregrine/include/controlled_root.py | 186 ++++++++++++++------------- 1 file changed, 94 insertions(+), 92 deletions(-) diff --git a/peregrine/include/controlled_root.py b/peregrine/include/controlled_root.py index 412678c..466a5e2 100644 --- a/peregrine/include/controlled_root.py +++ b/peregrine/include/controlled_root.py @@ -14,97 +14,99 @@ import cmath def controlled_root(N, T, BW): - # Input Parameters - # N [-] Loop Order - # T [s] Integration Time - # BW [Hz] Loop Bandwidth - # Output Parameters - # K [-] Loop constants + # Input Parameters + # N [-] Loop Order + # T [s] Integration Time + # BW [Hz] Loop Bandwidth + # Output Parameters + # K [-] Loop constants - K = [] - tol = 1.e-6 # Error tolerance - goal = BW*T # This is the BLT we want to solve - - # Few precomputed factorial parameters - if N > 1: - fac1 = factorial(N)/(factorial(1)*factorial(N-1)) - if N > 2: - fac2 = factorial(N)/(factorial(2)*factorial(N-2)) - fac3 = factorial(N-1)/(factorial(1)*factorial(N-1-1)) - fac4 = factorial(N-2)/(factorial(1)*factorial(N-2-1)) - - beta = 0.5 - step = 0.25 - done = True - ii = 1 - if N == 1: - while done: - z1 = exp(-beta) - K1 = 1.-z1 - blt = K1/(2.*(2.-K1)) - err = goal-blt - if abs(err) <= tol: - K = [K1] - done = False - if err > 0.: - beta = beta + step - step = step / 2. - if err < 0.: - beta = beta - step - step = step / 2. - if ii > 30: - 'Error - did not converge' - done = False - ii = ii + 1; - if N == 2: - while done: - z1 = cmath.exp(-beta*(1.+1.j)) - z2 = cmath.exp(-beta*(1.-1.j)) - K1 = 1.-z1*z2 - K1 = K1.real - K2 = fac1-K1-z1-z2 - K2 = K2.real - blt = (2.*K1*K1+2.*K2+K1*K2)/(2.*K1*(4.-2*K1-K2)) - err = goal-blt - if abs(err) <= tol: - K = K1, K2 - done = False - if err > 0.: - beta = beta + step - step = step / 2. - if err < 0.: - beta = beta - step - step = step / 2. - if ii > 30: - 'Error - did not converge' - done = False - ii = ii + 1; - if N == 3: - while done: - z1 = exp(-beta) - z2 = cmath.exp(-beta*(1.+1.j)) - z3 = cmath.exp(-beta*(1.-1.j)) - K1 = 1-z1*z2*z3 - K1 = K1.real - summ = z1*z2+z1*z3+z2*z3; - K2 = (fac2-fac3*K1-summ)/fac4 - K2 = K2.real - K3 = fac1-K1-K2-z1-z2-z3 - K3 = K3.real - blt = (4.*K1*K1*K2-4.*K1*K3+4.*K2*K2+2.*K1*K2*K2+4.*K1*K1*K3+4.*K2*K3+3*K1*K2*K3+K3*K3+K1*K3*K3)/(2.*(K1*K2-K3+K1*K3)*(8.-4.*K1-2.*K2-K3)) - err = goal-blt - if abs(err) <= tol: - K = K1, K2, K3 - done = False - if err > 0.: - beta = beta + step - step = step / 2. - if err < 0.: - beta = beta - step - step = step / 2. - if ii > 30: - 'Error - did not converge' - done = False - ii = ii + 1; + K = [] + tol = 1.e-6 # Error tolerance + goal = BW*T # This is the BLT we want to solve - return K + # Few precomputed factorial parameters + if N > 1: + fac1 = factorial(N)/(factorial(1)*factorial(N-1)) # eq.(45) + if N > 2: + fac2 = factorial(N)/(factorial(2)*factorial(N-2)) # eq.(46) + fac3 = factorial(N-1)/(factorial(1)*factorial(N-1-1)) # eq.(46) + fac4 = factorial(N-2)/(factorial(1)*factorial(N-2-1)) # eq.(46) + + beta = 0.5 + step = 0.25 + done = True + ii = 1 + if N == 1: + while done: + z1 = exp(-beta) # eq.(50) + K1 = 1.-z1 # eq.(49) + blt = K1/(2.*(2.-K1)) # Table IV + err = goal-blt + if abs(err) <= tol: + K = [K1] + done = False + if err > 0.: + beta = beta + step + step = step / 2. + if err < 0.: + beta = beta - step + step = step / 2. + if ii > 30: + 'Error - did not converge' + done = False + ii = ii + 1; + if N == 2: + while done: + z1 = cmath.exp(-beta*(1.+1.j)) # eq.(50) + z2 = cmath.exp(-beta*(1.-1.j)) # eq.(50) + K1 = 1.-z1*z2 # eq.(49) + K1 = K1.real + K2 = fac1-K1-z1-z2 # eq.(45) + K2 = K2.real + blt = (2.*K1*K1+2.*K2+K1*K2)/(2.*K1*(4.-2*K1-K2)) # Table IV + err = goal-blt + if abs(err) <= tol: + K = K1, K2 + done = False + if err > 0.: + beta = beta + step + step = step / 2. + if err < 0.: + beta = beta - step + step = step / 2. + if ii > 30: + 'Error - did not converge' + done = False + ii = ii + 1; + if N == 3: + while done: + z1 = exp(-beta) # eq.(50) + z2 = cmath.exp(-beta*(1.+1.j)) # eq.(50) + z3 = cmath.exp(-beta*(1.-1.j)) # eq.(50) + K1 = 1-z1*z2*z3 # eq.(49) + K1 = K1.real + summ = z1*z2+z1*z3+z2*z3; + K2 = (fac2-fac3*K1-summ)/fac4 # eq.(46) + K2 = K2.real + K3 = fac1-K1-K2-z1-z2-z3 # eq.(45) + K3 = K3.real + blt = (4.*K1*K1*K2-4.*K1*K3+4.*K2*K2+2.*K1*K2*K2+4.*K1*K1*K3+4.*K2*K3 + +3*K1*K2*K3+K3*K3+K1*K3*K3)/(2.*(K1*K2-K3+K1*K3)*(8. + -4.*K1-2.*K2-K3)) # Table IV + err = goal-blt + if abs(err) <= tol: + K = K1, K2, K3 + done = False + if err > 0.: + beta = beta + step + step = step / 2. + if err < 0.: + beta = beta - step + step = step / 2. + if ii > 30: + 'Error - did not converge' + done = False + ii = ii + 1; + + return K From 6e89807ad15b21324ab82b6108cf85b7f18d835b Mon Sep 17 00:00:00 2001 From: Tommi Paakki Date: Tue, 2 Aug 2016 16:23:32 +0300 Subject: [PATCH 3/3] Address review comments --- peregrine/include/controlled_root.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/peregrine/include/controlled_root.py b/peregrine/include/controlled_root.py index 466a5e2..50af7ce 100644 --- a/peregrine/include/controlled_root.py +++ b/peregrine/include/controlled_root.py @@ -1,15 +1,20 @@ -# -*- coding: utf-8 -*- -""" -Created on Thu Jul 14 14:29:34 2016 - -@author: tpaakki +# Copyright (C) 2016 Swift Navigation Inc. +# Contact: Tommi Paakki +# +# This source is subject to the license found in the file 'LICENSE' which must +# be be distributed together with this source. All other rights reserved. +# +# THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, +# EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. +""" This script generates controlled-root loop parameters, based on "Stephens, S. A., and J. C. Thomas, "Controlled-Root Formulation for Digital Phase-Locked Loops," IEEE Trans. on Aerospace and Electronics Systems, January 1995" - """ + from math import factorial, exp import cmath