view core/Kalman.cpp @ 151:e9c9404e3d1f ClockSync

Pff partially working. No PID. When setting the audio clock on the bbb to 44098 the master and slave clock keep diverging instead of converging ...

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