ToA-based Localization Algorithm for Mitigating Positioning Error in NLoS Channel