[Python知识库]二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器

本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分五个小节介绍:

一.卡尔曼滤波器开发实践之一: 五大方程

二.卡尔曼滤波器开发实践之二: ?一个简单的位置估计卡尔曼滤波器 ? ? 也就是本文

三.卡尔曼滤波器(EKF)开发实践之三: ?基于三个传感器的海拔高度数据融合

四.卡尔曼滤波器(EKF)开发实践之四: ?ROS系统位姿估计包robot_pose_ekf详解

五.卡尔曼滤波器(EKF)开发实践之五: ?编写自己的EKF替换robot_pose_ekf中EKF滤波器

--------------------------------正文开始------------------------------------------
本文是此系列文章第二小节,将用python语言展现一个简单卡尔曼滤波器完整实现.之所以用python语言有两个原因:

一是此代码是在参考文献的作者的源码基础上改进过来的(本人也写了C++版本,合适时间再发出来);

二是python语言书写的代码表达方式,和卡尔曼五大公式在形式上很接近,非常适合新手入门.

下面将介绍在2维屏幕上模拟一个小车,跟随鼠标运动的例子.? 本例子目的重在演示卡尔曼公式代码化,故系统模型都很简单.? 系统模型分析如下:

小车的在屏幕上坐标Point(x,y),作为小车的系统状态值和. 很明显它是一个2维列向量:

? 即: 系统状态值个数n=2,? ?初始状态值

测量值为当前鼠标的坐标Point(x,y), 小车在每次预测更新时,使用当前鼠标位置更新自己的位置.?

显然,测量值是一个2维列向量:

即: 测量值个数m=2

根据上一节对五大状态方程的应用分析.我们很容易得到:

1.因为在鼠标输入的测量值不参与计算时,系统状态值的变化为:?=,得:?

2.因为此例子中小车的运动没有速度和加速度控制. 所以控制矩阵和控制向量省略不考虑.

3.关于系统不确定性协方差矩阵, 初始值(0,0)或鼠标的位置测量值均认为比较可靠. 我们设置的方差较小的初始协方差矩阵:

?4.系统噪音协方差矩阵,我们设置一个较小值:?

5.传感器噪声协方差矩阵:

6.状态向量到测量值向量转换矩阵?:? 因为都是同一度量单位,不存在复杂变换关系,基本是1:1的恒等关系.得:

?综上,? 该设置的向量和矩阵,我们都有了,下面上核心基类EKF代码:

import numpy as np


class EKF(object):
    """
    A abstrat class for the Extended Kalman Filter, based on the tutorial in
    http://home.wlu.edu/~levys/kalman_tutorial.
    """

    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.
        """
        # 预测状态变换矩阵,比如依据系统运动学方程或几何学方程得出的变换矩阵,维数n,取自状态向量个数n
        # F_k = np.eye(n)  # 返回的是一个二维的数组(N,N)，对角线的地方为1，其余的地方为0.

        # Set up covariance matrices for process noise
        self.Q_k = np.eye(n) * qval  # 系统过程噪声协方差矩阵

        # Current state is zero,
        # F_k(n,n) * X^_k-1(n,1) --> x^_k(n,1) 新时刻状态向量
        self.x = np.zeros(n)  # 上一时刻(k-1)或当前时刻k的状态向量:  n个元素向量,或nx1矩阵

        # 协方差矩阵中间状态: P_k
        # No current prediction noise covariance
        self.P_current = None
        # 当前时刻最优估计协方差矩阵,下一次预测和更新会复制给P_k-1
        self.P_result = np.eye(n) * pval

        """
        For update
        """

        # 传感器自身测量噪声带来的不确定性协方差矩阵
        # Set up covariance matrices for measurement noise
        self.R_k = np.eye(m) * rval

        # 单位矩阵I, 这里当数字1使用.  P_k = P_k - K_k*H_k*P_k = (I - K_k*H_k)*P_k
        # Identity matrix(单位矩阵) will be useful later
        self.I = np.eye(n)

    def updateR(self, R):
        self.R_k = R

    # 输入当前测量值,  返回最优估计值
    def step(self, z):  # 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 ----------------------------------------------------

        # 根据上一时刻系统状态值X_k-1,预测当前时刻X_k. 主要完成方程(1)
        # (1). X_k = F_k * X_k-1 + B_k * u_k
        # F_k: 根据系统模型设计的预测矩阵或状态变量转移矩阵nxn
        # B_k: 系统外部控制矩阵 维数确保可以和前面F_k相乘
        # u_k: 系统外部控制向量,例如加速度,转向等.  维数确保可以和前面F_k相乘
        self.x, F_k = self.stateTransitionFunction(self.x)

        # 根据上一时刻协方差矩阵P_k-1,预测当前时刻协方差矩阵P_k. 主要完成方程(2)
        # (2). P_k = F_k * P_k-1 * F_k^T + Q_k
        # Q_k: 各状态量对应的噪音协方差矩阵nxn
        self.P_current = F_k * self.P_result * F_k.T + self.Q_k  # P_k

        # Update -----------------------------------------------------
        # 返回状态值到测量值的变换矩阵,把系统状态量转换为与测量值向量同样的单位或度量,
        # 必要时需要非线性函数线性化(返回雅各比矩阵)
        # 1. 经雅各比矩阵H_k * 当前状态值向量得 -> 到的预估测量值向量:zz_k(mx1)
        # H_k: 预测状态值转测量值变换函数的雅各比矩阵
        zz_k, H_k = self.stateToMeasurementTransitionFunction(self.x)

        # 卡尔曼增益: K_k
        # (3). K_k = P_k * H_k^T * (H_k * P_k * H_k^T + R_k)^-1
        # R_k: 传感器噪声(不确定性)协方差矩阵mxm
        K_k = np.dot(self.P_current.dot(H_k.T), np.linalg.inv(H_k.dot(self.P_current).dot(H_k.T) + self.R_k))

        # 最终,最优预测状态向量值
        # z_k: 传感器读数或均值
        # (4). X^_k = X_k + K_k * (z_k - H_k * X_k)
        # self.x += np.dot(K_k, (np.array(z) - H_k.dot(self.x)))

        # zz_k: 由状态转测量值函数stateToMeasurementTransitionFunction返回的值
        # (4). X^_k = X_k + K_k * (z_k - zz_k)    # Note:  此处的zz_k = H_k * X_k)
        self.x += np.dot(K_k, (np.array(z) - zz_k))

        # 最后,最优预测协方差矩阵
        # (5). P^_k = P_k - K_k * H_k * P_k = (I - K_k * H_k) * P_k
        self.P_result = np.dot(self.I - np.dot(K_k, H_k), self.P_current)

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

    """
    根据系统运动学方程或几何学方程,更新预测状态向量(nx1)
    # (1). X_k = F_k * X_k-1 + B_k * u_k
    # F_k: 根据系统模型设计的预测矩阵或状态变量转移矩阵nxn
    # B_k: 系统外部控制矩阵
    # u_k: 系统外部控制向量,例如加速度,转向等.
    # 这里注意: B_k * u_k的结果维数要保持和X_k同样的维数,以便做矩阵相加
        
    参数x是含有n个元素的当前(k-1时刻)状态值(Current state)向量,
    返回:
    X_k: 依据方程(1)计算的含有n个元素的新的(k)预测状态向量(nx1)
    F_k: 一个依据系统运动学方程或几何学方程设计的预测状态变换模型(n*n矩阵)
    """

    @abstractmethod
    def stateTransitionFunction(self, x):
        raise NotImplementedError()
        
    """
    预测状态值变换函数,把预测状态值向量转换为与测量值向量同样的单位或度量,
    必要时需要非线性函数线性化(返回雅各比矩阵)
    方程(4). X^_k = X_k + K_k * (z_k - zz)     # zz = H_k * X_k
    
    参数x是含有n个元素的当前(k-1时刻)状态值(Current state)向量,
    返回:
    1. 预估测量值向量ZZ_K: 一个m个元素的预估测量值向量: 经 经雅各比矩阵 * 当前状态值向量 得到的 "预估测量值向量"
    2. H_k: 一个m*n雅各比矩阵H_k,矩阵的元素为: 状态值转换为观测值的非线性函数的一阶导数,或有限差分, 或连续差分的比值
    m为测量值个数, n为状态量个数, 用处1: H_k(mxn) * X(nx1) = ZZ_k(mx1),以便下一步方程(4)执行: Z_k - ZZ_K
    """

    @abstractmethod
    def stateToMeasurementTransitionFunction(self, x):
        raise NotImplementedError()

?上面是本文简单卡尔曼滤波器的核心基类 EKF代码. 为了根据实际项目实现具体的功能,我们需要从EKF继承一个之类TrackerEKF,用来实现基类EKF中的两个抽象方法:

import numpy as np

import EKF

class TrackerEKF(EKF):

    def __init__(self):
        EKF.__init__(self, 2, 2) # 状态值个数: n=2,  测量值个数: m=2

    # 返回:  X_k, F_k
    def stateTransitionFunction(self, x):
        # State-transition function is identity
        # X_k这里直接返回了当前状态值X_k-1的相同值
        return np.copy(x), np.eye(2)  # 返回的是一个nxn矩阵，对角线的地方为1，其余的地方为0.

    # 返回: 预估测量值向量ZZ_K, H_k
    def stateToMeasurementTransitionFunction(self, x):
        H_k = np.eye(2)  # 状态值转换为测量值的函数为: y= f(x) = x,基本是恒等关系,故返回2x2单位矩阵
        return H_k.dot(x), H_k   # 返回经状态转换函数变换后的测量值ZZ_k(mx1) = H_k(mxn) * X(nx1)

?如上: 重要的代码实现和分析,都在代码中做了注释.

上面只是滤波器部分代码. 实际运行需要基于openCV在图形界面操作.相关代码稍后附上,您也可以参考原作者在github上的源码见参考文献. 程序运行效果图:

?蓝色轨迹为鼠标移动轨迹, 绿色轨迹为模拟小车以鼠标位置为测量值的最优评估轨迹.

参考文献:

本文源码基于The Extended Kalman Filter: An Interactive Tutorial for Non-Experts?的作者放在github上的TinyEKF改进而来. 再次特表感谢!