In this thesis we describe algorithms for localization of mobile agents using a stereo camera. The development of algorithms was motivated by the EuRoC competition, Challenge 3: Plant Servicing and Inspection with micro aerial robots. First, we describe the newly developed visual odometry algorithm dubbed stereo odometry with feature tracking (SOFT), which has been holding the first place on the rankings table of the KITTI dataset for more than two years. The SOFT algorithm is based on a careful feature selection and tracking through time in subpixel resolution. Moreover, SOFT combines rotation estimation using a monocular camera with stereo based translation estimation in order to improve robustness to imperfect stereo calibration. Furthermore, in the thesis we also present the extension of the SOFT odometry to simultaneous localization and mapping (SOFT-SLAM). The SOFT-SLAM algorithm relies on the high accuracy of the visual odometry, while the global map consistency is ensured by optimizing the camera pose graph. The approach was developed with the goal of being executed on an embedded computer with limited resources. Given that, it is organized in two separate threads: the synchronous thread for visual odometry operating in real-time and asynchronous thread for mapping and loop closing to ensure global consistency. SOFT-SLAM runs in constant time at 20 Hz frequency on two processor cores. In the EuRoC competition, Challenge 3, Stage IIa - Benchmarking, Task 2: State Estimation and Control System Accuracy, among the five competing finalist SOFT-SLAM achieved the highest score in localization accuracy. In the thesis, we also compare the proposed algorithm to two state-of-the-art approaches, namely ORB-SLAM2 and LSD-SLAM, on two relevant datasets, namely the EuRoC and KITTI dataset. Comparison showed that on average SOFT-SLAM achieves the highest score using significantly less computing resources. Furthermore, in the thesis we also describe development of a new inertial stabilization platform for perception sensors to be mounted on a multirotor flying robot. As opposed to standard solutions, the developed inertial stabilization platform successfully stabilizes sensors largely varying in weight, where the effect of stabilization is proportional to the weight of the sensor. The developed platform significantly reduces the effects of flight dynamics on the accuracy of localization and mapping of the flying robot.