EKF测量更新原理及其实现方式解析

扩展卡尔曼滤波（Extended Kalman Filter, EKF）是卡尔曼滤波算法的一种扩展形式，它能够处理非线性系统的状态估计问题。在许多实际应用中，尤其是导航系统中，系统的动态模型和观测模型往往是非线性的。因此，EKF提供了一种对这些非线性模型进行有效估计的方法。 在EKF的测量更新过程中，关键的步骤和知识点可以详细解释如下： 1. GPS定位定速结果（GPSDat）：GPSDat包含了当前时刻的GPS定位和速度信息，这些信息是通过GPS系统得到的。其维数为1*7，包括时间戳、位置（在n0系下的m坐标）以及速度（在n系下的m/s单位）。在进行EKF的测量更新时，GPS数据提供了一个关键的测量值，用于更新和校正滤波器对系统状态的估计。 2. SINS惯导积分解算结果结构体（MIMURes）：MIMU（惯性测量单元）提供加速度和角速度的测量，SINS（惯性导航系统）则利用这些数据进行积分计算以估计载体的状态。结构体中的各个字段分别描述了如下信息： - MIMUTime：当前时刻的MIMU测量数据的时刻值。 - dt：当前时刻的积分时间间隔。 - Cb2n：当前时刻MIMU所在载体坐标系b到地理坐标系NED（北-东-天）n的方向余弦矩阵。 - Qua：当前时刻NED坐标系到载体坐标系的旋转四元数。 - Att：当前时刻NED坐标系到载体坐标系的旋转欧拉角（使用321转序表示，即先滚动角，再俯仰角，最后偏航角），其量纲为弧度（rad）。 - Vel：当前时刻载体坐标系相对于地理坐标系的平动速度，单位为m/s。 - BLH：当前时刻载体所在位置的大地坐标（纬度、经度和高度），单位为弧度和米。 3. 导航相关的地球物理参数结构体（NavEarthPar）：这个结构体包含了与导航系统相关的地球物理参数，通常有9个元素。例如，Grav_n表示n系（地理坐标系）中的重力加速度。这些参数是影响导航精度的重要因素，需要根据具体情况调整和使用。 在实际的EKF算法中，测量更新步骤通常包括以下步骤： - 预测步骤：根据上一时刻的系统状态估计和模型的动态信息，预测当前时刻的系统状态估计和状态估计误差的协方差。 - 更新步骤：将预测得到的状态估计与新获取的测量值（如GPS和MIMU数据）进行比较，然后通过卡尔曼增益来计算新状态估计，以更新系统状态估计和误差协方差。 在处理GPS和MIMU数据时，EKF需要处理非线性的状态转移函数和观测函数。这通常通过泰勒级数展开到一阶项来实现，也就是所谓的线性化过程。在EKF中，雅可比矩阵（Jacobi矩阵）是用来对非线性函数进行线性化的主要工具。 EKF在测量更新中面临的挑战主要包括数值稳定性问题，以及在处理高度非线性系统时的近似误差。为了改善这些状况，有时会采用无迹卡尔曼滤波（Unscented Kalman Filter, UKF）作为EKF的替代方案，因为它使用一组精心挑选的点（称为Sigma点）来更准确地逼近非线性函数。 了解以上知识点后，对于使用EKF进行导航系统设计和状态估计的工程师而言，可以更准确地处理GPS和MIMU的数据，从而提供更可靠的导航解决方案。