A Novel INS/USBL Integrated Navigation Scheme Via Factor Graph Optimization