這是原理介紹後的第二篇：MALTAB程式設計。

這是原文連結：https://blog.csdn.net/heyijia0327/article/details/17667341

這裡主要講了卡爾曼濾波。等下次空閒的時候我會把EKF和UKF做一個簡單的介紹。畢竟現實生活中的很多場景都是非線性的。

編寫卡爾曼濾波程式時需注意一點：在做融合的時候，你是不知道真實值的，你擁有的只有測量值，測量值裡面是已經包含了噪聲的。不需要另外再加噪聲項。這兩篇文章原理和matlab程式設計對於掃盲是足夠了，感謝原作者，我也是想把了解的寫下來。

以下是原文內容：

該文是自我總結性文章，有紕漏，請指出，謝謝。           --白巧克力

這部分主要是通過對第一部分中提到的勻加速小車模型進行位移預測。

先來看看狀態方程能建立準確的時候，狀態方程見第一部分分割線以後內容，小車做勻加速運動的位移的預測模擬如下。

1. clc  
2. clear all  
3. close all  
4.   
5. % 初始化引數  
6. delta_t=0.1;   %取樣時間  
7. t=0:delta_t:5;  
8. N = length(t); % 序列的長度  
9. sz = [2,N];    % 訊號需開闢的記憶體空間大小  2行*N列  2:為狀態向量的維數n  
10. g=10;          %加速度值  
11. x=1/2*g*t.^2;      %實際真實位置  
12. z = x + sqrt(10).*randn(1,N); % 測量時加入測量白噪聲  
13.   
14. Q =[0 0;0 9e-1]; %假設建立的模型  噪聲方差疊加在速度上 大小為n*n方陣 n=狀態向量的維數  
15. R = 10;    % 位置測量方差估計，可以改變它來看不同效果  m*m      m=z(i)的維數  
16.   
17. A=[1 delta_t;0 1];  % n*n  
18. B=[1/2*delta_t^2;delta_t];  
19. H=[1,0];            % m*n  
20.   
21. n=size(Q);  %n為一個1*2的向量  Q為方陣  
22. m=size(R);  
23.   
24. % 分配空間  
25. xhat=zeros(sz);       % x的後驗估計  
26. P=zeros(n);           % 後驗方差估計  n*n  
27. xhatminus=zeros(sz);  % x的先驗估計  
28. Pminus=zeros(n);      % n*n  
29. K=zeros(n(1),m(1));   % Kalman增益  n*m  
30. I=eye(n);  
31.   
32. % 估計的初始值都為預設的0，即P=[0 0;0 0],xhat=0  
33. for k = 9:N           %假設車子已經運動9個delta_T了，我們才開始估計  
34.     % 時間更新過程  
35.     xhatminus(:,k) = A*xhat(:,k-1)+B*g;  
36.     Pminus= A*P*A'+Q;  
37.       
38.     % 測量更新過程  
39.     K = Pminus*H'*inv( H*Pminus*H'+R );  
40.     xhat(:,k) = xhatminus(:,k)+K*(z(k)-H*xhatminus(:,k));  
41.     P = (I-K*H)*Pminus;  
42. end  
43.    
44. figure  
45. plot(t,z);  
46. hold on  
47. plot(t,xhat(1,:),'r-')  
48. plot(t,x(1,:),'g-');  
49. legend('含有噪聲的測量', '後驗估計', '真值');  
50. xlabel('Iteration');  

綠線為真實值，藍色的為噪聲很大的測量值，紅線為估計值。由此可以看出卡爾曼濾波確實相當犀利，提供了一個順滑的最優的估計。並請注意程式碼中，特地使得估計是從第9個才開始預測,就像雷達跟蹤一樣，假設一開始我們沒有發現這個東西，它已經運行了一段時間，我們在雷達測量和自己預測後得到估計結果，從圖中可看出效果確實很好。

但這裡請注意影象中畫紅圈部分，由於一開始你預測值為0，而實際上不是（它已經運動9個時間間隔了），所以估計出的效果不好。在這裡回憶前面討論過的K值大小和估計的關係，既然預測不準，那麼一開始我就先相信測量唄。這就涉及估計值誤差協方差初值的選取。在第一部分中我們知道卡爾曼增益K與預測誤差協方差矩陣正相關，由第一部分推導知道預測誤差協方差陣：

它又和估計誤差協方差矩陣有關，在Q，A確定的情況下，和成正比。所以如果我們給的初值大的話，那麼遞推第一步中計算出的卡爾曼增益K就大。K大意味著更相信測量值。

修改初值P=[2 0;0 2],估計影象如下，可以看到初始估計明顯改進了。（兩幅圖中，測量值相同，只改變了P）。這幅圖中紅色水平線那部分是前9個時間段，你還沒開始雷達追蹤，所以是水平的為0。

好了，到第二個問題，當狀態方程建立不正確的又會怎樣呢？實際應用中很多時候我們不能建立正確的狀態方程。

我們假設建立的狀態方程如下：

轉換矩陣A,B,H都等於1.這個模型明顯是不正確的。

注意這個時候的系統噪聲，就不單單只是系統內部產生的，還包括你建立狀態方程的不正確性。你建立的越不正確，根據你模型進行的預測就不正確，從這個角度來說，相當於你的噪聲增大了。所以這個時候系統噪聲W的方差應該增大。理解這一點，對改進實際估計效果有好處。接下來通過對比不同的W方差值設定給出對比，貼出這部分模擬如下。

1. clc  
2. clear  
3. close all  
4.   
5. % 初始化引數  
6. delta_t=0.1;  
7. t=0:delta_t:5;  
8. g=10;%加速度值  
9. n_iter = length(t); % 序列的長度  
10. sz = [n_iter, 1]; % 訊號需開闢的記憶體空間大小  
11. x=1/2*g*t.^2;  
12. x=x';  
13. z = x + sqrt(10).*randn(sz); % 測量時加入測量白噪聲  
14.   
15. Q = 0.9; % 過程激勵噪聲方差     
16.          %注意Q值得改變  待會增大到2，看看效果。對比看效果時，修改程式碼不要改變z的值  
17. R = 10; % 測量方差估計，可以改變它來看不同效果  
18.    
19. % 分配空間  
20. xhat=zeros(sz);      % x的後驗估計  
21. P=zeros(sz);         % 後驗方差估計  
22. xhatminus=zeros(sz); % x的先驗估計  
23. Pminus=zeros(sz);    % 先驗方差估計  
24. K=zeros(sz);         % Kalman增益  
25.    
26. % 估計的初始值  
27. xhat(1) = 0.0;  
28. P = 1.0;  
29. for k = 2:n_iter   %  
30.     % 時間更新過程  
31.     xhatminus(k) = xhat(k-1);  
32.     Pminus(k) = P(k-1)+Q;  
33.       
34.     % 測量更新過程  
35.     K(k) = Pminus(k)/( Pminus(k)+R );  
36.     xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k));  
37.     P(k) = (1-K(k))*Pminus(k);  
38. end  
39.    
40. figure  
41. plot(t,z);  
42. hold on  
43. plot(t,xhat,'r-')  
44. plot(t,x,'g-');  
45. legend('含有噪聲的測量', '後驗估計', '真值');  
46. xlabel('Iteration');  

圖(a)圖(b)

兩個圖中測量值是一樣的，只是第二圖中將系統噪聲方差Q增大到2。對比可以看出，特別是影象後半段，圖(b)比圖(a)效果更接近真實值。

至此，從推導到應用接近尾聲了，但我在這裡還有一個問題就是，你隨便給的x的預測初值，模型建立也不正確，kalman filter 竟然依然這麼犀利，那麼他收斂性怎麼證明呢？寫這文章的時候，筆者沒有看詳細的數學證明，但是由前面說到的kalman filter和數值分析裡遞推求解方程組時用的Gauss-Seidel 迭代法，兩者真的很相近，於是我直觀的認為卡爾曼的收斂性和Gauss-Seidel 一樣。Gauss-Seidel迭代法裡權重的選取能使得遞推收斂真實值，因此卡爾曼濾波里增益K的每次計算就是卡爾曼收斂的重要保證。

最後再說一句個人可能不正確觀點，拋磚迎玉，卡爾曼濾波最後收斂得到的方程就是維納濾波，卡爾曼濾波是一步一步遞推然後收斂到真實值，維納濾波是直接計算出估計值，不是一步一步的結果，但兩者都是最小均方差的思想在裡面。因此，我在學卡爾曼的時候，想到數字影象裡的維納濾波，直觀的想到，維納濾波能做到的，卡爾曼應該也能做到。但這我也沒去驗證，倒是確實有這方面的論文。

 全文完