Diff: driver_mbed_KalmanFilter.cpp

Revision:

0:2ee0bc82e7b8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/driver_mbed_KalmanFilter.cpp	Wed Mar 29 03:09:38 2017 +0000
@@ -0,0 +1,93 @@
+/*
+ * 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 <driver_mbed_KalmanFilter.h>
+// #include <inttypes.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+AnalogIn ain(D0);
+/* 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)ain.read());
+	/* 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;
+}