Fig. 1. Algorithm flow chart

Fig. 2. Postural transformation

Fig. 3. Comparison chart of iPCA-based planar optimization

Fig. 4. Block diagram of tightly coupled nonlinear optimization

Fig. 5. Plane constraints

Fig. 6. Experimental hardware platform

Fig. 7. Realistic environments for different scenarios

Fig. 8. Plane extraction. (a) Before extraction; (b) After extraction

Fig. 9. Comparison of the estimated trajectories of room_02 and hall_01 sequence algorithms with the real trajectories

Fig. 10. Comparison between the estimated trajectory of the Indoor_01 sequence algorithm and the actual trajectory

Fig. 11. Running time analysis of each part of this algorithm