Where to start to write my own task code

Hello, I am new to UAV and I am using NAVIO2 for my drone. I am going to do a research related to vision based control for UAV. Could anyone tell me where should I start or what material should I refer to to write my own code to realize the control in the following picture?


I would start by trying to get a simulation working.


You can see some commands here that you can use to get started


Thank you. I will try that.