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?


Thanks!

I would start by trying to get a simulation working.

http://ardupilot.org/dev/docs/simulation-2.html

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

http://ardupilot.org/dev/docs/copter-sitl-mavproxy-tutorial.html#taking-off

Thank you. I will try that.