卡尔曼滤波器

卡尔曼滤波器的主要作用是估计系统的状态,并根据未来的预测值来对系统进行控制。它可以帮助我们从含有噪声和误差的传感器数据中提取出准确的信息,并且具有良好的实时性和可靠性。

Hat{xk}为第k次取平均得到的对真实数据的估计，zk为第k次的测量结果

新的估计值与上一次的估计值有关，这是一个递推式。
卡尔曼滤波器不需要追溯很久以前的数据，而只需要上一次的数据结果即可，这是一种递归的思想。
估计误差eest=估计值-真实值，估计误差是变化的。
测量误差emea=max(测量值-真实值)，测量误差是固定的，由测量仪器决定。这也决定了测量结果必然是在真实值附近的，以测量误差为阈值的随机波动（且通常呈现正态分布），且只有满足这一输入情况的测量数据才能应用卡尔曼滤波器滤波。

当k-1次的估计误差远大于k次的测量误差时，
当k-1次的估计误差远小于k次的测量误差时，
卡尔曼滤波器的循环递归过程如下所示：

协方差Cov描述了两个变量之间的相关性，在数学意义下，认为当Cov>0时两变量呈现正相关，即X增大Y也增大，当Cov<0时认为两变量呈现负相关，即X增大Y减小，当Cov=0时认为两变量不相关，即X增大与减小和Y增大与减小之间无关联。在统计学意义下，认为当Cov>>0时两变量呈现正相关，当Cov<<0时认为两变量呈现负相关，当Cov->0时认为两变量不相关。协方差的公式为：
Cov(X,Y) = E[XY] - E[X]E[Y]=E[(X - E(X))(Y - E(Y))]
协方差矩阵是一个对称矩阵

对于一个离散模型而言，我们可以离散状态空间方程的形式去表示它：

其中X为状态变量，U为输入量，Z为输出量/测量量（可观测）
当然，上述模型是一个理想模型，在实际的模型中，我们的系统会有噪声，测量也会不可避免的出现误差，因而对于一个实际模型而言，我们一般会为其加上过程噪声和测量噪声，即：

过程噪声和测量噪声是不可测的，但他们都被认为符合正态分布，即：

这里假设ω的协方差矩阵为Q，v的协方差矩阵为R
对于期望为0的正态分布，其协方差矩阵有如下计算公式：

推导如下，以两变量的协方差矩阵为例：

由于方差=平方的期望-期望的平方，而对于期望为0的正态分布变量ω1，ω2而言，其期望的平方E2(ω1)和E2(ω2)显然均为0。故其平方的期望E(ω12)和E(ω22)就等于方差Var(ω1)和Var(ω2)。而又由协方差公式Cov(ω1,ω2) = E[ω1ω2] - E[ω1]E[ω2]=E[ω1ω2]，因而可以看到，上式就可以改写成：

证毕。

当我们使用理想模型得到xk的计算值和测量值时，此时这两个值都不是精确解，而仅仅只是一个估计值，但很显然，由于噪声的存在，我们永远都不可能得到这个精确解。此时我们就要使用数据融合的相关知识来得到一个最优解：
先从一个例子入手，我们要测得一个物体的真实质量，现在我们有两个秤，一个秤的标准差为2g，测量结果为30g；另一个秤的标准差为4g，测量结果为32g。那么这个物体的真实质量的最优解是多少呢？

首先我们要认清一个概念，最优解是什么意思？显然，应该是在该解处的方差最小，说明了该解是最精确的。回到题目，我们虽然测量出了两个结果，但由于秤1的方差比秤2的方差更低，从定性上看我们应更信任秤1的结果。为此，引入信任参数K（这个参数也就是卡尔曼增益），引入最优解函数hat{z}=z1+k(z2-z1)。显然，当k->0时，我们更信任z1；当k->1时，我们更信任z2。则现在的求解关键就是求K使得hat{z}的方差最小，为此，我们就要求hat{z}的方差并令导数为零即可，具体推导过程如下：

回到正题，我们的计算值hat{xk-}和测量值hat{xkmea}显然也满足上述的数据融合理念：

现在的问题很简单，我们如何利用不准确的计算值和测量值来得到一个最优解作为k时刻的值，为此我们依照数据融合的知识构建最优解函数（注意上面算出来的是先验估计结果，经过最优解函数求解得到的是后验估计结果，注意状态量方程的公式是：本次的先验估计结果=A上一次的后验估计结果+B输入值，这个公式告诉我们，我们通过状态量方程得到的先验估计结果必须经过最优解函数得到后验估计结果，而下一次的先验估计结果则由本次的后验估计结果决定而非本次的先验估计结果）：

上述红框中的表达式即为卡尔曼滤波器标准方程，现在的目标就是寻找最优解：

为了让计算的参数符合期望为0的正态分布，我们引入第k次的误差ek并假设其协方差矩阵为Pk：
，
故此时的最优解即为ek的方差最小，由于此时的ek为矩阵，其方差最小也就是各变量的方差之和最小，也就是协方差矩阵Pk的迹最小（方阵的迹：方阵X的主对角线上的所有元素之和称之为X的迹，记为tr(X)，即tr(X)=∑xii）。
我们先利用公式求解协方差矩阵Pk：

将hat{xk}利用卡尔曼滤波器公式带入：

则上式等于：

先看第二项的期望，由于I-KkH和Kk都是常数，因而第二项的期望也就是下式的期望：

而由于ek和vk是互相独立的变量，且两者的期望均为0。故E(ek-vkT)=E(ek-)E(vkT)=0。
同理第三项的结果也为0，则上式可以进一步写作：

由于E(ek-ek-T)=Pk-，E(vkvkT)=R（Pk-是第k次的先验误差协方差矩阵，R为测量误差协方差矩阵，由于测量误差恒定，故R为一常数，不随k而改变），故进一步展开：

得到了Pk的表达式后，下一步就是求其迹的极值，即导数为0的Kk值：
在此之前，我们先补充几个矩阵导数公式：

同理可以推导：

我们发现，由于Pk-为协方差矩阵，协方差矩阵是一个对称矩阵，而对称矩阵的转置为其本身，故上式的第二项和第三项是转置关系，而两个互为转置的矩阵的迹相等，于是合并第二项与第三项并使用公式1，对第四项和第五项使用公式2，有：

进一步推导：

可以看到，这就是卡尔曼增益Kk估计/(估计+测量)的公式由来（严格来说，上式的除法应该写作矩阵的逆，除法只在矩阵为1*1的数时成立，但为了与标准形式对照便如此写）
接下来计算一下Pk-的值，首先我们要得到ek-的值：

则：

由于ek-1和ωk-1互相独立，而E(ek-1)和E(ωk-1)都等于0，故第二项和第三项为0，则上式：

而：

蓝色项将Kk代入就是：

故：

于是卡尔曼滤波的一般过程即：
1.根据上一次的后验结果hat{xk-1}和输入uk-1得到这一次的先验结果hat{xk-}
2.根据上一次的后验误差协方差Pk-1得到这一次的先验误差协方差Pk-
3.根据这一次的先验误差协方差Pk-得到这一次的卡尔曼增益Kk
4.根据这一次的先验结果hat{xk-}、卡尔曼增益Kk和测量值zk得到这一次的后验结果hat{xk}，而后验结果hat{xk}也就是我们需要的最优解
5.根据这一次的先验误差协方差Pk-和卡尔曼增益Kk得到这一次的后验误差协方差Pk
6.进入下一次迭代
可以看到卡尔曼滤波是一个递归过程，每一次的计算均只与上一次的结果有关。
其中需要用到的参数常量为状态矩阵A，输入矩阵B，过程噪声协方差矩阵Q，测量噪声协方差矩阵R，输出矩阵H。此外，我们还要给定递归起点，即初始状态hat{x0}和P0。

IIR滤波器(Infinite Impulse Response Filter)是一种数字滤波器，它的输出信号是由过去的输入信号和输出信号的加权和组成的。