初始化（GNSS与VIO融合）
GNSS初始化（coarse-to-fine）
- GNSS 初始化决定了state的预测结果
- 在线初始化可以从任意情况下恢复初始state
- anchor point 的局部和全局坐标已知（用于GNSS 全局系和VIO的局部系融合）
- 前提： a. VIO系统已经完成初始化，如重力向量，初始速度，初始IMU bias，尺度）
b. 局部地图系上的光滑轨迹可用于GNSS-VI 的初始化 - 步骤：
a. Coarse anchor point localization: 使用GNSS 的SPP算法（输入：滑窗内的所有伪距），由于滑窗是位于一段空间和时间上的，SPP的结果是当前串口的平均位置
b. Yaw offset calibration: 校准位于ENU frame和local frame间的yaw offset航线偏移（使用低噪声的Doppler Measurement），通过下式可获得初始yaw offset和接收器时钟漂移速率
n为滑窗的大小，pk为第k个epoch观察到的的卫星数量。滑窗内，假设速度恒为VIO求得的结果，k时刻的时钟漂移率恒定。上一步求得的 Coarse anchor point localization用于计算方向向量和旋转矩阵。优化变量为Yaw offset ψ和滑窗内平均时钟漂移速率。优化后，ENU frame和local frame完成校准。
c. Anchor Point Refinement: 完善上一步得到的coarse anchor point,并调整local frame下的轨迹实现在ECEF frame中的alignment. Anchor Point由之前ENU frame的原点变为local frame的原点。 图示见下图：
Frame介绍
- sensor frame(传感器系)
- local world frame（局部系:VIO系统所在系，原点可为任意点，Z轴垂直与lcoal ground plane–红色虚线
- ECEF frame（Earth-Centered,Earth-Fixed:相对于地球的Cartesian坐标系，原点在地球中心处，Z轴垂直于赤道指向正北，下，x,y轴与赤道相交，x轴指向本初子午线—蓝色实线
- ENU frame(Earth-nurth up) 站心地平坐标系:作用是连接local frame和全局ECEF frame，x指向东，y指向北，z向上。对于任意一个ECEF上的点，都有唯一的ENU frame（此ENU原点在ECEF目标点上—黑色实线
- 注： 对于local frame和ENU frame，Z轴都平行于重力方向
ECEF 和local frame间转化关系
- 已知接收器在ECEF下坐标，欲求接收器在local下的坐标，需通过anchor point
- ENU坐标的建立就为了解决此问题
- 已知anchor point在ECEF下坐标，从ENU到ECEF的旋转矩阵为：
φ and λ是纬度和经度
从ENU到local的旋转矩阵R为yaw offset ψ
因此，ECEF到local frame
方程为：接收器在ECEF位置=旋转矩阵*接收器与anchor在local 下位置差+anchor在ECEF中位置
理解：
- 需要求接收器ECEF上位置，已知local 位置，所以需要R矩阵
- 因为接收器在移动，所以R矩阵需要从Anchor确定
- anchor用于ENU的引入，因为anchor的local 和ECEF坐标都已知，所以可以作为frame转换的中间媒介
- anchor值固定，R矩阵也因此可以得出