I'm using the ComplementaryFilter code in https://github.com/ccny-ros-pkg/imu_tools/blob/indigo/imu_complementary_filter/src/complementary_filter.cpp. I wanted to make sure I understood how it worked and how you've aligned the axes so I did a quick test. Specifically, I'd construct a new ComplementaryFilter and then I'd call update 5 times passing in [1, 0, 0] for my accelerations and [0, 0, 0] for the gyros indicating a dt of 1 second (e.g. cf.update(1, 0, 0, 0, 0, 0, 1)
). So, in this example, I should be telling the ComplementaryFilter that I'm seeing exactly 1 force of gravity in the x direction. I then ask for the orientation and I compute the pitch and roll. I then repeated the above experiment but passing [0, 1, 0] (1G in the y-direction) and [0, 0, 1] (1G in the z direction).
What I saw was that 1G in the x direction yields a pitch of 90 degrees and no roll which would indicate that x points "forward". However, 1G in the y or z directions yields no pitch and no roll. It makes sense that 1G in the z direction would produce no pitch or roll as z is, I think, up/down so that indicates that nothing has changed. However, if 1G in the x direction is pitch I'd expect 1G in the y direction to be roll but it isn't.
To do this experiment I wrote a tiny Python/cython wrapper for your code:
from libcpp cimport bool
import numpy as np
import quaternion as quat
cdef extern from "complementary_filter.h" namespace "imu_tools":
cdef cppclass ComplementaryFilter:
ComplementaryFilter() except +
void setDoAdaptiveGain(bool do_adaptive_gain)
void update(double ax, double ay, double az,
double wx, double wy, double wz,
double dt)
void getOrientation(double& q0, double& q1, double& q2,
double& q3) const
void setOrientation(double q0, double q1, double q2, double q3)
cdef class OrientationFilter:
"""A Python wrapper for the ComplementaryFilter. Note that I didn't do anything to
correct for the differences in axis definitions. See the RevlOrientation class for
a more convenient way to use this code."""
cdef ComplementaryFilter cf
def __cinit__(self, do_adaptive_gain=True):
"""Constructor.
Args:
do_adaptive_gain: if True the gain from the accelerometers will be
dynamically adjusted so it matters more when the device isn't
being accelerated and less when it is.
"""
self.cf = ComplementaryFilter()
self.cf.setDoAdaptiveGain(do_adaptive_gain)
def update(self, double ax, double ay, double az,
double wx, double wy, double wz,
double dt):
"""Pass new data to the filter to have the estimates updated.
Args:
ax, ay, az: acclerations in the x, y, and z direction. Units are
multiples of the force of gravity so that 1.0 means 1G.
wx, wy, wz: angular velocity in radiaans / second.
dt: time delta in seconds.
"""
self.cf.update(ax, ay, az, wx, wy, wz, dt)
@property
def orientation_quat(self):
"""Returns the orientation of the camera with respect to the world
as a quaternion. The return value is a quaternion instance."""
cdef double q0 = 0.0
cdef double q1 = 0.0
cdef double q2 = 0.0
cdef double q3 = 0.0
self.cf.getOrientation(q0, q1, q2, q3)
return quat.quaternion(q0, q1, q2, q3)
And my test code looks like this:
import orientation_filter as of
from revl_orientation import RevlOrientation
import quaternion as quat
import numpy as np
def single_move(ax, ay, az, rx, ry, rz):
f = of.OrientationFilter()
for x in range(10):
f.update(ax, ay, az, rx, ry, rz, 1)
return f.orientation_quat
def to_pitch(q):
q = q.components
return np.degrees(np.arcsin(-2 * (q[0] * q[2] - q[3] * q[1])))
def to_roll(q):
q = q.components
tmp = 2 * (q[1] * q[2] + q[0] * q[3])
tmp = tmp / (q[3]**2 + q[2]**2 - q[1]**2 - q[0]**2)
return np.degrees(np.arctan(tmp))
def pnr(cw_quat):
pitch = to_pitch(cw_quat)
roll = to_roll(cw_quat)
return (pitch, roll)
xg = single_move(1, 0, 0, 0, 0, 0)
yg = single_move(0, 1, 0, 0, 0, 0)
zg = single_move(0, 0, 1, 0, 0, 0)
xpr = pnr(xg)
ypr = pnr(yg)
zpr = pnr(zg)
print('When gravity is +x:', xpr)
print('When gravity is +y:', ypr)
print('When gravity is +z:', zpr)
the output is:
When gravity is +x: (90.0, -0.0)
When gravity is +y: (-0.0, 0.0)
When gravity is +z: (0.0, -0.0)
Is there a bug in the code or my understanding?
Thanks!!