One euro Caml

diff --git a/oneeuro.ml b/oneeuro.ml
new file mode 100644 (file)
index 0000000..feed28f
--- /dev/null
+++ b/oneeuro.ml
@@ -0,0 +1,49 @@
+type lowPassFilter = { mutable firsttimelp: bool ; mutable hatxprev: float };;
+
+type oneEuroFilter = { mutable firsttimeoe: bool;
+       rate: float;
+       mincutoff: float;
+       beta: float;
+       xfilt: lowPassFilter;
+       dcutoff: float;
+       dxfilt: lowPassFilter };;
+       
+let pi = 3.1415926;;
+
+let alpha freq cutoff =
+       let tau = 1.0 /. (2. *. pi *. cutoff)
+       and te = 1.0 /. freq in
+       1.0 /. (1.0 +. tau /. te);;
+       
+let lowpassfilter filter x alpha =
+       if filter.firsttimelp then
+       begin
+               filter.firsttimelp <- false;
+               filter.hatxprev <- x
+       end;
+       let hatx = alpha *. x +. (1.0 -. alpha) *. filter.hatxprev in
+       filter.hatxprev <- hatx;
+       hatx;;
+       
+let oneeurofilter filter x =
+       let dx = 
+       if (filter.firsttimeoe) then
+       begin
+               filter.firsttimeoe <- false;
+               0.0
+       end
+       else
+               (x -. filter.xfilt.hatxprev) *. filter.rate
+       in
+       let edx = lowpassfilter filter.dxfilt dx (alpha filter.rate filter.dcutoff) in
+       let cutoff = filter.mincutoff +. filter.beta *. abs_float edx in
+       lowpassfilter filter.xfilt x (alpha filter.rate cutoff);;
+
+let createfilter freq mincutoff        beta dcutoff = { 
+       firsttimeoe = true;
+       rate = freq;
+       mincutoff = mincutoff;
+       beta = beta;
+       xfilt = { firsttimelp = true ; hatxprev = 0.0 };
+       dcutoff = dcutoff;
+       dxfilt = { firsttimelp = true ; hatxprev = 0.0 } }

diff --git a/oneeuro.mli b/oneeuro.mli
new file mode 100644 (file)
index 0000000..83420d8
--- /dev/null
+++ b/oneeuro.mli
@@ -0,0 +1,18 @@
+type oneEuroFilter;;
+
+(*
+       oneeurofilter filter value
+       returns the value fintered with the given filter
+*)
+val oneeurofilter: oneEuroFilter -> float -> float;;
+
+(*
+       createfilter freq mincutoff beta dcutoff
+       
+       Creates a filter with these parameters:
+       freq: 
+       mincutoff       
+       beta 
+       dcutoff
+*)
+val createfilter: float -> float -> float -> float -> oneEuroFilter;;
\ No newline at end of file

diff --git a/testfilter.ml b/testfilter.ml
new file mode 100644 (file)
index 0000000..d82024c
--- /dev/null
+++ b/testfilter.ml
@@ -0,0 +1,44 @@
+#use "oneeuro.ml";;
+open Graphics;;
+
+let parabole a b c = 
+  let rec tr = function
+    | 0 -> []
+    | n -> let y = a *. float_of_int(n * n) +. b *. float_of_int(n) +. c in
+             y :: (tr (n - 1))
+  in tr;;
+
+let drawlist l =
+       let nb = List.length l in
+       let rec aux n = function
+       | [] -> ()
+       | x :: l' -> if (x >= 0.0 && x < 600.0) then
+                       plot n (int_of_float(x));
+               aux (n - 1) l'
+       in aux nb l;;
+
+let rec noisify range = function
+       | [] -> []
+       | x :: l -> (x +. Random.float range -. range /. 2.0) :: noisify range l;;
+
+let rec filtered filter = function
+       | [] -> []
+       | x :: l -> (oneeurofilter filter x) :: (filtered filter l);;
+
+let p = parabole (-.0.002) 1.0 400.0 800;;
+let p' = noisify 50.0 p;;
+let f = createfilter 100.0 1.0 0.0 1.0;;
+let p'' = filtered f p';;
+
+open_graph " 800x600";;
+clear_graph();;        
+
+set_color black;;
+drawlist p;;
+
+set_color red;;
+drawlist p';;
+
+set_color blue;;
+drawlist p'';;
+