Udacity毫米波，激光数据融合项目。

激光测量：【vx, vy】

毫米波测量：【ro, rhi,r_dot】

当前数据可能是激光，也可能是毫米波，穿插着来。

毫米波观测空间->状态空间变换是非线性的，可以采用EKF将h(x)近似线性化，经F矩阵转换到当前预测，线性转换后为高斯。也可以通过取上一帧观测sigma点，分别通过h(x)计算转换后的sigma，用这些sigma点来近似拟合一个高斯分布。

激光雷达观测->状态空间是线性变换，可以直接写出来F矩阵。

预测当前位置和速度，状态变量【px, py, vx, vy】

流程：

• 第一帧数据初始化EKF，包括状态量x_k-1，不确定性P_k-1
• 后续每帧数据，计算F矩阵和过程噪声Q（，预测当前状态x_k, 不确定性P_k(状态空间)
• 预测结果转换到观测空间（H矩阵）
• 当前观测z和不确定度R,求残差（z-Hx_k）,残差不确定度（HQ*H(T)+R）,卡尔曼增益K
• 计算融合后的状态量x'和不确定度P'

预测当前位置、速度、偏航角及变化率，状态变量【px, py, v, yaw, yaw_rate】

• 第一帧数据初始化EKF，包括状态量x_k-1，不确定性P_k-1，sigma点权重
• 根据h(x)计算sigma点转换后的点集矩阵（维度*点集）
• 预测系统状态（sigma矩阵权重），预测不确定度P(P_K-1+权重维度变化量)
• 计算拟合的高斯参数，根据观测计算残差、残差不确定度
• 计算融合后的状态量x'和不确定度P'

修改ekf_main.cpp和ukf_main.cpp中输入文件地址和输出文件地址，丢在catkin下编译即可。

rosrun kalman_filter  ekf_node
rosrun kalman_filter  ukf_node