Phone sensor fusion demo

[haversin]

Phyphox app was used to send accelerometer, magnetometer and gyro data over wi-fi using Http requests to a Python script running on a pc. The phone orientation in space was calculated two ways: 1) using accelerometer and magnetometer data only and 2) using a Complementary filter on the accelerometer, magnetometer and gyro data. The results are shown in this video.

[Fumeaux]

That looks quite cool. Would you mind sharing your code? I'm interested in it.

[haversin]

Here is the Python code. you need the Phyphox app on the phone.  I have an android not sure about iphone. I wrote my own quaternion module let me know if you need that.

Code: [Select]

import sys
import pygame
from pygame.locals import *
from OpenGL.GL import *
from OpenGL.GLU import *
from math import *
import numpy as np
import requests
import quaternions as qu

x,y,z = 1.25,2.5,.25
vertices = (
    (x, -y, -z),
    (x, y, -z),
    (-x, y, -z),
    (-x, -y, -z),
    (x, -y, z),
    (x, y, z),
    (-x,-y, z),
    (-x, y, z)
    )

faces = (
    (0,1,2,3),
    (3,2,7,6),
    (6,7,5,4),
    (4,5,1,0),
    (1,5,7,2),
    (4,0,3,6)
    )

colors = (
    (0.,1.,0.),
    (1.,0.,0.),
    (0.,0.,1.),
    (1.,1.,0.),
    (1.,0.,1.),
    (0.,1.,1.)
    )

def quads():
    cindx = 0
    glBegin(GL_QUADS)
    for face in faces:
        glColor3fv(colors[cindx])
        for vertex in face:
            glVertex3fv(vertices[vertex])
        cindx += 1
    glEnd()       
           
# phyphox comm settings
PP_ADDRESS = "http://192.168.1.3:8080"
PP_CHANNELS = ["accX", "accY", "accZ","gyr_time","gyrX", "gyrY", "gyrZ",
            "magX", "magY", "magZ"]
#PP_CHANNELS = ["acc_time","accX", "accY", "accZ"]# accelerometer data names
#PP_CHANNELS = ["gyr_time","gyrX", "gyrY", "gyrZ"] # gyro data names
#PP_CHANNELS = ["magX", "magY", "magZ"] # magnetometer data names
start = PP_ADDRESS +"/control?cmd=start"
stop = PP_ADDRESS +"/control?cmd=stop"
clear = PP_ADDRESS +"/control?cmd=clear"
url = PP_ADDRESS + "/get?" + ("&".join(PP_CHANNELS))
ax,ay = 0.,0.


def main():
    m, e, u, qf = np.zeros((3,3)),np.zeros(3),np.zeros(3),np.zeros(4)
    b, n, q, dqg = np.zeros(3),np.zeros(3),np.zeros(4),np.zeros(4)
    wu = np.zeros(3)
    wtl = 0.
    crd, twopi = 180./pi, 2*pi
    fac = .9  #  filter parameter
    pygame.init()
    display = (1333,1000)
#    aspr = float(display[0])/display[1]
    aspr = 1.5
    pygame.display.set_mode(display, DOUBLEBUF|OPENGL)
    glEnable(GL_CULL_FACE)
    glFrontFace(GL_CW) # face vertex winding order is CW
#    gluPerspective(45, (display[0]/display[1]), .1, 50.)
    s = 7.
    glOrtho(-s*aspr,s*aspr,-s,s,0.1,50.)
    glTranslatef(0,0,-5)
    glRotated(-90.,1,0,0)
    glRotated(-90.,0,0,1)

   
    data = requests.get(start).json() # start comm with phyphox
   
    while True:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()
                data = requests.get(stop).json() # stop comm with phyphox
                quit()

        data = requests.get(url=url).json()
        ax = data["buffer"][PP_CHANNELS[0]]["buffer"][0]
        ay = data["buffer"][PP_CHANNELS[1]]["buffer"][0]
        az = data["buffer"][PP_CHANNELS[2]]["buffer"][0]
        wt = data["buffer"][PP_CHANNELS[3]]["buffer"][0]
        wx = data["buffer"][PP_CHANNELS[4]]["buffer"][0]
        wy = data["buffer"][PP_CHANNELS[5]]["buffer"][0]
        wz = data["buffer"][PP_CHANNELS[6]]["buffer"][0]
        bx = data["buffer"][PP_CHANNELS[7]]["buffer"][0]
        by = data["buffer"][PP_CHANNELS[8]]["buffer"][0]
        bz = data["buffer"][PP_CHANNELS[9]]["buffer"][0]

        if str(ax) != 'None' and str(wt) != 'None' and str(bx) != 'None' :

# use acceleromter and magnetometer data to construct rotation matrix
            u[:] = ax, ay, az
            umag = sqrt(u.dot(u))
            u /= umag
            b[:] = bx, by, bz
            e = np.cross(b,u)
            emag = sqrt(e.dot(e))
            e /= emag
            n = np.cross(u,e)
            m[0] = e
            m[1] = n
            m[2] = u
# convert rotation matrix to quaternion qam
            qam = qu.mat2q(m)
            if wtl == 0.:
              qf, wtl = qam, wt
# compute delta rotation quaternion from gyro data
            dwt = wt - wtl
            if dwt > .2:
              dwt = .2
            print(wx, wy, wz, wt, wtl, dwt)
            wtl = wt
            wu[:] = wx, wy, wz
            wmag = sqrt(wu.dot(wu))
            wu /= wmag
            thao2 = wmag*dwt/2.
            sthao2 = sin(thao2)
            dqg[:] = cos(thao2), wu[0]*sthao2, wu[1]*sthao2, wu[2]*sthao2
            qg = qu.q_mult(dqg,qf)
            qf = qu.Slerp(qam,qg,fac)

        glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT)
# draw object with it's own screen positon           
        glPushMatrix()
        glTranslatef(0,-4.25,0)
        tha = 2*acos(qam[0])
        glRotated(tha*crd,qam[1],qam[2],qam[3])
        quads()
        glPopMatrix()


# draw object with it's own screen positon           
        glPushMatrix()
        glTranslatef(0,4.25,0)
        tha = 2*acos(qf[0])
        glRotated(tha*crd,qf[1],qf[2],qf[3])
        quads()
        glPopMatrix()

        pygame.display.flip()
        pygame.time.wait(1)   
main()