LSM330DLC傳感器問題

sam183132

我使用的是LSM330DLC 這顆sensor，而我使用其陀螺儀吐出的角速度，做積分算出其旋轉角度!!我使用的方式是

//dt 為每個角速度的間隔時間(週期)
//gyro 是其累加的角速度數值
angle = angle +gyro *dt ;   //dt 為每個角速度的間隔時間(週期), gyro 是其累加的角速度數值

使用這樣離散的方式做累加(積分)，所產生的角度會有其很飄移與偏差!!

我的實驗是使用sensor在水平時可以使用gyroscope來計算其旋轉角，我找到的 kalman filter  和 Complementary filter 來做filter 希望可以減少其飄移與誤差，但是我發現此兩種方法都需要加速度計算出的傾斜角來幫作輔助運算，而且加速度計算出的傾斜角在水平時無法準確的運算(X軸加速度傾斜角的參考值為加速度X軸與Y軸，且水平時X軸與Y軸的數值會在 0 上下浮動，故無法真正計算其正確的傾角)，所以我想說可以使用high pass filter來幫助運算(就是過濾低頻的訊號，使高頻的信號通過)，可使發現我使用的High-pass filter 跟預期的不一樣(發現反而是我寫)，所以想問各位高手，我使用的High-pass filter 使否正確??或是有其他的方法可以減少其陀螺儀的飄移與誤差??


Complementary filter :


//CF_gyro Complementary filter之後的數值
//gyro 累加的加速度值
//ACC_angle  用加速度計算的傾斜角


CF_gyro = 0.98 * (CF_gyro(Previous) + (gyro * 0.023)) + (0.02 * ACC_angle);


low-pass filter:
//LFg_2 low-pass filter之後的數值
//angle 輸入的角度值
LFg_2 = ((angle * 0.1) + (LFg_2 * 0.9));   

High-pass filter
//HFg_2 High-pass filter之後的數值
//angle 輸入的角度值

HFg_2 = angle - LFg_2;                              

sam183132

抱歉上面有一段我沒有寫完，再次補充:
發現我使用的High-pass filter 跟預期的不一樣(發現我寫的low-pass filter比較接近high-pass filter)，所以才想問一下各位高手，是否我寫的high-pass filter寫錯?那有怎麼做修改?謝謝

vegewell

回復 2# sam183132

高通濾波器只是輸入減去低通濾波器的輸出。
我看你的高通濾波器還好,
反倒是低通濾波器有問題,
low-pass filter: example:
output = (1 -a) * input + a * previous_output, //0 < a < 1

sam183132

謝謝回復~!我會先試試看這個公式~感謝!^^