Python 卡爾曼濾波器實現

去年我們在設計一款新產品的時候，由於選用定製開發的乙個soc器件，導致我們在用adc讀取經由這個soc晶元放大後的訊號時，出現了極其不穩定的情況。正常情況下adc讀取出來的訊號應當為一條平穩的直線，而現實上讀取出來的訊號確上下波動極其大，遠遠超出了我們理論計算水平。

雖然後來通過大量的研究分析，得出時soc極其容易受到emi干擾，在新增遮蔽片後成功解決了這個問題，但是在研究過程中我們發現卡爾曼濾波器在處理這種干擾時有著媲美硬體濾波器的結果，還是感到非常驚訝。

下面介紹由andrew d. straw提供的基於python語言的卡爾曼濾波濾波器實現。

#coding:utf-8  

# python3.7
import numpy  
import pylab
def kalmanfilter(z,  n_iter = 20):  
#這裡是假設a=1，h=1的情況  
# intial parameters  
sz = (n_iter,) # size of array   
#q = 1e-5 # process variance  
q = 1e-6 # process variance   
# allocate space for arrays  
xhat=numpy.zeros(sz)      # a posteri estimate of x  
p=numpy.zeros(sz)         # a posteri error estimate  
xhatminus=numpy.zeros(sz) # a priori estimate of x  
pminus=numpy.zeros(sz)    # a priori error estimate  
k=numpy.zeros(sz)         # gain or blending factor  
r = 0.1**2 # estimate of measurement variance, change to see effect  
# intial guesses  
xhat[0] = 0.0  
p[0] = 1.0  
a = 1
h = 1
for k in range(1,n_iter):  
# time update  
xhatminus[k] = a * xhat[k-1]  #x(k|k-1) = ax(k-1|k-1) + bu(k) + w(k),a=1,bu(k) = 0  
pminus[k] = a * p[k-1]+q      #p(k|k-1) = ap(k-1|k-1)a' + q(k) ,a=1  
# measurement update  
k[k] = pminus[k]/( pminus[k]+r ) #kg(k)=p(k|k-1)h'/[hp(k|k-1)h' + r],h=1  
xhat[k] = xhatminus[k]+k[k]*(z[k]-h * xhatminus[k]) #x(k|k) = x(k|k-1) + kg(k)[z(k) - hx(k|k-1)], h=1  
p[k] = (1-k[k] * h) * pminus[k] #p(k|k) = (1 - kg(k)h)p(k|k-1), h=1  
return xhat
if __name__ == '__main__':
with open("raw_data.txt", "r", encoding="utf-8") as f:
text = f.readline().split(",")
print(text)
raw_data = list()
for x in text:
print(int(x))
xhat = kalmanfilter(raw_data, n_iter=len(raw_data))
pylab.plot(raw_data, 'k-', label='raw measurement')  # 測量值
pylab.plot(xhat, 'b-', label='kalman estimate')  # 過濾後的值
pylab.legend()
pylab.xlabel('iteration')
pylab.ylabel('adc reading')
pylab.show()

專案後來不用卡爾曼濾波器而大費周折採用成本更高的硬體濾波器，主要還是產品需要乙個快速響應輸出，而卡爾曼濾波器在這方面有點滯後了，如果對於效能要求不高的產品，這個濾波器是絕佳的利器。

卡爾曼濾波器

協方差 用於表示兩個變數的總體誤差，如果兩個變化趨勢一致則協方差為正值，變化趨勢不一致則為負值。從直觀上來看，協方差表示的是兩個變數總體誤差的期望。由k 1時刻的最優值和系統輸入計算k時刻的系統 值。根據k 1時刻的系統協方差 k時刻系統協方差。根據 k時刻 協方差矩陣的 值計算卡爾曼增益。根據狀態...

Python 實現 卡爾曼濾波器 非常簡單

整體思路很簡單，卡爾曼濾波器就是做資料融合的，先給乙個gps的資料 z 和乙個里程計資料 u 讓他們融合吧。usr bin env python3 coding utf 8 created on tue dec 18 19 37 13 2018 author sc args explanition ...

卡爾曼濾波器原理和matlab實現

以其中的一篇參考資料為例 卡爾曼濾波器的遞迴過程 估計時刻k 的狀態 x k ax k 1 bu k 這裡,u k 是系統輸入，在專案中，一維輸入訊號a 1 計算誤差相關矩陣p,度量估計值的精確程度 p k a p k 1 a q 這裡,q e 是系統雜訊的協方差陣，即系統框圖中的wj的協方差陣,q...