Question : extension of OpenVINS for velocity update

[arthurBricq]

Hello,

First, thanks a lot for this great research platform !

I was wondering about the possibility to add to OpenVINS a new update from a measure of speed. I have found the function StateHelper::EKFUpdate(<...>) and I see that it is easy to implement such update,since the jacobian and the observation model are trivial (H = identity matrix, res = z - state.imu.vel). The 'type-based EKF Update' really makes the code simple. This kind of update is available for ROVIO, and in this case it was showed to greatly improve the filter, which is why I am trying it with OpenVINS.

However, it seems like it is not that easy. I have quickly implemented it and a problem occurs. For the first few seconds, the filter accepts the new updates and everything works fine. But after some time, the residual increases quickly, and then some negative elements appear in the state's covariance. This problem does not occur without the speed update. It's quite hard to tell where the problem comes from. I hint that it is caused by timestamp synchronization, because the 'camera-imu timeoffset' also explodes (>10s). In my specific case, this new measure of speed is not synchronized with the IMU / camera system, but the measure is still pretty reliable.

So, if I wanted to add this 'basic' update, would I have to care of timestamp synchronization / propagation ? I have seen in your code that most updates are performed after a step of state propogation, which uses the time-offset synchronisation. Could this be the problem ? Should I try to do the same ? Are there some other problem that might appear with what I am doing ?

I hope you're able to help me out, I really hope to make it work Thanks a lot for your help and for this great project, Best Regards

[goldbattle]

How are you setting the noise for this update? Likely this is causing your issue. Are you implementing this inside the simulator? If so you can try without timeoffset first and with known noise distributions. I recommend nailing down your exact measurement model.

Additionally in the realworld if this is an external sensor, then you need to perform a frame transformation of the velocity in the sensor frame into the IMU frame. To handle the synchronization the most simple way is to propagate to the measurement time and then insert a historical clone or directly update the _imu state at that timestep. Hope this helps you go in the right direction.

[carolo44]

I have implemented the velocity update for open_vins. It just works fine. I would suggest you tune your measurement noise and the chi-square threshold first. If you are sure that the problem comes from the timestamp deviation, you can build a buffer to save the more fine-grained imu states, and then pick out the best step to calculate the residual. Whats more, try to turn off the timeoffset, ex/intrinsics calibration when you are trying something "new", they are increasing the complexity and maybe confusing.

[goldbattle]

Feel free to reopen if you have any more questions.