第3课 扩展Kalman滤波及多传感器融合1

【扩展Kalman滤波】是线性Kalman滤波在处理非线性系统时的扩展，主要用于解决实际中遇到的非线性问题。在传统的线性Kalman滤波中，系统模型和观测模型都是线性的，但在实际应用中，很多系统如机器人定位、飞行器导航等，其动态行为往往具有非线性特性。 在本课中，通过一个简单的多传感器融合实例——使用GPS和气压计来获取机器人高度信息，展示了如何应用扩展Kalman滤波进行融合。观测模型表示为: \( z_k = C_x^{k-1} + v_k \)，其中\( z_k \)是观测值，\( C_x^{k-1} \)是状态向量的非线性函数，\( v_k \)是观测噪声。GPS和气压计分别给出了不同的高度观测，通过线性平均初步融合数据。 线性化是扩展Kalman滤波的关键步骤，利用泰勒级数将非线性函数展开并保留到一阶项。对于非线性函数\( h(x_k) \)，在预测和更新步骤中，使用其在当前估计值\( x_{k-1} \)处的泰勒展开。预测步骤为: \( x_k = x_{k-1} + w_k \)，更新步骤为: \( x_k = x_{k-1} + g_k(z_k - h(x_k)) \)，其中\( g_k \)是卡尔曼增益，计算时需要用到非线性函数的一阶导数。 在多传感器融合中，每个传感器的观测值都被纳入到滤波过程中，通过卡尔曼增益分配不同传感器的权重，实现信息的最优融合。例如，代码片段展示了对两个传感器观测值的融合，通过迭代计算卡尔曼增益和状态估计，实现了对传感器数据的平滑和校正。 扩展Kalman滤波的优缺点也是需要注意的。优点是能够处理非线性问题，且无需预先计算标称轨迹。但缺点在于，当非线性较强或者滤波误差较大时，线性化的误差也会增大，可能导致滤波效果下降。 此外，课程还提到了一个经典的案例——加速度计和陀螺仪融合，这是一个典型的非线性系统，通常需要使用扩展Kalman滤波或更高级的滤波算法如UKF（无迹卡尔曼滤波）来处理。EKF的局限性在于它依赖于线性化，对于高度非线性的情况，可能会导致较大的误差。尽管如此，EKF仍然是处理非线性滤波问题的常用工具，尤其在工程实践中。 总结起来，扩展Kalman滤波是处理非线性动态系统的一种重要方法，通过线性化非线性函数，将非线性滤波问题转化为近似的线性问题，从而实现对系统状态的优化估计。在多传感器融合场景中，EKF可以有效地融合来自不同传感器的数据，提高估计精度。然而，对于高度非线性问题，可能需要考虑更高级的滤波算法。