giuliomoro@149: /*
giuliomoro@149:  * Kalman.cpp
giuliomoro@149:  *
giuliomoro@149:  *  Created on: 20 Sep 2015
giuliomoro@149:  *      Author: giulio
giuliomoro@149:  */
giuliomoro@149: #include "Kalman.h"
giuliomoro@149: void KalmanOne::init(double newQ, double newR, double newX){
giuliomoro@149: 	A = 1;
giuliomoro@149: 	H = 1;
giuliomoro@149: 	Q = newQ;  // covariance of the error on the prediction
giuliomoro@149: 	R = newR; // covariance of the measurement error
giuliomoro@149: 	x = newX; // 5805.09230769231; % predicted x
giuliomoro@149: 	P = 6; // predicted covariance
giuliomoro@149: 	return;
giuliomoro@149: }
giuliomoro@149: double KalmanOne:: process(double z){
giuliomoro@149: 	double xp = A*x; // I. Prediction of the estimate
giuliomoro@149: 	double Pp = A*P*A + Q; // Prediction of the error covariance
giuliomoro@149: 	double K = Pp*H/(H*Pp*H + R); // II. Computation of Kalman gain
giuliomoro@149: 	x = xp + K*(z - H*xp); // III. Computation of the estimate
giuliomoro@149: 	P = Pp - K*H*Pp; // IV. Computation of the error covariance
giuliomoro@149: 	return x;
giuliomoro@149: }