Kalman濾波器原理和實現

kalman filter

Kalman濾波器的直觀理解^[1]

假設我們要測量一個房間下一刻鍾的溫度。據經驗判斷，房間內的溫度不可能短時大幅度變化，也就是說可以依經驗認為下一刻鍾的溫度等於現在的溫度。但是經驗是存在誤差的，下一刻的真實溫度可能比我們預測溫度上下偏差幾度，這個偏差可以認為服從高斯分布。另外我們也可以使用溫度計測量溫度，但溫度計測量的是局部空間的溫度，沒辦法准確的度量整間房子的平均溫度。測量值和真實值得偏差也認為服從高斯分布。

現在希望由經驗的預測溫度和溫度計的測量值估算房間的真實平均溫度。

首先我們由時刻的溫度可以推測下一刻鍾溫度，例如k時刻的溫度是23°C，那么預測k+1刻鍾的溫度也為23°C,同時假設估計偏差是5°C，然后到了k+1時刻使用溫度計測量得到的溫度是25°C，溫度計的偏差是4°C。真實的溫度以較大概率位於23℃ 和25℃ 之間 ,所以可以通過這兩個值的方差來判定誰更可靠，方差越小說明可信度越高，那么真實溫度接近該值的可能性越大。所以這里可以認為真實溫度為 ℃。可以看出最終選擇的溫度值更偏向於偏差較小的量。

現在k+1時刻的溫度可以認為是24.11攝氏度，那么預測k+2時刻的溫度時就可以依據經驗認為溫度是24.11，那么偏差是多少呢？ ，其中被稱為卡爾曼增益,可以發現現在估算的偏差小了好多。於是可以類似估算k+1時刻溫度那樣較准確的估算第k+2時刻的溫度，這是一個迭代的過程。

Kalman理論推導^[2]

現在有一個運動系統

該式稱為系統的預測方程，其中

1. 是t-1時刻下目標的狀態，而是估算的t時刻的狀態，比如位移，速度

2. 矩陣是狀態轉移矩陣

3. 是第t時刻系統新加入的變量，是輸入控制矩陣，是對當前輸入的處理矩陣

4. 是噪聲矩陣，可以認為是高斯噪聲。

在上例中, A=1,·,B=0,沒有輸入，偏差.

系統的測量方程為

其中表示t時刻的真實狀態，是觀測矩陣，因為的變量空間不一定相同，所以有一個觀測矩陣，使真實狀態映射到觀測空間中。是高斯噪聲。如果能直接測量，那么.

現在來推導Kalman過程：
設預測過程中噪聲,測量過程中噪聲, 分別是協方差矩陣。

1. 預測
   預測值

   

   最小均方誤差矩陣

   

   這里,
   是期望符號。
   為了理解這個式子啊，要明白真實值是沒法獲得的，我們得到的都是估計值，因為錯誤可以避免，誤差一定存在。預測使用的是初步估計值，然后使用測量值對估計值修正之后還是估計值，只不過是更准確的估計值~ 當然系統狀態方程是不變的，真實的狀態運動到真實的狀態,而估計的狀態值運動到估計的狀態.

2. 修正
   誤差增益

   

   修正估計

   

   這兩個式子可以通過上面的那個例子理解，這里的矩陣主要是用來將觀測空間映射到狀態空間。

   最小均方誤差矩陣修正

   

這里推出來的結果和網上的都不一樣，暫時還沒發現是哪里出現問題了，再慢慢看，可是想一想協方差矩陣應該是對稱矩陣，而網上給出的怎么保證是對稱矩陣呢？

---

均方誤差中的道道

• 估計值和測量值的偏差都服從高斯分布

• Kalman濾波器結合了估計值和觀測值得到更精確的估計值~即使偏差更小

• Kalman濾波器需要初始化第一幀的狀態。

---

matlab代碼

1.  

   ---

2. function [ curSample,P] = kalmanfilter(initSample,observeSample,initP,A,H,Q,R,boundary) 

   ---

3. % 基於kalman濾波的目標追蹤方法實現 

   ---

4. % structure of sample: (x,y,vx,vy,hx,hy,sc) 

   ---

5. % x -x方向坐標 

   ---

6. % y -y方向坐標 

   ---

7. % vx -x方向的速度 

   ---

8. % vy -y方向的速度 

   ---

9. % hx -區域寬度的一半 

   ---

10. % hy -區域高度的一半 

    ---

11. % sc -尺度變換scale 

    ---

12.  

    ---

13. % 系統狀態方程：x(n)=A*x(n-1)+w(n) 

    ---

14. % 系統測量方程：z(n)=H*x(n)+v(n) 

    ---

15. % 其中 w(n)和v(n)均服從獨立正態分布 

    ---

16.  

    ---

17. % 鑒於連續幀之間時間間隔很短，假設兩幀之間目標勻速運動 

    ---

18.  

    ---

19. % inputs: 

    ---

20. % initSample -前一幀檢測到的區域，作為當前幀的輸入 

    ---

21. % observeSample -當前幀觀測到的區域 

    ---

22. % initP -前一幀的均方誤差矩陣 

    ---

23. % A -狀態轉移矩陣 

    ---

24. % H -系統觀測矩陣 

    ---

25. % Q -過程噪聲的協方差矩陣 

    ---

26. % R -測量噪聲的協方差矩陣 

    ---

27. % boundary -圖像的大小[width,height] 

    ---

28. % outputs: 

    ---

29. % curSample -修正后的觀測值，作為輸出的檢測區域 

    ---

30. % P -當前幀的均方誤差矩陣，作為下一幀的輸入 

    ---

31.  

    ---

32. %[A,Q,H,R]=initialize(); 

    ---

33. [curSample,P]=predict(initSample,A,Q,initP); 

    ---

34. [curSample,P]=update (curSample,P,observeSample,H,R); 

    ---

35. if isValidate(curSample,boundary)==0 

    ---

36. curSample=initSample; 

    ---

37. end 

    ---

38. end 

    ---

39.  

    ---

40. function flag=isValidate(sample,boundary) 

    ---

41. % 判定選擇的區域是否越界 

    ---

42. % inputs: 

    ---

43. % sample -待判定的樣本 

    ---

44. % boundary -圖像的邊界[width,height] 

    ---

45. % outputs: 

    ---

46. % flag -1有效，0無效 

    ---

47. width =boundary(1); 

    ---

48. height =boundary(2); 

    ---

49. x0=sample(1)-sample(5);% 窗口左上角的x坐標 

    ---

50. y0=sample(2)-sample(6);% 窗口左上角的y坐標 

    ---

51. flag=1; 

    ---

52. if x0<1||y0<1||x0>width-2*sample(5)-1||y0>height-2*sample(6)-1 

    ---

53. flag=0; 

    ---

54. end 

    ---

55. end 

    ---

56.  

    ---

57. function [curSample,P]=predict(preSample,A,Q,preP) 

    ---

58. % kalman濾波的預測階段 

    ---

59. % inputs: 

    ---

60. % preSample -前一時刻的狀態，即x(t-1) 

    ---

61. % A -狀態轉移矩陣 

    ---

62. % Q -過程噪聲的協方差矩陣 

    ---

63. % preP -前一時刻的誤差協方差矩陣P(n-1) 

    ---

64. % outputs; 

    ---

65. % curSample -預測的狀態值,即x(n|n-1) 

    ---

66. % P -預測狀態的協方差矩陣P(n|n-1) 

    ---

67. curSample=A*preSample; 

    ---

68. P=A*preP*A'+Q; 

    ---

69. end 

    ---

70.  

    ---

71. function [curSample,P]=update(curSample,P,observeSample,H,R) 

    ---

72. % kalman濾波的修正階段 

    ---

73. % inputs: 

    ---

74. % curSample -預測階段的狀態 

    ---

75. % P -預測階段的協方差矩陣 

    ---

76. % observeSample -當前時刻運動目標的觀測值 

    ---

77. % H -觀測狀態使用的觀測矩陣 

    ---

78. % R -測量噪聲的協方差矩陣 

    ---

79. % outputs: 

    ---

80. % curSample -修正之后的目標狀態 

    ---

81. % P -修正之后的誤差協方差矩陣 

    ---

82.  

    ---

83. temp=H*P*H'+R; 

    ---

84. K=P*H'/temp; % kalman增益 

    ---

85. curSample=curSample+K*(observeSample-H*curSample); 

    ---

86. temp=K*H; 

    ---

87. I=eye(size(temp)); 

    ---

88. P=(I-temp)*P; 

    ---

89. end 

    ---

90.  

    ---

---

1. 卡爾曼濾波的原理說明 ↩

2. Kalman濾波器從原理到實現 ↩