diff core/Kalman.cpp @ 149:134bff10e561 ClockSync

Added simple one-variable one-measurement Kalman filter, Pid controller(which output is not used). Virtual clock is now much more precise and reactive for period. Still it is lagging behind a bit on the overall offset.

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/core/Kalman.cpp	Mon Sep 21 03:12:21 2015 +0100
@@ -0,0 +1,24 @@
+/*
+ * Kalman.cpp
+ *
+ *  Created on: 20 Sep 2015
+ *      Author: giulio
+ */
+#include "Kalman.h"
+void KalmanOne::init(double newQ, double newR, double newX){
+	A = 1;
+	H = 1;
+	Q = newQ;  // covariance of the error on the prediction
+	R = newR; // covariance of the measurement error
+	x = newX; // 5805.09230769231; % predicted x
+	P = 6; // predicted covariance
+	return;
+}
+double KalmanOne:: process(double z){
+	double xp = A*x; // I. Prediction of the estimate
+	double Pp = A*P*A + Q; // Prediction of the error covariance
+	double K = Pp*H/(H*Pp*H + R); // II. Computation of Kalman gain
+	x = xp + K*(z - H*xp); // III. Computation of the estimate
+	P = Pp - K*H*Pp; // IV. Computation of the error covariance
+	return x;
+}