When I used to develop Kalman filters, visualizing the covariances was the best way to understand/debug the setup. Two great libraries that I used were eigen3 (

When I used to develop Kalman filters, visualizing the covariances was the best way to understand/debug the setup. Two great libraries that I used were eigen3 (for the filter) and point cloud library (https://pointclouds.org/ https://pointclouds.org/) for visuals.