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

(4)

(5)

(6)

视觉目标函数可写为

(7)

根据经验值选取权值初值PLi,Cq=0.1、PLi,Cp=0.01和PLi,CV=0.01^[9]；θ是旋转矩阵的欧拉角；i是滑窗内数据索引(i=1, 2，…10)；pi,C、Vi,C和qi,C是VIO位置、速度和旋转向量；pi,C、pi,s、qi,C和qi,s为后端输出数据；pi,Ci 1、Vi,Ci 1和qi,Ci 1分别为VIO相邻两帧间的平移、速度和旋转变化量。VIO相对于各参数雅克比向量见参考文献[11]。

1.2.2 伪卫星约束

伪卫星约束计算公式为