One paper is accepted by IEEE Transactions on Robotics (TRO)