-----------------------------------------------------------------------------
-- |
-- Module      :  DSP.Estimation.Frequency.QuinnFernandes
-- Copyright   :  (c) Matthew Donadio 2003
-- License     :  GPL
--
-- Maintainer  :  m.p.donadio@ieee.org
-- Stability   :  experimental
-- Portability :  portable
--
-- This is an implementation of the Quinn-Fernandes algorithm for
-- estimating the frequency of a real sinusoid in noise.
--
-----------------------------------------------------------------------------

module DSP.Estimation.Frequency.QuinnFernandes (qf) where

import Data.Array

-- | The Quinn-Fernandes algorithm

qf :: (Ix a, Integral a, RealFloat b) => Array a b -- ^ y
      -> b -- ^ initial w estimate
      -> b -- ^ w

qf :: forall a b. (Ix a, Integral a, RealFloat b) => Array a b -> b -> b
qf Array a b
y b
w = forall a b. (Ix a, Integral a, RealFloat b) => Array a b -> b -> b
qf' Array a b
y (b
2 forall a. Num a => a -> a -> a
* forall a. Floating a => a -> a
cos b
w)

qf' :: (Ix a, Integral a, RealFloat b) =>
       Array a b
    -> b
    -> b
qf' :: forall a b. (Ix a, Integral a, RealFloat b) => Array a b -> b -> b
qf' Array a b
y b
a | forall a. Num a => a -> a
abs (b
aforall a. Num a => a -> a -> a
-b
b) forall a. Ord a => a -> a -> Bool
< b
eps = forall a. Floating a => a -> a
acos(b
0.5 forall a. Num a => a -> a -> a
* b
b)
	| Bool
otherwise       = forall a b. (Ix a, Integral a, RealFloat b) => Array a b -> b -> b
qf' Array a b
y b
b
    where z :: Array a b
z = forall i e. Ix i => (i, i) -> [(i, e)] -> Array i e
array (-a
2,a
nforall a. Num a => a -> a -> a
-a
1) ([ (-a
2, b
0), (-a
1, b
0) ] forall a. [a] -> [a] -> [a]
++ [ (a
i, Array a b
yforall i e. Ix i => Array i e -> i -> e
!a
i forall a. Num a => a -> a -> a
+ b
a forall a. Num a => a -> a -> a
* Array a b
zforall i e. Ix i => Array i e -> i -> e
!(a
iforall a. Num a => a -> a -> a
-a
1) forall a. Num a => a -> a -> a
- Array a b
zforall i e. Ix i => Array i e -> i -> e
!(a
iforall a. Num a => a -> a -> a
-a
2)) | a
i <- [a
0..(a
nforall a. Num a => a -> a -> a
-a
1)] ])
	  b :: b
b = forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum [ (Array a b
zforall i e. Ix i => Array i e -> i -> e
!a
i forall a. Num a => a -> a -> a
+ Array a b
zforall i e. Ix i => Array i e -> i -> e
!(a
iforall a. Num a => a -> a -> a
-a
2)) forall a. Num a => a -> a -> a
* Array a b
zforall i e. Ix i => Array i e -> i -> e
!(a
iforall a. Num a => a -> a -> a
-a
1) | a
i <- [a
0..(a
nforall a. Num a => a -> a -> a
-a
1)] ] forall a. Fractional a => a -> a -> a
/ forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum [ (Array a b
zforall i e. Ix i => Array i e -> i -> e
!(a
iforall a. Num a => a -> a -> a
-a
1))forall a b. (Num a, Integral b) => a -> b -> a
^(Int
2::Int) | a
i <- [a
0..(a
nforall a. Num a => a -> a -> a
-a
1)] ]
	  eps :: b
eps = b
1.0e-6
	  n :: a
n = forall a b. (a, b) -> b
snd (forall i e. Array i e -> (i, i)
bounds Array a b
y) forall a. Num a => a -> a -> a
+ a
1