-----------------------------------------------------------------------------
-- |
-- Module      :  DSP.Estimation.Frequency.FCI
-- Copyright   :  (c) Matthew Donadio 2003
-- License     :  GPL
--
-- Maintainer  :  m.p.donadio@ieee.org
-- Stability   :  experimental
-- Portability :  portable
--
-- This module contains a few simple algorithms for interpolating the
-- peak location of a DFT\/FFT.
--
-----------------------------------------------------------------------------

-- TODO: confirm that quinn2 needs log10 and not ln

module DSP.Estimation.Frequency.FCI (quinn1, quinn2, quinn3, jacobsen, macleod3, macleod5, rv) where

import DSP.Basic((^!))
import Data.Array
import Data.Complex

log10 :: Floating a => a -> a
log10 :: forall a. Floating a => a -> a
log10 = forall a. Floating a => a -> a -> a
logBase a
10

-- | Quinn's First Estimator (FCI1)

quinn1 :: (Ix a, Integral a, RealFloat b) => Array a (Complex b) -- ^ X[k]
       -> a -- ^ k
       -> b -- ^ w

quinn1 :: forall a b.
(Ix a, Integral a, RealFloat b) =>
Array a (Complex b) -> a -> b
quinn1 Array a (Complex b)
x a
k = b
2 forall a. Num a => a -> a -> a
* forall a. Floating a => a
pi forall a. Num a => a -> a -> a
* ((forall a b. (Integral a, Num b) => a -> b
fromIntegral a
k) forall a. Num a => a -> a -> a
+ b
d) forall a. Fractional a => a -> a -> a
/ (forall a b. (Integral a, Num b) => a -> b
fromIntegral a
n)
    where d :: b
d | b
dp forall a. Ord a => a -> a -> Bool
> b
0 Bool -> Bool -> Bool
&& b
dm forall a. Ord a => a -> a -> Bool
> b
0 = b
dp
	    | Bool
otherwise        = b
dm
	  dp :: b
dp = -b
ap forall a. Fractional a => a -> a -> a
/ (b
1 forall a. Num a => a -> a -> a
- b
ap)
 	  dm :: b
dm =  b
am forall a. Fractional a => a -> a -> a
/ (b
1 forall a. Num a => a -> a -> a
- b
am)
 	  ap :: b
ap = forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
+a
1)) forall a. Fractional a => a -> a -> a
/ forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k)
 	  am :: b
am = forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
-a
1)) forall a. Fractional a => a -> a -> a
/ forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k)
 	  n :: a
n = forall a b. (a, b) -> b
snd (forall i e. Array i e -> (i, i)
bounds Array a (Complex b)
x) forall a. Num a => a -> a -> a
+ a
1

-- | Quinn's Second Estimator (FCI2)

quinn2 :: (Ix a, Integral a, RealFloat b) => Array a (Complex b) -- ^ X[k]
       -> a -- ^ k
       -> b -- ^ w

quinn2 :: forall a b.
(Ix a, Integral a, RealFloat b) =>
Array a (Complex b) -> a -> b
quinn2 Array a (Complex b)
x a
k = b
2 forall a. Num a => a -> a -> a
* forall a. Floating a => a
pi forall a. Num a => a -> a -> a
* ((forall a b. (Integral a, Num b) => a -> b
fromIntegral a
k) forall a. Num a => a -> a -> a
+ b
d) forall a. Fractional a => a -> a -> a
/ (forall a b. (Integral a, Num b) => a -> b
fromIntegral a
n)
    where d :: b
d = (b
dp forall a. Num a => a -> a -> a
+ b
dm) forall a. Fractional a => a -> a -> a
/ b
2 forall a. Num a => a -> a -> a
+ forall a. Floating a => a -> a
tau(b
dpforall a. Num a => a -> Int -> a
^!Int
2) forall a. Num a => a -> a -> a
- forall a. Floating a => a -> a
tau(b
dmforall a. Num a => a -> Int -> a
^!Int
2)
          dp :: b
dp = -b
ap forall a. Fractional a => a -> a -> a
/ (b
1 forall a. Num a => a -> a -> a
- b
ap)
 	  dm :: b
dm =  b
am forall a. Fractional a => a -> a -> a
/ (b
1 forall a. Num a => a -> a -> a
- b
am)
 	  ap :: b
ap = forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
+a
1)) forall a. Fractional a => a -> a -> a
/ forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k)
 	  am :: b
am = forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
-a
1)) forall a. Fractional a => a -> a -> a
/ forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k)
 	  tau :: a -> a
tau a
y = a
0.25 forall a. Num a => a -> a -> a
* forall a. Floating a => a -> a
log10(a
3forall a. Num a => a -> a -> a
*a
yforall a. Num a => a -> Int -> a
^!Int
2 forall a. Num a => a -> a -> a
+ a
6 forall a. Num a => a -> a -> a
* a
y forall a. Num a => a -> a -> a
+ a
1) forall a. Num a => a -> a -> a
- (forall a. Floating a => a -> a
sqrt a
6) forall a. Fractional a => a -> a -> a
/ a
24 forall a. Num a => a -> a -> a
* forall a. Floating a => a -> a
log10 ((a
y forall a. Num a => a -> a -> a
+ a
1 forall a. Num a => a -> a -> a
- forall a. Floating a => a -> a
sqrt (a
2forall a. Fractional a => a -> a -> a
/a
3)) forall a. Fractional a => a -> a -> a
/ (a
y forall a. Num a => a -> a -> a
+ a
1 forall a. Num a => a -> a -> a
+ forall a. Floating a => a -> a
sqrt (a
2forall a. Fractional a => a -> a -> a
/a
3)))
 	  n :: a
n = forall a b. (a, b) -> b
snd (forall i e. Array i e -> (i, i)
bounds Array a (Complex b)
x) forall a. Num a => a -> a -> a
+ a
1

-- | Quinn's Third Estimator (FCI3)

quinn3 :: (Ix a, Integral a, RealFloat b) => Array a (Complex b) -- ^ X[k]
       -> a -- ^ k
       -> b -- ^ w

quinn3 :: forall a b.
(Ix a, Integral a, RealFloat b) =>
Array a (Complex b) -> a -> b
quinn3 Array a (Complex b)
x a
k = b
2 forall a. Num a => a -> a -> a
* forall a. Floating a => a
pi forall a. Num a => a -> a -> a
* ((forall a b. (Integral a, Num b) => a -> b
fromIntegral a
k) forall a. Num a => a -> a -> a
+ b
d) forall a. Fractional a => a -> a -> a
/ (forall a b. (Integral a, Num b) => a -> b
fromIntegral a
n)
    where d :: b
d = (b
dm forall a. Num a => a -> a -> a
+ b
dp) forall a. Fractional a => a -> a -> a
/ b
2 forall a. Num a => a -> a -> a
+ (b
dp forall a. Num a => a -> a -> a
- b
dm) forall a. Num a => a -> a -> a
* (b
3forall a. Num a => a -> a -> a
*b
dtforall a. Num a => a -> Int -> a
^!Int
3 forall a. Num a => a -> a -> a
+ b
2forall a. Num a => a -> a -> a
*b
dt) forall a. Fractional a => a -> a -> a
/ (b
3forall a. Num a => a -> a -> a
*b
dtforall a. Num a => a -> Int -> a
^!Int
4forall a. Num a => a -> a -> a
+b
6forall a. Num a => a -> a -> a
*b
dtforall a. Num a => a -> Int -> a
^!Int
2forall a. Num a => a -> a -> a
+b
1)
	  dt :: b
dt | b
dm forall a. Ord a => a -> a -> Bool
> b
0 Bool -> Bool -> Bool
&& b
dp forall a. Ord a => a -> a -> Bool
> b
0 = b
dp
	     | Bool
otherwise        = b
dm
	  dp :: b
dp = -b
ap forall a. Fractional a => a -> a -> a
/ (b
1 forall a. Num a => a -> a -> a
- b
ap)
 	  dm :: b
dm =  b
am forall a. Fractional a => a -> a -> a
/ (b
1 forall a. Num a => a -> a -> a
- b
am)
 	  ap :: b
ap = forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
+a
1)) forall a. Fractional a => a -> a -> a
/ forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k)
 	  am :: b
am = forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
-a
1)) forall a. Fractional a => a -> a -> a
/ forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k)
 	  n :: a
n = forall a b. (a, b) -> b
snd (forall i e. Array i e -> (i, i)
bounds Array a (Complex b)
x) forall a. Num a => a -> a -> a
+ a
1

-- | Eric Jacobsen's Estimator

jacobsen :: (Ix a, Integral a, RealFloat b) => Array a (Complex b) -- ^ X[k]
       -> a -- ^ k
       -> b -- ^ w

jacobsen :: forall a b.
(Ix a, Integral a, RealFloat b) =>
Array a (Complex b) -> a -> b
jacobsen Array a (Complex b)
x a
k = b
2 forall a. Num a => a -> a -> a
* forall a. Floating a => a
pi forall a. Num a => a -> a -> a
* ((forall a b. (Integral a, Num b) => a -> b
fromIntegral a
k) forall a. Num a => a -> a -> a
+ b
d) forall a. Fractional a => a -> a -> a
/ (forall a b. (Integral a, Num b) => a -> b
fromIntegral a
n)
    where d :: b
d = forall a. Complex a -> a
realPart ((Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
-a
1) forall a. Num a => a -> a -> a
- Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
+a
1)) forall a. Fractional a => a -> a -> a
/ (Complex b
2 forall a. Num a => a -> a -> a
* Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k forall a. Num a => a -> a -> a
- Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
-a
1) forall a. Num a => a -> a -> a
- Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
+a
1)))
 	  n :: a
n = forall a b. (a, b) -> b
snd (forall i e. Array i e -> (i, i)
bounds Array a (Complex b)
x) forall a. Num a => a -> a -> a
+ a
1

-- | MacLeod's Three Point Estimator

macleod3 :: (Ix a, Integral a, RealFloat b) => Array a (Complex b) -- ^ X[k]
       -> a -- ^ k
       -> b -- ^ w

macleod3 :: forall a b.
(Ix a, Integral a, RealFloat b) =>
Array a (Complex b) -> a -> b
macleod3 Array a (Complex b)
x a
k = b
2 forall a. Num a => a -> a -> a
* forall a. Floating a => a
pi forall a. Num a => a -> a -> a
* ((forall a b. (Integral a, Num b) => a -> b
fromIntegral a
k) forall a. Num a => a -> a -> a
+ b
d) forall a. Fractional a => a -> a -> a
/ (forall a b. (Integral a, Num b) => a -> b
fromIntegral a
n)
    where rm1 :: b
rm1 = forall a. Complex a -> a
realPart (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
-a
1) forall a. Num a => a -> a -> a
* forall a. Num a => Complex a -> Complex a
conjugate (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k))
 	  r :: b
r   = forall a. Complex a -> a
realPart (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k     forall a. Num a => a -> a -> a
* forall a. Num a => Complex a -> Complex a
conjugate (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k))
 	  rp1 :: b
rp1 = forall a. Complex a -> a
realPart (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
+a
1) forall a. Num a => a -> a -> a
* forall a. Num a => Complex a -> Complex a
conjugate (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k))
	  d :: b
d = (forall a. Floating a => a -> a
sqrt (b
1 forall a. Num a => a -> a -> a
+ b
8 forall a. Num a => a -> a -> a
* b
gforall a. Num a => a -> Int -> a
^!Int
2) forall a. Num a => a -> a -> a
- b
1) forall a. Fractional a => a -> a -> a
/ b
4 forall a. Fractional a => a -> a -> a
/ b
g
 	  g :: b
g = (b
rm1 forall a. Num a => a -> a -> a
- b
rp1) forall a. Fractional a => a -> a -> a
/ (b
2 forall a. Num a => a -> a -> a
* b
r forall a. Num a => a -> a -> a
+ b
rm1 forall a. Num a => a -> a -> a
+ b
rp1)
 	  n :: a
n = forall a b. (a, b) -> b
snd (forall i e. Array i e -> (i, i)
bounds Array a (Complex b)
x) forall a. Num a => a -> a -> a
+ a
1

-- | MacLeod's Three Point Estimator

macleod5 :: (Ix a, Integral a, RealFloat b) => Array a (Complex b) -- ^ X[k]
       -> a -- ^ k
       -> b -- ^ w

macleod5 :: forall a b.
(Ix a, Integral a, RealFloat b) =>
Array a (Complex b) -> a -> b
macleod5 Array a (Complex b)
x a
k = b
2 forall a. Num a => a -> a -> a
* forall a. Floating a => a
pi forall a. Num a => a -> a -> a
* ((forall a b. (Integral a, Num b) => a -> b
fromIntegral a
k) forall a. Num a => a -> a -> a
+ b
d) forall a. Fractional a => a -> a -> a
/ (forall a b. (Integral a, Num b) => a -> b
fromIntegral a
n)
     where rm2 :: b
rm2 = forall a. Complex a -> a
realPart (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
-a
2) forall a. Num a => a -> a -> a
* forall a. Num a => Complex a -> Complex a
conjugate (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k))
	   rm1 :: b
rm1 = forall a. Complex a -> a
realPart (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
-a
1) forall a. Num a => a -> a -> a
* forall a. Num a => Complex a -> Complex a
conjugate (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k))
 	   r :: b
r   = forall a. Complex a -> a
realPart (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k     forall a. Num a => a -> a -> a
* forall a. Num a => Complex a -> Complex a
conjugate (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k))
 	   rp1 :: b
rp1 = forall a. Complex a -> a
realPart (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
+a
1) forall a. Num a => a -> a -> a
* forall a. Num a => Complex a -> Complex a
conjugate (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k))
 	   rp2 :: b
rp2 = forall a. Complex a -> a
realPart (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
+a
2) forall a. Num a => a -> a -> a
* forall a. Num a => Complex a -> Complex a
conjugate (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k))
	   d :: b
d = b
0.4041 forall a. Num a => a -> a -> a
* forall a. Floating a => a -> a
atan (b
2.93 forall a. Num a => a -> a -> a
* b
g)
 	   g :: b
g = (b
4 forall a. Num a => a -> a -> a
* (b
rm1 forall a. Num a => a -> a -> a
- b
rp1) forall a. Num a => a -> a -> a
+ b
2 forall a. Num a => a -> a -> a
* (b
rm2 forall a. Num a => a -> a -> a
- b
rp2)) forall a. Fractional a => a -> a -> a
/ (b
12 forall a. Num a => a -> a -> a
* b
r forall a. Num a => a -> a -> a
+ b
8 forall a. Num a => a -> a -> a
* (b
rm1 forall a. Num a => a -> a -> a
+ b
rp1) forall a. Num a => a -> a -> a
+ b
rm2 forall a. Num a => a -> a -> a
+ b
rp2)
 	   n :: a
n = forall a b. (a, b) -> b
snd (forall i e. Array i e -> (i, i)
bounds Array a (Complex b)
x) forall a. Num a => a -> a -> a
+ a
1

-- | Rife and Vincent's Estimator

rv :: (Ix a, Integral a, RealFloat b) => Array a (Complex b) -- ^ X[k]
       -> a -- ^ k
       -> b -- ^ w

rv :: forall a b.
(Ix a, Integral a, RealFloat b) =>
Array a (Complex b) -> a -> b
rv Array a (Complex b)
x a
k = b
2 forall a. Num a => a -> a -> a
* forall a. Floating a => a
pi forall a. Num a => a -> a -> a
* ((forall a b. (Integral a, Num b) => a -> b
fromIntegral a
k) forall a. Num a => a -> a -> a
+ b
d) forall a. Fractional a => a -> a -> a
/ (forall a b. (Integral a, Num b) => a -> b
fromIntegral a
n)
    where d :: b
d = forall a b. (Integral a, Num b) => a -> b
fromIntegral a
at forall a. Num a => a -> a -> a
* forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
+a
at) forall a. Fractional a => a -> a -> a
/ Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k) forall a. Fractional a => a -> a -> a
/ (b
1 forall a. Num a => a -> a -> a
+ forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
+a
at) forall a. Fractional a => a -> a -> a
/ Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!a
k))
	  at :: a
at | (forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
+a
1)))forall a. Num a => a -> Int -> a
^!Int
2 forall a. Ord a => a -> a -> Bool
> (forall a. RealFloat a => Complex a -> a
magnitude (Array a (Complex b)
xforall i e. Ix i => Array i e -> i -> e
!(a
kforall a. Num a => a -> a -> a
-a
1)))forall a. Num a => a -> Int -> a
^!Int
2 =  a
1
	     | Bool
otherwise                                         = -a
1
 	  n :: a
n = forall a b. (a, b) -> b
snd (forall i e. Array i e -> (i, i)
bounds Array a (Complex b)
x) forall a. Num a => a -> a -> a
+ a
1