3、卡尔曼滤波器-------卡尔曼滤波公式的详细推导过程
作者：互联网
参考内容：B站的DR_CAN的卡尔曼滤波器视频
1、状态空间方程
（1）
xk是状态变量，A是状态矩阵，B是控制矩阵，uk是控制变量，wk-1是过程噪声，其中过程噪声是不可测的。但通常将其假设为符合正态分布。即它的概率分布
其中0是期望，Q是协方差矩阵；其中Q的求解可写成下面公式的形式。
举个例子，假设w=[w1;w2];再根据方差公式VAR(X)=E(X2)-(E(X))2，又因为w的概率分布E(X)=0，上面的公式可以写成：
上面的
、
分别代表w1、w2的方差，
、
代表w1、w2之间的协方差。
与过程噪声wk一样，测量噪声vk的概率分布同样也符合期望为0，协方差矩阵为R的正态分布。
在建模的过程中由于过程噪声和测量噪声本身的不确定性，无法对其进行建模。能够真实掌握的是公式1中除了噪声的其他项。即：
（2）
在不加噪声得到的结果，往往只是估计值。根据上面的公式2得到的估计值，称为先验估计值
。
（3）
其中
是估计值，
是测量的估计值。得到公式3的两个式子，不管是测量得到的还是计算得到的，二者由于都不包含噪声项，所以都是估计结果，都不是准确的值。这时候就能体现出kalman滤波的好处，通过kalman滤波的方法，将两个不准确的结果进行一个融合来得到一个更加准确的结果。具体可参照KALMAN滤波器------数学基础_数据融合_协方差矩阵中的数据融合的部分。
经过数据融合后会得到一个新的估计值
，把它称为后验估计值（或者称为最终的估计值）。
最终估计值=算出来的结果（
）+系数*（测量的结果-算出来的结果）。
当G=0时，最终的估计值=算出来的结果；当G=1时，最终估计值=测出来的结果
化成kalman滤波的公式需要进行一个简单的变换即G=KkH。
这就是标准的kalman滤波的公式。
到了这一步我们就有了一个目标，就是找到一个Kk值，使得最终的估计值
趋近于实际值
。Kk的选择明显和噪声有关。例如要是测量的噪声更大的话，会更加相信计算的结果；要是计算的噪声更大的话，会更加相信测量的结果。因此Kk
的取值和误差相关。至于如何量化这个误差。我们引入一个新的变量，误差ek
ek也是一个概率分布服从正态分布,它的期望为0，协方差矩阵为P。
其中协方差矩阵P的计算为（假设ek=[e1;e2]）：
另外，若我们新估计的值距离实际值越小呢，距离实际的方差就越小。方差越小，证明越接近于期望值，所以我们希望选取合适的Kk使得P矩阵的迹最小，也就是上面公式的对角线相加和最小。
综合以上信息，我们得到一个更加明确的目标，那就是选取合适的Kk使得P矩阵的迹最小。
通过上面的计算就能得到一个使得协方差矩阵的迹达到最小的卡尔曼增益Kk。从公式可以看出卡尔曼增益和测量误差协方差R存在一定的关系。当R越到时，Kk越趋近于0，当R越大时，Kk趋近与H-1。也就是说当测量误差小的时候更相信测量的结果，当测量误差较大的时候更相信计算的结果。
到这我们得到了三个方程：
公式中A和B已知，上一次的结果
也是知道的，输入uk-1是可控变量，也是知道的。所以先验估计是已知的。后验估计中的
是已知的，H和zk也是已知的，若Kk是已知就能得到后验估计值。上面只有卡尔曼增益中的先验协方差矩阵估计
是未知，那么接下来对
进行求解。
所以现在得到一个关于协方差矩阵的先验误差
。
在做完校正之后需要更新一下误差协方差矩阵Pk,又因为：
所以最后得到的五个kalman滤波方程为：
到这卡尔曼滤波的所有方程的推导已经全部结束了。后面将从matlab程序进一步学习卡尔曼滤波算法。B站上老师最后的视频上的那个excel文件，由于老师设置了限制没法操作，所以后面自己按照老师的方法自己写了一个。文件在https://i.cnblogs.com/files里面，有需要的可自行下载学习。
标签：推导,Kk,噪声,卡尔曼滤波,估计值,-------,协方差,公式,矩阵 来源： https://www.cnblogs.com/sbb-first-blog/p/16684520.html