Skip to content

Alternative Madgwick filter for IMU #45

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Alternative Madgwick filter for IMU #45

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8120ffd
Implement Madgwick filter for IMU
sfe-SparkFro Jul 29, 2023
7431ace
Add reset_angles() to imu.py
sfe-SparkFro Jul 30, 2023
4171af2
Flip roll and pitch axes for IMU AHRS algorithm
sfe-SparkFro Aug 9, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
178 changes: 117 additions & 61 deletions XRPLib/imu.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,10 +87,17 @@ def _reset_member_variables(self):
self._acc_scale_factor = 1
self._gyro_scale_factor = 1

# Angle integrators
self.running_pitch = 0
self.running_yaw = 0
self.running_roll = 0
# Angles
self.pitch = 0
self.yaw = 0
self.roll = 0
self.pitchComputed = True
self.yawComputed = True
self.rollComputed = True

# Madgwick values
self.beta = 0.1
self.q = [1, 0, 0, 0]

def _int16(self, d):
return d if d < 0x8000 else d - 0x10000
Expand Down Expand Up @@ -308,16 +315,22 @@ def get_pitch(self):
:return: The pitch of the IMU in degrees
:rtype: float
"""
return self.running_pitch

if self.pitchComputed == False:
self.pitch = math.asin(-2 * (self.q[1]*self.q[3] - self.q[0]*self.q[2])) * 57.23
self.pitchComputed = True
return self.pitch

def get_yaw(self):
"""
Get the yaw (heading) of the IMU in degrees. Unbounded in range

:return: The yaw (heading) of the IMU in degrees
:rtype: float
"""
return self.running_yaw
if self.yawComputed == False:
self.yaw = math.atan2(self.q[1]*self.q[2] + self.q[0]*self.q[3], 0.5 - self.q[2]*self.q[2] - self.q[3]*self.q[3]) * 57.23
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The yaw reading is fine most of the time, and works while the bot is at any angle other than vertical. When the robot is rotated such that roll is near 90 (or -90), the yaw jumps up/down by a fair margin. Here's an example of it jumping from -90 to -165ish
image

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yaw jumps up/down by a fair margin

Again, this has to do with the singularity condition when near vertical. Think of a vector pointing straight out the front of the robot, projected onto the horizontal plane; the direction of that vector is what's reported as the yaw. Image the robot RPY starts at (0,0,0), then it's pitched up to 89 degrees (almost vertical). The vector pointing out from the front of the robot would get projected into the horizontal plane, and is still pointing at 0 degrees, so the yaw would still get reported as 0. Now imagine the robot is pitched 2 more degrees, going through vertical. Now projecting that vector onto the horizontal plane would cause it to point the opposite direction from the origin, so the yaw gets reported as 180 degrees (or -180).

This is all to be expected with a proper AHRS algorithm, since RPY has a singularity around the vertical directions, which is why quaternions are superior for this. Probably also worth noting that the pitch is actually bounded between [-90, 90], so when the robot gets rotated that extra 2 degrees, the pitch would still get reported as 89 degrees instead of 91 like you might expect. It's very much related to spherical coordinates.

self.yawComputed = True
return self.yaw

def get_heading(self):
"""
Expand All @@ -326,7 +339,7 @@ def get_heading(self):
:return: The heading of the IMU in degrees, bound between [0, 360)
:rtype: float
"""
return self.running_yaw % 360
return self.get_yaw() % 360

def get_roll(self):
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In testing, roll and pitch seem to be swapped. I'm also seeing unexpected results for roll (when calling pitch) when the bot is rotated more than 90 degrees to the left or right. Here's me rotating the bot 180 degrees to the right (rotating to the left is the same but negative):
image

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

roll and pitch seem to be swapped

Yeah, they're flipped from what's intuitive (thinking of an airplane). I can fix this by adjusting which axes are used for X and Y, stay tuned!

unexpected results for roll

I think this is an artifact of the axes being flipped, test again once I update the axes. Should also note that some values may rapidly change when pointing nearly straight up - this is a singularity condition that quaternions handle well, but RPY values can suddenly change. I also expect most people won't be driving their robots straight up/down 😉

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ok, just pushed a commit that flips the axes, the roll and pitch should make more sense now!

"""
Expand All @@ -335,52 +348,16 @@ def get_roll(self):
:return: The roll of the IMU in degrees
:rtype: float
"""
return self.running_roll

def reset_pitch(self):
"""
Reset the pitch to 0
"""
self.running_pitch = 0

def reset_yaw(self):
"""
Reset the yaw (heading) to 0
"""
self.running_yaw = 0

def reset_roll(self):
"""
Reset the roll to 0
"""
self.running_roll = 0

def set_pitch(self, pitch):
"""
Set the pitch to a specific angle in degrees
if self.rollComputed == False:
self.roll = math.atan2(self.q[0]*self.q[1] + self.q[2]*self.q[3], 0.5 - self.q[1]*self.q[1] - self.q[2]*self.q[2]) * 57.23
self.rollComputed = True
return self.roll

:param pitch: The pitch to set the IMU to
:type pitch: float
def reset_angles(self):
"""
self.running_pitch = pitch

def set_yaw(self, yaw):
"""
Set the yaw (heading) to a specific angle in degrees

:param yaw: The yaw (heading) to set the IMU to
:type yaw: float
"""
self.running_yaw = yaw

def set_roll(self, roll):
"""
Set the roll to a specific angle in degrees

:param roll: The roll to set the IMU to
:type roll: float
Reset all angles to 0
"""
self.running_roll = roll
self.q = [1, 0, 0, 0]

def temperature(self):
"""
Expand Down Expand Up @@ -481,6 +458,7 @@ def gyro_rate(self, value=None):

# Update timer frequency
self.timer_frequency = int(value.rstrip('Hz'))
self.timer_period = 1 / self.timer_frequency
self._start_timer()

def calibrate(self, calibration_time:float=1, vertical_axis:int= 2):
Expand Down Expand Up @@ -536,15 +514,93 @@ def _start_timer(self):
def _stop_timer(self):
self.update_timer.deinit()

def _invSqrt(self, x):
return x ** -0.5

def _update_imu_readings(self):
# Called every tick through a callback timer
self.get_gyro_rates()
delta_pitch = self.irq_v[1][0] / 1000 / self.timer_frequency
delta_roll = self.irq_v[1][1] / 1000 / self.timer_frequency
delta_yaw = self.irq_v[1][2] / 1000 / self.timer_frequency

state = disable_irq()
self.running_pitch += delta_pitch
self.running_roll += delta_roll
self.running_yaw += delta_yaw
enable_irq(state)
# Function converted to Python from https://github.com/arduino-libraries/MadgwickAHRS

# Get IMU measurements
cur_vals = self.get_acc_gyro_rates()

# Acceleration, from mg's to g's
ax = cur_vals[0][0] * 0.001
ay = cur_vals[0][1] * 0.001
az = cur_vals[0][2] * 0.001

# Rotation rate, from mdps to rad/sec
gx = cur_vals[1][0] * 0.0000174533
gy = cur_vals[1][1] * 0.0000174533
gz = cur_vals[1][2] * 0.0000174533

# Roll and pitch are measured about the x and y axes respectively. The
# IMU on the XRP has the x and y axes pionting to the right and forwards,
# meaning roll and pitch are swapped from what's intuitive. This axis swap
# effectively rotates the x and y axes by -90 degrees
ax, ay = (-ay, ax)
gx, gy = (-gy, gx)

# Rate of change of quaternion from gyroscope
qDot1 = 0.5 * (-self.q[1] * gx - self.q[2] * gy - self.q[3] * gz)
qDot2 = 0.5 * (self.q[0] * gx + self.q[2] * gz - self.q[3] * gy)
qDot3 = 0.5 * (self.q[0] * gy - self.q[1] * gz + self.q[3] * gx)
qDot4 = 0.5 * (self.q[0] * gz + self.q[1] * gy - self.q[2] * gx)

# Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(not ((ax == 0) and (ay == 0) and (az == 0))):
# Norm`alise accelerometer measurement
recipNorm = self._invSqrt(ax * ax + ay * ay + az * az)
ax *= recipNorm
ay *= recipNorm
az *= recipNorm

# Auxiliary variables to avoid repeated arithmetic
_2q0 = 2 * self.q[0]
_2q1 = 2 * self.q[1]
_2q2 = 2 * self.q[2]
_2q3 = 2 * self.q[3]
_4q0 = 4 * self.q[0]
_4q1 = 4 * self.q[1]
_4q2 = 4 * self.q[2]
_8q1 = 8 * self.q[1]
_8q2 = 8 * self.q[2]
q0q0 = self.q[0] * self.q[0]
q1q1 = self.q[1] * self.q[1]
q2q2 = self.q[2] * self.q[2]
q3q3 = self.q[3] * self.q[3]

# Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay
s1 = _4q1 * q3q3 - _2q3 * ax + 4 * q0q0 * self.q[1] - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az
s2 = 4 * q0q0 * self.q[2] + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az
s3 = 4 * q1q1 * self.q[3] - _2q1 * ax + 4 * q2q2 * self.q[3] - _2q2 * ay
recipNorm = self._invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3) # normalise step magnitude
s0 *= recipNorm
s1 *= recipNorm
s2 *= recipNorm
s3 *= recipNorm

# Apply feedback step
qDot1 -= self.beta * s0
qDot2 -= self.beta * s1
qDot3 -= self.beta * s2
qDot4 -= self.beta * s3

# Integrate rate of change of quaternion to yield quaternion
self.q[0] += qDot1 * self.timer_period
self.q[1] += qDot2 * self.timer_period
self.q[2] += qDot3 * self.timer_period
self.q[3] += qDot4 * self.timer_period

# Normalise quaternion
recipNorm = self._invSqrt(self.q[0] * self.q[0] + self.q[1] * self.q[1] + self.q[2] * self.q[2] + self.q[3] * self.q[3])
self.q[0] *= recipNorm
self.q[1] *= recipNorm
self.q[2] *= recipNorm
self.q[3] *= recipNorm

# Update RPY flags
self.pitchComputed = False
self.yawComputed = False
self.rollComputed = False