卡尔曼滤波EKF

一、概述

二、卡尔曼滤波的5个公式

三、应用案例：汽车运动

四、应用案例：温度估计

五、总结

一、概述

初学者对于卡尔曼滤波5个公式有点懵，本文先接地气地介绍5个公式，然后举两个常用例子加强理解，同时附有Matlab代码。卡尔曼滤波在大学课程《现代控制理论》当中有涉及详细讲解。卡尔曼滤波使用条件有：

1、线性系统；
2、系统中噪声(不确定性)服从高斯分布。下文中的方差、误差、偏差、协方差都指不确定性的意思。误差 = 偏差；方差 = 偏差 ²。

在无人驾驶导航定位当中，需要多传感器对汽车位姿进行检测；在多传感器融合方面，使用卡尔曼滤波理论较多。对预测公式新的理解：第一个公式求解的是状态变量的期望值。第二个公式求解的是状态变量的方差。

二、卡尔曼滤波的5个公式

以下内容有手写word，都是原创哈。

三、应用案例：汽车运动

完整的matlab代码：

clear all;
close all;
clc;
Z = (1:100); %观测值
noise = randn(1,100);%1行100列高斯白噪声
Z = Z + noise;

X = [0;0];%状态值
P = [1 0; 0 1];%状态协方差矩阵
F = [1 1; 0 1];%状态转移矩阵
Q = [0.0001, 0; 0 0.0001];%状态转移协方差矩阵
H = [1 0];%观测矩阵
R = 1;%观测噪声方差

figure;
%hold on;
speed = [];
distance = [];
for i =1:100
   %% 预测
   X_ = F*X;
   P_ = F*P*F' + Q;
   %% 更新
   K = P_*H'/(H*P_*H' + R);
   X = X_ + K*(Z(i) - H*X_);
   P = (eye(2) - K*H)*P_;

   speed(i) = X(2);
   distance(i) = X(1);
   %%plot(X(1), X(2));
end
plot(distance, speed);

四、应用案例：温度估计

五、总结

在线性高斯系统中，卡尔曼滤波器构成了该系统中的最大后验概率估计。而且，由于高斯分布经过线性变换后仍服从高斯分布，所以整个过程中我们没有进行任何的近似。可以说，卡尔曼滤波器构成了线性系统的最优无偏估计。

SLAM 中的运动方程和观测方程通常是非线性函数，尤其是视觉 SLAM 中的相机模型，需要使用相机内参模型以及李代数表示的位姿，更不可能是一个线性系统。一个高斯分布，经过非线性变换后，往往不再是高斯分布，所以在非线性系统中，我们必须取一定的近似，将一个非高斯的分布近似成一个高斯分布。我们希望把卡尔曼滤波器的结果拓展到非线性系统中来，称为扩展卡尔曼滤波器（Ex-tended Kalman Filter，EKF）。通常的做法是，在某个点附近考虑运动方程以及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。

EKF的局限性：

1. 首先，滤波器方法在一定程度上假设了马尔可夫性，也就是 k 时刻的状态只与 k −1时刻相关，而与 k − 1 之前的状态和观测都无关（或者和前几个有限时间的状态相关）。这有点像是在视觉里程计中，只考虑相邻两帧关系一样。如果当前帧确实与很久之前的数据有关（例如回环），那么滤波器就会难以处理这种情况。而非线性优化方法则倾向于使用所有的历史数据。它不光考虑邻近时刻的特征点与轨迹关系，更会把考虑很久之前的状态也考虑进来，称为全体时间上的 SLAM（Full-SLAM）。在这种意义下，非线性优化方法使用了更多信息，当然也需要更多的计算。
2.与第六章介绍的优化方法相比，EKF 滤波器仅在 k−1时刻相机位姿处做了一次线性化，然后就直接根据这次线性化结果，把后验概率给算了出来。这相当于在说，我们认为该点处的线性化近似，在后验概率处仍然是有效的。而实际上，当我们离开工作点较远的时候，一阶泰勒展开并不一定能够近似整个函数，这取决于运动模型和观测模型的非线性情况。如果它们有强烈的非线性，那线性近似就只在很小范围内成立，不能认为在很远的地方仍能用线性来近似。这就是 EKF 的非线性误差，是它的主要问题所在。在优化问题中，尽管我们也做一阶（最速下降）或二阶（G-N 或 L-M）的近似，但每迭代一次，状态估计发生改变之后，我们会重新对新的估计点做泰勒展开，而不像EKF 那样只在固定点上做一次泰勒展开。这就导致优化方法适用范围更广，则在状态变化较大时亦能适用。
3.从程序实现上来说，EKF 需要存储状态量的均值和方差，并对它们进行维护和更新。如果把路标也放进状态的话，由于视觉 SLAM 中路标数量很大，这个存储量是相当可观的，且与状态量呈平方增长（因为要存储协方差矩阵）。因此，EKF SLAM 普遍认为不可适用于大型场景。