Are you talking about a new control mode or a new mode of estimating the vehicle position?
As you mention GPS and IMU it sounds like the latter.
Current estimators (LPE and EKF2) already fuse GPS and IMU data, so I'm not quite sure how your method would be different.
If you want to develop a new state estimator to take place of the existing ones, you will have to write your own module that outputs the state estimates in the same manner LPE and EKF2 do. You can look at e.g. the LPE module for reference on which topics you need to publish and so on. Then just make sure your module is started instead of the existing state estimator.
