本文仅用于个人学习的总结。

0. 参考资料

图说卡尔曼滤波，一份通俗易懂的教程：https://zhuanlan.zhihu.com/p/39912633
在此之上的一个解释：https://zhuanlan.zhihu.com/p/81404580

1. 基础概念

当物体在运动时，根据当前时刻的位置，和运动方程，可以推测出下一个时刻的位置。当到达下一个位置时，能够通过传感器观测到一个位置，两个位置进行融合，得到最终的位置。

2. 数学

2.1 符号定义：

x^k\hat{x}_kx^k：k时刻的待估计状态
FkF_kFk：k时刻的状态转移矩阵
PkP_kPk：k时刻状态的协方差矩阵
BkB_kBk：k时刻控制矩阵
uku_kuk：k时刻控制向量
QkQ_kQk：k时刻环境的不确定度协方差矩阵
zkz_kzk：观测数据
RkR_kRk：观测数据的协方差矩阵
HkH_kHk：观测矩阵
KKK：卡尔曼增益

2.2 预测

所谓预测，是指在根据前一个状态预测下一个状态，所需要预测的内容包括：位置与相应的协方差矩阵。我们认为状态量是一个高斯分布。即：(μ,Σ)∼N(x^,P)(\mu, \Sigma) \sim \mathcal N(\hat{x}, P)(μ,Σ)∼N(x^,P)
最理想情况
状态转移：x^k=Fkx^k−1\hat{x}_k=F_k \hat{x}_{k-1}x^k=Fkx^k−1
协方差矩阵递推：Pk=FkPk−1FkTP_k=F_k P_{k-1} F_k^TPk=FkPk−1FkT
（注意：若Cov(x)=Σ,则Cov(Ax)=AΣATCov(x)=\Sigma, 则Cov(Ax)=A \Sigma A^TCov(x)=Σ,则Cov(Ax)=AΣAT）
当有外部输入时，比如有外力作用，状态不仅仅是一个简单的转移，需要加上运动量 BkukB_k u_kBkuk，此时更新为：
状态转移：x^k=Fkx^k−1+Bkuk\hat{x}_k=F_k \hat{x}_{k-1}+B_k u_kx^k=Fkx^k−1+Bkuk
当环境存在不确定性时
即虽然可能运动方程没有错误，但受到环境扰动，最终到达位置是不确定的。这个不确定和之前的不确定并不一样，之前的协方差矩阵表示当前位置的不确定，而这里的环境不确定性则是假设之前位置确定后重新推算出的位置不确定，所以可以理解成在原来“不确定的分布区间”中的每一个点在映射过程中仍有了新的不确定性，“不确定的分布区间”从而变大。
此时协方差递推为：Pk=FkPk−1FkT+QkP_k=F_k P_{k-1} F_k^T+Q_kPk=FkPk−1FkT+Qk
一般来说，QkQ_kQk 是非负定的，即不肯能由于噪声而使不确定度更小。

2.3 修正（更新）

所谓更新，或修正，是指在原有推测的状态基础上，根据传感器的观测数据，得到新的状态和协方差的递推。我们依旧认为观测数据是一个正态分布，满足(μ,Σ)∼N(z^,R)(\mu, \Sigma) \sim \mathcal N(\hat{z}, R)(μ,Σ)∼N(z^,R)。
这里的观测数据是由当前状态 x^\hat{x}x^ 测得的，通过观测矩阵HHH，获得实际的观测到的zzz，从而以上分布可以写成：
(μ,Σ)∼N(Hx^,R)(\mu, \Sigma) \sim \mathcal N(H\hat{x}, R) (μ,Σ)∼N(Hx^,R) 那么预测结果如何和观测结果合并呢？
采用相乘的方式，得到新的正态分布：N(x,μ0,Σ0)∗N(x,μ1,Σ1)=N(x,μ′,Σ′)\mathcal N(x, \mu_0, \Sigma_0)*\mathcal N(x, \mu_1,\Sigma_1) = \mathcal N(x, \mu', \Sigma')N(x,μ0,Σ0)∗N(x,μ1,Σ1)=N(x,μ′,Σ′) 其中 K=Σ0(Σ0+Σ1)−1K=\Sigma_0(\Sigma_0+\Sigma_1)^{-1}K=Σ0(Σ0+Σ1)−1 μ′=μ0+K(μ1−μ0)\mu'=\mu_0+K(\mu_1-\mu_0)μ′=μ0+K(μ1−μ0) Σ′=Σ0−KΣ0\Sigma'=\Sigma_0-K\Sigma_0 Σ′=Σ0−KΣ0 带入实际的分布，可以得到：Hkx^k′=Hkx^k+K(zk−Hkx^k)H_k \hat{x}_k'=H_k\hat{x}_k+K(z_k -H_k\hat{x}_k) Hkx^k′=Hkx^k+K(zk−Hkx^k) HkPk′HkT=HkPkHkT−KHkPkHkTH_kP_k'H_k^T=H_kP_kH_k^T-KH_kP_kH_k^THkPk′HkT=HkPkHkT−KHkPkHkT 其中卡尔曼增益为：K=PkHkT(HkPkHkT+Rk)−1K=P_kH_k^T(H_kP_kH_k^T+R_k)^{-1}K=PkHkT(HkPkHkT+Rk)−1 整理得到：x^k′=x^k+K(zk−Hkx^k)\hat{x}_k'=\hat{x}_k+K(z_k -H_k\hat{x}_k) x^k′=x^k+K(zk−Hkx^k) Pk′=Pk−KHkPkP_k'=P_k-KH_kP_k Pk′=Pk−KHkPk K=PkHkT(HkPkHkT+Rk)−1K=P_kH_k^T(H_kP_kH_k^T+R_k)^{-1} K=PkHkT(HkPkHkT+Rk)−1 即完成了更新部分

2.4 总结

由于假设了预测是正态分布，更新时的观测也是正态分布，可以直接将两个正态分布合并成一个新的正态分布，作为下一次预测的前一个时刻，从而不断进行。
正是由于假设正态分布，才能够不断递推进行。其中从协方差外加状态转移矩阵时可以看出，矩阵作为线性变换是不影正态性的。

总结卡尔曼滤波五个等式
预测：x^k=Fkx^k−1+Bkuk\hat{x}_k=F_k \hat{x}_{k-1}+B_k u_kx^k=Fkx^k−1+Bkuk Pk=FkPk−1FkT+QkP_k=F_k P_{k-1} F_k^T+Q_kPk=FkPk−1FkT+Qk 修正：x^k′=x^k+K(zk−Hkx^k)\hat{x}_k'=\hat{x}_k+K(z_k -H_k\hat{x}_k)x^k′=x^k+K(zk−Hkx^k) Pk′=Pk−KHkPkP_k'=P_k-KH_kP_kPk′=Pk−KHkPk K=PkHkT(HkPkHkT+Rk)−1K=P_kH_k^T(H_kP_kH_k^T+R_k)^{-1}K=PkHkT(HkPkHkT+Rk)−1

当状态转移、观测等不是线性时，就无法保持正态性，所以需要采用扩展卡尔曼滤波(EKF)。

3. 扩展卡尔曼滤波

当状态转移和控制，不是简单的矩阵，而是非线性的函数关系时，例如：xk=f(xk−1,uk−1)+wkx_k=f(x_{k-1}, u_{k-1})+w_kxk=f(xk−1,uk−1)+wk，此时不能够继续用正态分布叠加。
此时将f函数进行taylor展开，采用一阶近似:f(x)≈f(μ)+∂f(μ)∂x(x−μ)f(x) \approx f( \mu )+\frac{\partial f(\mu)}{\partial x} (x-\mu)f(x)≈f(μ)+∂x∂f(μ)(x−μ)

对于向量，一阶导即雅克比矩阵。我们使用雅克比矩阵代替卡尔曼滤波中的线性矩阵FFF等即可。从而得到扩展卡尔曼滤波。

卡尔曼滤波与扩展卡尔曼滤波(EKF)相关推荐

行驶车辆状态估计，无迹卡尔曼滤波，扩展卡尔曼滤波（EKF UKF）
行驶车辆状态估计,无迹卡尔曼滤波,扩展卡尔曼滤波(EKF UKF) 软件使用:Matlab Simulink 适用场景:采用扩展卡尔曼滤波和无迹卡尔曼滤波EKF UKF进行行驶车辆的"车速, ...
卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理
所有滤波问题其实都是求感兴趣的状态的后验概率分布,只是由于针对特定条件的不同,可通过求解递推贝叶斯公式获得后验概率的解析解(KF.EKF.UKF),也可通过大数统计平均求期望的方法来获得后验概率(PF ...
【概率机器人】3.1 卡尔曼滤波、扩展卡尔曼滤波和无迹卡尔曼滤波
这一章将介绍卡尔曼滤波.扩展卡尔曼滤波以及无迹卡尔曼滤波,并从贝叶斯滤波的角度来进行分析并完成数学推导.如果您对贝叶斯滤波不了解,可以查阅相关书籍或阅读 [概率机器人 2 递归状态估计]. 这三种滤波 ...
【camera-lidar-radar】基于卡尔曼滤波和扩展卡尔曼滤波的相机、激光雷达、毫米波雷达多传感器后融合
[camera-lidar-radar]基于卡尔曼滤波和扩展卡尔曼滤波的相机.激光雷达.毫米波雷达多传感器后融合代码下载地址(C++ and Python):下载地址红点和蓝点分别表示radar和 ...
使用卡尔曼滤波和扩展卡尔曼滤波进行毫米波雷达和激光雷达数据融合示例
前言在<无人驾驶技术入门(十三)| 手把手教你写卡尔曼滤波器>的分享中,我以激光雷达的数据为例介绍了卡尔曼滤波器(KF)的七个公式,并用C++代码实现了激光雷达障碍物的跟踪问题:在< ...
扩展卡尔曼滤波python_扩展卡尔曼滤波（MRPT）
#include #include#include#include#include using namespacemrpt;using namespacemrpt::bayes;using names ...
卡尔曼滤波（KF）和扩展卡尔曼滤波（EKF）相应推导
从上个世纪卡尔曼滤波理论被提出,卡尔曼滤波在控制论与信息论的连接上做出了卓越的贡献.为了得出准确的下一时刻状态真值,我们常常使用卡尔曼滤波.扩展卡尔曼滤波.无迹卡尔曼滤波.粒子滤波等等方法,这些方法在 ...
路面附着系数估计_无迹?扩展卡尔曼滤波（UKF EKF）
路面附着系数估计_无迹?扩展卡尔曼滤波(UKF EKF) 软件使用:Matlab Simulink 适用场景:采用无迹扩展卡尔曼滤波UKF进行路面附着系数估计,可实现"不变路面,对接路面和 ...
路面附着系数估计_无迹扩展卡尔曼滤波（UKF EKF）
路面附着系数估计_无迹扩展卡尔曼滤波(UKF EKF) 软件使用:Matlab Simulink 适用场景:采用无迹扩展卡尔曼滤波UKF进行路面附着系数估计,可实现"不变路面,对接路面和对 ...
路面附着系数估计_无迹扩展卡尔曼滤波（UKF/EKF）采用无迹/扩展卡尔曼滤波UKF进行路面附着系数估计，可实现“不变路面，对接路面和对开路面”等工况的路面附着系数估计
路面附着系数估计_无迹?扩展卡尔曼滤波(UKF/EKF) 软件使用:Matlab/Simulink 适用场景:采用无迹/扩展卡尔曼滤波UKF进行路面附着系数估计,可实现"不变路面,对接路面和 ...

卡尔曼滤波与扩展卡尔曼滤波(EKF)