卡尔曼滤波

​ 本文主要记录一下卡尔曼滤波的通俗一些的解释,毕竟研究SLAM有一段时间了,但是还是对卡尔曼滤波的理解不透彻,有的时候貌似看懂了,过后又忘记了,还是写一篇文章毕竟好。

一、基本认知

1.1 自动驾驶闭环最常用的算法—卡尔曼滤波

参考文献

​ 那作为自动驾驶工程师,各式各样的常用算法就是构建起整个系统的基础,而卡尔曼滤波算法作为贯穿感知、定位、决策和控制整个自动驾驶系统闭环中最常用的算法之一,自然是学习的重中之重。

目标追踪需要用到卡尔曼滤波,多传感器融合需要用到卡尔曼滤波,定位建图需要用到卡尔曼滤波,轨迹跟踪与控制还是会用到卡尔曼滤波。

​ 这一伟大的算法自上世纪六十年代诞生以来已经应用于无数的工程学术项目中,无论你是学控制的、电子的、通信的、计算机的还是车辆、机械的,一定曾经或多或少听说过、学习过这一算法。但是相信你也会和我有一样的感觉,就是老师讲的总是感觉差了那么点意思,总感觉没有深刻理解其精髓。

下面开始正式的卡尔曼滤波学习~

卡尔曼滤波器是一种最优化递归数据处理算法。开篇标明其实际含义。卡尔曼是发明者的名字,滤波是一种方法,二者结合对于初学者而言并不知道这是一种什么算法,甚至“滤波”二字还会产生干扰和误解,让人误以为这是一种和低通滤波器类似的滤除某一类信号的东西。而在之前的学习中,“滤波”的含义已经被解释得非常清楚,即滤除的并非某一类信号,而是不确定性

当我们去描述一个系统的时候,不确定性主要体现在三个方面:

1、不存在完美的数学模型。世界是复杂多变的,人们可以找到数学模型去描述一个系统,但是总是需要各种各样的理想条件,因此没有一个系统能用数学模型完美描述,只是近似程度高与低的差别;

2、系统的扰动不可控也很难建模。例如车辆行驶在路上突遇暴雨,这种扰动如何建模、如何表示?很难;

3、测量传感器存在误差。这也很好理解,传感器就像量物体长度的尺子,用不同的尺子测总会存在不一样的结果。

接下来就通过上面举的尺子的例子来看一下递归算法的实现。我认为这一例子举得是非常巧妙的,通过简单的数学推导让人领悟到卡尔曼滤波思想的精髓。

​ 我们假设用一把尺子量硬币的长度,记第k次的测量结果为$Z_k$,现在需要你去估计一下这个硬币的真实长度(估计值我们记为$\widehat{X}_K$)。很自然的你会想到把这k次的结果求平均,它还有一个高大上的名字叫做均值滤波,因为从另一个角度看求平均值代表着你为每一次测量提供了相同的权重。

image-20230418220007922

很简单的求取平均值的表达式,接下来我们对它做一些变形:

image-20230418220021344

​ 这个变形用到一点小技巧,和高中学的数学归纳法类似。最终得到一个递归的等式,分析这个等式,可以发现,当k增加,$\frac{1}{k}$趋近于0,$\widehat{X}K$就趋近于$\widehat{X}{k-1}$。其背后的逻辑也很好理解,随着测量次数的增加,测量的结果也就不再重要,预测已经能够很好地估计出真实值;同样的,当k很小的时候,例如时k=1,$\widehat{X}_K = z_k$ ,,因为此时测量次数很少,预测还很不确定,测量值起到的作用比较大。

​ 到这里其实就和卡尔曼滤波的核心思想很接近了。卡尔曼滤波分两步,预测和更新,最终的结果实际上就是预测值和观测值之间的加权和,而每次决定两者权重的就是卡尔曼的核心——卡尔曼增益kalman Gain

因此我们把上式改写一下成为

image-20230418220415168

​ 它的实际意义就是当前的估计值等于上一次的估计值+系数x(当前的测量值-上一次的估计值)。

​ 所以卡尔曼滤波只与上一时刻的信息有关,而不需要在全过程记录以往的数据,这就使得其在内存及计算能力有限的嵌入式设备中广为应用。

1.2 基于Kalman滤波器的跟踪算法设计

参考文献[1]俞佳莹. 车辆侧向目标的超声波传感器阵列感知研究[D].清华大学,2016.

Kalman跟踪原理

​ 前常见的应用较为广泛的对目标物进行定位跟踪的算法主要有Kalman滤波器(Kalman Filter,KF)、扩展Kalman滤波器(Extended Kalman Filter,EKF)、无迹 Kalman 滤波器(Unscented Kalman Filter,UKF)以及粒子滤波器(Particle Filter,PF)。其中,根据 Sebastian Thurn 在书 Probabilistic robotics的定义和分类,Kalman 滤波器、扩展 Kalman 滤波器及无迹 Kalman 滤波器均属于参数滤波器粒子滤波器属于**非参数滤波器[**24]。下面重点介绍本文涉及的参数化滤波器 Kalman 滤波器。

(一)状态空间理论和假设

​ Kalman 滤波利用状态空间理论进行对物理系统的数学建模,将物理系统的运行过程看成是一个状态转移过程。系统状态用该状态空间(m 维)的一个向量$X\in R^m$表示。

(1) 离散化基本模型

​ 由于观测过程一般为离散时间过程。可在一定的假设框架下建立离散化系统状态观测模型。必要假设为:1.物体系统的状态转换过程可以描述为一个离散时间的随机过程。2.系统状态受控制输入的影响。3.系统状态及观测过程受到噪声影响。4.对系统状态是非直接可观测。得到离散化的状态转移和观测方程:

image-20230419214935280

​ 其中,$X_k$为系统状态变量,$X\in R^m,U_k$为系统控制输入,$W_K$为系统过程噪声,$Z_k$为观测量$Z_k\in R^n$,$V_k$为观测噪声。函数$f()$为系统转移方程,函数$h()$为测量方程

二、卡尔曼滤波器

参考文献

2.1 前言

​ 由于传感器本身的特性,任何测量结果都是有误差的。以障碍物检测为例,如果直接使用传感器的测量结果,在车辆颠簸时,可能会造成障碍物测量结果的突变,这对无人车的感知来说是不可接受的。因此需要在传感器测量结果的基础上,进行跟踪,以此来保证障碍物的位置、速度等信息不会发生突变。

​ 最经典的跟踪算法莫过于卡尔曼老爷子在1960年提出的卡尔曼滤波器。在无人车领域,卡尔曼滤波器除了应用于障碍物跟踪外,也在车道线跟踪、障碍物预测以及定位等领域大展身手。

2.2 正文

​ 在介绍卡尔曼滤波器数学原理之前,先从感性上看一下它的工作原理。简单来讲,卡尔曼滤波器就是根据上一时刻的状态,预测当前时刻的状态,将预测的状态与当前时刻的测量值进行加权,加权后的结果才认为是当前的实际状态,而不是仅仅听信当前的测量值

初始化(Initialization)

三、协方差矩阵

概念

协方差(Covariance)在概率论和统计学中用于衡量两个变量的总体误差。而方差是协方差的一种特殊情况,即当两个变量是相同的情况。 这个解释摘自维基百科,看起来很是抽象,不好理解。其实简单来讲,协方差就是衡量两个变量相关性的变量。当协方差为正时,两个变量呈正相关关系(同增同减);当协方差为负时,两个变量呈负相关关系(一增一减)。而协方差矩阵,只是将所有变量的协方差关系用矩阵的形式表现出来而已。通过矩阵这一工具,可以更方便地进行数学运算。

  • 版权声明: 本博客所有文章除特别声明外,著作权归作者所有。转载请注明出处!
  • Copyrights © 2022-2024 lk
  • 访问人数: | 浏览次数:

请我喝杯咖啡吧~

支付宝
微信