卡尔曼滤波是非常经典的预测追踪算法能够在系统存在噪声干扰的情况下进行系统状态的最优估计广泛使用在导航、制导、控制相关的领域。1. 一个简单的场景假设我们开发了一台无人机假设它的名字是Eva想要用它来在城市中送快递Eva身上有一些传感器。可以让我们知道它的速度v比如三维空间中沿x, y, z各轴的速度大小同时Eva身上还有GPS系统、气压计等设备可以获知它的位置p比如经纬度海拔等也就是说我们可以实时观测Eva的状态。那么我们可以把Eva的某一个时刻的状态表示为一个向量不确定性和相关性虽然我们比较肯定Eva此时的状态但是无论如何系统总是会存在误差的无论是计算上还是传感器的检测上所以我们只能认为当前状态是当前真实状态的一个最优估计。那么我们不妨认为Eva的当前状态服从一个高斯分布如下图所示高斯分布的中心就是图中的对于方差也就是图中的椭圆的范围因为我们有两个变量所以可以用一个协方差矩阵来表示如果对协方差矩阵还不了解戳此了解所以Eva的真实状态可能就位于上图椭圆的范围内位于圆心的概率最大。2. 预测下一个位置的系统状态和系统误差Ok接下来我们需要通过Eva当前的状态运用一些物理学的知识来预测它的下一个状态通过简单的物理学知识通过k-1时刻的位置和速度可以推测下一个时刻的状态为写成矩阵形式就是此处的Fk就是状态转移矩阵。Eva的系统误差通过协方差矩阵来表示根据协方差矩阵的性质那么我们所预测的Eva下一个时刻的状态误差为3.考虑系统内部控制为了能让Eva到达任何地方毫无疑问我们需要对它进行控制比如加速和减速假设某个时刻我们施加给Eva的加速度是a那么下一时刻的位置和速度则应该为因此我们的状态预测方程更新为Ok新方程中的Bk我们称为状态控制矩阵而uk称为状态控制向量含义很明显前者表明的是加速减速如何改变Eva的状态而后者则表明控制的力度大小和方向。4. 考虑系统外部影响但是外界可能有很多影响因素导致我们对Eva实施控制的时候并不总是如我们所愿有时候会逆风有时候则是顺风。在此我们猜测外部的不确定因素对Eva造成的系统状态误差服从均值为0高斯分布至此我们就能得到Kalman滤波中完整的状态预测方程因为为0所以有的文章可能会忽略不写但是如果明确知道均值不为零的是时候就需要注意了这要看实际应用时候的场景理解了它的原理就能对各部分的变化有深入体会。5. 此时应该观测到什么前面我们通过Eva的上一个状态对它的当前状态做了缜密的预测此时我们要考虑我们事先安装在Eva身上的各种传感器应该能够观测到什么Eva当前的状态和我们观测到的传感器数据应该具备特定的关系假设这个关系通过矩阵表示为如下图所示在此前对Eva所做的预测状态下我们应该观测到传感器的观测值为因此我们就完成了对观测值的预测预测其结果服从如下高斯分布6. 考虑实际观测的结果好的我们不仅推测了Eva当前的状态还推测了我们应该观测到的传感器数据但是现实和理想之间必然是存在差距的我们预测的观测结果和实际的观测结果可能如下图所示上图中的表示实际观测的结果但是观测的结果肯定也是不准确的所以我们认为其观测噪声是一个均值为0协方差矩阵为的高斯分布即其实也就是说我们对Eva的观测值服从高斯分布Eva真实的情况应该存在以为椭圆心的椭圆内即观测结果服从高斯分布终于来到了最关键的一步卡尔曼滤波需要做的最重要的最核心的事就是融合预测和观测的结果充分利用两者的不确定性来得到更加准确的估计。通俗来说就是怎么从上面的两个椭圆中来得到中间淡黄色部分的高斯分布看起来这是预测和观测高斯分布的重合部分也就是概率比较高的部分。7. 两个高斯概率密度函数的乘积一维的高斯分布通过高斯概率密度函数来表示在坐标轴上画出来是一个类似草帽的形状。下面给出两个高斯概率密度函数相乘的直观的结果。对比标准的高斯概率密度函数相乘的结果是一个乘了特定系数的新的高斯概率密度函数这个系数在后面的演示代码中会计算并且我们可以求解得到这个新的高斯分布的均值和方差分别为通过MATLAB我们可以计算两个高斯概率密度函数的乘积以及通过上述公式计算得到的新的高斯概率密度函数以下是相关的代码和运行截图clear all x-1:0.01:2.5; mu0 0.3; mu1 0.8; sigma0 0.2; sigma1 0.5; sigma0_sq 0.04; sigma1_sq 0.25; y1normpdf(x,mu0,sigma0); y2normpdf(x,mu1,sigma1); y3y1.*y2; k sigma0_sq / (sigma0_sq sigma1_sq); mu (mu0*sigma1_sq mu1*sigma0_sq) / (sigma0_sq sigma1_sq); sigma sqrt((sigma0_sq * sigma1_sq) / (sigma0_sq sigma1_sq)); scale (1.0 / (sqrt(2*pi*(sigma0_sq sigma1_sq)))) * exp(-1.0 * ((mu0-mu1)^2/(2.0 * (sigma0_sq sigma1_sq)))); y4 normpdf(x,mu,sigma);% * scale; figure; plot(x,y1,x,y2,x,y3, x, y4,MarkerSize,20,LineWidth,5); grid; tip1 sprintf((\\mu_0,\\sigma_0) (%.4f, %.4f), mu0, sigma0); tip2 sprintf((\\mu_1,\\sigma_1) (%.4f, %.4f), mu1, sigma1); tip3 (\mu_0,\sigma_0)*(\mu_1,\sigma_1); tip4 sprintf((\\mu^\\prime,\\sigma^\\prime) (%.4f, %.4f), mu, sigma); legend({tip1, tip2, tip3, tip4});运行截图因为MATLAB的函数normpdf使用的参数是均值和标准差所以代码和截图中也是使用均值和标准差表示一个高斯分布。图中蓝色和橙色两个波形的直接乘积是黄色这个波形而它其实可以通过紫色的波形乘上一个系数得到也就是前面代码中的scale这个变量计算公式在上面的已经提供。如果在计算y4紫色的波形的时候乘上这个系数你会发现它的波形就和黄色的波形 (y3) 完全重合了。把对应行稍作修改即可y4 normpdf(x,mu,sigma) * scale;8.新的高斯分布那么我们把关注点放在这个乘积中这个新的高斯概率密度函数其实它就描述了一个新的高斯分布这正是卡尔曼滤波想要的最优估计。在新的均值和方差计算公式中我们令那么可以得到将它们写成矩阵形式就是前面我们已经得到了预测结果和观测结果服从的两个高斯分布如下所以我们可以进行如下推导来得到卡尔曼滤波对当前状态基于预测和观测的最优估计的计算方程好的两边化简下注意K可以展开于是可以得到此处的K′就是传说中的卡尔曼增益9. 实际实现时的计算步骤在实际使用卡尔曼滤波的时候计算的步骤一般为这里把下标去掉了因为在实现的时候即使下标不一样我们用的其实就是一个变量注意和前面的方程进行比对上面的y是测量余量 (measurement residual)S是测量余量协方差矩阵。最重要的是我们要时刻关注不断迭代的系统变量分别是系统的状态x其误差协方差矩阵P和卡尔曼增益K。在实际应用时对Q和R的选择要依据实际情况来定可以不断调试来寻找一个最优解也可以是可变的只要最终效果能够更好。10. 实践中的主要难点10.1. 运动学模型的建立也就是预测方程。这个一方面可以通过比较细的系统辨识来做这方面要结合具体的应用背景不一而足也是普遍做的比较差的地方。10.2. 噪声的统计特性我们得到了2或多个个反馈同一件事的信息A和B但不知道该相信哪个结果所以简单的将两个信息相加除以2加权平均得到一个结果0.5A0.5B。但这个方法过于粗暴不能反映出水平。现在知道了其中一个信息可靠度更好而另一个可靠度不好就要更相信好的信息如现在给可信度好的B更大的权重得到0.2A0.8B这个结果会好于第一个。但仍然觉得low因为A与B的参数是根据经验设置的并不知道是否是最合理的是否是最优化的。所以高级的用优化的方法根据A与B的高斯分布特征去融合A和B信息这个就是卡尔曼。因为建模误差不知道。观测方程到还可以测一下ZRO算一下均值和方差。然后下一步就是大家喜闻乐见的调参的阶段据说是群魔乱舞花样百出。参考文献卡尔曼滤波算法总结_miraito_wa的博客-CSDN博客实际工程项目中是怎么用卡尔曼滤波的?实际工程项目中是怎么用卡尔曼滤波的?