卡尔曼滤波 KF | 扩展卡尔曼滤波 EKF (思路流程和计算公式)

简介: 本文分析卡尔曼滤波和扩展卡尔曼滤波,包括:思路流程、计算公式、简单案例等。滤波算法,在很多场景都有应用,感觉理解其思路和计算过程比较重要。

前言

本文分析卡尔曼滤波和扩展卡尔曼滤波,包括:思路流程、计算公式、简单案例等。滤波算法,在很多场景都有应用,感觉理解其思路和计算过程比较重要。


一、卡尔曼滤波

卡尔曼滤波思想由 kalman于 1960 年提出,该方法:

假设状态噪声与观测噪声符合高斯分布;

通过观测数据 对 状态量 进行最优估计。

其实质是计算最大后验概率问题,只能应用于线性系统。


1.1 KF计算公式

卡尔曼滤波法的核心问题是:如何建立滤波所需的状态方程和观测方程。在实际工作过程中,卡尔曼滤波法将为系统提供最优估计,其前提是系统应具备线性模型,使用卡尔曼滤波法可以简化计算量,并且节数据存储。

在定义系统的 状态方程 和 观测方程 分别如下:

image.gif

第一条公式:定义 k-1 时刻的状态为 ,其对应的协方差为。k时刻(当前时刻)的状态为;思路:先用上一时刻的状态,来预估当前时刻的状态。

第二条公式:定义为当前时刻的观测值。思路:通过预估当前时刻的状态 和观测变量,来计算观察值


1.2 KF迭代过程

(1)预测:

image.gif

(2)更新:

首先更新卡尔曼增益 K:

image.gif

然后基于卡尔曼增益 K再更新 更新后验概率分布(即:状态值、协方差)

image.gif


1.3 KF使用说明

卡尔曼滤波仅适用于线性高斯系统, 目前大多数情况下机器人所涉及的状态估计是非线性的, 卡尔曼滤波的结果并不可靠。

针对卡尔曼滤波的缺陷,学者们提出了许多改进方法, 如扩展卡尔曼滤波(EKF)、无损卡尔曼滤波(UKF)、压缩扩展 卡尔曼滤波(CEKF)……。


二、卡尔曼滤波案例——多目标跟踪

多目标跟踪算法的经典算法DeepSort,它是一个两阶段的算法,达到实时跟踪效果,曾被应用于工业开发。

核心思想:使用递归的卡尔曼滤波和逐帧的匈牙利数据关联。

假定跟踪场景是定义在 8 维状态空间(u, v, γ, h, ẋ, ẏ, γ̇, ḣ)中, 边框中心(u, v),宽高比 γ,高度 h 和和它们各自在图像坐标系中的速度。

这里依旧使用的是匀速运动模型,并把(u,v,γ,h)作为对象状态的直接观测量。

image.gif编辑

在目标跟踪中,需要估计目标的以下两个状态:

•均值(Mean):包含目标的位置和速度信息,由 8 维向量(u, v, γ, h, ẋ, ẏ, γ̇, ḣ)表示,其中每个速度值初始化为 0。均值 Mean 可以通过观测矩阵 H 投影到测量空间输出(u,v,γ,h)。

•协方差(Covariance):表示估计状态的不确定性,由 8x8 的对角矩阵表示,矩阵中数字越大则表明不确定性越大。


2.1 卡尔曼滤波器——预测阶段

•step1:首先利用上一时刻 k-1 的后验估计值,通过状态转移矩阵 F 变换,得到当前时刻 k 的先验估计状态

image.gif编辑

其中,状态转移矩阵 F如下:

image.gif编辑

•step2:然后使用上一时刻 k-1 的后验估计协方差来计算当前时刻 k 的先验估计协方差

image.gif编辑

通过上一时刻的后验估计均值和方差 估计 当前时刻的先验估计均值x和协方差P

实现代码如下:

def predict(self, mean, covariance):
    # mean, covariance 相当于上一时刻的后验估计均值和协方差
    std_pos = [
        self._std_weight_position * mean[3],
        self._std_weight_position * mean[3],
        1e-2,
        self._std_weight_position * mean[3]]
    std_vel = [
        self._std_weight_velocity * mean[3],
        self._std_weight_velocity * mean[3],
        1e-5,
        self._std_weight_velocity * mean[3]]
    # 初始化噪声矩阵 Q
    motion_cov = np.diag(np.square(np.r_[std_pos, std_vel]))
    # x' = Fx
    mean = np.dot(self._motion_mat, mean)
    # P' = FPF^T + Q
    covariance = np.linalg.multi_dot((
        self._motion_mat, covariance, self._motion_mat.T)) + motion_cov
    # 返回当前时刻的先验估计均值 x 和协方差 P
    return mean, covariance

image.gif


2.2 卡尔曼滤波器——更新阶段

    • step1:首先利用先验估计协方差矩阵 P 和观测矩阵 H 以及测量状态协方差矩阵 R 计算出卡尔曼增益矩阵 K

    image.gif

      • step2:然后将卡尔曼滤波器的先验估计值 x 通过观测矩阵 H 投影到测量空间,并计算出与测量值 z 的残差 y

      image.gif

      image.gif

        • step3:将卡尔曼滤波器的预测值和测量值按照卡尔曼增益的比例相融合,得到后验估计值 x

        image.gif

          • step4:计算出卡尔曼滤波器的后验估计协方差

          image.gif

          卡尔曼滤波器更新阶段,代码实现:

          def update(self, mean, covariance, measurement):
              # 将先验估计的均值和协方差映射到检测空间,得到 Hx' 和 HP'
              projected_mean, projected_cov = self.project(mean, covariance)
              chol_factor, lower = scipy.linalg.cho_factor(
                  projected_cov, lower=True, check_finite=False)
              # 计算卡尔曼增益 K
              kalman_gain = scipy.linalg.cho_solve(
                  (chol_factor, lower), np.dot(covariance, self._update_mat.T).T,
                  check_finite=False).T
              # y = z - Hx'
              innovation = measurement - projected_mean
              # x = x' + Ky
              new_mean = mean + np.dot(innovation, kalman_gain.T)
              # P = (I - KH)P'
              new_covariance = covariance - np.linalg.multi_dot((
                  kalman_gain, projected_cov, kalman_gain.T))
              # 返回当前时刻的后验估计均值 x 和协方差 P
              return new_mean, new_covariance

          image.gif

          总结一下:

          在目标跟踪中,需要估计目标的以下两个状态:

          •均值(Mean):包含目标的位置和速度信息,由 8 维向量(u, v, γ, h, ẋ, ẏ, γ̇, ḣ)表示,其中每个速度值初始化为 0。均值 Mean 可以通过观测矩阵 H 投影到测量空间输出(u,v,γ,h)。

          •协方差(Covariance):表示估计状态的不确定性,由 8x8 的对角矩阵表示,矩阵中数字越大则表明不确定性越大。

          predict 阶段和 update 阶段都是为了计算出卡尔曼滤波的估计均值 x 和协方差 P,不同的是前者是基于上一历史状态做出的先验估计,而后者则是融合了测量值信息并作出校正的后验估计。

          详细参考:多目标跟踪算法 | DeepSort_一颗小树x的博客-CSDN博客_deepsort


          三、扩展卡尔曼滤波 EKF

          扩展卡尔曼滤波可以通过泰勒展开来解决非线性情况下的问题。

          3.1  EKF计算公式

          image.gif

          其中image.gif编辑是展开后得到的一阶偏导项。 在展开过程中省略了次数较高的项,将状态估计方程转化为 近似的线性化方程。

           

          3.2 EKF迭代过程

          迭代过程与卡尔曼滤波大致相似,

          (1)预测:

          image.gif

          (2)更新:

          首先更新卡尔曼增益 K:

          image.gif

          然后基于卡尔曼增益 K再更新 更新后验概率分布(即:状态值、协方差)

          image.gif

          其中 R'与 Q' 为线性化后状态方程对应的新的协方差。


          3.3 EKF使用说明

          扩展卡尔曼滤波可以在非线性的场景下工作。但是,它在计算的时候忽略了展开式的高次项,因此估计会有误差,有可能会导致滤波器发散。

          几种滤波器的对比说明

          image.gif



          参考文献:

          [1] 李庆玲,许茂洲,张慧祥. SLAM 后端优化原理研究[J]. 技术设计与试验应用,2020.

          [2] 危双丰,庞 帆,刘振彬, 师现杰. 基于激光雷达的同时定位与地图构建方法综述[J]. 计算机应用研究,2020.


          本文直供大家参考学习,谢谢!

          相关文章
          |
          数据可视化 中间件 API
          FreeRTOS记录(一、熟悉开发环境以及CubeMX下FreeRTOS配置)
          熟悉 在 STM32 CubeMX 下面的 FreeRTOS 使用
          6760 2
          FreeRTOS记录(一、熟悉开发环境以及CubeMX下FreeRTOS配置)
          |
          传感器 资源调度 算法
          【数据融合】【状态估计】基于KF、UKF、EKF、PF、FKF、DKF卡尔曼滤波KF、无迹卡尔曼滤波UKF、拓展卡尔曼滤波数据融合研究(Matlab代码实现)
          【数据融合】【状态估计】基于KF、UKF、EKF、PF、FKF、DKF卡尔曼滤波KF、无迹卡尔曼滤波UKF、拓展卡尔曼滤波数据融合研究(Matlab代码实现)
          1173 0
          |
          8月前
          |
          XML 编译器 C语言
          在 MATLAB 中手动安装 MinGW-w64 编译器的详细教程
          本教程详解MATLAB中手动安装MinGW-w64编译器的完整流程,涵盖版本兼容性检查、MSYS2或预编译包安装、PATH配置、mex-setup设置及编译验证。特别提醒:R2020b起官方已不支持MinGW-w64,推荐优先使用内置编译器(如Visual C++)。
          4306 1
          |
          传感器 机器学习/深度学习 分布式计算
          卡尔曼滤波的多传感器数据融合算法
          卡尔曼滤波的多传感器数据融合算法
          2107 0
          |
          机器人 API 定位技术
          具身智能干货|ROS2理论与实践系列(二):ROS2通信机制核心
          机器人是一种高度复杂的系统性实现,一个完整的机器人应用程序可能由若干功能模块组成,每个功能模块可能又包含若干功能点,在不同功能模块、不同功能点之间需要频繁的进行数据交互。比如以导航中的路径规划模块为例: 路径规划时就需要其他功能模块输入数据,并输出数据以被其他模块调用。 输入的数据有地图服务提供的地图数据、定位模块提供的机器人位姿数据、人机交互模块提供的目标点数据......。 输出的路径信息则被运动控制订阅或是回显在人机交互界面上。 那么这些相对独立的功能模块或功能点之间是如何实现数据交互的呢?在此,我们就需要介绍一下ROS2中的通信机制了。
          3337 62
          |
          机器学习/深度学习 人工智能 算法
          Python+YOLO v8 实战:手把手教你打造专属 AI 视觉目标检测模型
          本文介绍了如何使用 Python 和 YOLO v8 开发专属的 AI 视觉目标检测模型。首先讲解了 YOLO 的基本概念及其高效精准的特点,接着详细说明了环境搭建步骤,包括安装 Python、PyCharm 和 Ultralytics 库。随后引导读者加载预训练模型进行图片验证,并准备数据集以训练自定义模型。最后,展示了如何验证训练好的模型并提供示例代码。通过本文,你将学会从零开始打造自己的目标检测系统,满足实际场景需求。
          17350 1
          Python+YOLO v8 实战:手把手教你打造专属 AI 视觉目标检测模型
          |
          机器学习/深度学习 编解码 监控
          目标检测实战(六): 使用YOLOv8完成对图像的目标检测任务(从数据准备到训练测试部署的完整流程)
          这篇文章详细介绍了如何使用YOLOv8进行目标检测任务,包括环境搭建、数据准备、模型训练、验证测试以及模型转换等完整流程。
          30791 59
          目标检测实战(六): 使用YOLOv8完成对图像的目标检测任务(从数据准备到训练测试部署的完整流程)
          |
          安全 Linux 虚拟化
          VMware Tools 12.5.1 下载 - 虚拟机必备组件 (驱动和交互式服务)
          虚拟机必备组件 (驱动和交互式服务)
          27948 14
          VMware Tools 12.5.1 下载 - 虚拟机必备组件 (驱动和交互式服务)
          |
          网络协议 开发工具 git
          解决 git 报错 “fatal: unable to access ‘https://github.com/.../.git‘: Recv failure Connection was rese
          在使用 Git/Git小乌龟 进行代码管理的过程中,经常会遇到各种各样的问题,其中之一就是在执行 git clone 或 git pull 等操作时出现 “fatal: unable to access ‘https://github.com/…/.git’: Recv failure Connection was reset” 的报错。这个问题通常是由网络连接问题或代理设置不正确导致的。在我的个人使用经验中,我亲自尝试了四种方法,它们都能够有效地解决这个报错。个人比较推荐方法二。
          10937 1
          |
          Ubuntu Python
          全网最简约的Vscode配置Anaconda环境(百分百成功)
          全网最简约的Vscode配置Anaconda环境(百分百成功)
          40300 0
          全网最简约的Vscode配置Anaconda环境(百分百成功)