from sympy import *
from sympy.utilities.codegen import codegen
init_printing(use_latex="mathjax")


## Introduction

In this notebook we’ll implement a simple orientation filter for a MARG sensor, explaining some of the theory along the way, doing it all in python using the awesome Sympy library. Finally we’ll use sympy’s codegen utility to generate c code so our filter can be compiled to run on virtually any microcontroller.

## Overview

Let’s say you want to determine the orientation of some object using a MARG sensor. These sensors are equipped to measure acceleration, angular rate, and the magnetic field around them. So how do you actually turn these readings into an orientation, and how do you use prior state estimates and sensor fusion to filter out noise?

Using just the acceleration and magnetic field reading we can determine the orientation of a static body fairly accurately. To do this we imagine our sensor aligned with the earth, what kind of readings would we expect to see? In that scenario our acceleration would be equal to the gravitational acceleration, and the magnetic field would be pointed north (and slightly into the earth). Using an iterative algorithm, like gradient descent, we find the orientation that best aligns the sensor-frame readings with what we’d expect to see in the earth-frame.

That’s simple enough but what if we start moving our sensor around? This introduces an applied acceleration and we can no longer rely on those two readings to determine orientation. What about the angular rate? If we could simply fuse that reading with our previously estimated orientation, surely we could get a more accurate final estimate. How we actually go about doing this is dependent on the filter we choose to implement, luckily there are many solutions to this problem including Kalman Filters (UKF or EKF), the Complementary Filter, etc. We’ll be implementing the filter described by S. Madgwick here, as it’s simple to understand and computationally less expensive than the alternatives.

## Sensor Frame

First thing’s first, let’s define the MARG readings for the sensor-frame. These are acceleration, angular rate, and the magnetic field. We’ll also define the time between readings as it’s necesarry for when we want to integrate the angular velocity later.

a = Matrix(symbols('a_x, a_y, a_z')) # accelerometer reading
m = Matrix(symbols('m_x, m_y, m_z')) # magnetomer reading
w = Matrix(symbols('w_x, w_y, w_z')) # gyroscope reading

dt = symbols('dt') # timestep


## Earth Frame

What about the Earth frame? Well let’s define variables for the Earth’s gravitational acceleration (in g’s) and magnetic field as what we’d expect to see if the sensor was oriented with the Earth’s reference frame.

G = Matrix([0, 0, 1]) # Earth-frame gravitational acceleration

bx, bz = symbols('b_x, b_z')
B = Matrix([bx, 0, bz]) # Earth-frame magnetic field


## Orientation and Quaternions

We’ll be using unit quaternons to represent orientation and rotations in 3 dimensional space. To do this we define a Quaternion class that extends a sympy Matrix with some helper functions:

Quaternion.fromRV: A unit quaternion with real part $r$ and vector part $\pmb{v}$.

Quaternion.conj: The conjugate of a quaternion $\pmb{q}$:

Quaternion.prod: The product of two quaternions, $\pmb{q_1}$ and $\pmb{q_2}$:

Quaternion.rot: And the rotation of a vector $\pmb{W} = (0, \pmb{w})$ by a quaternion $\pmb{q}$:

class Quaternion(Matrix):
@property
def r(self):
return self[0]

@property
def v(self):
return self[1:,:]

@classmethod
def fromRV(cls, r, v):
return Quaternion([r, v[0], v[1], v[2]])

def conj(self):
return Quaternion.fromRV(self.r, -1 * self.v)

def prod(self, other):
return Quaternion.fromRV(self.r * other.r - self.v.dot(other.v),
self.r * other.v + other.r * self.v + self.v.cross(other.v))

def rot(self, w):
W = Quaternion.fromRV(0, w)
return q.prod(W).prod(q.conj())[1:,:]


As explained above, all our gradient descent algorithm does is minimize the error when rotating vectors from the sensor-frame to the earth-frame. Actually it’s implemented backwards, but it’s the same principle really. We’re going to be using our two vectors in the earth-frame and rotating them into the sensor-frame by some best guess quaternion, and subtracting our expected vector. Then we’ll compute the gradient of these residual functions and subtract that from our last best guess quaternion by some scaling factor. This process is repeated until a sufficient convergence is reached.

To do this we must first define two residual functions $\pmb{f_1}$ and $\pmb{f_2}$, as well as some best guess quaternion $\pmb{q}$

q = Quaternion(symbols('q_0, q_1, q_2, q_3'))

f_1 = q.conj().rot(G) - a # Earth-to-Sensor frame acceleration rotation error
f_2 = q.conj().rot(B) - m # Earth-to-Sensor frame magnetic field rotation error


Then at iteration $k$ we compute the error gradient $\pmb{\nabla F(q_k)} = \pmb{J_1^T f_1} + \pmb{J_2^T f_2}$

and subtract it from our prior prediction by some scaling factor $\alpha$.

J_1 = f_1.jacobian(q)
J_2 = f_2.jacobian(q)

fgrad = (J_1.T * f_1) + (J_2.T * f_2) # Error Gradient

alpha = symbols('alpha') # Scaling Factor



## Sensor Fusion

Compute the angular velocity from the angular rate

Then estimate the angular velocity from the computed angular rate and the previous orientation estimate

then integrate this over the change in time to get our new orientation estimate

beta = symbols('beta')

# angular rate to angular velocity
dq = 0.5 * q.prod(Quaternion.fromRV(0, w))

dq_est = dq - beta * fgrad

q_est = q + dq_est * dt


## Code Generation

Auto-generating c code from the above sympy equations is as easy as calling codegen with the right arguments. The codegen module is described in detail here

prefix = "autogen"
project = "imu"

funcs =  [
]

[(_, c_code), (_, h_code)] = codegen(funcs,
language="C",
prefix=prefix,
project=project,