卡尔曼滤波(KF-EKF-UKF)总结学习笔记。

卡尔曼滤波

卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。

为什么使用卡尔曼滤波

在自动驾驶领域，传感器返回值的数据不是$100$%准确的，或者误差并不小于阈值。

在自动驾驶领域，卡尔曼滤波器管饭被用于通过特征级融合激光雷达和毫米波雷达的数据，估计汽车的位置和速度。简单来说，就是将传感器返回的有偏差的测量结果与卡尔曼滤波提供的预测结果相结合。从而获得对准确位置的最佳估计。

例如：假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是$100$%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算$k$时刻的是实际温度值。首先你要根据$k-1$时刻的温度值，来预测$k$时刻的温度。因为你相信温度是恒定的，所以你会得到$k$时刻的温度预测值是跟$k-1$时刻一样的，假设是$23$度，同时该值的高斯噪声的偏差是$5$度（$5$是这样得到的：如果$k-1$时刻估算出的最优温度值的偏差是$3$，你对自己预测的不确定度是$4$度，他们平方相加再开方，就是$5$）。然后，你从温度计那里得到了$k$时刻的温度值，假设是$25$度，同时该值的偏差是$4$度。

由于我们用于估算$k$时刻的实际温度有两个温度值，分别是$23$度和$25$度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为$Kg^2=5^2/(5^2+4^2)$，所以$Kg=0.78$，我们可以估算出$k$时刻的实际温度值是：$23+0.78*(25-23)=24.56$度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到$k$时刻的最优温度值了，下一步就是要进入$k+1$时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入$k+1$时刻之前，我们还要算出$k$时刻那个最优值（$24.56$度）的偏差。算法如下：$((1-Kg)5^2)0.5=2.35$。这里的$5$就是上面的$k$时刻你预测的那个$23$度温度值的偏差，得出的$2.35$就是进入$k+1$时刻以后$k$时刻估算出的最优温度值的偏差（对应于上面的$3$）。

就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值。

卡尔曼滤波器算法

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述： \(X(k)=A X(k-1)+B U(k)+W(k)\) 再加上系统的测量值：

\[Z(k)=H X(k)+V(k)\]

上两式子中，$X(k)$是$k$时刻的系统状态，$U(k)$是$k$时刻对系统的控制量。$A$和$B$是系统参数，对于多模型系统，他们为矩阵。$Z(k)$是$k$时刻的测量值，$H$是测量系统的参数，对于多测量系统，$H$为矩阵。$W(k)$和$V(k)$分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是$Q$，$R$（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出。

首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是$k$，根据系统的模型，可以基于系统的上一状态而预测出现在状态： \(X(k|k-1)=A X(k-1|k-1)+B U(k)\) 式(1)中，$X(k|k-1)$是利用上一状态预测的结果，$X(k-1|k-1)$是上一状态最优的结果，$U(k)$为现在状态的控制量，如果没有控制量，它可以为$0$。

到现在为止，我们的系统结果已经更新了，可是，对应于$X(k|k-1)$的covariance还没更新。我们用$P$表示covariance： \(P(k|k-1)=A P(k-1|k-1) A’+Q\) 式(2)中，$P(k|k-1)$是$X(k|k-1)$对应的covariance，$P(k-1|k-1)$是$X(k-1|k-1)$对应的covariance，$A’$表示$A$的转置矩阵，$Q$是系统过程的covariance。式子$1$，$2$就是卡尔曼滤波器$5$个公式当中的前两个，也就是对系统的预测。

现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态$(k)$的最优化估算值$X(k|k)$： \(X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))\) 其中$Kg$为卡尔曼增益(Kalman Gain)： \(Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)\) 到现在为止，我们已经得到了k状态下最优的估算值$X(k|k)$。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新$k$状态下$X(k|k)$的covariance： \(P(k|k)=(I-Kg(k) H)P(k|k-1)\) 其中$I $为$1$的矩阵，对于单模型单测量，$I=1$。当系统进入$k+1$状态时，$P(k|k)$就是式子(2)的$P(k-1|k-1)$。这样，算法就可以自回归的运算下去。

卡尔曼滤波器的原理基本描述了，式子$1$，$2$，$3$，$4$和$5$就是他的$5$ 个基本公式。根据这$5$个公式，可以很容易的实现计算机的程序。