Position control in OFFBOARD mode failed Erratically

Hi there !
What I am trying to do is use mavros+Vicon+ offboard mode for position control. I have already managed after changing some params in px4.config to get the /mavros/local_position/pose and /mavros/mocap/pose to match perfectly using lpe.(You can see the params in the log files below ). My PX4 firmware is the last from px4 repo and I am using Snapdragon as flight controller.

The problem now is that when I want to make an altitude hold and send some value via offboard mode, the quad start to climb and then goes on the wall. For some reason he gains pitch or roll speed while I am just asking he to go up.
There is the logs I got while flying: Flight Review or this one : Flight Review

At first I though I have to do tuning but even when the set point is equal to the start location of the quad the drone start to roll or pitch as in the following logs: Flight Review and Flight Review.

I don’t even understand why the estimated yaw is that much different of the desired yaw ??? Has someone has the same behavior ?

Thanks in advance for your help.