NWU to ENU conversion for a homemade IMU
Hello,
I made an IMU sensor, that takes advantage of ROS's complementary filter ported to TM4C123 arm micro controller. Both the software and hardware are open source. Here is the link to github: https://github.com/altineller/fximu2/
The sensor outputs data with NWU orientation. In order to use robot_localization package I need it in ENU format. After doing the preliminary research, I stumbled on https://github.com/ros-perception/imu... with you can convert between NED to NWU. Which is not what I needed exactly, but reading the code and experimenting, I figured out how to rotate a pose by multiplying with a quaternion, and to convert NWU to ENU, you need to rotate the frame by 90 degrees, counterclockwise. As such:
// get orientiation from filter
filter_.getOrientation(q0, q1, q2, q3);
float q0_temp, q1_temp, q2_temp, q3_temp;
quaternionMultiplication(0.707106f, 0.0f, 0.0f, 0.707106f, q0, q1, q2, q3, q0_temp, q1_temp, q2_temp, q3_temp);
Rotates the orientation pose defined by q0,q1,q2,q3 by 90 degrees, counterclockwise.
But however, this is when the filter is fed with NWU, and the output it converted to ENU. In order to take advantage of the robot_localization package, I must also convert ax,ay,az,gx,gy,gz,mx,my,mz by swapping and negating coordinates.
The relevant part of code is pasted below:
// store gyro values
gx = (float) (gyroRD.x * _GYRO_SENSITIVITY * SENSORS_DPS_TO_RADS);
gy = (float) (gyroRD.y * _GYRO_SENSITIVITY * SENSORS_DPS_TO_RADS);
gz = (float) (gyroRD.z * _GYRO_SENSITIVITY * SENSORS_DPS_TO_RADS);
// store accel values
ax = (float) (accelRD.x * _ACCEL_SENSITIVITY * SENSORS_GRAVITY_EARTH);
ay = (float) (accelRD.y * _ACCEL_SENSITIVITY * SENSORS_GRAVITY_EARTH);
az = (float) (accelRD.z * _ACCEL_SENSITIVITY * SENSORS_GRAVITY_EARTH);
// process mag values
float x = (float) (magRD.x * MAG_UT_LSB);
float y = (float) (magRD.y * MAG_UT_LSB);
float z = (float) (magRD.z * MAG_UT_LSB);
// apply mag offset compensation (base values in uTesla)
x = x - mag_offsets[0];
y = y - mag_offsets[1];
z = z - mag_offsets[2];
// apply mag soft iron error compensation
mx = x * mag_softiron_matrix[0][0] + y * mag_softiron_matrix[0][1] + z * mag_softiron_matrix[0][2];
my = x * mag_softiron_matrix[1][0] + y * mag_softiron_matrix[1][1] + z * mag_softiron_matrix[1][2];
mz = x * mag_softiron_matrix[2][0] + y * mag_softiron_matrix[2][1] + z * mag_softiron_matrix[2][2];
// update the filter
filter_.update(ax, ay, az, gx, gy, gz, mx, my, mz, dt);
This is where we get the values from the sensors, after interrupt handlers fill accelRD, gyroRD, and magRD with data from sensors.
If we are rotating 90degrees counterclockwise about Z axis: we should have: (pseudo code)
enu_ax = -ay
enu_ay = ax
enu_az = az
enu_gx = -gy
enu_gy = gx
enu_gz = gz
enu_mx = -my
enu_my = mx
enu_mz = mz
But when I change sensor values, in this fashion the filter breaks it does not output a meaningful orientation. I am doing something wrong swapping parameters, but what is it?
All the relevant code above belongs to: https://github.com/altineller/fximu2/...
Best Regards, C.Altineller
