From 3641c14175fee4b60b7930fc6675efc280e26393 Mon Sep 17 00:00:00 2001
From: Thomas Pietrzak <thomas.pietrzak@gmail.com>
Date: Sun, 23 Sep 2018 14:41:32 +0200
Subject: [PATCH 1/1] One euro Caml

---
 oneeuro.ml    | 49 +++++++++++++++++++++++++++++++++++++++++++++++++
 oneeuro.mli   | 18 ++++++++++++++++++
 testfilter.ml | 44 ++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 111 insertions(+)
 create mode 100644 oneeuro.ml
 create mode 100644 oneeuro.mli
 create mode 100644 testfilter.ml

diff --git a/oneeuro.ml b/oneeuro.ml
new file mode 100644
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
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
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'';;
+
-- 
2.30.2