基于ESKF算法的惯性和光学组合跟踪定位方法及系统

本公开提供了一种基于ESKF算法的惯性和光学组合跟踪定位方法及系统，可以应用于光学定位、惯性导航追踪、以及手术器械的组合导航定位与实时导航技术领域。该方法包括：通过光学定位追踪模块获取手术器械的光学位姿数据、通过惯性测量模块获取手术器械的惯性位姿数据；利用ESKF滤波器，根据惯性位姿数据预测每个时刻下手术器械的标称状态和误差状态，根据光学位姿数据对每个时刻的误差状态进行更新；根据标称状态和更新后的误差状态确定每个时刻下手术器械的真实状态，其中，真实状态用于指示手术器械的真实位姿。