1 files changed, 63 insertions, 0 deletions
diff --git a/lib/kalman_filter.c b/lib/kalman_filter.c
new file mode 100644
index 0000000..f7adf6f
--- /dev/null
+++ b/lib/kalman_filter.c
@@ -0,0 +1,63 @@
+// Kalman Filter
+//
+// kalman_filter
+//
+// Kalman Filter
+//
+// Optimal state estimation via predict-update cycle
+//
+// Inputs:    1
+// Outputs:   1
+// Constants: 2
+
+
+#include <math.h>
+#include "ts.h"
+
+typedef struct {
+    fp_t Q;
+    fp_t R;
+} ts_kalman_filter_params_t;
+
+ts_parameter_t const ts_kalman_filter_p = { .n_inputs = 1, .n_outputs = 1, .n_constants = 2, .params_size = sizeof(ts_kalman_filter_params_t) };
+
+uint_t ts_kalman_filter_offset( ts_constant_t const *c )
+{
+	( void )c;
+	return ( 0 );
+}
+
+int ts_kalman_filter(
+	fp_t * const *y, uint_t y_n,
+	fp_t const * const *x, uint_t x_n,
+	uint_t n, ts_constant_t const *c
+)
+{
+	TS_PROLOGUE( ts_kalman_filter_p );
+	if ( !c ) return -1;
+	TS_CHECK_INPUT(x, x_n, n);
+    fp_t Q = ((ts_kalman_filter_params_t *)c)->Q;
+    fp_t R = ((ts_kalman_filter_params_t *)c)->R;
+
+    if (Q < 0 || R < 0) return -1;
+    if (isnan(Q) || isnan(R)) return -1;
+    if (n == 0) return -1;
+    fp_t x_est = x[0][0];
+    /* NOTE: P=1.0 is a reasonable default for normalized data.
+     * For price data in the range of 100-3000, consider initializing
+     * P to the variance of the first few samples. */
+    fp_t P = 1.0;
+
+    y[0][0] = x_est;
+    for ( uint_t i = 1; i < n; ++i )
+    {
+        P = P + Q;
+        fp_t denom = P + R;
+        fp_t K = denom != 0 ? P / denom : 0;
+        x_est = x_est + K * (x[0][i] - x_est);
+        P = (1 - K) * P;
+        y[0][i] = x_est;
+    }
+	return 0;
+}
+