visual/inertial Sensor fusion with an extended Kalman filter.

I need an extended kalman filter written in c++ for sensor fusion. I have a visual/inertial system, providing positional XYZ and quaternion rotation from the visual, and gyro/accel data from the IMU. At the end of the project, I will need a full EKF, in c++, as a .cpp and a .h file, that I can easily stream data into, and embed in my existing code. As output, i expect quaternions, in relation to the initial interial orientation. The rotation truth will always be the IMU, the visual rotation data will only be used to correct the drift. There will be an offset between the two sets of rotational data. The visual data may sometimes drop out. When this happens the visual output will be (for quaternions) 0,0,0,0. The aim is a very high repeatability, and a very very low drift. Please bid ONLY if you have experience with kalman sensor fusion. Thank you.