卡尔曼滤波器
在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测。
对于一个物体，它具有一个状态，表示位置和速度，通过GPS传感器我们可以获取实时的状态信息，但是同时我们需要预测机器人将会如何去运动。
我们并不知道实际的位置和速度，他们之间可能有很多中不用的组合，但是我们可以根据数据的分布得知，有一些组合的可能性相对大一些。
卡尔曼滤波器假设两个变量都是随机的，并且服从高斯分布，每个变量都有一个均值u，表示随机分布的中心，以及方差C，表示不确定性。
对于速度和位置两个信息，两者之间是有关联的，观测特定位置的可能性取决于当前的速度。如果速度过高，根据旧位置估计新位置则可能已经移动很远，如果移动速度缓慢，则移动距离并不会很远。两个变量之间的关系会给我们带来更多的信息，我们可以通过一个测量值来推算其他变量的可能值，这就是卡尔曼滤波的目的，即尽可能的在包含不确定性的测量数据中提取更多的信息。
这种相关性用协方差矩阵来表示，矩阵中每个元素(i,j)表示第i个和第j个状态变量之间的相关度。
基本问题
我们基于高斯分布来建立状态变量，在时刻k我们需要最佳估计X以及协方差矩阵P两个信息。我们需要根据当前状态来预测下一状态。
基于运动学公式我们可以得到：
即
现在我们有了一个预测矩阵表示下一时刻的状态，但我们不知道应该如何更新协方差矩阵。
所以我们可以得到
外部控制量
除了速度和位置以外，我们还可以得知加速度的值，它可以被看作成一个控制向量。
外部干扰
我们的预测并不会100%的准确，对于位置的预测，我们加上对于加速度的考虑，有速度以及位置的信息，但仍然不能保证他的位置不会其他因素的影响，比如风的干扰。于是在每次预测之后，我们可以添加一些新的不确定性来模拟来自于外部的干扰。
我们通过简单地添加Q得到扩展地协方差
新的最优估计是根据上一最优估计预测得到的，并加上已知外部控制量的修正。而新的不确定性由上一不确定性预测得到，并加上外部环境的干扰。
结合传感器数据
我们会有多个传感器来测量当前地状态，我们通过矩阵H来表示传感器的数据。
卡尔曼滤波可以处理传感器噪声，将原始估计中的每个状态可以和一定范围内的传感器读数对应起来。从测量到的传感器数据中，我们可以得到系统当前处于大致的状态，但由于存在不确定性，某些状态比我们得到的读书更接近真实状态。
我们将这种不确定性用协方差R表示，该分布的均值就是我们读取到的传感器数据，记为z。
预测值满足一个高斯分布，传感器的读数也遵守一个高斯分布。预测值和传感器测量值重叠的部分就是两个估计最可能的值，即给定的所有信息中的最优估计，它遵守一个新的具有独立均值和方差的高斯分布。
根据定义的两类值符合的高斯分布的参数协方差以及均值，我们可以得到
矩阵K成为卡尔曼增益。
Kalman Filter
我们有两个高斯分布，预测部分以及测量部份，将他们与上述式子结合可以计算得出他们之间的重叠部分。
等式两边同时乘H矩阵转置的逆，
对于非线性系统来说，我们使用扩展卡尔曼滤波，区别在与EKF多了一个把预测和测量部份进行线性化的过程。