ukf性能评估,国内的ukf真实度

(16)

(17)

(18)

由于VIO随着时间误差会累积，因此，为了更加准确地求取视觉世界坐标系到全局坐标系的转换，在每次优化完成后重新求取pi,CG和qi,CG。

1.3 有效性检核

为增加定位的稳健性，减少粗差给融合定位带来影响，本文使用下列方案检验数据的有效性。

已知第i 1帧相对视觉世界坐标系(视觉初始化时的参考坐标系)下的平移量pi 1,C，又已知第i帧视觉世界坐标系向全局坐标系的旋转矩阵qi,CG和平移矩阵pi,CG，可以得到第i 1帧全局坐标系下的坐标估值为

(19)

假设伪卫星在i 1时刻的坐标为pi 1,S，方差为σ²，可以建立检核方程为