卡尔曼滤波与扩展卡尔曼滤波

1. 背景介绍
   卡尔曼滤波（Kalman filtering）一种线性系统状态方程，对输入输出的观测数据进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。故求解最优估计的算法就称之为卡尔曼滤波算法。目前在机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等应用较广。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测，计算机视觉等等。

2. 卡尔曼滤波算法
   a. 上述所说，滤波器在有噪声的线性系统中，跟踪目标值状态的递归滤波器。它基于贝叶斯(Bayes)概率，预先假定一个模型，并使用这个模型从以前的状态预测（Prediction）当前状态。然后这个预测值与由外部测量仪器获得的实际测量值之间的误差来执行一个补偿（update）过程，这个过程利用误差值推定更准确的状态。
   
   通俗讲解–（可借用上述模型）
   Step 1: 假设在t−1时刻我们预测房间的温度为23度，预测的误差为3度。假设它是服从高斯分布，我们将3度视为温度标准差。（Pk-1=23, Xk-1=3）
   　　Step 2: 根据我们的经验，在没有外界干扰的情况下房间的温度将会恒定不变。于是我们预测在t时刻房间的温度为23度。但是预测本身也会存在误差。我们把这个误差看做是服从高斯分布。假设其标准差为4度。值得一提的是对于4度和3度，两个高斯分布是相互独立的。综合这两个高斯分布，通过我们的经验可以得到t时刻的气温为23度，其标准差为5度。(利用物理模型，预测阶段)
   　　Step 3: 为了更精准的测量室内温度，我们会使用温度计去测量温度。在t时刻温度计的读数为25度。温度计的误差为4度，其服从高斯分布。（当前测定值）
   　　Step 4: 此时我们对t时刻的气温值有两个估计，一个是人为经验的预测，另一个是温度计测量读数。它们的误差都服从高斯分布且相互独立。
   　　我们将其视为两个传感器然后进行加权求得我们最终的估计值。通过一系列计算，我们最终算的房间温度为24.56度，误差为2.35度。（测定值与预测值，补偿阶段）
   　　Step 5: 将Step 4中算出的结果作为t时刻的最终结果，重复Step 1至Step 4便可以算出以后任意时刻房间的气温。（进入下一时刻）
   　 b. 卡尔曼滤波的数学形式
   卡尔曼滤波的整个过程可以被描述为5个公式。
   （1）状态的一步预测方程：X̂ t,t−1=At,t−1X̂ t−1（A作为矩阵，对X进行线性变换）
   （2）均方误差的一步预测：Pt,t−1=At,t−1Pt−1ATt,t−1+Q（对新误差进行标准差更新）
   （3）滤波增益方程（权重）：Ht=Pt,t−1CTt[CtPt,t−1CTt+R]−1（计算权重，决定实际测量值和预估值的计算权重）
   （4）滤波估计方程（T时刻的最优值）：X̂ t=X̂ t,t−1+Ht[Zk−CkX̂ t,t−1]（计算最优值）
   （5）滤波均方误差更新矩阵（T时刻的最优均方误差）：Pt=[I−HtCt]pt,t−1（计算新的方差误差）

3. 扩展卡尔曼滤波算法（EKF）
   上述已经说过，卡尔曼滤波算法是线性算法，而对许多应用是值得改进的。扩展卡尔曼滤波就出现了，这里使用所有时刻采集到的数据进行状态估计，一般对图像而言，相同的地点像素和明暗度是基本不变的，所以数据估计反而更优，在视觉slam中是主流方法。
   a. SLAM中的运动方程和观测方程通常是非线性系统，需要使用相机内参模型及李代数表示的位姿，更不可能是线性系统。经典的KF算法，里面线性结果，则符合高斯分布。而非线性变换，高斯分布也就不再是高斯分布。这时KF需要扩展。
   b. 扩展方式
   通常的做法是，在某个点附近考虑运动方程及观测方程展开成一阶泰勒，只保留一阶项，即线性的部分，然后按线性系统进行推导。
   5步骤中，依次对几个过程进行泰勒转换取其中的线性部分。