Driver for HP20X sensor
Dependents: 8-0_OneNet_IoT_demo
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; +}