Loss of coordinate frame in quadrotor_dynamics

[eric-wieser]

This line strikes me as incorrect.

Vr = cross(o,D(:,i)) + v;

D is in body coordinates, but o and v are in world coordinates, so this doesn't make any sense.

Assuming Vr should be in body coordinates, I think this should be

Vr = inv(R) * (cross(o,R*D(:,i)) + v);

[eric-wieser]

Or if o is actually body rate, then

Vr = cross(o,D(:,i)) + inv(R) * v;