Over the past week I have been researching how to implement a kalman filter into the Lidar localization system. As a reminder the goal of this project is to investigate whether it is possible to drop the angular velocity measurement of the Lidar as it completes a sweep of the room and still maintain an accurate localization estimate of the system in space. We therefore want to predict what the angular velocity is such that it can be used in our localization calculations. The Kalman filter it perfectly suited to this task. The details of its implementation have been omitted from this blog post but there are many online resource which can be referred to. My favourite are: Matlab Kalman Filter Tutorial Series and Student Dave Kalman Filter Explained.
The image below shows the performance of the Kalman filter. This particular example has excessive measurement noise added to it in order to demonstrate the filters performance under these conditions.
The angular displacements generated from the Kalman filter were then used as inputs to the lidar sweep simulator. The first image shows the room layout and the Lidar intercepts. The second image shows the actual and perceived distance profile given the actual and perceived angular displacement of the system.
This simulation of the effects of inaccurate velocity and displacement measurements will be helpful in the future when developing an algorithm to perform localization.
