视觉SLAM笔记(50) 线性系统和 KF


1. 简化最大似然估计

当假设了马尔可夫性,从数学角度会发生哪些变化呢?
首先,当前时刻状态只和上一个时刻有关,式中等式右侧第一部分
在这里插入图片描述
可进一步简化:
在这里插入图片描述
这里,由于 k 时刻状态与 k − 1 之前的无关
所以就简化成只与 xk−1 和 uk 有关的形式,与 k 时刻的运动方程对应

第二部分可简化为:
在这里插入图片描述
这是考虑到 k 时刻的输入量 uk 与 k − 1 时刻的状态无关,所以把 uk 拿掉

可以看到,这一项实际是 k − 1 时刻的状态分布
于是,这一系列方程说明了,实际在做的是“如何把 k − 1 时刻的状态分布推导至 k 时刻”这样一件事
也就是说,在程序运行期间,只要维护一个状态量,对它进行不断地迭代和更新即可


2. 线性高斯系统

进一步,如果假设状态量服从高斯分布,那只需考虑维护状态量的均值和协方差即可

从形式最简单的线性高斯系统开始,最后会得到卡尔曼滤波器
线性高斯系统是说,运动方程和观测方程可以由线性方程来描述:
在这里插入图片描述
并假设所有的状态和噪声均满足高斯分布
记这里的噪声服从零均值高斯分布:
在这里插入图片描述
为了简洁省略了 R 和 Q 的下标
现在,利用马尔可夫性,假设知道了 k − 1时刻的后验(在 k − 1 时刻看来)状态估计x^\hat{x}k−1 和它的协方差 P^\hat{P}k−1
现在要根据 k 时刻的输入和观测数据,确定 xk 的后验分布

为区分推导中的先验和后验,在记号上作一点区别:
以尖帽子 x^\hat{x}k 表示后验,以横线 xˉ\bar{x} 表示先验分布


3. 卡尔曼滤波器

卡尔曼滤波器的第一步,通过运动方程确定 xk 的先验分布
根据高斯分布的性质,显然有:

在这里插入图片描述
这一步称为预测
它显示了如何从上一个时刻的状态,根据输入信息(但是有噪声),推断当前时刻的状态分布
这个分布也就是先验
记这里的:
在这里插入图片描述
另一方面,由观测方程,可以计算在某个状态下,应该产生怎样的观测数据:

在这里插入图片描述

为了得到后验概率,想要计算它们的乘积
然而,虽然知道最后会得到一个关于 xk 的高斯分布,但计算上是有一丁点儿麻烦的
先把结果设为 xk ∼ N(x^\hat{x}k, P^\hat{P}k),那么:
在这里插入图片描述


4. 推导KF

这里稍微用点讨巧的方法
既然已经知道等式两侧都是高斯分布
那就只需比较指数部分即可,而无须理会高斯分布前面的因子部分
指数部分很像是一个二次型的配方,来推导一下

首先把指数部分展开,有:
在这里插入图片描述
为了求左侧的 x^\hat{x}kP^\hat{P}k,把两边展开,并比较 xk 的二次和一次系数
对于二次系数,有式1:
在这里插入图片描述
该式给出了协方差的计算过程
为了便于后边列写式子,定义一个中间变量,则式2:
在这里插入图片描述

则在式1中左右各乘 P^\hat{P}k,有:
在这里插入图片描述
于是有:
在这里插入图片描述
然后再比较一次项的系数,有:
在这里插入图片描述
整理(取系数并转置)得:
在这里插入图片描述
两侧乘以 P^\hat{P}k 并代入式2,得:
在这里插入图片描述
于是又得到了后验均值的表达
总而言之,上面的两个步骤可以归纳为“预测”(Predict)和“更新”(Update)两个步骤:

在这里插入图片描述
至此,推导了经典的卡尔曼滤波器的整个过程
事实上卡尔曼滤波器有若干种推导方式,而使用的是从概率角度出发的最大后验概率估计的形式
在线性高斯系统中,卡尔曼滤波器构成了该系统中的最大后验概率估计
而且,由于高斯分布经过线性变换后仍服从高斯分布,所以整个过程中没有进行任何的近似
可以说,卡尔曼滤波器构成了线性系统的最优无偏估计


参考:

《视觉SLAM十四讲》


相关推荐:

视觉SLAM笔记(49) 后端状态估计
视觉SLAM笔记(48) 局部地图
视觉SLAM笔记(47) 优化 PnP 的结果
视觉SLAM笔记(46) 基本的 VO
视觉SLAM笔记(45) 搭建 VO 框架


谢谢!

发布了265 篇原创文章 · 获赞 358 · 访问量 337万+
展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie

分享到微信朋友圈

×

扫一扫,手机浏览