Radio Simultaneous Localization and Mapping in the Terahertz Band