Hello,
I know that this topic is discussed in depth in many places on the internet and I think I understand the theory behind using a quaternion to generate a rotation matrix that converts from NED (either local- or body-) to body fixed (front, right, down).
My question is however related to the precise implementation of this in the PX4 stack. I am having difficulty getting linear velocities in the body-fixed frame from those reported in the NED frame. This is what I have done so far and it is not working:
math::Vector<3> linear_rates(vlp_topic.vx, vlp_topic.vx, vlp_topic.vx);
math::Quaternion q_NED(v_att_topic.q[0],v_att_topic.q[1],v_att_topic.q[2],v_att_topic.q[3]);
math::Matrix<3,3> R_NED = q_NED.to_dcm();
math::Vector<3> linear_rates_body = R_NED * linear_rates;
Where vlp__topic is the vehicle__local__position topic and v__att__topic is the vehicle__attitude topic.
When I say that this is not working, I mean that the results I am getting are not the results I expected (velocities of x,y,z in the body fixed frame). For example, I am reading z velocities when moving my flight controller across a flat desk.
Perhaps I am using the wrong quaternion?
Thanks,
Jon