卡尔曼（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  = = 1	= 1 /（1+0.1）  = 0.909  = 0.909（ 0.390-0） = 0.35  =（1-0.909）1  = 0.091	0.355	0.091
2	0.500	0.355	0.091	= 0.355  = 0.091	= 0.091 /(0.091+0.1）  = 0.476  = 0.355+0.476 （0.500-0.355）  = 0.424  =（1-0.476）0.091  = 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.csdn.net/download/hshiya0412/11003126