Sonar and video fusion for vehicle trajectory estimation in under-ice environments

Localization and navigation in an under-ice underwater environment presents a difficult challenge. GPS, the standard means of localization for aerial and terrestrial vehicles, is not available underwater. Common underwater navigation solutions such as compass sensors and acoustic triangulation are not available under the ice or require overwhelming support infrastructure. Decreased localization error in such missions would increase the mission capabilities of the underwater vehicle. Proposed here is a method for utilizing common camera and sonar sensors already onboard many underwater vehicles to bound the error drift rates encountered in many navigation systems. A factor graph framework is used to synergistically combine relative pose estimates from these two independent and complementary sensors. A novel noise model is proposed to weight each pose estimate in the overall graph. Optimization of the factor graph over all estimates and corresponding weights can be performed to obtain a robust estimated vehicle trajectory over the entire mission duration. Such a self-contained and robust method can help aid in bounding the error rates of an under-ice navigation system.