二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器

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

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

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

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

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

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

六.卡尔曼滤波器(UKF)开发实践之六: 无损卡尔曼滤波器(UKF)进阶-白话讲解篇

七.卡尔曼滤波器(UKF)开发实践之七: 无损卡尔曼滤波器(UKF)进阶-实例篇

--------------------------------正文开始------------------------------------------
本文是此系列文章第二小节,将用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改进而来. 再次特表感谢!

  • 7
    点赞
  • 26
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
二维扩展滤波器(Extended Kalman Filter,EKF)是对滤波器的扩展,用于非线性系统的状态估计滤波器是一种递归滤波算法,通过观测数据和系统模型来估计系统的状态。然而,对于非线性系统,滤波器的线性化假设不再成立,因此需要使用扩展滤波器来处理非线性系统。 在二维扩展滤波器中,系统的状态和观测向量都是二维的。与普通的滤波器类似,扩展滤波器也通过预测和更新两个步骤来进行状态估计。预测步骤使用系统模型(通常是非线性的)来预测当前时刻的状态,并计算预测误差协方差矩阵。更新步骤使用观测数据来校正预测的状态,并更新状态估计和误差协方差矩阵。 在预测和更新步骤中,需要对系统模型进行线性化,即通过在当前状态点处对非线性函数进行一阶泰勒展开来近似非线性函数。这样可以得到线性化的系统模型和观测模型,然后可以使用滤波器的预测和更新公式进行状态估计。 需要注意的是,二维扩展滤波器是一种近似方法,对于高度非线性的系统,可能会存在估计误差较大的情况。此外,对于更复杂的非线性系统,还可以考虑使用其他扩展滤波器的变种,如无迹滤波器(Unscented Kalman Filter,UKF)或粒子滤波器(Particle Filter)等。
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值