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

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

先來看看狀態(tài)方程能建立準(zhǔn)確的時候，狀態(tài)方程見第一部分分割線以后內(nèi)容，小車做勻加速運(yùn)動的位移的預(yù)測仿真如下。

[plain] view plaincopy

1. clc  
2. clear all  
3. close all  
4.   
5. % 初始化參數(shù)  
6. delta_t=0.1;   %采樣時間  
7. t=0:delta_t:5;  
8. N = length(t); % 序列的長度  
9. sz = [2,N];    % 信號需開辟的內(nèi)存空間大小  2行*N列  2:為狀態(tài)向量的維數(shù)n  
10. g=10;          %加速度值  
11. x=1/2*g*t.^2;      %實(shí)際真實(shí)位置  
12. z = x + sqrt(10).*randn(1,N); % 測量時加入測量白噪聲  
13.   
14. Q =[0 0;0 9e-1]; %假設(shè)建立的模型  噪聲方差疊加在速度上 大小為n*n方陣 n=狀態(tài)向量的維數(shù)  
15. R = 10;    % 位置測量方差估計(jì)，可以改變它來看不同效果  m*m      m=z(i)的維數(shù)  
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的后驗(yàn)估計(jì)  
26. P=zeros(n);           % 后驗(yàn)方差估計(jì)  n*n  
27. xhatminus=zeros(sz);  % x的先驗(yàn)估計(jì)  
28. Pminus=zeros(n);      % n*n  
29. K=zeros(n(1),m(1));   % Kalman增益  n*m  
30. I=eye(n);  
31.   
32. % 估計(jì)的初始值都為默認(rèn)的0，即P=[0 0;0 0],xhat=0  
33. for k = 9:N           %假設(shè)車子已經(jīng)運(yùn)動9個delta_T了，我們才開始估計(jì)  
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('含有噪聲的測量', '后驗(yàn)估計(jì)', '真值');  
50. xlabel('Iteration');  

綠線為真實(shí)值，藍(lán)色的為噪聲很大的測量值，紅線為估計(jì)值。由此可以看出卡爾曼濾波確實(shí)相當(dāng)犀利，提供了一個順滑的最優(yōu)的估計(jì)。并請注意代碼中，特地使得估計(jì)是從第9個才開始預(yù)測,就像雷達(dá)跟蹤一樣，假設(shè)一開始我們沒有發(fā)現(xiàn)這個東西，它已經(jīng)運(yùn)行了一段時間，我們在雷達(dá)測量和自己預(yù)測后得到估計(jì)結(jié)果，從圖中可看出效果確實(shí)很好。

但這里請注意圖像中畫紅圈部分，由于一開始你預(yù)測值為0，而實(shí)際上不是（它已經(jīng)運(yùn)動9個時間間隔了），所以估計(jì)出的效果不好。在這里回憶前面討論過的K值大小和估計(jì)的關(guān)系，既然預(yù)測不準(zhǔn)，那么一開始我就先相信測量唄。這就涉及估計(jì)值誤差協(xié)方差初值的選取。在第一部分中我們知道卡爾曼增益K與預(yù)測誤差協(xié)方差矩陣正相關(guān)，由第一部分推導(dǎo)知道預(yù)測誤差協(xié)方差陣：

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

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

好了，到第二個問題，當(dāng)狀態(tài)方程建立不正確的又會怎樣呢？實(shí)際應(yīng)用中很多時候我們不能建立正確的狀態(tài)方程。

我們假設(shè)建立的狀態(tài)方程如下：

轉(zhuǎn)換矩陣A,B,H都等于1.這個模型明顯是不正確的。

注意這個時候的系統(tǒng)噪聲，就不單單只是系統(tǒng)內(nèi)部產(chǎn)生的，還包括你建立狀態(tài)方程的不正確性。你建立的越不正確，根據(jù)你模型進(jìn)行的預(yù)測就不正確，從這個角度來說，相當(dāng)于你的噪聲增大了。所以這個時候系統(tǒng)噪聲W的方差應(yīng)該增大。理解這一點(diǎn)，對改進(jìn)實(shí)際估計(jì)效果有好處。接下來通過對比不同的W方差值設(shè)定給出對比，貼出這部分仿真如下。

[plain] view plaincopy

1. clc  
2. clear  
3. close all  
4.   
5. % 初始化參數(shù)  
6. delta_t=0.1;  
7. t=0:delta_t:5;  
8. g=10;%加速度值  
9. n_iter = length(t); % 序列的長度  
10. sz = [n_iter, 1]; % 信號需開辟的內(nèi)存空間大小  
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; % 測量方差估計(jì)，可以改變它來看不同效果  
18.    
19. % 分配空間  
20. xhat=zeros(sz);      % x的后驗(yàn)估計(jì)  
21. P=zeros(sz);         % 后驗(yàn)方差估計(jì)  
22. xhatminus=zeros(sz); % x的先驗(yàn)估計(jì)  
23. Pminus=zeros(sz);    % 先驗(yàn)方差估計(jì)  
24. K=zeros(sz);         % Kalman增益  
25.    
26. % 估計(jì)的初始值  
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('含有噪聲的測量', '后驗(yàn)估計(jì)', '真值');  
46. xlabel('Iteration');  

圖(a)圖(b)

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

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

最后再說一句個人可能不正確觀點(diǎn)，拋磚迎玉，卡爾曼濾波最后收斂得到的方程就是維納濾波，卡爾曼濾波是一步一步遞推然后收斂到真實(shí)值，維納濾波是直接計(jì)算出估計(jì)值，不是一步一步的結(jié)果，但兩者都是最小均方差的思想在里面。因此，我在學(xué)卡爾曼的時候，想到數(shù)字圖像里的維納濾波，直觀的想到，維納濾波能做到的，卡爾曼應(yīng)該也能做到。但這我也沒去驗(yàn)證，倒是確實(shí)有這方面的論文。

 全文完