qq_44207633的博客

原创 IMM-UKF代码阅读

k时刻的预测状态均值在CTRV(X_sig)中进行了计算，X_ctrv发生更新，已经变为状态sigma采样点在k时刻的预测值的均值，与。对k时刻预测状态进行sigma采样，得到X_predict_meas_sig，不过这次没有对角度进行正则化，可能考虑到后续的观测方程也没用到角度，猜测是用了lidar的观测模型，这里用到的P_ctrv已经更新为k时刻预测sigma点拟合的方差了。直接赋值给了Z_sig，应该是代表对当前k时刻预测状态的预测状态观测，只取了x,y，可能是采用了Lidar的线性观测模型。