1、基本公式推导
卡尔曼滤波是目前应用很广泛的一种滤波方法，最早由Kalman老先生在1960年提出，网上可以找到原文。这种方法最开始用在航天领域，作为轨道矫正的一种方法，有很好的效果。
卡尔曼滤波的方法的核心思想，就是用另一个测量空间的观测值去纠正当前空间对被测量的量的估计。简单来说，就是用一种方法去测量一个量。同时建立一个模型去估计这个测量的量，最后，按权重的方式求这两种方式的和，就是滤波之后的量的值。而这个权重的大小，就是卡尔曼系数。
首先，我们假设要测量的量为, 这个量有一个模型去描述其随时间的变化，例如计算每天的温度变化，可以大致根据之前几天的温度变化规律得到一个计算矩阵，这里也有一个计算模型去计算这个变量
（1）
其中为转换矩阵，
表示
时刻的噪声，且该噪声服从高斯分布。在其他的卡尔曼滤波公式推导中，会有一个额外的控制量，这里不考虑这个量。
对于测量矩阵，也有一个公式去转换。例如测量温度可以用温度传感器来测量，但是温度传感器的测量是因为温度改变了电阻的阻值，所以根据电压电流以及电阻随温度的变化曲线而计算出来的。在卡尔曼模型中，这一公式可以表示为如下等式
（2）
其中，
是通过测量的量，对应到上述的例子中，就是温度传感器的电阻阻值，
就是温度。
是测量矩阵，用来将测量的量转换成要估计的量。
是测量过程中存在的误差。同样的，
也是服从高斯分布的白噪声。
然后就是卡尔曼滤波的核心思想了，有了这两种方法得到的，那么怎么得到一个更准确的估计值。所以需要将两种方法得到的估计值进行算一下加权平均，就得到了最优的估计值。所以卡尔曼滤波的方法如下：
1: 首先根据模型计算当前时刻的估计值
（3）
2: 然后根据测量矩阵计算当前的测量值的估计值
（4）
3: 然后计算测量值和测量估计值之间的差，并以此作为对最终估计值的调整。从这里可以看出，如果估计的很准，就是说此时
的值和
的值相差很小，那么
对于
的修正也就越少。但是如果估计值和测量值相差很大，那么
对于
的修正也就越大。其中，
是卡尔曼增益，表示滤波器对测量值的信任程度。
（5）
那么如何估计卡尔曼增益，可以用贝叶斯估计的方法推导，也可以用最小二乘法的方式推导，这里用最小二乘法的方式推导
我们假设真实值是，那么卡尔曼滤波计算得到的估计值和真实值之间的协方差
（6）
卡尔曼滤波的估计值和模型的估计值之间的协方差，用来评估两种估计的差别 （7）
根据卡尔曼的估计公式以及测量公式，可以得到
把上述等式展开，可以得到
所以，如果我们要估计的更准确，那么就要更小，就是说真实值和卡尔曼滤波的估计值之间的协方差最小。不考虑估计值之间的相关，那么协方差矩阵的对角线元素就表示了卡尔曼估计值和真实值之间的方差。接下来就是求方差最小的情况下对应的卡尔曼增益
。可以用矩阵的迹的方法求解
可以看出，是
的二次函数，所以根据二次函数求极值的方法，对
求导，得到
令，所以有
（8）
把t的结果带入到
的表达式中，有
（9）
所以根据上述的推导计算，可以得到卡尔曼滤波的计算过程：
- 1、首先，根据已知的模型，以及上一时刻的卡尔曼估计值，计算当前时刻的模型预测值
- 2、根据当前的模型预测值，计算对应的协方差
- 3、根据当前的协方差和测量空间的转换矩阵，计算当前时刻的卡尔曼增益 （8）
- 4、根据卡尔曼增益和测量值，计算当前时刻的卡尔曼估计值
- 5、计算了当前时刻的卡尔曼估计值之后，还需要计算当前的估计值和真实值的协方差矩阵，方便下一次计算 （9）
以上就是卡尔曼滤波的基本过程，以及一些简单的推导。总体上说理解卡尔曼滤波应该算一种最优估计的算法。也是应用很广泛的，然后卡尔曼滤波的推导方法也有很多，除了最小二乘法，也可以从贝叶斯估计的角度推导。两者是类似的。
Code
2、具体说明
先从下面的公式来开始讲解：
其中下标k是指状态，这里我们可以把它看成间隔，比如k=1指1ms，k=2指2ms。我们的目的就是要找到在状态k时X的估计。在公式中就是真实测量值。我们要牢记，这些测量值并不可靠。（如果这些测量值是可靠的，那么我们就没必要学kalman filter了：））。
指卡尔曼增益（这是整个公式中的核心点）。
就是上一状态时的估计。
在上述公式中，我们唯一未知的量就是卡尔曼增益。像测量值及上一状态估计，我们都已经知道了。当然了，求出卡尔曼增益并不简单，但是我们有方法来解决它。
我们从另一角度来思考，让我们假设卡尔曼增益为0.5，我们会得到什么呢？它就是个简单的平均！换句话说，我们应该找到对应于每个状态的更加智能化的卡尔曼增益系数。归根结底就是：
卡尔曼滤波器为每个结果状态找到最优的平均因子。 不知怎的，还记得一些关于过去的状态。
Step1 构建模型
Step2 开始处理
如果你成功的将你的模型拟合到卡尔曼滤波器中，那么接下来的步骤就是决定必要的参数及你的初始值。
我们有两个不同的方程集合：时间更新（预测）及测量更新（校准）。两个方程集合都是应用在第k
个状态
我们在步骤一中建立模型，因此我们知道矩阵A,B和H。很可能，他们都是数值常量。并且他们极可能都是数值1.
我建议你们重新写下这些公式，并看看如何去简化这些公式。
.
上述公式中最痛苦的事儿就是确定R和Q了。R还相对比较容易找出，因为，通常来讲，我们对环境中的噪声还是比较确定的。但是寻找Q就并不明显了。在这个阶段，我们并不能给你一个指定的方法。
为了开始这个处理，我么需要知道和
的估计
Step3 迭代
在我们收集了我们需要的所有信息后，我们开始处理流程，现在我们可迭代估计。我们要牢记上一状态的估计将成为当前状态的估计的输入。
这里，先验估计指测量更新校准前的粗略估计。这里我们使用在测量更新公式中的先验值
在测量更新公式中，我们找到了在状态k时x的估计。并且找到了为了状态k+1估计所需要的值。
我们计算的卡尔曼增益，在下一个迭代步骤中并不需要。 这是一组方程中隐藏的，神秘的，最重要的部分。
我们在测量更新阶段评估的值也称为后验值。这也是说得通的。
一个简单的例子
如果小伙伴们在看到这里还有点稀里糊涂的，那么接下来我们找个例子来加深下理解。
现在，让我们努力去估计一个标量随机常数，例如来自某个信号源的“电压读数”。假设它有一个恒定的a V电压，但是我们读数会不准确，可能会读高了，也有可能读低了。我们假设测量噪声的的标准差为0.1 V。
现在我们来构建我们的模型：
就像我之前说的，我们可以把方程简化成非常简单的形式。
- 首先，我们有一个一维的信号的问题，因此我们模型中的每个系数都是数值，而不是矩阵。
- 我们并没有控制信号uk
- 因为信号是一个常数，那么常数A就是1，因为我们已经知道下一个值等于先前的值。我们很幸运，在这个例子中我们有一个恒定的值，但是即使它具有其他任何线性性质，我们都可以将它假设成1.
- 值H=1,因为我们知道测量值是由状态值和一些噪声组成的。你很少会遇到现实生活中，H与1不相同的情况
最后，我们假设我们得到了下面的测量值