卡尔曼（Kalman）滤波及C++例程
卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。
构建模型：
第一个是线性随机微分方程。其中 为k时刻的系统状态变量，
是k-1时刻的系统状态变量。A是状态转移矩阵或者过程增益矩阵，是算法对状态变量进行预测的依据。B是可选的控制输入增益，在大多数实际情况下并没有控制增益，所以这一项通常为零。
是k时刻系统的控制量。
是过程噪声，是期望为0，协方差为Q的高斯白噪声。
第二个是系统测量值。是k时刻的观测值。H是测量系统参数。
是观测噪声，是期望为0，协方差为R的高斯白噪声。
算法流程：
|
时间更新 （预测） |
测量更新 （更正） |
对于上面未解释的变量有如下解释。
表示k时刻先验状态估计值，这是算法根据前次迭代结果（就是上一次循环的后验估计值）做出的不可靠估计。
、
：分别表示k时刻、k-1时刻后验状态估计值，也就是要输出的该时刻最优估计值，这个值是卡尔曼滤波的结果。
表示k时刻的先验估计协方差，这个协方差矩阵只要确定了一开始的
，后面都可以递推出来，而且初始协方差矩阵
只要不是为0，它的取值对滤波效果影响很小，都能很快收敛。
、
：分别表示k时刻、k-1时刻的后验估计协方差，是滤波结果之一。
：表示卡尔曼增益，是滤波的中间结果。
Q：表示过程激励噪声的协方差。
R：表示测量噪声协方差。
下面是计算迭代过程的一个图：
有些公式比较复杂，可以看状态矩阵退出后的方程（H等于1），从这个式子里可以比较好的理解卡尔曼滤波的核心思想：根据当前仪器的测量值和上一刻的预测值和误差，计算当前的最优值，再计算下一个量。
例子：
现在让我们尝试估计一个标量随机常数，例如来自源的“ 电压读数 ”。因此，让我们假设它具有恒定值 aV（伏）。假设测量噪声的标准偏差为0.1 V。
然后我们来构建我们的模型。
-
这是一个一维信号问题，因此我们模型中的每个实体都是数值，而不是矩阵。
-
例子中没有控制信号，即
不存在。
-
由于信号是常数值，常数A只是1，因为我们已经知道下一个值与前一个值相同。我们很幸运，在这个例子中我们有一个恒定的值，但即使它是任何其他线性性质，我们也可以轻易地假设值A=1。
-
值H = 1，测量由状态值和一些噪声组成。很少会遇到H与1不同的真实案例。
假设，测量值如下
| 时间（毫秒） | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 |
| 价值（V） | 0.39 | 0.50 | 0.48 | 0.29 | 0.25 | 0.32 | 0.34 | 0.48 | 0.41 | 0.4 |
从k = 0开始。应该找到或假设一些初始状态。假设 = 0的估计值，
= 1.那么我们为什么不选择
= 0？这很简单。如果我们选择了这种方式，这将意味着环境无噪音，这个假设将导致所有的结果
=0 （剩余的初始状态）。
再此例子中，我们的方程可以简化如下：
|
时间更新 （预测） |
测量更新 （更正） |
据此，我们可以迭代计算了，可以得到如下结果：
| ķ | 时间更新 | 测量更新 | |||||
|---|---|---|---|---|---|---|---|
| 1 | 0.390 | 0 | 1 |
|
= 0.909 = 0.35 = 0.091 |
0.355 | 0.091 |
| 2 | 0.500 | 0.355 | 0.091 |
|
= 0.476 = 0.424 = 0.048 |
0.424 | 0.048 |
| 3 | 0.480 | 0.424 | 0.048 | 0.442 | 0.032 | ||
| 4 | 0.290 | 0.442 | 0.032 | 0.405 | 0.024 | ||
| 5 | 0.250 | 0.405 | 0.024 | 0.375 | 0.020 | ||
| 6 | 0.320 | 0.375 | 0.020 | 0.365 | 0.016 | ||
| 7 | 0.340 | 0.365 | 0.016 | 0.362 | 0.014 | ||
| 8 | 0.480 | 0.362 | 0.014 | 0.377 | 0.012 | ||
| 9 | 0.410 | 0.377 | 0.012 | 0.380 | 0.011 | ||
| 10 | 0.450 | 0.380 | 0.011 | 0.387 | 0.010 |
此例子的C++程序，需下载Eigen库：https://download.****.net/download/hshiya0412/11003126