3.3.3 卡尔曼滤波
1）卡尔曼滤波算法的基本思想。
卡尔曼滤波（Kalman filtering），一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。因为观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作滤波过程。算法的核心思想是，根据当前的仪器测量值和上一刻的预测量和误差，计算得到当前的最优量，再预测下一刻的量。里面比较突出的是观点是把误差纳入计算，而且分为预测误差和测量误差两种，统称为噪声。还有一个非常大的特点是误差独立存在，始终不受测量数据的影响。
2）卡尔曼滤波算法的原理及推导公式。
我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程来描述：
再加上系统的测量值：
上两式子中，X（k）是k时刻的系统状态；U（k）是k时刻对系统的控制量；A 和B是系统参数，对于多模型系统，它们为矩阵；Z（k）是k 时刻的测量值；H 是测量系统的参数，对于多测量系统，H 为矩阵；W（k）和V（k）分别表示过程和测量的噪声。
它们被假设成高斯白噪声（white Gaussian noise），它们的协方差分别是Q，R（这里假设它们不随系统状态变化而变化）。当满足上面的条件时（线性随机微分系统中产生的噪声都是高斯白噪声），卡尔曼滤波器是最优的信息处理器。
下面用它们结合协方差来估算系统的最优化输出。首先，我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
式（3-1）中，X（k∣k－1）是利用上一状态预测的结果，X（k－1∣k－1）是上一状态最优的结果，U（k）为现在状态的控制量，如果没有控制量，它可以为0。到现在为止，系统结果已经更新了，可是对应于X（k∣k－1）的协方差还没更新。
用P 表示协方差：
式（3-2）中，P（k∣k－1）是X（k∣k－1）对应的协方差，P（k－1∣k－1）是X（k－1∣k－1）对应的协方差，A′表示A 的转置矩阵，Q 是系统过程的协方差。
式（3-1）（3-2）就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。有了现在状态的预测结果，再收集现在状态的测量值。结合预测值和测量值，可以得到现在状态（k）的最优化估算值X（k∣k）：
其中g K 为卡尔曼增益（Kalman gain）：
到现在为止，已经得到了（k）状态下最优的估算值X（k∣k）。但是为了让卡尔曼滤波器不断地运行下去直到系统过程结束，还要更新（k）状态下X（k∣k）的协方差：
其中I为1的矩阵，对于单模型单测量，I＝1。当系统进入（k＋1）状态时，P（k∣k）就是式（3-2）的P（k－1∣k－1）。
这样，算法就可以自回归地运算下去。这就是卡尔曼滤波器原理的基本描述。式（3-1）（3-2）（3-3）（3-4）和式（3-5）就是他的5个基本公式。根据这5个公式，可以很容易地实现计算机的程序。