/*
 * File name  : KalmanFilter.cpp
 * Description: Kalman Filter class 
 * Author     : Oliver Wang from Seeed studio
 * Version    : V0.1
 * Create Time: 2014/04
 * Change Log :
*/

/****************************************************************************/
/***        Include files                                                 ***/
/****************************************************************************/
#include <Arduino.h>
#include <KalmanFilter.h>
#include <inttypes.h>
#include <stdlib.h>
#include <stdio.h>

/* random number table */
float Rand_Table[100]={
0.5377,1.8339,-2.2588,0.8622,0.3188,-1.3077,-0.4336,0.342,3.5784, 
2.7694,-1.3499,3.0349,0.7254,-0.0631,0.7147,-0.2050,-0.1241,1.4897, 
1.4090,1.4172,0.6715,-1.2075,0.7172,1.6302,0.4889,1.0347,0.7269, 
-0.3034,0.2939,-0.7873,0.8884,-1.1471,-1.0689,-0.8095,-2.9443,1.4384, 
0.3252,-0.7549,1.3703,-1.7115,-0.1022,-0.2414,0.3192,0.3129,-0.8649, 
-0.0301,-0.1649,0.6277,1.0933,1.1093,-0.8637,0.0774,-1.2141,-1.1135, 
-0.0068,1.5326,-0.7697,0.3714,-0.2256,1.1174,-1.0891,0.0326,0.5525, 
1.1006,1.5442,0.0859,-1.4916,-0.7423,-1.0616,2.3505,-0.6156,0.7481, 
-0.1924,0.8886,-0.7648,-1.4023,-1.4224,0.4882,-0.1774,-0.1961,1.4193, 
0.2916,0.1978,1.5877,-0.8045,0.6966,0.8351,-0.2437,0.2157,-1.1658, 
-1.1480,0.1049,0.7223,2.5855,-0.6669,0.1873,-0.0825,-1.9330,-0.439, 
-1.7947};

/* Extern variables */
KalmanFilter kalmanFilter;

KalmanFilter::KalmanFilter()
{
    X_pre = 0;
	P_pre = 0;	 
	X_post = 0;
	P_post = 0;
	K_cur = 0;
}

float KalmanFilter::Gaussian_Noise_Cov(void)
{
    int index = 0;
	float tmp[10]={0.0};
	float average = 0.0;
	float sum = 0.0;
	/* Initialize random number generator */
	srand((int)analogRead(0));
	
	/* Get random number */
	for(int i=0; i<10; i++)
	{
	    index = (int)rand()%100;
        tmp[i] = Rand_Table[index];
        sum += tmp[i];		
	}
	
	/* Calculate average */
	average = sum/10;
	
	/* Calculate Variance */
	float Variance = 0.0;		
	for(int j = 0; j < 10; j++)
	{
	    Variance += (tmp[j]-average)*(tmp[j]-average);
	}
	Variance/=10.0;
	
	return Variance;
}

float KalmanFilter::Filter(float origin)
{
    float modelNoise = 0.0;
	float observeNoise = 0.0;
	
	/* Get model and observe Noise */
	modelNoise = Gaussian_Noise_Cov();
	observeNoise = Gaussian_Noise_Cov();
	
	/* Algorithm */
	X_pre = X_post;
	P_pre = P_post + modelNoise;
	K_cur = P_pre/(P_pre + observeNoise);
	P_post = (1 - K_cur)*P_pre;
	X_post = X_pre + K_cur*(origin - X_pre);
	
	return X_post;
}