This paper presents an improved indoor localization system based on RF and ultrasonic signal which we named SNSH system. This system composes of a transmitter mounted in a mobile target, and a series of receiver nodes which are managed by a coordinator. By measuring Time Delay of Arrival (TDoA) of RF and ultrasonic signal from the transmitter, distance from target to receiver node is calculated and sent to the coordinator through CAN network, and all the information are gathered in PC to estimate 3D position of the target. Sine wave detector and dynamic threshold filter are applied to provide excellent accuracy of range measurement from TDoA result, and multilateration algorithms is realized to optimize coordinate determination accuracy. Specifically, Linear Least Square and Non-linear Least Square techniques are implemented to contrast their performances in target coordinate estimation. RF signal encoding/decoding time, time delay in CAN network and math calculating are carefully considered to ensure optimal system performance and get ready for field application. Experiments show that sine wave detector algorithm has greatly improved range measurement accuracy, with mean error at 2.2mm and maximum error at 6.7 mm, for distance below 5m. In addition, 3D position accuracy is greatly enhanced by multilateration methods, with mean error in position stay under 15mm, and 90% confident error values at 23mm for LLS, 20mm for NLS. Overall system update period has been verified in real system operation, with maximum rate at 25ms, and compared with other existing researches.