卡尔滤波（从小白到自动驾驶系统工程师15）

想象你驾驶着一艘船航行在大海里，你知道自己昨天是从中国台湾省的高雄港出发的，可以通过航向、船速、洋流速度等推算当前位置，你也可以通过仪器（如GPS）测量得到当前位置，两个位置通常是不重合的，而且都含有误差，我们该怎样确定一个可靠的当前位置呢？

或者再想象你看到花丛中有一只蝴蝶在飞来飞去，你只能隐约、断续地看到蝴蝶的闪现，并不能连续看到蝴蝶的运动轨迹，该怎么猜测蝴蝶现在会出现在哪里以及下一刻它会飞到哪里呢？

你还可以想象你在一个房间中，根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度。可是你的经验也不是100%准确的，会有几度的偏差。另外，房间里有一个温度计，但是这个温度计也是不准确的，测量值和实际值相比也有一定的偏差。怎样才能根据自己的经验和这个不太准确的温度计推测出一个相对准确的当前温度呢？

为了解决这类问题，人们设计出了一种特殊的表示方法和推理算法，那就是卡尔曼滤波器。

卡尔滤波（从小白到自动驾驶系统工程师15）(1)

基本概念

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。

Kalman Filter 算法，是一种递推预测滤波算法，算法中涉及到滤波，也涉及到对下一个时刻数据的预测。Kalman Filter 由一系列递归数学公式描述。它提供了一种高效可计算的方法来估计过程的状态，并使估计均方误差最小。卡尔曼滤波器应用广泛且功能强大：它可以估计信号的过去和当前状态，甚至能估计将来的状态，即使并不知道模型的确切性质。

滤波一词起源于通信理论，它是从含有干扰的接收信号中提取有用信号的一种技术，是抑制和防止干扰的一项重要措施。

线性linear，指量与量之间按比例、成直线的关系，在数学上可以理解为一阶导数为常数的函数；非线性则指不按比例、不成直线的关系，一阶导数不为常数。

线性系统是指同时满足叠加性与均匀性（又称为齐次性）的系统。所谓叠加性是指当几个输入信号共同作用于系统时，总的输出等于每个输入单独作用时产生的输出之和；均匀性是指当输入信号增大若干倍时，输出也相应增大同样的倍数。对于线性连续控制系统，可以用线性的微分方程来表示。不满足叠加性和均匀性的系统即为非线性系统。

状态方程：描述系统状态变量间或状态变量与系统输入变量间关系的一个一阶微分方程组(连续系统)或一阶差分方程组（离散系统），称为状态方程。

卡尔滤波（从小白到自动驾驶系统工程师15）(2)

卡尔曼滤波的5个公式

假设我们要研究的对象是一个房间的温度，首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程来描述：

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)分别表示过程和测量的噪声，他们被假设成高斯白噪声，他们的协方差分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

X(k|k-1)=A X(k-1|k-1) B U(k) ……………….….… (1)

P(k|k-1)=A P(k-1|k-1) A’ Q …………………….…. (2)

X(k|k)= X(k|k-1) Kg(k) (Z(k)-H X(k|k-1)) ……...… (3)

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ R) …………. (4)

P(k|k)=（I-Kg(k) H）P(k|k-1) ………………….… (5)

式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。

到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的协方差还没更新。

我们用P表示协方差，式(2)中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差，A’表示A的转置矩阵，Q是系统过程的协方差。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)。

X(k|k)= X(k|k-1) Kg(k) (Z(k)-H X(k|k-1))

其中Kg为卡尔曼增益，到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的协方差。

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ R)