Robust Stereo Visual Inertial Navigation System based on Multi-Stage Outlier Removal in Dynamic Environments