view 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.

/*
 * 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;
}