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