如何用卡爾曼濾波演算法求解電池SOC（基礎篇）

匈牙利數學家Rudolf Emil Kalman在NASA研究中心訪問時發現他提出遞推濾波演算法有助於提高航天領域對軌道預測的精度。現在在航天、通信、導航、控制、圖像處理等計算機技術應用上都少不了對卡爾曼濾波演算法（Kalman Filter）的應用。在新能源汽車動力電池的設計中採用卡爾曼濾波演算法進行SOC計算也成了比較主流的研究領域。不過我發現目前在網上查閱到的論文、書籍凡是涉及到了這部分內容不是晦澀難懂，就是乾脆一筆帶過，所以利用周末時間寫這篇文章就是想用一種易懂的方式解釋卡爾曼濾波演算法（以下簡稱KF演算法）。

1. 解決什麼問題

如果從哲學的角度理解，我覺得KF演算法本質上是解決信任度的問題。我們人類每天獲取海量的信息，每時每刻都在思考我該相信什麼，該做什麼，並在行動後獲得新的反饋。計算機也和我們一樣，面對相同的困惑。RAM里還保留著上一秒「可疑」的運算的結果，執行器卻自信地反饋「他已經有效完成了規定動作」，此時感測器氣喘吁吁的跑來報告「我們已經偏離了預定軌道」。這麼多答案放在計算機的面前，to be or not to be: that is the question。

解決信任問題有什麼好方法么？一般首先想到的會是基於大數據的演算法。比如google機器翻譯技術就是通過抓取大量網頁內容建立龐大的統計學模型從而使得這個產品僅僅開發兩年就一舉超過眾多老牌翻譯系統，解決了機器對上下文語境的有效翻譯。其實機器並非理解了整段文字，而是根據概率分析出了最優的解釋。但是在控制系統上的處理器往往不具備高速聯網、大容量存儲、大規模計算的能力，同時對計算的時效性和可靠性又有著極高的要求。因此需要採用一種簡單高效的演算法排除干擾，消除誤差。如平均值濾波、中位值濾波、一階滯後濾波等都是常見的方法。但這些演算法運算較為機械，對複雜的控制問題難以得到理想的效果。KF演算法則通過持續的遞推運算逐漸趨近「真相」，非常適合嵌入式控制系統演算法的應用。

2. 含義理解

我看過解釋得最通俗易懂的例子是Ramsey Faragher在2012年發表於Signal Processing Magazine上的一篇文章。以小車運動定位為模型，通過幾張圖片就表達了KF演算法的含義。

圖1中有一輛停在A點（初始點）的小車準備向前行駛，經過T秒後處於B點。小車上裝有速度感測器，因此理論上我們可以根據A點的坐標，計算出B點的坐標。但速度感測器必然存在一定的誤差，導致B點坐標並非一個確定的點，而是符合正態分布的坐標可能性。

圖2中出現了一個小車定位裝置，根據定位裝置的評估得到了C點坐標。同樣定位裝置也存在誤差，C點坐標的可能性也滿足正態分布。此時我們得到了兩個小車坐標，應該信任哪一個呢？

圖3中顯示了利用B點和C點兩處的正態分布求出了小車實際上更有可能所處的D點坐標，並且D點符合方差（σ）更小的正態分布。同時在後續的計算中D點坐標作為下一次計算中的初始點（A點）。通過不斷的迭代向更為準確的坐標逼近。

3.公式的初級應用

卡爾曼濾波演算法的5項公式如下，其中1式和2式為時間更新方程，意為根據上一次計算的結果推算單位時間後的情況（即(u1, σ1^2)變化為(u2, σ2^2)的過程）。

先驗估算值X: Xp(t) = A*X(t-1) + B*U(t) + W(t)

（1） 說明：W(t)為過程雜訊

先驗協方差P: Pp(t) = A*P(t-1)*A』+ Q(t)

（2） 說明：Q(t)為過程雜訊的協方差

3式、4式、5式為測量更新方程，意為根據實際測量情況分別求出信任權重、最優估計值、最新協方差（即求出(u4, σ4^2））。

卡爾曼增益Kg: Kg(t) = Pp(t)*H』/ (H*Pp(t)*H』+ R(t))

（3） 說明：R(t)為測量雜訊的協方差

最優估計值X: Xo(t) = Xp(t) + Kg(t)(Z(t) - H*Xp(t))

（4） 說明：Z(t)為實際測量值: Z(t) = H*X(t) + V(t)

最新協方差P: Po(t) = ( I - Kg(t) *H) * Pp(t)

（5）

大多數介紹KF演算法的文章難於理解的關鍵在於沒有將公式對應到具體的物理意義上。如果我們拿前面提到的小車運動案例來分析，並將計算值與其中的物理量明確對應起來則明了許多。同時H該案例為單模型（即僅有位置坐標），因此狀態轉移矩陣A，輸入矩陣B都為1。並且測量方式為直接獲得位置坐標而無需公式轉換，所以觀測矩陣H也為1。

公式1的物理意義：B點坐標的期望（u2）。根據A點坐標（u1）、位移計算（B*U(t)）、位移計算誤差（W(t)）求出。

公式2的物理意義：B點誤差的協方差（σ2^2）。根據A點坐標的協方差（σ1^2）、位移計算誤差的協方差（Q(t)）求出。

公式3的物理意義：B點與C點的信任權重，該權重使（σ4^2）最小。根據B點誤差的協方差（σ2^2）和實際測量誤差的協方差（R(t) ）求出。

公式4的物理意義：A』點坐標的期望（u4）。根據B點坐標的期望（u2）、權重係數（Kg(t)）、實際測量值（u3）求出。

公式4的物理意義：A』點誤差的協方差（σ4^2）。根據權重係數（Kg(t)）、B點坐標的協方差（σ2^2）求出。我們可以直觀的這麼理解，若Kg越大，則說明此次預測值更可信，所以Po越小。

接下去我們可以做一道應用題。設小車以勻速10m/s的速度行駛，經過A點（u1=0,σ1^2=1），1秒後到達A』點，此時感測器測得該點位置為11。已知小車位移計算誤差的協方差Q(t)為0.5，實際測量誤差的協方差R(t) =1。求A』點的最優估計坐標。

首先需要求出B點的信息：

Xp(t) = u2 = u1 + U(t) = 0 + 10 = 10

Pp(t) = σ2^2 = σ1^2 + Q(t) = 1 + 0.5 = 1.5

同時根據實際觀測數據我們也知道了C點信息：u3 = 11

然後求信任權重Kg：Kg = σ2^2 / ( σ2^2 + R(t) ) = 1.5 /(1.5 + 1) = 0.6

這樣我們就可以得到最優估計A』點坐標期望：Xo(t) = u4 = u2 + Kg（u3 - u2）=10 + 0.6*（11 - 10）=10.6

但為了下一次迭代計算還需要知道A』誤差協方差：Po(t) = σ4^2 = （1 - Kg）* σ2^2 = （1 - 0.6）* 1.5 =0.6

由此我們也可以發現A』是比A更「瘦」的正態分布，因此通過持續的迭代，坐標期望u將逐漸趨近真實值。

看到這裡讀者可能有一個疑問，Q(t)和R(t)通常是怎麼得到的？過程雜訊協方差Q(t)和測量雜訊協方差R(t)往往比較難以獲取。在實際應用中可以通過試驗進行標定。從便於理解的角度看可以認為若是相信預測結果則Q(t)較小，若相信測量結果則R(t)較小。

在實際應用中能把KF演算法用好的關鍵其實就在於標定調參上，如同頂級賽車的性能關鍵取決於經驗豐富的標定工程師的悉心調校。未來汽車工業在硬體上將逐漸走向全球供應鏈，軟體開發上將有越來越成熟豐富的庫函數，但即便如此也不代表所有車企都將在同一技術水平線上。一流的性能並非僅僅是軟硬體的堆砌，基於海量實驗的積累及對演算法的深度理解才能給機器注入靈魂。

未完待續

參考文獻：

[1]Ramsey Faragher.Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation.2012

作者：葉磊Ray