卡尔曼滤波器

首先从工程上看卡尔曼滤波算法。

引入一个离散控制过程的系统，该系统可用一个线性随机微分方程（linear stochastic difference equation）来描述：
X(k)=A⋅X(k−1)+B⋅U(k)+W(k)X(k) = A \cdot X(k-1) + B \cdot U(k) + W(k)X(k)=A⋅X(k−1)+B⋅U(k)+W(k)

这里：X(k)X(k)X(k) 是 kkk 时刻的系统状态，U(k)U(k)U(k) 是 kkk 时刻对系统的控制量。AAA 和 BBB 是系统参数，对于多模系统，它们为矩阵。

再加上系统的测量值：
Z(k)=H⋅X(k)+V(k)Z(k) = H \cdot X(k) + V(k)Z(k)=H⋅X(k)+V(k)

这里：Z(k)Z(k)Z(k) 是 kkk 时刻的测量值，HHH 是测量参数，对于多模系统，HHH 为矩阵。W(k)W(k)W(k) 和 V(k)V(k)V(k) 分别表示过程和测量的噪声，它们被假设成高斯白噪声，它们的 covariance 分别是 Q,RQ, RQ,R。

对于满足上面条件（线性随机微分系统，过程和测量都是高斯白噪声），卡尔曼滤波器是最优的信息处理器。

下面介绍卡尔曼滤波器算法流程及核心公式。

首先利用系统过程模型，来预测下一个状态的系统：
X(k∣k−1)=A⋅X(k−1∣k−1)+B⋅U(k)(1)X(k | k-1) = A \cdot X(k-1 | k-1) + B \cdot U(k) \tag{1}X(k∣k−1)=A⋅X(k−1∣k−1)+B⋅U(k)(1)

X(k∣k−1)X(k | k-1)X(k∣k−1) 是利用上一状态预测的结果，X(k−1∣k−1)X(k-1 | k-1)X(k−1∣k−1) 是上一状态的最优结果。U(k)U(k)U(k) 为现在状态的控制量，若没有，则为 000。

现在更新 X(k∣k−1)X(k | k-1)X(k∣k−1) 的 covariance，用 PPP 表示：
P(k∣k−1)=A⋅P(k−1∣k−1)⋅A′+Q(2)P(k | k-1) = A \cdot P(k-1 | k-1) \cdot A' + Q \tag{2}P(k∣k−1)=A⋅P(k−1∣k−1)⋅A′+Q(2)

这里：P(k∣k−1)P(k | k-1)P(k∣k−1) 是 X(k∣k−1)X(k | k-1)X(k∣k−1) 对应的 covariance，P(k−1∣k−1)P(k-1 | k-1)P(k−1∣k−1) 是 X(k−1∣k−1)X(k-1 | k-1)X(k−1∣k−1) 对应的 covariance。A′A'A′ 表示 AAA 的转置，QQQ 是系统的 covariance。

式（1）（2）用来对系统进行预测。

然后再收集系统的观测值 Z(k)Z(k)Z(k)，则最优值：
X(k∣k)=X(k∣k−1)+Kg(k)⋅(Z(k)−H⋅X(k∣k−1))(3)X(k | k) = X(k | k-1) + K_g(k) \cdot (Z(k) - H \cdot X(k | k-1)) \tag{3}X(k∣k)=X(k∣k−1)+Kg(k)⋅(Z(k)−H⋅X(k∣k−1))(3)

这里 KgK_gKg 为卡尔曼增益（Kalman Gain）。

Kg(k)=P(k∣k−1)⋅H′H⋅P(k∣k−1)⋅H′+R(4)K_g(k) = \frac{P(k | k-1) \cdot H'}{H \cdot P(k | k-1) \cdot H' + R} \tag{4}Kg(k)=H⋅P(k∣k−1)⋅H′+RP(k∣k−1)⋅H′(4)

到目前为止，已经得到 kkk 状态下的最优估算值 X(k∣k−1)X(k | k-1)X(k∣k−1)。

但是为了要卡尔曼滤波器不断得运行下去直到系统过程结束，需要更新 kkk 状态下 X(k∣k−1)X(k | k-1)X(k∣k−1) 的 covariance：
P(k∣k)=(I−Kg(k)⋅H)P(k∣k−1)(5)P(k | k) = (I - K_g(k) \cdot H) P(k | k-1) \tag{5}P(k∣k)=(I−Kg(k)⋅H)P(k∣k−1)(5)

这里 III 为 111 的矩阵，对于单模系统 I=1I = 1I=1。

当系统进入 k+1k+1k+1 状态时，P(k∣k)P(k | k)P(k∣k) 就是式（2）中的 P(k−1∣k−1)P(k-1 | k-1)P(k−1∣k−1)。这样，算法就可以自回归地运算下去。

扩展卡尔曼滤波器

由上述的介绍可以得知，卡尔曼滤波器只适用于线性系统模型，然而实际中的系统往往都是非线性模型。所示必须对卡尔曼滤波器进行修改。

首先要了解一下线性化卡尔曼滤波。它和线性卡尔曼滤波器在滤波器的算法方面有同样的算法结构。不一样的地方在于这两者的系统模型不同。线性卡尔曼滤波器的系统本身就是线性系统，而线性化卡尔曼滤波器的系统本身是非线性系统，但是机智的大神们将非线性的系统进行了线性化，于是卡尔曼滤波就可以用在非线性系统中了。对于一个卡尔曼滤波器的设计者，就可以不管模型到底是一开始就是线性系统还是非线性系统线性化得到的线性系统，反正只要是线性系统就好了。

但是线性化卡尔曼滤波器会发散。为什么会发散呢？是这样，我们在对非线性系统进行线性化的过程中，只有被线性化的那个点附近的线性化模型和真实的模型相近，远的误差就大了，那么这个时候卡尔曼滤波器的效果就不好。所以线性化的这个限制要时刻考虑，这也就是为什么要把线性卡尔曼滤波器和线性化卡尔曼滤波器区分开的理由。而决定一个线性化滤波器成功与否的关键就在于这个滤波器系统模型线性化得好不好。

EKF 基于线性化系统的思想，将系统函数的非线性函数作一阶Taylor展开，得到线性化系统方程。扩展的卡尔曼滤波器算法就是适用于非线性系统的卡尔曼滤波器。它与经典的线性卡尔曼滤波器很相似，算法步骤和结构都相同。不同在于系统模型和矩阵 AAA 和 HHH。

协方差

协方差（英语：Covariance），在概率论与统计学中用于衡量两个随机变量的联合变化程度。

若变量X的较大值主要与另一个变量Y的较大值相对应，而两者的较小值也相对应，则可称两变量倾向于表现出相似的行为，协方差为正。在相反的情况下，当一个变量的较大值主要对应于另一个变量的较小值时，则两变量倾向于表现出相反的行为，协方差为负。即协方差之正负号显示著变量的相关性。

方差为协方差的一种特殊情况，即该变量与其自身之协方差。

期望值分别为 E(X)=μE(X) = \muE(X)=μ 和 E(Y)=υE(Y) = \upsilonE(Y)=υ 的两个具有有限二阶矩的实数随机变量 XXX 和 YYY 之间的协方差定义为：
cov(X,Y)=E((X−μ)(Y−υ))=E(X⋅Y)−μυ\text{cov}(X,Y) = E( (X - \mu) (Y - \upsilon) ) = E(X \cdot Y) - \mu \upsiloncov(X,Y)=E((X−μ)(Y−υ))=E(X⋅Y)−μυ

协方差表示的是两个变量的总体的误差，这与只表示一个变量误差的方差不同。如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值，另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值。如果两个变量的变化趋势相反，即其中一个大于自身的期望值，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。

Ref：

MATLAB实现卡尔曼滤波器(KF、EKF)
协方差 - WikiPedia
卡尔曼滤波（Kalman Filter）原理与公式推导

【UWB】Kalman filter, KF卡尔曼滤波, EKF 扩展卡尔曼滤波相关推荐

始卡尔曼滤波算法（KF）、扩展卡尔曼滤波算法（EKF）以及无迹卡尔曼滤波算法（UKF）三者之间的区别？
原始卡尔曼滤波算法(KF).扩展卡尔曼滤波算法(EKF)以及无迹卡尔曼滤波算法(UKF)三者之间的区别? 原文:https://www.zhihu.com/question/22714163/answ ...
卡尔曼滤波（KF）和扩展卡尔曼滤波（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）、扩展卡尔曼滤波（EKF）以及无迹卡尔曼滤波（UKF）
由于研究需要,最近在看卡尔曼滤波,做个小总结. 最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Kолмогоров等人的研究工作,后人统称为维纳滤波理论.从理论上说,维纳滤波的最大 ...
使用卡尔曼滤波和扩展卡尔曼滤波进行毫米波雷达和激光雷达数据融合示例
前言在<无人驾驶技术入门(十三)| 手把手教你写卡尔曼滤波器>的分享中,我以激光雷达的数据为例介绍了卡尔曼滤波器(KF)的七个公式,并用C++代码实现了激光雷达障碍物的跟踪问题:在< ...
扩展卡尔曼滤波python_扩展卡尔曼滤波（MRPT）
#include #include#include#include#include using namespacemrpt;using namespacemrpt::bayes;using names ...

【UWB】Kalman filter, KF卡尔曼滤波, EKF 扩展卡尔曼滤波

文章目录

卡尔曼滤波器

扩展卡尔曼滤波器

协方差

Ref：

【UWB】Kalman filter, KF卡尔曼滤波, EKF 扩展卡尔曼滤波相关推荐

最新文章

热门文章