Rotations from NED to Body Fixed Frame

1

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

2

Hi @Jonathan_Wittmer

Check here to get an example of how to convert a vector from one frame to an other using quaternions or rotation matrices in PX4.
It looks that in your case, you just need to transpose your rotation matrix (or even better, use directly the quaternions. Passing by a rotation matrix is an unnecessary step).

Tell me if it still doesn’t works,
Good luck!

P.s.: you also use three times vx in your vector, but I assume that this is a typo

3

Thanks @bresch! I had read through that link before but missed that I had to transpose the rotation matrix. That fixed my problems :slight_smile:

Jon