之前有关卡尔曼滤波的例子都比较简单，只能用于简单的理解卡尔曼滤波的基本步骤。现在让我们来看看卡尔曼滤波在实际中到底能做些什么吧。这里有一个使用卡尔曼滤波在窗口内跟踪鼠标移动的例子，原作者主页:http://home.wlu.edu/~levys/
首先，第一步是选取状态变量，这里选择系统状态变量为x=[x, y]T ，即状态变量选为鼠标在窗口内的位置。通过鼠标事件响应的回调函数可以获得鼠标当前位置，即观测值z = [x, y]T.对于这一问题外界控制量u=0. 观测噪声和系统噪声的选择需要靠实验或其它方式确定，这里先采用默认值以简化问题。则系统状态方程可以写为：
同样可以写出观测值和状态变量之间的关系，可知矩阵F和矩阵H均为2阶单位矩阵。
下面使用Python和OpenCV来解决这一问题。tinyekf.py文件中定义了EKF抽象类，使用时需要自己定义一个类(继承EKF)，并实现非线性函数f(x)，h(x)和雅可比矩阵JF，JH的计算。扩展卡尔曼滤波(EKF)同样能解决线性问题。
tinyekf.py:
1 ''' 2 Extended Kalman Filter in Python 3 ''' 4 import numpy as np 5 from abc import ABCMeta, abstractmethod 6 7 class EKF(object): 8 __metaclass__ = ABCMeta 9 def __init__(self, n, m, pval=0.1, qval=1e-4, rval=0.1): 10 ''' 11 Creates a KF object with n states, m observables, and specified values for 12 prediction noise covariance pval, process noise covariance qval, and 13 measurement noise covariance rval. 14 ''' 15 # No previous prediction noise covariance 16 self.P_pre = None 17 18 # Current state is zero, with diagonal noise covariance matrix 19 self.x = np.zeros((n,1)) 20 self.P_post = np.eye(n) * pval 21 22 # Get state transition and measurement Jacobians from implementing class 23 self.F = self.getF(self.x) 24 self.H = self.getH(self.x) 25 26 # Set up covariance matrices for process noise and measurement noise 27 self.Q = np.eye(n) * qval 28 self.R = np.eye(m) * rval 29 30 # Identity matrix will be usefel later 31 self.I = np.eye(n) 32 33 def step(self, z): 34 ''' 35 Runs one step of the EKF on observations z, where z is a tuple of length M. 36 Returns a NumPy array representing the updated state. 37 ''' 38 # Predict ---------------------------------------------------- 39 self.x = self.f(self.x) 40 self.P_pre = self.F * self.P_post * self.F.T + self.Q 41 42 # Update ----------------------------------------------------- 43 G = np.dot(self.P_pre * self.H.T, np.linalg.inv(self.H * self.P_pre * self.H.T + self.R)) 44 self.x += np.dot(G, (np.array(z) - self.h(self.x).T).T) 45 self.P_post = np.dot(self.I - np.dot(G, self.H), self.P_pre) 46 47 # return self.x.asarray() 48 return self.x 49 50 @abstractmethod 51 def f(self, x): 52 ''' 53 Your implementing class should define this method for the state transition function f(x), 54 returning a NumPy array of n elements. Typically this is just the identity function np.copy(x). 55 ''' 56 raise NotImplementedError() 57 58 @abstractmethod 59 def getF(self, x): 60 ''' 61 Your implementing class should define this method for returning the n x n Jacobian matrix F of the 62 state transition function as a NumPy array. Typically this is just the identity matrix np.eye(n). 63 ''' 64 raise NotImplementedError() 65 66 @abstractmethod 67 def h(self, x): 68 ''' 69 Your implementing class should define this method for the observation function h(x), returning 70 a NumPy array of m elements. For example, your function might include a component that 71 turns barometric pressure into altitude in meters. 72 ''' 73 raise NotImplementedError() 74 75 @abstractmethod 76 def getH(self, x): 77 ''' 78 Your implementing class should define this method for returning the m x n Jacobian matirx H of the 79 observation function as a NumPy array. 80 ''' 81 raise NotImplementedError()
kalman_mousetracker.py:
1 # -*- coding: utf-8 -*- 2 ''' 3 kalman_mousetracker.py - OpenCV mouse-tracking demo using TinyEKF 4 ''' 5 6 # This delay will affect the Kalman update rate 7 DELAY_MSEC = 20 # 卡尔曼滤波计算时间间隔,单位为ms 8 9 WINDOW_NAME = 'Kalman Mousetracker [ESC to quit]' # 窗口名称 10 WINDOW_SIZE = 500 # 窗口大小 11 12 import cv2 13 import numpy as np 14 from sys import exit 15 from tinyekf import EKF 16 17 class TrackerEKF(EKF): 18 ''' 19 An EKF for mouse tracking 20 ''' 21 def __init__(self): 22 EKF.__init__(self, 2, 2, pval=1, qval=0.001, rval=0.1) 23 24 def f(self, x): 25 # State-transition function is identity 26 return np.copy(x) 27 28 def getF(self, x): 29 # So state-transition Jacobian is identity matrix 30 return np.eye(2) 31 32 def h(self, x): 33 # Observation function is identity 34 return x 35 36 def getH(self, x): 37 # So observation Jacobian is identity matrix 38 return np.eye(2) 39 40 41 class MouseInfo(object): 42 ''' 43 A class to store X,Y points 44 ''' 45 def __init__(self): 46 self.x, self.y = -1, -1 47 48 # If you print an object then its __str__ method will get called 49 # The __str__ is intended to be as human-readable as possible 50 def __str__(self): 51 return '%4d %4d' % (self.x, self.y) 52 53 54 def mouseCallback(event, x, y, flags, mouse_info): 55 ''' 56 Callback to update a MouseInfo object with new X,Y coordinates 57 ''' 58 mouse_info.x = x 59 mouse_info.y = y 60 61 62 def drawCross(img, center, r, g, b): 63 ''' 64 Draws a cross a the specified X,Y coordinates with color RGB 65 ''' 66 d = 5 # 调整d改变X标记大小 67 thickness = 2 # 线宽 68 color = (r, g, b) # 标记颜色 69 ctrx = center[0] # 标记中心点的x坐标 70 ctry = center[1] # 标记中心点的y坐标 71 72 # Python： cv2.line(img, pt1, pt2, color[, thickness[, lineType[, shift ] ] ])--> None 73 # lineType参数之一： CV_AA - antialiased line 74 cv2.line(img, (ctrx - d, ctry - d), (ctrx + d, ctry + d), color, thickness, cv2.CV_AA) 75 cv2.line(img, (ctrx + d, ctry - d), (ctrx - d, ctry + d), color, thickness, cv2.CV_AA) 76 77 78 def drawLines(img, points, r, g, b): 79 ''' 80 Draws lines 81 ''' 82 # Python: cv2.polylines(img, pts, isClosed, color[, thickness[, lineType[, shift ] ] ]) -->None 83 # 参数pts: Array of polygonal curves 84 cv2.polylines(img, [np.int32(points)], isClosed=False, color=(r, g, b)) 85 86 87 def newImage(): 88 ''' 89 Returns a new image 90 ''' 91 return np.zeros((500,500,3), np.uint8) # 创建矩阵，用于保存图像内容 92 93 94 if __name__ == '__main__': 95 # Create a new image in a named window 96 img = newImage() 97 cv2.namedWindow(WINDOW_NAME) 98 99 # Create an X,Y mouse info object and set the window's mouse callback to modify it 100 mouse_info = MouseInfo() # mouse_info用于存贮当前鼠标位置 101 102 # 设置鼠标事件回调函数 103 # 参数1：name – Window name 104 # 参数2：onMouse – Mouse callback. 105 # 参数3：param – The optional parameter passed to the callback. 106 cv2.setMouseCallback(WINDOW_NAME, mouseCallback, mouse_info) 107 108 # Loop until mouse inside window 109 while True: 110 if mouse_info.x > 0 and mouse_info.y > 0: # 鼠标进入窗口内 111 break 112 cv2.imshow(WINDOW_NAME, img) # 鼠标没进入窗口内则一直显示黑色背景 113 if cv2.waitKey(1) == 27: # 检测是否按下ESC键 114 exit(0) 115 116 # These will get the trajectories for mouse location and Kalman estiamte 117 measured_points = [] # 测量值列表 118 kalman_points = [] # 估计值列表 119 120 # Create a new Kalman filter for mouse tracking 121 kalfilt = TrackerEKF() 122 123 # Loop till user hits escape 124 while True: 125 # Serve up a fresh image 126 img = newImage() 127 128 # Grab current mouse position and add it to the trajectory 129 measured = (mouse_info.x, mouse_info.y) 130 measured_points.append(measured) # 注意：程序运行时间越长（或者计算间隔越小）列表长度会越大 131 132 # Update the Kalman filter with the mouse point, getting the estimate. 133 estimate = kalfilt.step((mouse_info.x, mouse_info.y)) 134 135 # Add the estimate to the trajectory 136 estimated = [int(c) for c in estimate] 137 kalman_points.append(estimated) # kalman_points为2D point列表,存放每次计算出的估计值坐标 138 139 # Display the trajectories and current points 140 drawLines(img, kalman_points, 0, 255, 0) # 绘制跟踪点移动路径 141 drawCross(img, estimated, 255, 255, 255) # X标记点，代表卡尔曼滤波估计位置 142 drawLines(img, measured_points, 255, 255, 0) # 绘制鼠标移动路径 143 drawCross(img, measured, 0, 0, 255) # X标记点，代表鼠标当前位置 144 145 # Delay for specified interval, quitting on ESC 146 cv2.imshow(WINDOW_NAME, img) # image每隔DELAY_MSEC毫秒就刷新一次 147 if cv2.waitKey(DELAY_MSEC) & 0xFF == 27: 148 break 149 150 # close the window and de-allocate any associated memory usage. 151 cv2.destroyAllWindows()
程序执行效的果如下图所示：
跟踪的效果与程序中的两个参数有关：qval代表了模型噪声(即模型准确度，obviously all moedls are not able to describe the motion perfectly,thus each model contains a probabilistic part)，rval代表了观测噪声。使用观测值修正预测值的表达式为：
其中卡尔曼增益K的表达式为：
从上面公式可以看出当R趋于0，即测量误差非常小时，估计的结果更接近测量值。当P趋向0时，估计的结果更接近预测值。因此，当程序中rval的值相比qval小得多时跟踪进行的很流畅(卡尔曼滤波的估计值更偏向观测值)。但是当rval比qval小时（从感性上理解这个小），跟踪就出现了滞后，这说明需要更合理的鼠标运动模型，如Constant Velocity Model或Constant Acceleration Model。可以参考论文: Empirical evaluation of vehicular models for ego motion estimation.2011 IEEE Intelligent Vehicles Symposium (IV), 534–539. doi:10.1109/IVS.2011.5940526. 论文中提到不同的模型主要适用于不同的情境，例如高速公路上汽车加减速过程较少，而在闹市区汽车需要频繁加减速，此时用CA模型就比用CV模型合适。In general,more sophisticated models outperform simpler ones, especially in situations where the assumption of the simple models are no longer true. 这就是我认为卡尔曼滤波中最难的部分：根据问题建立合理的模型。
根据参考[3]中的叙述：If you have a badly defined model, you will not get a good estimate. But you can relax your model by increasing your estimated error. This will let the Kalman filter rely more on the measurement values, but still allow some noise removal.
即当你建立的模型不准确时，可以将与模型误差有关的参数Q调大，此时估计值将更加依赖观测值而不是通过模型得到的预测值。既然模型不准确，为什么还要用卡尔曼滤波，直接从传感器获取观测值不就好了吗？可是有时传感器获得的数据也不那么准确，而且传感器测量精度越高价格也越贵：Sensors are noisy. The world is full of data and events that we want to measure and track, but we cannot rely on sensors to give us perfect information. 因此，就我目前浅显的理解卡尔曼滤波是一种折衷(trade off)的方法，当模型更准时估计值更接近模型的预测，当传感器测量值更准时估计值更接近测量值。
参考：
[1] http://home.wlu.edu/~levys/kalman_tutorial/
[3] https://www.cs.cornell.edu/courses/cs4758/2012sp/materials/mi63slides.pdf
扫一扫
870
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
