My personal ukf test and data fusion
kalman filter
参数选取原则
首先按照动力学方程建立F方程
用H来选择有几个输入参数，这里我有四个参数，分别是y位置y速度,z位置z速度，xy轴的数据已经在之前的融合做完了所以这里H直接返回四个参数。
dim_x = 4， 不知道表示什么，
dim_z = 4 表示观测方程z有四个数
std_y, std_z，表示yz两轴的测量误差，我感觉激光雷达应该有3cm的精度，所以用了0.03
vstd表示速度测量误差，我写了个0.2发现结果还不错，但是感觉误差应该没那么大，但是速度测量结果其实不可信，所以写高了结果就不太好
Q噪声项，var这里我发现用0.2的结果比较好，目前还不知道为什么
P里头分别填上前面y位置y速度，z位置z速度能达到的最大值。
ukf.x = np.array([0., 0., 0., 0.])表示起始位置和起始速度都是0