double compfilter( double angleout , double newangle , double newgyro , double dt ,float tau )
* dt：兩次取樣時間間隔
* tau：反應慣性濾波的係數
* angleout：上次角度輸出值
* newangle：角速度計得到的角度值
* newgyro：陀螺儀的角速度值
* 輸出
* 互補濾波計算得到的角度值
先驗**方差
kalman增益矩陣的計算
增益矩陣
後驗**值
後驗**方差
//之前需要定義的變數
float q_angle = 0.01;
float q_gyro = 0.0003;
float r_angle = 0.01;
float x_bias = 0;
float p_00 = 0, p_01 = 0, p_10 = 0, p_11 = 0;
float y, s; float k_0, k_1;
double kalmanfilter(double angleout , double newangle, double newrate,double dt)
濾波演算法總結
1 限幅濾波法 又稱程式判斷濾波法 a 方法 根據經驗判斷，確定兩次取樣允許的最大偏差值 設為a 每次檢測到新值時判斷 如果本次值與上次值之差 a,則本次值有效 如果本次值與上次值之差 a,則本次值無效,放棄本次值,用上次值代替本次值 b 優點 能有效克服因偶然因素引起的脈衝干擾 c 缺點 無法抑制...
幾種濾波演算法，總結一下
最近在做武術擂台，發現對於紅外測距感測器的返回值速度很快，但是誤差值很大，經過簡單函式調校之後，發現還是有誤差，有干擾資料，於是匯入了math.h，進行的絕對值濾波，但是用迴圈暫存了十組資料，進行簡單的加權演算法，發現還是不行，於是去找了一些經典的濾波演算法，算是簡單記錄一下。分享給大家。1 限幅濾...
濾波演算法 遞推平均濾波法
基本方法 採用佇列作為測量資料儲存器,設佇列的長度為n 每進行一次測量,把測量結果放於隊尾,而扔掉原來隊首的乙個資料,這樣在佇列中始終就有n個 最新 的資料。當計算平均值時,只要把佇列中的n個資料進行算數平均,就可得到新的算數平均值。這樣每進行一次測量,就可得到乙個新的算術平均值。其程式如下所示 i...