卡爾曼濾波—Simple Kalman Filter for 2D tracking with OpenCV

之前有關卡爾曼濾波的例子都比較簡單，只能用於簡單的理解卡爾曼濾波的基本步驟。現在讓我們來看看卡爾曼濾波在實際中到底能做些什么吧。這里有一個使用卡爾曼濾波在窗口內跟蹤鼠標移動的例子，原作者主頁: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)和雅可比矩陣J_F，J_H的計算。擴展卡爾曼濾波(EKF)同樣能解決線性問題。

tinyekf.py:

'''
    Extended Kalman Filter in Python
'''
import numpy as np
from abc import ABCMeta, abstractmethod

class EKF(object):
    __metaclass__ = ABCMeta
    def __init__(self, n, m, pval=0.1, qval=1e-4, rval=0.1):
        '''
        Creates a KF object with n states, m observables, and specified values for 
        prediction noise covariance pval, process noise covariance qval, and 
        measurement noise covariance rval.
        '''
        # No previous prediction noise covariance
        self.P_pre = None

        # Current state is zero, with diagonal noise covariance matrix
        self.x = np.zeros((n,1))
        self.P_post = np.eye(n) * pval

        # Get state transition and measurement Jacobians from implementing class
        self.F = self.getF(self.x)
        self.H = self.getH(self.x)

        # Set up covariance matrices for process noise and measurement noise
        self.Q = np.eye(n) * qval
        self.R = np.eye(m) * rval
 
        # Identity matrix will be usefel later
        self.I = np.eye(n)

    def step(self, z):
        '''
        Runs one step of the EKF on observations z, where z is a tuple of length M.
        Returns a NumPy array representing the updated state.
        '''
        # Predict ----------------------------------------------------
        self.x = self.f(self.x)
        self.P_pre = self.F * self.P_post * self.F.T + self.Q

        # Update -----------------------------------------------------
        G = np.dot(self.P_pre * self.H.T, np.linalg.inv(self.H * self.P_pre * self.H.T + self.R))
        self.x += np.dot(G, (np.array(z) - self.h(self.x).T).T)
        self.P_post = np.dot(self.I - np.dot(G, self.H), self.P_pre)

        # return self.x.asarray()
        return self.x

    @abstractmethod
    def f(self, x):
        '''
        Your implementing class should define this method for the state transition function f(x),
        returning a NumPy array of n elements.  Typically this is just the identity function np.copy(x).
        '''
        raise NotImplementedError()    

    @abstractmethod
    def getF(self, x):    
        '''
        Your implementing class should define this method for returning the n x n Jacobian matrix F of the 
        state transition function as a NumPy array.  Typically this is just the identity matrix np.eye(n).
        '''
        raise NotImplementedError()    

    @abstractmethod
    def h(self, x):
        '''
        Your implementing class should define this method for the observation function h(x), returning
        a NumPy array of m elements. For example, your function might include a component that
        turns barometric pressure into altitude in meters.
        '''
        raise NotImplementedError()    

    @abstractmethod
    def getH(self, x):
        '''
        Your implementing class should define this method for returning the m x n Jacobian matirx H of the 
        observation function as a NumPy array.
        '''
        raise NotImplementedError()    

kalman_mousetracker.py:

# -*- coding: utf-8 -*-
'''
kalman_mousetracker.py - OpenCV mouse-tracking demo using TinyEKF
'''

# This delay will affect the Kalman update rate
DELAY_MSEC = 20        # 卡爾曼濾波計算時間間隔,單位為ms

WINDOW_NAME = 'Kalman Mousetracker [ESC to quit]'    # 窗口名稱
WINDOW_SIZE = 500                                    # 窗口大小

import cv2
import numpy as np
from sys import exit
from tinyekf import EKF        

class TrackerEKF(EKF):                        
    '''
    An EKF for mouse tracking
    '''
    def __init__(self):
        EKF.__init__(self, 2, 2, pval=1, qval=0.001, rval=0.1)

    def f(self, x):
        # State-transition function is identity
        return np.copy(x)

    def getF(self, x):
        # So state-transition Jacobian is identity matrix
        return np.eye(2)

    def h(self, x):
        # Observation function is identity
        return x

    def getH(self, x):
        # So observation Jacobian is identity matrix
        return np.eye(2)

        
class MouseInfo(object):
    '''
    A class to store X,Y points
    '''
    def __init__(self):
        self.x, self.y = -1, -1
    
    # If you print an object then its __str__ method will get called
    # The __str__ is intended to be as human-readable as possible
    def __str__(self):
        return '%4d %4d' % (self.x, self.y)

        
def mouseCallback(event, x, y, flags, mouse_info):
    '''
    Callback to update a MouseInfo object with new X,Y coordinates
    '''
    mouse_info.x = x
    mouse_info.y = y


def drawCross(img, center, r, g, b):
    '''
    Draws a cross a the specified X,Y coordinates with color RGB
    '''
    d = 5               # 調整d改變X標記大小
    thickness = 2        # 線寬
    color = (r, g, b)    # 標記顏色
    ctrx = center[0]    # 標記中心點的x坐標
    ctry = center[1]    # 標記中心點的y坐標

    # Python： cv2.line(img, pt1, pt2, color[, thickness[, lineType[, shift ] ] ])--> None
    # lineType參數之一： CV_AA - antialiased line
    cv2.line(img, (ctrx - d, ctry - d), (ctrx + d, ctry + d), color, thickness, cv2.CV_AA) 
    cv2.line(img, (ctrx + d, ctry - d), (ctrx - d, ctry + d), color, thickness, cv2.CV_AA)


def drawLines(img, points, r, g, b):
    '''
    Draws lines
    '''
    # Python: cv2.polylines(img, pts, isClosed, color[, thickness[, lineType[, shift ] ] ]) -->None
    # 參數pts: Array of polygonal curves
    cv2.polylines(img, [np.int32(points)], isClosed=False, color=(r, g, b))


def newImage():
    '''
    Returns a new image
    '''
    return np.zeros((500,500,3), np.uint8) # 創建矩陣，用於保存圖像內容


if __name__ == '__main__':
    # Create a new image in a named window
    img = newImage()
    cv2.namedWindow(WINDOW_NAME)

    # Create an X,Y mouse info object and set the window's mouse callback to modify it
    mouse_info = MouseInfo()    # mouse_info用於存貯當前鼠標位置
    
    # 設置鼠標事件回調函數
    # 參數1：name – Window name
    # 參數2：onMouse – Mouse callback.
    # 參數3：param – The optional parameter passed to the callback.
    cv2.setMouseCallback(WINDOW_NAME, mouseCallback, mouse_info)

    # Loop until mouse inside window
    while True:
        if mouse_info.x > 0 and mouse_info.y > 0:    # 鼠標進入窗口內
            break
        cv2.imshow(WINDOW_NAME, img) # 鼠標沒進入窗口內則一直顯示黑色背景
        if cv2.waitKey(1) == 27:     # 檢測是否按下ESC鍵            
            exit(0)

    # These will get the trajectories for mouse location and Kalman estiamte
    measured_points = []    # 測量值列表
    kalman_points = []      # 估計值列表

    # Create a new Kalman filter for mouse tracking
    kalfilt = TrackerEKF()

    # Loop till user hits escape
    while True:
        # Serve up a fresh image
        img = newImage()    

        # Grab current mouse position and add it to the trajectory
        measured = (mouse_info.x, mouse_info.y)
        measured_points.append(measured)    # 注意：程序運行時間越長（或者計算間隔越小）列表長度會越大

        # Update the Kalman filter with the mouse point, getting the estimate.
        estimate = kalfilt.step((mouse_info.x, mouse_info.y))

        # Add the estimate to the trajectory
        estimated = [int(c)     for c in estimate]
        kalman_points.append(estimated)        # kalman_points為2D point列表,存放每次計算出的估計值坐標

        # Display the trajectories and current points
        drawLines(img, kalman_points,   0,   255, 0)    # 繪制跟蹤點移動路徑
        drawCross(img, estimated,       255, 255, 255)    # X標記點，代表卡爾曼濾波估計位置
        drawLines(img, measured_points, 255, 255, 0)    # 繪制鼠標移動路徑
        drawCross(img, measured, 0,   0,   255)            # X標記點，代表鼠標當前位置

        # Delay for specified interval, quitting on ESC
        cv2.imshow(WINDOW_NAME, img)    # image每隔DELAY_MSEC毫秒就刷新一次
        if cv2.waitKey(DELAY_MSEC) & 0xFF == 27:
            break
            
    #  close the window and de-allocate any associated memory usage.
    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/

[2] http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/

[3] https://www.cs.cornell.edu/courses/cs4758/2012sp/materials/mi63slides.pdf