// Kalman Filter  
/*

  (1)Kalman Filter       較舊 作者：Kristian Lauszus   https://www.arduino.cc/reference/en/libraries/kalman-filter-library/


  (2)SimpleKalmanFilter  較新 作者 denyssene https://github.com/denyssene/SimpleKalmanFilter

   e_mea：測量不確定度       我們對測量變化的期望有多大
   e_est：估計不確定性       可以使用與 e_mea 相同的值初始化，因為卡爾曼濾波器將調整其值。
   q：    過程方差-過程噪音   通常是 0.001 到 1 之間的一個小數字 - 您的測量移動的速度有多快。
                            推薦 0.01。應該根據您的需要進行調整。

  SimpleKalmanFilter kf = SimpleKalmanFilter(e_mea, e_est, q);

  while (1) {
    float x = analogRead(A0);
    float estimated_x = kf.updateEstimate(x);
  
    // ...
  } 


  (3)最新  作者 Romain Fétick    https://github.com/rfetick/Kalman

*/

#include <SimpleKalmanFilter.h>
#define PI 3.1415926535897932384626433832795

int e_mea = 2;      // 測量不確定度  遲後且波形振幅變小
int e_est = 2;      // 估計不確定性  不遲後，校正不明顯
float q   = 0.03;   //  過程噪音 越大則越接近原值。越小則修正越好 但遲後且波形振幅變小

SimpleKalmanFilter kf = SimpleKalmanFilter(e_mea, e_est, q);

const long SERIAL_REFRESH_TIME = 300;
long refresh_time;

int angle = 0;
int count = 0;
float real_value;
float angle_rad;



void setup() {
  Serial.begin(115200);
}

void loop() {

  // sin 波
  angle_rad = angle * PI / 180;
  real_value = 20 * sin(angle_rad);    

  // 斜波
  // real_value = angle;

  // 0波
  // real_value = 0;

  count++;
  angle++;

  if (angle > 360){
    angle = 0;
    count = 0;
  }

  
  //為參考值添加噪聲並用作測量值
  float measured_value = real_value + random (-7 , 7 ) ;

  //用卡爾曼濾波器計算估計值
  float estimated_value = kf.updateEstimate(measured_value);



  Serial.print(real_value);
  Serial.print("\t");
  // Serial.print(measured_value);
  // Serial.print("\t");
  Serial.print(estimated_value);
  Serial.println("\t");


  delay(10);

}