参考:详解卡尔曼滤波原理
核心思想:将基于预测(先验)和基于测量(后验)的两个高斯分布融合,得到最终的预测值。
什么是卡尔曼滤波:
卡尔曼滤波(Kalman filtering)是一种用于估计系统状态的递归滤波器。它是基于线性动态系统和高斯噪声假设,通过结合系统模型和测量信息,提供最优估计值和最小均方误差的状态估计方法。
卡尔曼滤波器的核心思想是将先验信息(系统模型)和后验信息(测量信息)进行融合,从而获得对系统状态的最优估计。它通过两个主要步骤来实现这一目标:预测和更新。
在预测步骤中,卡尔曼滤波器使用系统的状态转移模型来预测当前时刻的状态。这个预测基于先前的状态估计和系统的动态方程。预测步骤输出两个重要的结果:预测状态和预测协方差。
在更新步骤中,卡尔曼滤波器使用测量模型来结合实际的测量值和预测状态,从而修正预测并获得更准确的状态估计。更新步骤输出的结果是更新后的状态估计和更新后的协方差。
卡尔曼滤波器的关键特点是它能够同时处理系统模型的不确定性和测量的不确定性,并通过递归的方式进行状态估计。它通过动态地调整权衡先验和后验信息的权重,以最优的方式利用可用的信息来估计系统的状态。
从卡尔曼滤波名字就可以看出来,其更倾向于滤波。即对系统噪声和测量噪声进行过滤优化,得出更优的结果。如果系统噪声比较强,那么最终结果就会倾向于测量结果,而当测量噪声强时,最终结果就倾向于系统状态方程描述的结果
。
卡尔曼滤波是个迭代计算过程,不断重复这两步,预测和校正(更新)。校正就是用测量结果和状态方程结果不断计算卡尔曼增益K。K越大越相信测量结果,K越小越相信状态方程描述的结果。
总体流程
步骤(1)预测当前状态x_k和协方差矩阵P_k,得到基于预测的高斯分布
步骤(2) 获取测量值,得到基于测量值的高斯分布
步骤(3)融合两个高斯分布,期间可以得到卡尔曼增益,并得到最终的预测状态和协方差矩阵
1. 预测当前时刻状态
用状态转移矩阵 F_k 来表示这个预测过程。
预测的状态x_k和协方差P_k:
引入外部控制量和外部干扰:
2. 用测量值来修正估计值
传感器读取的数据的单位和尺度有可能与我们要跟踪的状态的单位和尺度不一样,我们用测量矩阵 H_k 来表示传感器的数据。
从测量到的传感器数据中,我们大致能猜到系统当前处于什么状态。但是由于存在不确定性,某些状态可能比我们得到的读数更接近真实状态。我们将这种不确定性(例如:传感器噪声)用协方差R_k,该分布的均值就是我们读取到的传感器数据,称之为z_k描述。
现在我们有了两个高斯分布,一个是在预测值附近,一个是在传感器读数附近。
剩下的就是重叠部分了,这个重叠部分的均值就是两个估计最可能的值,也就是给定的所有信息中的最优估计。
3. 融合高斯分布(卡尔曼增益怎么来的)
以上所有公式中,你只需要用到式(7)、(18)、(19)。(如果忘了的话,你可以根据式(4)和(15)重新推导一下)
我们可以用这些公式对任何线性系统建立精确的模型,对于非线性系统来说,我们使用扩展卡尔曼滤波,区别在于EKF多了一个把预测和测量部分进行线性化的过程。
具体AEC任务