robot_localization - accessing estimate covariance matrix P
I'm experimenting the ekf implementation of robot_localization to infer velocity from an IMU sensor. The ultimate goal is to use that velocity to intuitively telecontrol a manipulator with kinematic control.
To prevent the integration to drift, I send an Odometry message with 0 twist every time I detect that the device (attached to a human arm) is not moving. For this purpose I've implemented a simple zero-velocity detector and it actually works pretty well, effectively preventing the linear velocity to increase in magnitude undefinitely.
The problem now is about the motion direction: the kalman filter is not able to understand the correct direction of motion from solely accelerometer data.
To overcome this issue I'm thinking about a calibration step of the filter aimed at correctly parametrizing the covariance matrix P. The idea is to make the manipulator moving randomly and asking the user to imitate its moves. Since I have access to the robot's kinematic model, I'm able to get the current speed and position of the end-effector in any moment. I would then fuse those information as an Odometry message along with the IMU acceleration using the ekf. The filter should (hopefully and after a while) be able to understand how to treat the linear acceleration signals in order to obtain the velocity and position provided by the manipulator. I would say that a good criterion to stop this calibration step is to sense whether the estimate covariance matrix P has changed significantly between the filter's updates. Once that's not the case anymore, we may say that the filter has converged to an optimal estimate of P. The problem I'm facing is that I have no idea on how to read the matrix P at runtime. The package allows to set the initial_estimate_covariance, but then I need to read it during the calibration process. I've noticed that when run in debug mode, the ekf_localization_node actually writes that matrix in the debug message. One could exploit that by parsing the debug file looking for the matrix P, but it seems a very dirty solution. Is there a way to access the covariance matrix P programmatically, for example using the APIs provided by rospy?
