卡爾曼濾波學習及python仿真

在這裏我就不介紹卡爾曼的數學推算了，網上的數學推導一抓一大把，如果想了解推導過程的小夥伴可以去大佬的博客。如果你是想直接簡單運用卡爾曼濾波來處理mpu6050的數據，或者是處理ADC的數據，那麼我希望這篇筆記可以幫助到你。

卡爾曼濾波（Kalman filtering）是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。

卡爾曼濾波簡介：你可能經常聽學長學姐提起這個算法，你是否會覺得它很普通？但實際上它是一種非常強大的算法，早在1969年，NASA（美國宇航局）就將它運用在飛船軌跡預測上了。卡爾曼濾波算法就是這樣偉大而平凡，即爲人類文明發展做出巨大貢獻，也爲普通開發者提供智囊，讓我們一起感謝卡爾曼先生吧。

卡爾曼濾波的基本作用：

1. 去除噪聲（噪聲是除有用信號以外的一切不需要的信號的總稱）
2. 進行最優估計
3. 儘可能的還原真實值

舉個栗子：比如你讀陀螺儀的角度數據，我們知道陀螺儀有時會存在數據漂移的情況，它實際的角度是60°，但是返回的角度數據卻是90°，這時用卡爾曼濾波後，可能返回的數據不是90°了而是62°或者是65°，向真實值靠近。

卡爾曼濾波最重要的是它的5大公式：

從上至下爲公式1-5
含義解釋：
還不會數學公式編輯，下次一定，猛哭。
x(k)^- ：k時刻系統的狀態值
x(k)^ ：k時刻的最優估計值
u(k) ：外力干預
A,B : 各部分所佔權重，和爲1
P(k)-：k時刻與k-1時刻的狀態誤差
P(k)：k時刻與k-1時刻誤差的協方差
Q：狀態的協方差
Kk：k時刻卡爾曼增益
H：狀態變換矩陣，有單位不統一時使用
R：測量時噪聲的協方差
最後一個公式裏面不是字母I是數字1

公式解釋：

1. 公式一：預測系統狀態值
2. 公式二：預測新的誤差
3. 公式三：計算卡爾曼增益
4. 公式四：進行更新校正，得到該時刻最優值
5. 公式五：更新誤差協方差，爲下一時刻預測誤差做準備

利用公式進行濾波的步驟：

1. 預測的數據值
2. 測量的數據值
3. 卡爾曼增益
4. 最終估算的數據值

最後來看一下python仿真：

import numpy as np
import matplotlib.pyplot as plt

Q = 0.00001    #噪聲的協方差，也可以理解爲兩個時刻之間噪聲的偏差
R = 0.1        #狀態的協方差，可以理解爲兩個時刻之間狀態的偏差
P_k_k1 = 1     #上一時刻狀態協方差
Kg = 0         #卡爾曼增益
P_k1_k1 = 1
x_k_k1 = 0     #上一時刻狀態值
ADC_OLD_Value = 0    #上一次的ADC值
kalman_adc_old = 0   #上一次的卡爾曼濾波的到的最優估計值

def kalman(ADC_Value):
    global kalman_adc_old   #定義兩個全局變量，可以在程序中直接改變其中的值
    global P_k1_k1
    Z_k = ADC_Value          #測量值
    
    if (abs(kalman_adc_old-ADC_Value)>=80):      #上一狀態值與此刻測量值差距過大，進行簡單的一階濾波，0618黃金比例可以隨意定哦
        x_k1_k1= ADC_Value*0.382 + kalman_adc_old*0.618
    else:                 #差距不大直接使用
        x_k1_k1 = kalman_adc_old;

    x_k_k1 = x_k1_k1      #測量值
    P_k_k1 = P_k1_k1 + Q      #公式二
    Kg = P_k_k1/(P_k_k1 + R)  #公式三

    kalman_adc = x_k_k1 + Kg * (Z_k - kalman_adc_old)    #計算最優估計值
    P_k1_k1 = (1 - Kg)*P_k_k1  #公式五
    P_k_k1 = P_k1_k1     #更新狀態協方差

    ADC_OLD_Value = ADC_Value   
    kalman_adc_old = kalman_adc
    return kalman_adc


plt.figure(figsize = (20,8))    #創建畫布大小爲（20,8）
a= [100]*200    #生成包含200個值爲100的列表
array = np.array(a)

s = np.random.normal(0, 25, 200)     #生成200個符合正太分佈的隨機數

test_array = array + s
plt.plot(test_array,label = 'Noise')      #畫出干擾噪聲
adc=[]
for i in range(200):
    adc.append(kalman(test_array[i]))


plt.plot(adc,label = 'Kalman')     #卡爾曼濾波後的圖像
plt.plot(array,label = 'Original')    #理想效果曲線
plt.legend()   #顯示圖例
plt.show()

站在巨人的肩膀上讓我能快速瞭解卡爾曼濾波算法，感謝幾位前輩給我的參考，這幾篇博客都是深度好文！！！
參考博客：
利用Python實現卡爾曼濾波算法：https://blog.csdn.net/moge19/article/details/82531119
通俗理解卡爾曼濾波及其算法實現（實例解析）：
https://blog.csdn.net/tiandijun/article/details/72469471
卡爾曼濾波器的公式推導及示例代碼編寫：
https://gitchat.csdn.net/activity/5d37aeb1b5590f6f299a9cdc