(8)

式中，PLi,S_p和PLi,SV为伪卫星的定位和测速的精度因子；ωGCC为陀螺仪测量出的视觉相对于伪卫星全局坐标系的角速率；LCGC为伪卫星接收机天线相对于载体坐标系的平移向量；(·)^×表示反对称矩阵；Vi,S可以根据瞬时差分多普勒求得。因为伪卫星无法求出旋转角度，因此只利用位置和速度信息进行约束。δpi,S关于pi,G，θi,G，Vi,G，bi,g和bi,a的雅克比矩阵为

(9)

δVi,S关于pi,G、θi,G、Vi,G、bi,g和bi,a的雅克比矩阵为

(10)

1.2.3 基于扩展卡尔曼滤波的全局坐标初始化

pk,G和qk,G为全局坐标系下的平移和旋转矩阵。由于伪卫星只能提供全局位置和速度约束，因此若想求得当前坐标系相对全局坐标系下的旋转矩阵，至少需要提供4个历元的位置约束。假设滑窗宽度为10，首先利用扩展卡尔曼滤波初始化全局坐标。

状态预测方程为

(11)

状态更新方程为