Simulink inertial navigation system for quadcopter

I am making a flight control system for a drone, using Matlab(Simulink) and the hardware I am testing it on is a Pixhawk. It's an inertial navigation system, so I am not allowed to use GPS or Opticflow sensors! The Pixhawk has a built in Gyroscopes and Accelerometers to get IMU readings. Using IMU the simulink model will double integrate and find distance and its position relative to home. It has a barometer for altitude. The inertial navigation system should be able to guide the drone with a particular level of accuracy and perform an autonomous mission. There are 2 ways to do this, first method is, changing the code of the pixhawk. That will require an experienced coder. The other simpler approach is to use Simulink. The Simulink models and toolchain is all available, after following the PDF guide you can install and open the model in Simulink. What needs to be done, change the necessary Simulink blocks so that, the Pixhawk calculate its position using IMU readings from the accelerometers. Build and Run the model. Then make it perform an autonomous mission using local coordinates. Once I get the working Simulink model I can test it