i am currently working on HIL using default px4 firmware and a simple matlab flight-dynamic-simulation(Flight Dynamics(quadrotor)+ Mavlink Send/Receive). At the moment i am stuck because there is unexpected behavior when there is the transition from hover to straight and level flight. In the attached log you can see that the drone ascends and descends when pitch is increased. After evaluating the logged topics i found that vel_d from the vehicle_gps_position and vz from vehicle_local_position start to diverge when pitch is increased to gain velocity( They should probably be quite similiar ?; Angle-estimates correspond with the matlab-simulation angles).
Left:jMavsim(for comparison); Right: Matlab-Simulation
Furthermore vel_pos_innov.02(velocity vz) graph seems like inverted compared to the jMavsim-log; which i used as reference(same maneuver: takeoff–>straight-and-level flight).
vel_pos_innov.02 during takeoff:
Left:jMavsim; Right: Matlab-Simulation
How is the vel_pos_innov data generated in the ekf? And which of the inputs are perhaps wrong, because i send the same set of messages(HIL_SENSOR and HIL_GPS) like jmavsim. The geometry and mass properties of my matlab-simulation are the same as for the jMavsim-Quad.
Would be very thankful for any advices on this.