自动驾驶-卡尔曼滤波，从CF->KCF->UCF土味讲解

这篇文章目的在于让读者能够从整体思路概念上完全掌握kalman filter，同时对于一些具体细节有一个明确认知。讲解方法主要是结合参数说明的算法讲解。

卡尔曼滤波是什么

这里是让大家对卡尔曼有一个映像，后面通过原理说明来解释。

官方说明

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，根据系统输入（观测数据），对系统状态进行最优估计，更新系统估计状态，输出系统估计状态的方法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

土味说明

首先明确，卡尔曼滤波器中有一个系统状态量，用于记录最正确的结果；结合系统输入，用两个量计算出一个更精确的值，用这个值替代原来的系统状态量，完成一次卡尔曼滤波，而这个值也就是当次卡尔曼滤波的输出。

上图为kalman filter的算法步骤以及参量意义说明，下面针对其进行详细说明。
从上面可以直观的发现，x,P即系统状态量及其确定性协方差，不会没被释放，一直在迭代更新输出。

卡尔曼滤波原理

卡尔曼滤波可以简单分成预测和测量更新两个步骤。下面分别对两个阶段进行说明讲解。

Prediction

F是关于时间间隔的状态变换矩阵，可以用上一帧X计算当帧的预测状态量X’。而u是设计模型所带来的预测误差。
P是用来描述X的协方差矩阵，其中对角代表各参数值的精确度，越小越精确；对角外其他数值代表两个参数间的相关性，0代表不相关，数值越大相关性越强。

下面配合例子讲解。

——prediction例子

状态量X=[x,y,vx,vy]，表示目标的速度和位置信息，假设模型为匀速模型，基于上一帧X预测当前帧的X’=[x,y,vx,vy]。公式如下所示：

首先，由于是匀速模型，那么当然不用考虑 a_x,a_y 带来的影响，直接把v当作0进行运算即可，即u=[0,0,0,0]
由于模型为匀速模型，则误差是由于忽略加速度而引起的参数偏差，明确这个误差并无规律性，那么满足ν～N(0,Q)。
a_x,a_y 通过差分得到，这里就不解释F变换矩阵了，一目了然。

那么这里的v有什么用呢，干嘛一会说直接把v当成0，计算出u；一会把u当成ν～N(0,Q)，好像没起到什么作用。
实际是为了计算预测不确定协方差，将在后面讲解。
这里再扩展说一下模型的作用，模型决定了预测的计算公式。只要是模型就一定有误差！但ν～N(0,Q)可能不成立，那么就要找到平均值完成运算，并在末端加上ν～N(0,Q)。总之要明确把误差零均值化！！！（算标准差也是先零均值化的）然后ν～N(0,Q)基于完成P’预测。

先说明Q，在说明一下FPF^T。
前面说明了P是用来说明X的数值精确度以及参量之间的相关性。P‘就是要更新P并考虑上预测误差。已知ν～N(0,Q)，v即为误差，那么根据协方差公式，求出Q，描述**X’**的协方差矩阵，其中对角代表各参数值的精确度，越小越精确；对角外其他数值代表两个参数间的相关性，0代表不相关，数值越大相关性越强。（你想想是不是这么回事）
FPF^T,通过对比说明，假设p～N(u,σ)，X就相当于这里的u，σ相当于P，表示p是关于X的方差为σ的分布。我们可以对于p进行拉伸、缩放、平移等操作，那么u，σ都会随之而变换，其中σ不会因平移而变化，但对于拉伸、缩放就会发生变化，而FPF^T就是这样的作用，F是状态变化，那么P‘=FPF^T+Q。

Measurement Update

Z是实际测量值，H是把X’与Z量纲统一的线性变换矩阵，比如说X记录使用的是s,km,N;测量值为h,m,kN,就可以通过线性变换完成量纲统一，从而求出y。
既然Hx与Z量纲统一，那么HP’H与R量纲统一，从而求出协方差合S。

下面通过对比说明K，比如你要把100g50元单价糖果合200g70元单价糖果混合成什锦糖果，那么定价一般想法肯定是加权平均嘛，price=1/3x50+2/3x70，那么这里的k也是类似的，用来表示P‘与S的比值，则X=(1-kH)X’+KZ’=X’+Ky。
ps.这里说一下为啥K=P’H^TS^-1，P’S^-1 代表占比值，这里的 H^T 是为了实现量纲统一，理解一下。。
这时你就发现不对了，K怎么用来描述Z的权重，(1-K)描述 X’ 的权重？？来回忆一下协方差的意义以及我们的目标，协方差用来描述参量的准确性，我们要让准确性越高的量占比更多，从而得到最准确的估计量。那么HP‘H^T占比越大代表X’应该占比更小，即P‘与权重成反比！所以这里干脆用K代表Z权重（理解一下！）

基于测量值Z更新完状态量X，下面更新P，既然通过X=X‘+Ky改变了状态量的值，那么P也发生相应变化。
这里也用一个对比说明，假设P，R为非矩阵常数量，那么下式成立。（当然这个变换在矩阵中是不成立的，只是用于理解而已。）

总而言之，P更新时差不多时发生这种变换，从而保证了P一定小于R和P’。

以上说完普通卡尔曼滤波。

扩展卡尔曼滤波

下面说说扩展卡尔曼，还是白话。如果你有注意到的话，KF是针对线性系统的，一些的变换累加都是基于线性系统，而扩展卡尔曼（Extend Kalman Filter）则把手伸向了非线性系统，实现在非线性系统上的滤波迭代作用。

简单说一下线性与非线性的区别，y=kx+b是直线，就是线性的，y=kx²+b就是非线性的，即应变量与变量之间的关系都是线性相关即为线性，否则为非线性。所以自然，三角变换，高次变换，logx,exp(x)都是非线性的。线性变换可以用矩阵表示，而非线性变换不可以！！！

如图所示，EKF与KF的区别只有个，量纲统一矩阵H改变了（改变原因是由于矩阵只能表示线性变化），其他并没有什么不同，下面针对这个变化做一个说明。
前面也说了，EKF使用场景是测量值与状态量不完全满足线性关系，这在多数据源融合的时候较常用到。所以如果能够得到一个近似的线性变换矩阵，那么KF也就可以work了。
下面就介绍一下如何求解这样一个近似的变换矩阵，也就是被称为Jacobian矩阵。

求法如上所示，其实就是关于Z求导X各个参量，直接看图上例子就好了。。。。

无损卡尔曼滤波

EKF解决了非线性系统卡尔曼滤波，KF解决了线性系统卡尔曼滤波，那不就完整了吗，那有无损卡尔曼滤波（Unsent Kalman Filter）什么事情？优化呀！KF已经够精简了，但EKF性能还可以提高。Jacobian矩阵涉及到求导，而求导对于处理器来说并不是一项简单的工作，那么用无损卡尔曼滤波，把非线性系统不通过求导的方式去拟合出一个变换矩阵，得到更好的结果。
写不动了，暂时不写了。。以后再写。。。