卡爾曼濾波python及陀螺儀例子
阿新 • • 發佈：2018-12-13
卡爾曼濾波在處理訊號的噪聲方面及其有用，最近看到一篇部落格，講解的通俗易懂，就不重複闡述了。附上個地址：卡爾曼濾波，通俗易懂
import matplotlib.pyplot as plt import numpy as np #建立噪聲 NUM = 300 np.random.seed(4) Noise= np.random.randn(NUM) #高斯分佈隨機噪聲 Noise_std = np.random.randn(NUM) #測量噪聲 X = [0]*NUM Y = [0]*NUM #建立資料集 for i in range(1,NUM): X[i] = np.sin(0.2*i) Noise_std_ = np.square(np.var(Noise_std)) #求方差 Noise_ = np.square(np.var(Noise)) P = [0]* NUM #每次的最優偏差 K = [0]* NUM #卡爾曼增益 S = X + Noise_std #測量值 for i in range(1,NUM): P[i] = np.square(P[i-1]) + 0.1*Noise_ K[i] = 0.1*np.sqrt( P[i]/( Noise_std_ + P[i])) Y[i] = Y[i-1] + K[i] * (S[i] - Y[i-1]) P[i] = np.sqrt((1-K[i])*P[i]) print(P[i]) plt.plot(X,color = 'r',label = '資料集') plt.plot(S,color = 'g',label = '資料測量獲取值（帶噪聲）') plt.plot(Y,color = 'b',label = '過濾後的資料') plt.show()
一般的對於我們嵌入式來說，在處理陀螺儀資料上面比較經典，有很多陀螺儀雖然都是自帶卡爾曼濾波處理，但是仍然值得學習嘗試處理。
#include "math.h" //在穩定情況下采集陀螺儀資料 //測定其噪聲的均值 float getNoiseGyro() { short i = 20; float temp = 0; for (; i > 0;i--) { temp+=GetXW(); //獲取單個軸的角速度 } temp /= i; return temp; } float XW[5] = {0}; short count = 0; float P = 0; float Noise = 0; //Noise = getNoiseGyro() float XA_ = 0; float getGyroSW() { //獲取的是濾波後的角速度 short i = 0; float temp = 0; float P1,K; count++; count %= 5; XW[count] = GetXW(); //跟新新增 for (; i < 5;i++) { //求取本狀態的穩定值，預測量 temp += XW[i]; } temp /= 5; P1 = P * P + 0.002; //0.002為角速度的閾值誤差 K = 0.1*P1 / (P1 + pow(Noise,2) ); //計算增益 XA_ = XA_ + K * (temp - XA_); P = sqrt((1-K) * P1 ); //跟新偏差 return XA_; }
以上就是一個簡單嘗試。