Kalman滤波器原理和实现

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滤波器从原理到实现 ↩