KF，EKF怎樣設置Q和R陣?

12-28

我用陀螺和測角敏感器建立了EKF模型。想用測角敏感器估計陀螺的漂移，但是按照常規的系統雜訊方差和測量雜訊方差來取Q陣和R陣時，結果總是不收斂。但是我把R陣取的非常大（超過一定程度比如1e7）的時候才收斂，但是也沒有收斂到預期值，請問這種情況下，一般該從哪入手進行排查和處理。R陣可以取的很大嗎，這樣合理不？

跑題一個，說幾個準則吧。只是準則，只能提供某個角度的參考，可能需要搭配試錯來用。

其實是模型誤差與測量誤差的大小，是模型預測值與測量值的加權。舉例而言，R固定，Q越大，代表越信任側量值，Q無窮代表只用測量值；反之，Q越小代表越信任模型預測值，Q為零則是只用模型預測。

再往背後挖，Q和R是歷史測量值與當前測量值的加權。

穩態時，可以分析系統的頻域特性的，跟Q和R有關。

從涉及反饋那一步入手，看成個反饋系統，然後可分析各個參數跟比如收斂速度等的關係。就是State estimate (a posteriori) 那一步。從反饋的角度來看，其實卡爾曼濾波等價於線性時變的「反饋控制」系統，且逐漸趨近於一個線性時不變的系統。因此，有些性質可以用控制系統相關的來分析。甚至可以直接用穩態解來當成LTI系統來分析，不過，當然會有誤差了～

其實上面說的模型誤差，不光包括系統模型誤差，還有信號模型誤差。什麼意思呢？雖然假設某個雜訊是零均值的，但實際可能有個常值偏差，這時候可以考慮類似前饋的做法，以圖消掉。其實有的些自適應濾波就是解決這個問題的。當然也有其他方法來做的，比如先做個最小二乘，把非零偏差估計出來減去。

---

補充一些跑題的。

雖然卡爾曼濾波核心代碼就那麼幾行，中間用到無非矩陣運算，最複雜可能也就算是矩陣求逆，但調參的區別可以非常之大，更不用說有很多小trick了。這點，電機控制的PID，也是如此。

卡爾曼濾波，不過是實現動態系統的迭代最小二乘，或是用在數據融合。電機的控制，可能也不過是串級PID。兩者貌似簡單，但真正能做好的，全世界也不多。電機控制比如就有三菱、現代等公司，有專門的老師傅專門負責調大功率電機的參數，算是絕活，可以吃一輩子。為什麼本科生就能做的事，還要專門養一個老師傅來做呢？到卡爾曼濾波的話，一直到現在，火星車上也需要有人人專門負責IMU。原因很簡單，同樣是Kalman，同樣是PID，調的水平可以差很多。再比如，現在的過程式控制制，很多中國企業花大價錢買最先進的硬體、軟體，知道殼牌、美孚用的是比如系統辨識，MPC，PID等方法，但就是各種參數不會調，有的甚至用default的，那控制效率怎麼可能跟殼牌、美孚等比呢？要知道，一個大的過控項目，光系統辨識可能就要花幾個月。還有，某射電望遠鏡，原來是用的PID，參數是大概調的，總是不滿意，也是後來專門請人用一些現代的調參方法，花幾個月優化的參數。

以卡爾曼濾波而言，初值怎麼選？Q，R怎麼設？雜訊不是高斯的怎麼辦？不是白雜訊怎麼辦？弱非線性怎麼辦？非線性強了怎麼辦？有干擾怎麼辦？干擾是能建模的的只怎麼辦？不能建模的怎麼辦？怎麼容錯（比如萬一數據介面接觸不良）？等等吧。最後還得保證在一個控制周期內完成整個運算。

回到電機控制，PID怎麼調，什麼時候只用P，只用I，只用D，只用PI，只用PD？如果以上幾種組合都不行怎麼辦？怎麼防止過飽和？怎麼提高響應速度（帶寬）？帶寬和精度、功率怎麼trade-off？Disturbance attenuation怎麼分析？魯棒性怎麼分析？怎麼容錯？等等吧。

而且這些所要求的不僅僅是理論，而是實打實的在工程中能用的演算法。不是理論上的分析，而是實際中的調參方法。

類似的演算法有很多？EKF？UKF？SRUKF？PF？還是非閉環、非反饋的濾波？還是頻域濾波？動態系統的狀態估計和信息融合，不論是導彈，航天飛船，火星車，衛星，GPS，潛艇，飛機，乃至到無人機、四旋翼，身邊的機器人，乃至平衡車，自動駕駛，等等吧，現在幾乎是卡爾曼濾波一統天下。看看他獲得過的獎吧，Kyoto，Draper。。。對了，香農的理論也是很「淺」啊，雖得過Kyoto獎，但也沒得過啥數學獎。

而對於上面說的，導彈，航天飛船，火星車，衛星，GPS，潛艇，飛機，乃至到無人機、四旋翼，身邊的機器人，乃至平衡車，自動駕駛，導航和控制兩個環節都是性命攸關的，當然還有建模。這點上，很多人有錯覺，以為硬體和軟體弄好了，演算法也在那了，就行了。其實，這只是完成了百分之三四十。演算法的調參那是相當重要。而且不是純01的東西，因為要控制硬體，是物理的，還有有採樣，有保持器，所以要結合物理來做。而且很多時候bug不在軟體中，甚至有時找不到bug，踢一腳就好了。

可悲的是，雖然絕大多數自動化的學生都「會」卡爾曼和PID，但能理解，能調好的，實在是寥寥無幾。因為都是記住公式，會套用，會所謂的實現（就些矩陣運算，還不簡單），但根本就不理解。所以也就不會調參。就卡爾曼的那幾行公式，怎麼從最大似然估計方面來分析，怎麼從反饋系統方面來分析，怎麼從殘差的innovation方面來分析？怎麼從動態規劃，馬爾可夫鏈方面來分析？怎麼從測量值與預測值的加權方面來分析？怎麼從歷史測量值的加權方面來分析？如果有非線性，EKF、UKF、PF等的優缺點是什麼？怎麼結合起來用？需要分析動態性能時怎麼辦？最小方差求不出來解析式，如何估計？PID更簡單，乘法加法而已，但一個簡單的問題，針對某個對象，PID最優能調到什麼程度（比如快速性，比如控制精度）？還有別的控制器能做的更好么？

工程（導航、控制）方面很多人就停留在「知道」上面，沒什麼深的理解，沒有在理論、演算法、工程之間來回幾次。

理論深刻與否，不是看數學上的艱深，而是看理論enable的東西。香農的資訊理論，愛因斯坦狹義相對論和質能方程，卡爾曼濾波（不過用到了里卡提方程），乃至動態規劃，都是很淺的數學，那比現在IEEE Transactions的東西數學簡單多了。但是大家都明白，現在發的所謂艱深論文，其實並不深刻，因為不是enabling的。在這一點上，卡爾曼濾波在維納動態系統頻域濾波的基礎上，開創了動態系統時域濾波這個方向，那就是極其深刻的，雖然數學上並不艱深。

實現卡爾曼濾波當然不是神級操作，分分鐘的事情，幾行代碼。但用得好就是神級操作，一輩子能成為專家也不錯。就是現在，能在火星車包括著落等這一級別的工程中，把卡爾曼濾波用好的，全世界有幾人？把特別考慮到登月是第一次，那就是神級操作。可參看這段歷史 Appendix A: Historical Perspectives

工程方面的調參，斷不是「麻煩」這麼簡單。說得高大上一點，很多時候，演算法的實現被稱為science，而調參是art。所謂art，就是要看人，看傳承的。就如同日本搞精密控制，為什麼咱就搞不出來？電機的理論就那麼些，不艱深，實現起來也不難，達不到神級操作。原因除了製造工藝之外，就是真正對物理熟悉的基礎上的調參，當然這裡調參是廣義的。日本的電機控制，有非常多的小trick，你說是理論吧，談不上；你說是經驗吧，但卻可以在一定程度上系統化。這樣的東西累積起來，就產生了巨大差別，產生了神級的超精密電機。機床方面也是如此。為什麼只有日本人行？然後就是登陸火星，只有美國的三次。查表的話，就是現在，也有用的。比如閉環有某個環節，有一定的非線性，最簡單的方式就是做個表。再往上，則有專家系統，模糊，都是專業的「查表」。但用的好了，就是神級的效果。就像梅西的過人，沒有花哨的動作，不如某某神級，也不如某某艱深，用最簡單的動作，最有效地達到了目的。跑題了。。。

理論很「淺」，不艱深，但深刻，極其有深度。如同狹義相對論和資訊理論一樣。

很多演算法都是實現不難，就幾行，現在都有庫，自己寫也不難。但難的是用好，是調參。最典型的例子，FIR和IIR的濾波器，實現起來，幾分鐘的事。但如何設計，卻是非常困難的。什麼都弄好了，實現的話，就幾行代碼，當然容易了，不要說本科生了，會編程的即可。其實PID更是簡單，但從實現來說，剛學編程也許都能實現。但為什麼很多世界頂尖公司都會養一些專家負責調參什麼的呢。。。

理論深刻與否，不是看艱深晦澀難懂，而是看enable的東西。這一點上，資訊理論，動態規劃，卡爾曼濾波，都不艱深，但極其深刻。很多人堅持理論一定要艱深晦澀難道才深刻。恰恰相反，深刻的理論往往是「淺」的，是簡單的。Nothing is more practical than a good theory，而good的theory往往都是小而美的。現在的推導繁雜的理論多了去了，反而多不深刻。

直到現在，大概也沒有內行敢說，卡爾曼濾波和導航只是一個項目里的子項目里的子項目里的子項目里的一個。而且當時被理論證明可行的濾波方法一大堆，當然當時多是頻域的。掌握相關演算法當然是應屆生都懂，但調的話呢？現在的新演算法也是一大堆，理論可行，但為什麼大家還在用Kalman呢？調參難度確實可以大到不可估計，如JPL火星車。就是如今，能在火星車包括其著陸中這樣級別的工程中調好的，也是寥寥無幾。

深刻的數學。如資訊理論，如質能方程，如動態規劃，既簡單，又深刻，不是偶然的。因為觸及到本質，所以深刻，而世界的本質往往（甚至可能全部）是簡單的。卡爾曼濾波也是如此。由此觀之，艱深繁雜的理論，幾乎是意味著還未到本質，還沒到最底層。就不舉例子了。，

而且即使從工程方面講，無論是卡爾曼，還是PID，這幾行代碼裡面的參數，物理意義要涉及到整個系統，比如整個登月車，整個火星車。並不是說這幾行程序就像比如求矩陣特徵值程序似的，其本身只具有數學或數值運算的性質，物理意義（如果有的話）是外界賦予的，因此可以作為一個模塊，用的時候調用即可。可以說卡爾曼和PID的參數本身就代表了整個系統的物理意義。

要等下一個大神，能做出，比如，真正的能用的自適應自學習容錯的控制演算法（現在大多數控制系統都是物理變了要人來重新調參的），或是真正的適用於複雜系統的演算法，或是真正能用的某些非線性系統的控制，等等吧。在工程上，如機器人，無人機，等吧，辨識、控制、估計等都還有很多問題解決地不夠透徹，或者說根本就不知道一個演算法的界限和極限在哪裡，還有在原演算法不滿足要求的時候有沒有更優的，還有調參系統性不夠，等等吧～

一方面很多人說模擬離工程太遠，一方面波音還有雷納等基於模擬做了很多實際中在用的東西。區別就在於這模擬夠不夠真，也就是模型與實際的區別。理論也是如此。

說到H2，其實本質上卡爾曼濾波就是H2估計。也因此對雜訊不夠魯棒。的卡爾曼有很多本質，比如是動態系統的最小二乘（最小方差），最大似然估計，逐次測量值的最優加權，最優殘差innovation榨取，等等吧。。。

最近一個比較驚艷的，也是實打實的，是資訊理論學者用控制這一套，解決了一個可以說是自香農提出資訊理論以來就懸而未決的問題，帶反饋信道的capacity，UC San Diego Professor Wins Top Information Theory Award

就如同FIR、IIR濾波器，實現起來簡直就是分分鐘的事，加乘而已。但如果問下做過FIR、IIR設計，即調參（其實上面所有評論中的調參換成設計，可能更好了），的話，會發現真是老大難，這麼多信號處理的書，這麼多各種變換，loosely speaking，本質都是這個問題。而且發展這麼多年，連一些很基本的問題也還是只能靠經驗，比如滾降和階數之間的tradeoff。

而卡爾曼濾波的設計可能更有過之。在整個登月系統設計好後，卡爾曼濾波設計者要獲得整個系統在登月各個時候段的模型，這點還好說，關鍵是還要獲得系統各個階段雜訊的協方差Q，R，但獲得這個Q、R要有登月系統從起飛到月球著落運行的真實數據，然後再建模和辨識，但第一次時，沒有試驗，怎麼獲得？只能靠分析（不知道還有別的可能嗎？）。即使在現在，在導彈，航天飛船，火星車，衛星，GPS，潛艇，飛機，乃至到無人機、四旋翼，身邊的機器人，乃至平衡車，自動駕駛等等中，如果能不靠實驗、而只靠分析設計好Q、R的（別說還有其它的參數了），相信行業絕對會稱之為大神，因為大部分人都是反反覆復地試才調好的。如果誰知道登月中的這些參數怎麼設計的，或是更牛的話，自己會設計，請私信，請他/她吃飯，哈哈哈～

控制、導航系統的bug，不指單純在軟體中，硬體中也有，還跟環境有關。因此，類似的如機器人debug，就是一遍遍試驗，然後再看bug出在哪裡，是軟體、硬體、還是借口，等等吧。但登月的，總不能一遍遍發射導彈，一遍遍著陸，一遍遍返回地球吧。登月是到了阿波羅11號，已經很不容易了。其實更廣義的debug，也就包括調參，就是某些參數理論、模擬或模擬中效果好，但實際中肯定有差別，怎麼辦？類似的如機器人，是一遍遍試驗，很多時候trial and error，但也有非常多的trick，很多經驗。但登月也是無法如此。當然，登月不止導航和控制。扯的遠一點，登月所用的那些理論、演算法，幾乎肯定不會失傳，但設計、調參，因為有很多的trick和經驗，其實是有「失傳」的危險的。有可能某個關鍵人物沒了就做不出來了。

其實直到今天，工程和實際中的問題一樣多，工程中能做出登月、火星車式的工程，理論上能提出下一個卡爾曼濾波式的結果，都是大神。兩者應該都指日可待，畢竟前者都提上日程了，後者的話，PID才100來年的歷史，卡爾曼濾波才50來年，不大可能控制就到此為止了吧～

個人看來，怎麼誇卡爾曼都不為過，他是和維納、香農一個級別的。甚至說他對系統科學的貢獻，argubaly，已經超過了維納。系統科學，簡單的說是人造系統的物理學，卡爾曼（當然也包括維納、香農）的公式，揭示了系統科學的本質和極限。做個可能不恰當的對比，就如同熱力學熵不減定律，絕對零度等，有人會說後者看起來「淺白簡單」而因此不深刻嗎？

Q和R是相對應設置的。Q：R決定了「相信」預測還是觀測，兩個值越小「相信」的成分越大。換句話說，Q和R分別代表預測和觀測時候誤差，因此誤差越小越值得「相信」。
如你所述，R需要設置的很大結果才收斂，說明你的方程只相信預測結果，並不相信觀測結果。

我感覺，你的問題出在觀測方程上，請檢查觀測方程以及相應的協方差矩陣。如果能夠單步調試，請首先校驗觀測方程的運算結果和其協方差矩陣的運算結果。當然這只是我得猜測，具體的問題得你自己慢慢剝，加油。

你的狀態模型不準，濾波器不知如何跟隨，所以就發散了。

你把r陣取得很大，意味著讓濾波器忽略測量，跟隨著你的狀態模型。也許不發散了，但結果肯定是錯的。

是你的模型有問題

題主可以看一下我寫的blog，對ekf怎麼用有詳細的講解，希望能幫到你 http://blog.csdn.net/lizilpl/article/details/45542907

——————原答案——————

我還沒有看到源代碼，但是可以根據工程經驗猜測一些可能經常發生的錯誤。

一個很可能的錯誤是雜訊的模型有誤。注意，加速度計或陀螺儀，其實他們的雜訊可以粗略地劃分為兩部分，一部分是隨機雜訊、一部分是長期漂移。這兩個雜訊在卡爾曼濾波中的位置和數值是不同的。如果弄混了很可能就發散了。通常隨機雜訊對應的那個Q矩陣元素比較大，長期漂移對應的那個Q矩陣元素很小。有的參考資料，模型中關於陀螺儀或加速度計的雜訊只有一項，這一項一般是長期漂移的那個雜訊而不是隨機雜訊的那個雜訊。注意檢查一下這個地方的公式和數值是不是對的。

另外，我猜測你的系統中，陀螺儀的速率比較高，而測角感測器的速率比較低，二者不是等採樣率的。這樣的系統是很常見的，比如常見的慣組和GPS組合，也是慣組採樣率高而GPS採樣率低。

對於同一個慣組（或陀螺儀），與不同採樣率的GPS（或測角感測器）組合，選取的Q是不同的。

我建議你把R矩陣維持正常的數值，而顯著減小Q矩陣。另外，注意初值，初值要準確一些。

——————20170805補充————————

我介紹一些關鍵的排查錯誤的經驗：

卡爾曼濾波，特別是擴展卡爾曼濾波，程序代碼是很容易寫錯的。排查順序應這樣做：

1.檢驗極性是否正確。觀察發散是震蕩發散還是單調發散。如果單調發散，很有可能是極性寫錯了。

2.檢查模型和方程是否正確。一個基本的檢驗方法是犧牲模型的精確性而縮小模型的維數，看看在較少的維數下是否可以大體收斂。然後逐步增加模型的維數、逐步改善精確性。不要直接驗算很複雜的模型。

3.Q從小向大調，R從大向小調。通常R比較容易知道而Q不容易知道，R可以先取正常的數值，然後讓Q為0，看看是否可以收斂。在收斂的基礎上，再適當增大Q。

4.Q可能和濾波的時間間隔有關。可以試圖讓時間間隔比較穩定，避免忽大忽小，看看是否可以得到正確的結果。

題主再好好檢查檢查吧，一定是
1 程序錯了
2 生成的或者測得的或者處理過的實驗數據錯了
3 模型寫錯了

Q和R應該都不會那麼大的

1.當QR 都取0時驗證EKF 是否正確，
2.QR 固定一個，看是Q還是R的問題，
3，再想為什麼是這樣。

R反映量測方程的準確程度，一般調節R會改變濾波的效果，還沒接觸過不收斂的。
可以參考以下的來思考：《機動目標跟蹤周宏仁》

瀉藥。

Q 矩陣描述系統端或者模型不確定性。R矩陣描述系統測量裝置精度。你應該是把陀螺儀drift當作未知常值，用擴張狀態方法估計的，這個時候也應該給他一定的不確定性（對應Q矩陣）。也叫stochastic walk模型。可以先模擬看看是否收斂。

系統非線性程度高嗎？Q矩陣是需要仔細選的。

again Simon Dan 在工業界20年總結了optimal filtering 那本書，值得仔細讀。

你說的這種情況，r取非常大，才收斂，其實根本不是收斂，而是相當程度上的依賴於你的初值，也就是你的初值給的准，你就回看到結果跟真值差不多，反之就差一些。你這個濾波器完全沒有起到濾波器的作用，肯定是設計或者編程環節有bug。