卡尔曼滤波matlab_汽车毫米波雷达距离测量中的一种扩展卡尔曼滤波实现

本文详细描述了卡尔曼滤波器在汽车毫米波距离测量应用中的工作原理，详细介绍了卡尔曼的工作原理，实际测量中的噪声来源，并在搭建好模型后运用MATLAB分析验证了模型的有效性。

毫米波雷达对待测目标进行间隔时间为T的一系列测量，获得距离，速度和角度信息，这些测量信息中因为包含测量噪声，所以我们不能完全采信这些测量结果。同时，我们假设待测目标按照匀加速度模型进行运动，那么当前状态下的待测目标距离可以按照待测目标上一状态的距离，速度信息依据牛顿线性模型计算得出，但是这种牛顿线性理想模型未能考虑其他能破坏模型的外力作用，比如待测目标的动力输出不可能是恒定不变的，地面阻力以及风的阻力也不是恒定不变的，因此按照牛顿线性模型推算的结果是缺损的，未能考虑其他外力作用。卡尔曼滤波器可以很好的平衡含冗余噪声的测量结果和缺少信息的过于理想的计算结果，并实现对待测目标的准确跟踪。

1、卡尔曼滤波介绍

卡尔曼早期最成功的应用案例是在阿波罗登月计划中成功解决了轨道预测问题。由于卡尔曼滤波是一种时域滤波方法，其算法采用递归形式，便于在计算机上实现，目前，卡尔曼滤波被广泛应用在惯性制导，地图导航，雷达测量等各个领域。总结起来，卡尔曼能解决的问题特征为：一是不能对待测目标进行准确建模，外力作用不稳定，但是掌握外力作用效果的统计值；二是虽然可进行一系列的测量，但是测量结果因含有噪声而不能完全采信。

毫米波雷达测量示意图如图1所示，假设雷达的测量周期T，时刻nT的测量向量记为u(n),

    (1)

r(n)代表目标相对于雷达的径向距离，φ(n)代表目标相对于雷达角度，代表目标相对于雷达径向速度。由于毫米波雷达测量得到的距离速度等信息都是径向的，因此测量向量u(n)是在极坐标系下获得的。

图1、雷达目标测量示意图

但是为了便于利用简单的牛顿线性预测模型对目标进行跟踪，也即对待测目标连续两次时间间隔为T的两个状态进行关联，我们需要获得笛卡尔坐标系下待测目标的距离，速度等信息。由于测量是在极坐标系中而状态跟踪是在笛卡尔坐标系，因此需要进行坐标系转换。由于这种坐标系的转换是非线性的，因此需要使用扩展卡尔曼滤波器。      

假设笛卡尔坐标系下待测目标在时刻nT的状态向量记为S(n):

    (2)

其中x(n),