PELLICANUS GNSS/INS navigation system dev board

Pellicanus is an open source INS/GPS integrated navigation system designed to be a handy, accessible development board.

ExpertWork in progressOver 41 days671

PELLICANUS GNSS/INS navigation system dev board

Things used in this project

Hardware components

Raspberry Pi rp2040

TDK Corporation IAM20680

MMC5983MA

TE Connectivity ms5637

sl871-gnss module

Software apps and online services

PyCharm

Story

Me and my friend Alperen Akküncü have been working on an open-source INS/GNSS system for a while. We came up with an open-source INS/GNSS module for implementing various navigation algorithms. Out of the box, It’s shipped with its default algorithm which is discrete architecture loosely coupled EKF/NCF. Pellicanus has castellated pins which makes it easy to integrat into your own projects or you can use the development board to play with algorithms that we provide or your own navigation algorithm.

Recently we started the pre-launch on CrowdSupply. Here is the link to our page if you want to subscribe:

https://www.crowdsupply.com/g-fusion/pellicanus

Pellicanus integrates the following components into a tiny SMD module:

· RP2040 MCU

· ROM

· Barometer

· IMU (gyro and acccelerometer)

· Magnetometer

· GNSS

· Antenna connector

SMD Module

This is 2D KiCAD view of Pellicanus SMD module. Its small size allows you to integrate it into your projects easily.

Features & Specifications

Accelerometer Meas Range: ±2 g, ±4 g, ±8 g, ±16 g
Gyro Meas Range: ±250 dps, ±500 dps, ±1000 dps, ±2000 dps
Mag Meas Range: ±8 gauss
Operation Temperature: -40°C to +80°C
Gyro Bias Instability: 7-8°/hr
Dynamic Heading Accuaricy: ±1°
Dynamic Pitch/Roll Accuaricy: ±0.1°
Position Accuaricy: ±4 meters
Pressure Sensor Meas Range: 300-1200 mbar
GNSS position, speed and heading correction

The main processor we use is RP2040 from Raspberry pi which can be programmed with C or micropython. We will be providing many micropython scripts fort he ease of development.

Both hardware and software are open-source sysyem. All of the algorithms and KiCAD Project will be shared on out github page. You can either implement your own algorithm or use our algorithm that we provide.

Here are some 3D pics of Pellicanus

SMD Module

This is the development board for pellicanus. It includes qiwiic connector, sdcard interdace, USB connector, various pins and LEDs.

Base Board

This system is an INS/GNSS module. It gives you the opportunity to implement and test many algorithms. PELLICANUS is a tactical level system. You can program it with the development board and use it with the development board or in your own bardu.

GUI

In addition, It has a GUI where you can test many AHRS and some Navigation algorithms and graph them.

Old Versiyon

This is the old version of pellicanus. We pivoted a little bit because we want pellicanus to be not only a development board for navigation but also for integrating to actual projects!

Default Navigation Algorithm

from machine import Timer,UART,Pin
import machine
import utime
import ustruct
import sys
import uctypes
from ulab import numpy as np
from math import *

##############################
uart1 = UART(0,baudrate = 115200, tx = Pin(12),rx = Pin(13))

##### REGISTER MAP #####
ACCEL_CONFIG = 0x1C
ACCEL_CONFIG2 = 0x1D
GYRO_CONFIG = 0x1B
GYRO_CONFIG2 = 0x1A

SELF_TEST_X_ACCEL = 0x0D
SELF_TEST_Y_ACCEL = 0x0E
SELF_TEST_Z_ACCEL = 0x0F

SELF_TEST_X_GYRO = 0x00
SELF_TEST_Y_GYRO = 0x01
SELF_TEST_Z_GYRO = 0X02

ACCEL_XOUT_H = 0x3B
ACCEL_XOUT_L = 0x3C
ACCEL_YOUT_H = 0x3D
ACCEL_YOUT_L = 0x3E
ACCEL_ZOUT_H = 0x3F
ACCEL_ZOUT_L = 0x40

GYRO_XOUT_H = 0x43
GYRO_XOUT_L = 0x44
GYRO_YOUT_H = 0x45
GYRO_YOUT_L = 0x46
GYRO_ZOUT_H = 0x47
GYRO_ZOUT_L = 0x48

PWR_MGMT_1 = 0x6B
PWR_MGMT_2 = 0x6C

ACCEL_WHO_AM_I = 0x75
GYRO_WHO_AM_I = 0x75

########################

GYRO_cs = machine.Pin(21, machine.Pin.OUT)
ACCEL_cs = machine.Pin(17, machine.Pin.OUT)


# Initialize SPI
spi = machine.SPI(0,
                  baudrate=1000000,
                  polarity=1,
                  phase=1,
                  bits=8,
                  firstbit=machine.SPI.MSB,
                  sck=machine.Pin(18),
                  mosi=machine.Pin(19),
                  miso=machine.Pin(20))


def reg_write(spi, cs, reg, data):

    """
    Write 1 byte to the specified register.
    """
    
    # Construct message (set ~W bit low, MB bit low)
    msg = bytearray()
    msg.append(0x00 | reg)
    msg.append(data)
    
    # Send out SPI message
    cs.value(0)
    spi.write(msg)
    cs.value(1)



def reg_read(spi, cs, reg, nbytes=1):

    """
    Read byte(s) from specified register. If nbytes > 1, read from consecutive
    registers.
    """
    
    # Determine if multiple byte (MB) bit should be set
    if nbytes < 1:
        return bytearray()
    elif nbytes == 1:
        mb = 0
    else:
        mb = 1
    
    # Construct message (set ~W bit high)
    msg = bytearray()
    msg.append(0x80 | (mb << 6) | reg)
    
    # Send out SPI message and read
    cs.value(0)
    spi.write(msg)
    data = spi.read(nbytes)
    cs.value(1)
    
    return data


# from stackoverflow J.F. Sebastian

def _twos_comp(val, bits=16):

    """compute the 2's complement of int val with bits"""
    if (val & (1 << (bits - 1))) != 0: # if sign bit is set
        val = val - (1 << bits)          # compute negative value
    return val

####ACCEL SET MAX RANGE########################

def ACCEL_set_2g():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG,0x00)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)


def ACCEL_set_4g():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG,0x01)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)


def ACCEL_set_8g():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG,0x10)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)


def ACCEL_set_16g():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG,0x11)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)

#####ACCEL SET LOW PASS FLTER#############

def ACCEL_set_5hz():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG2,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG2,0x06)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG2,1)

def ACCEL_set_10hz():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG2,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG2,0x05)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG2,1)

def ACCEL_set_20hz():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG2,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG2,0x04)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG2,1)

def ACCEL_set_40hz():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG2,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG2,0x03)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG2,1)

def ACCEL_set_100hz():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG2,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG2,0x02)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG2,1)

def ACCEL_set_200hz():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG2,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG2,0x01)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG2,1)


######GYRO SET MAX RANGE###########

def GYRO_set_250dps():
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)
    reg_write(spi,GYRO_cs,GYRO_CONFIG,0x00)
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)
    
def GYRO_set_500dps():
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)
    reg_write(spi,GYRO_cs,GYRO_CONFIG,0x01)
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)
    
def GYRO_set_1000dps():
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)
    reg_write(spi,GYRO_cs,GYRO_CONFIG,0x10)
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)


def GYRO_set_2000dps():
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)
    reg_write(spi,GYRO_cs,GYRO_CONFIG,0x11)
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)

#######GYRO SET LOW PASS FLTER#############

def ACCEL_data():
    
    #################### X axis ############################
    dataX = reg_read(spi, ACCEL_cs, ACCEL_XOUT_L, 1)
    dataX = dataX + reg_read(spi, ACCEL_cs, ACCEL_XOUT_H, 1)
    ax = int.from_bytes(dataX,'little')
    ax = _twos_comp(ax)
    
    #################### Y axis ############################
    dataY = reg_read(spi, ACCEL_cs, ACCEL_YOUT_L, 1)
    dataY = dataY + reg_read(spi, ACCEL_cs, ACCEL_YOUT_H, 1)
    ay = int.from_bytes(dataY,'little')
    ay = _twos_comp(ay)   
    
    #################### Z axis ############################
    dataZ = reg_read(spi, ACCEL_cs, ACCEL_ZOUT_L, 1)
    dataZ = dataZ + reg_read(spi, ACCEL_cs, ACCEL_ZOUT_H, 1)
    az = int.from_bytes(dataZ,'little')
    az = _twos_comp(az)
    
    return ax,ay,az


def GYRO_data():
    
    #################### X axis ############################
    dataX = reg_read(spi, GYRO_cs, GYRO_XOUT_L, 1)
    dataX = dataX + reg_read(spi, GYRO_cs, GYRO_XOUT_H, 1)
    wx = int.from_bytes(dataX,'little')
    wx = _twos_comp(wx)
    
    #################### Y axis ############################
    dataY = reg_read(spi, GYRO_cs, GYRO_YOUT_L, 1)
    dataY = dataY + reg_read(spi, GYRO_cs, GYRO_YOUT_H, 1)
    wy = int.from_bytes(dataY,'little')
    wy = _twos_comp(wy)
    
    #################### Z axis ############################
    dataZ = reg_read(spi, GYRO_cs, GYRO_ZOUT_L, 1)
    dataZ = dataZ + reg_read(spi, GYRO_cs, GYRO_ZOUT_H, 1)
    wz = int.from_bytes(dataZ,'little')
    wz = _twos_comp(wz)
    
    return wx,wy,wz

################################ MAHNETOMETER #############################

LIS3MDL_M_ADDRESS       = 0x1E
LIS3MDL_WHO_AM_I_M      = 0x0F
LIS3MDL_CTRL_REG1_M     = 0x20
LIS3MDL_CTRL_REG2_M     = 0x21
LIS3MDL_CTRL_REG3_M     = 0x22
LIS3MDL_CTRL_REG4_M     = 0x23
LIS3MDL_STATUS_REG_M    = 0x27
LIS3MDL_OUT_X_L_M       = 0x28
LIS3MDL_OUT_X_H_M       = 0x29
LIS3MDL_OUT_Y_L_M       = 0x2A
LIS3MDL_OUT_Y_H_M       = 0x2B
LIS3MDL_OUT_Z_L_M       = 0x2C
LIS3MDL_OUT_Z_H_M       = 0x2D
LIS3MDL_TEMP_OUT_L_M        = 0x2E
LIS3MDL_TEMP_OUT_H_M        = 0x2F
LIS3MDL_INT_CFG_M       = 0x30
LIS3MDL_INT_SRC_M       = 0x31
LIS3MDL_INT_THS_L_M     = 0x32
LIS3MDL_INT_THS_H_M     = 0x33
 
LIS3MDL_REG_CTL_1_TEMP_EN   = 0x80
LIS3MDL_REG_CTL_2_RESET     = 0x04
 
 
# mag_scale defines all possible FSR's of the magnetometer:
 
LIS3MDL_M_SCALE_4GS         = 0x20 # 00:  4Gs
LIS3MDL_M_SCALE_8GS     = 0x40 # 01:  8Gs
LIS3MDL_M_SCALE_12GS        = 0x60 # 10:  12Gs
LIS3MDL_M_SCALE_16GS        = 0x60 # 11:  16Gs

# mag_oder defines all possible output data rates of the magnetometer:
 
LIS3MDL_M_ODR_625       = 0x04 # 6.25 Hz 
LIS3MDL_M_ODR_125       = 0x08 # 12.5 Hz 
LIS3MDL_M_ODR_25        = 0x0C # 25 Hz 
LIS3MDL_M_ODR_5         = 0x10 # 50 
LIS3MDL_M_ODR_10        = 0x14 # 10 Hz
LIS3MDL_M_ODR_20        = 0x14 # 20 Hz
LIS3MDL_M_ODR_40        = 0x14 # 40 Hz
LIS3MDL_M_ODR_80        = 0x14 # 80 Hz

mRes = 4.0 / 32768.0   # 4G
SENSITIVITY_OF_MIN_SCALE = 27368.0 # (4 guass scale) * (6842 LSB/guass at 4 guass scale)
Scale = 16

i2c = machine.I2C(0,
                  scl=machine.Pin(13),
                  sda=machine.Pin(12),
                  freq=400000)

###############################################################################
# Functions

def reg_writei2c(i2c, addr, reg, data):

    """
    Write bytes to the specified register.
    """
    
    # Construct message
    msg = bytearray()
    msg.append(data)
    
    # Write out message to register
    i2c.writeto_mem(addr, reg, msg)
    
def reg_readi2c(i2c, addr, reg, nbytes=1):

    """
    Read byte(s) from specified register. If nbytes > 1, read from consecutive
    registers.
    """
    
    # Check to make sure caller is asking for 1 or more bytes
    if nbytes < 1:
        return bytearray()
    
    # Request data from specified register(s) over I2C
    data = i2c.readfrom_mem(addr, reg, nbytes)
    
    return data

def _twos_comp(val, bits=16):
    """compute the 2's complement of int val with bits"""
    if (val & (1 << (bits - 1))) != 0: # if sign bit is set
        val = val - (1 << bits)          # compute negative value
    return val 

###############################################################################

def initialise():
 
# initMag -- Sets up the magnetometer to begin reading.
 
    #User Register Reset Function
    reg_writei2c(i2c,LIS3MDL_M_ADDRESS, LIS3MDL_CTRL_REG2_M, LIS3MDL_REG_CTL_2_RESET)
    #Temperature Sensor Enabled
    reg_writei2c(i2c,LIS3MDL_M_ADDRESS, LIS3MDL_CTRL_REG1_M, LIS3MDL_REG_CTL_1_TEMP_EN )
    #Ultra High Performance Mode Selected for XY Axis
    reg_writei2c(i2c,LIS3MDL_M_ADDRESS, LIS3MDL_CTRL_REG1_M, 0x60)
    #Ultra High Performance Mode Selected for Z Axis
    reg_writei2c(i2c,LIS3MDL_M_ADDRESS, LIS3MDL_CTRL_REG4_M, 0x0C)
    #Output Data Rate of 80 Hz Selected
    reg_writei2c(i2c,LIS3MDL_M_ADDRESS, LIS3MDL_CTRL_REG1_M, 0x1C)
    #Continous Conversion Mode,4 wire interface Selected 
    reg_writei2c(i2c,LIS3MDL_M_ADDRESS, LIS3MDL_CTRL_REG3_M, 0x00)
    # 16 guass Full Scale 
    reg_writei2c(i2c,LIS3MDL_M_ADDRESS, LIS3MDL_CTRL_REG2_M, 0x60)
 
 
#Read the magnetometer output registers.
# This will read all six Magnetometer output registers.
 
# Reading the  Magnetometer X-Axis Values from the Register

def readMagx():
        Mag_l = reg_readi2c(i2c,LIS3MDL_M_ADDRESS,LIS3MDL_OUT_X_L_M)
        Mag_h = reg_readi2c(i2c,LIS3MDL_M_ADDRESS,LIS3MDL_OUT_X_H_M)
        Mag_total = (Mag_l[0] | Mag_h[0] <<8)
        
        return Mag_total  if Mag_total < 32768 else Mag_total - 65536
            
# Reading the  Magnetometer Y-Axis Values from the Register

def readMagy():
        Mag_l = reg_readi2c(i2c,LIS3MDL_M_ADDRESS,LIS3MDL_OUT_Y_L_M)
        Mag_h = reg_readi2c(i2c,LIS3MDL_M_ADDRESS,LIS3MDL_OUT_Y_H_M)
        Mag_total = (Mag_l[0] | Mag_h[0] <<8)
        return Mag_total  if Mag_total < 32768 else Mag_total - 65536
    
# Reading the  Magnetometer Z-Axis Values from the Register

def readMagz():
        Mag_l = reg_readi2c(i2c,LIS3MDL_M_ADDRESS,LIS3MDL_OUT_Z_L_M)
        Mag_h = reg_readi2c(i2c,LIS3MDL_M_ADDRESS,LIS3MDL_OUT_Z_H_M)
        Mag_total = (Mag_l[0] | Mag_h[0] <<8)
        return Mag_total  if Mag_total < 32768 else Mag_total - 65536



################ GNSS ##########################################

gps_module = UART(0,baudrate = 9600, tx = Pin(28), rx = Pin(29))

#Used to Store NMEA Sentences

buff = bytearray(255)
latitude = None
longitude = None
height = None
groundspeed = None
gpsyaw = None
geoidalseperation = None
hemisLon = None
hemisLat = None
ggaFix = False
vtgFix = False
vVFix = False

def gnss(timer2):
    
    global latitude,longitude,height,groundspeed,gpsyaw,geoidalseperation,hemisLon,hemisLat,ggaFix,vtgFix
    
    latitude = None
    longitude = None
    height = None
    groundspeed = None
    gpsyaw = None
    geoidalseperation = None
    hemisLon = None
    hemisLat = None
    ggaFix = False
    vtgFix = False
    

    gps_module.readline()
    
    buff = str(gps_module.readline())
    
    data = buff.split(",")
    
    if data[0] == "b'$GNGGA" and len(data) == 15:

        if len(data[2]) != 0 and len(data[4]) != 0 and len(data[9]) != 0 and len(data[11]) != 0:
            latitude = data[2]
            latdeg = latitude[0:2]
            latdeg1 = latitude[2:]
            latdeg1 = float(latdeg1)/60
            latitude = int(latdeg) + latdeg1
            longitude = data[4]
            londeg = longitude[0:3]
            londeg1 = longitude[3:]
            londeg1 = float(londeg1)/60
            longitude = int(londeg) + londeg1

            height = float(data[9])
            #print(latdeg,latdeg1,londeg,londeg1)
            hemisLat = data[3]
            hemisLon = data[5]
            geoidalseperation = float(data[11])
            ggaFix = True

    if data[0] == "b'$GNVTG" and len(data) == 10:
        if len(data[7]) != 0 and len(data[1]) != 0:
            groundspeed = float(data[7])
            gpsyaw = float(data[1])
            vtgFix = True
    
    #print(latitude,hemisLat,longitude,hemisLon,height,geoidalseperation,groundspeed,gpsyaw)
    #print(data)

################## TIME STEP ###################

t = 0
deltaT = 0.04

#####################################

axb = 0
ayb = 0
azb = 0
ax = 0
ay = 0
az = 0
wx = 0
wy = 0
wz = 0
Magx = 0
Magy = 0
Magz = 0
step1magx = 0
step1magy = 0
step1magz = 0
bx = 0
by = 0
cmagx = 0
cmagy = 0
cmagz = 0

def IMU_ecompensation():

    global axb,ayb,azb,ax,ay,az,wx,wy,wz,Magx,Magy,Magz,step1magx,step1magy,step1magz,cmagx,cmagy,cmagz

    axb,ayb,azb = ACCEL_data()
    axb=axb/16384
    ayb=ayb/16384
    azb=azb/16384
    axb = axb + 0.008859
    ayb = ayb - 0.007039
    azb = azb - 0.005115

    ax = 0.998103*axb - 0.000179*ayb - 0.000331*azb
    ay = -0.000179*axb + 1.001615*ayb + 0.000106*azb
    az = -0.000331*axb + 0.000106*ayb + 0.998466*azb

    #NED conflict
    ay = -ay
    az = -az

    #GYRO
    wx,wy,wz = GYRO_data()
    wx = radians((wx/32768)*500)
    wy = radians((wy/32768)*500)
    wz = radians((-wz/32768)*500)

    #NED conflict
    wy = -wy

    # Mag Read
    Magx = ((readMagx())*(Scale/SENSITIVITY_OF_MIN_SCALE))*100
    Magy = ((readMagy())*(Scale/SENSITIVITY_OF_MIN_SCALE))*100
    Magz = ((readMagz())*(Scale/SENSITIVITY_OF_MIN_SCALE))*100

    #NED conflict
    Magz = -Magz
    Magy = -Magy

    step1magx = (Magx - (-55.256433))
    step1magy = (Magy - (-4.367858))
    step1magz = (Magz - (9.826686))

    cmagx = step1magx*1.301028 + step1magy*-0.046316 + step1magz*0.014783
    cmagy = step1magx*-0.046316 + step1magy*1.282879 + step1magz*-0.033587
    cmagz = step1magx*0.014783 + step1magy*-0.033587 + step1magz*1.348008

################## AHRS ###################

gyro_quaternion = np.array([[0],
                            [0],
                            [0],
                            [0]],dtype = np.float)

ma_quaternion = np.array([[0],
                          [0],
                          [0],
                          [0]],dtype = np.float)


omega = np.array([[0,0,0,0],
                  [0,0,0,0],
                  [0,0,0,0],
                  [0,0,0,0]],dtype=np.float)
pitch = 0
roll = 0
yaw = 0
pitch_rd = 0
roll_rd = 0
yaw_rd = 0
bx = 0
by = 0
qw = 0
qx = 0
qy = 0
qz = 0
dcm = np.array([[0,0,0],
                [0,0,0],
                [0,0,0]],dtype = np.float)

alfa = 0.1



def euler2qua():
    global pitch_rd,roll_rd,yaw_rd,qw,qx,qy,qz

    p = pitch_rd/2
    r = roll_rd/2
    y = yaw_rd/2

    qw = cos(r)*cos(p)*cos(y) + sin(r)*sin(p)*sin(y)
    qx = sin(r)*cos(p)*cos(y) - cos(r)*sin(p)*sin(y)
    qy = cos(r)*sin(p)*cos(y) + sin(r)*cos(p)*sin(y)
    qz = cos(r)*cos(p)*sin(y) - sin(r)*sin(p)*cos(y)



def qua2euler():

    global pitch_rd,roll_rd,yaw_rd,qw,qx,qy,qz

    roll_rd = atan2((2*(qw*qx + qy*qz)),(1-2*qx*qx - 2*qy*qy))
    pitch_rd = asin(2*(qw*qy - qx*qz))
    yaw_rd = atan2((2*(qw*qz + qx*qy)),(1- 2*qy*qy - 2*qz*qz))



def AHRS():

    global t,pitch,pitch_rd,roll,roll_rd,yaw,yaw_rd,ax,ay,az,wx,wy,wz,dcm,bx,by,cmagx,cmagy,cmagz,qw,qx,qy,qz,deltaT,alfa,gyro_quaternion,ma_quaternion,omega
    
    pitch_rd = atan(ax/((ay*ay + az*az)**0.5))
    #pitch_rd = -asin(-ax)
    roll_rd = atan2(-ay,-az)
    bx = cmagx*cos(pitch_rd)+cmagy*sin(pitch_rd)*sin(roll_rd)+cmagz*sin(pitch_rd)*cos(roll_rd)
    by = cmagy*cos(roll_rd) - cmagz*sin(roll_rd)
    yaw_rd = atan2(-by,bx) + 0.09843
    #print(degrees(pitch_rd),degrees(roll_rd),degrees(yaw_rd))
    d = deltaT/2

    if t<1:

        euler2qua()

    else:

        gyro_quaternion = np.array([[qw-d*wx*qx-d*wy*qy-d*wz*qz],
                                    [qx+d*wx*qw-d*wy*qz+d*wz*qy],
                                    [qy+d*wx*qz+d*wy*qw-d*wz*qx],
                                    [qz-d*wx*qy+d*wy*qx+d*wz*qw]],dtype=np.float)*(1-alfa)


        euler2qua()
        ma_quaternion = np.array([[qw],
                                  [qx],
                                  [qy],
                                  [qz]],dtype=np.float)*(alfa)

        quat = gyro_quaternion + ma_quaternion

        qw = quat[0][0]
        qx = quat[1][0]
        qy = quat[2][0]
        qz = quat[3][0]

        qnorm = qw*qw + qx*qx + qy*qy + qz*qz

        qw = qw/qnorm
        qx = qx/qnorm
        qy = qy/qnorm
        qz = qz/qnorm
        qua2euler()

        roll = degrees(roll_rd)
        pitch = degrees(pitch_rd)
        yaw = degrees(yaw_rd)

        dcm = np.array([[(qw*qw + qx*qx - qy*qy -qz*qz),(2*(qx*qy - qz*qw)),(2*(qx*qz + qy*qw))],
                        [(2*(qx*qy + qz*qw)),(qw*qw - qx*qx + qy*qy -qz*qz),(2*(qy*qz - qx*qw))],
                        [(2*(qx*qz - qy*qw)),(2*(qy*qz + qx*qw)),(qw*qw - qx*qx -qy*qy + qz*qz)]],dtype=np.float)



################### EKF ##################
gravity = np.array([[0],
                    [0],
                    [-1]],dtype = np.int8)

estState = np.array([[0],
                     [0],
                     [0],
                     [0],
                     [0],
                     [0]],dtype = np.float)

corState = np.array([[0],
                     [0],
                     [0],
                     [0],
                     [0],
                     [0]],dtype = np.float)

estP = np.eye(6,dtype = np.float)*50

corrP = np.eye(6,dtype = np.float)

F = np.array([[1,0,0,0,0,0],
              [0,1,0,0,0,0],
              [0,0,1,0,0,0],
              [0,0,0,1,0,0],
              [0,0,0,0,1,0],
              [0,0,0,0,0,1]],dtype = np.float)

Rgga = np.eye(3)*0.025
Rvtg = np.eye(2)*0.05
RvV = np.eye(1)*0.05
R = None
noise = np.array([[2.81e-13],
                  [2.81e-13],
                  [2.81e-13],
                  [7.03e-10],
                  [7.03e-10],
                  [7.03e-10]],dtype = np.float)

#Qt = np.dot(np.eye(6),noise)
Qt = np.eye(6)*0.1
Hgga = np.array([[1,0,0,0,0,0],
                 [0,1,0,0,0,0],
                 [0,0,1,0,0,0]],dtype = np.int8)

Hvtg = np.array([[0,0,0,1,0,0],
                 [0,0,0,0,1,0]],dtype = np.int8)

HvV = np.array([[0,0,0,0,0,1]],dtype = np.int8)

H = None

kG = np.eye(6,dtype=np.float)

Z = np.array([[0],
              [0],
              [0]],dtype = np.float)

Vt = np.array([[0],
               [0],
               [0]],dtype = np.float)

accnoise = 86e-4

posnoiseconst = accnoise*deltaT*deltaT
velnoiseconst = accnoise*deltaT

NEDpos = np.array([[0],
                   [0],
                   [0]],dtype = np.float)

NEDvel = np.array([[0],
                   [0],
                   [0]],dtype = np.float)

a = np.array([[0],
              [0],
              [0]],dtype = np.float)

mNEDX = 0
mNEDY = 0
mNEDZ = 0
mNVELX = 0
mNVELY = 0
vVelocity = 0
estimating = np.array([[0],
                       [0],
                       [0]],dtype = np.float)

def estimatingState():

    global ax,ay,az,a,dcm,deltaT,estState,corState,posnoiseconst,velnoiseconst,NEDpos,NEDvel

    a = np.array([[ax],
                  [ay],
                  [az]],dtype = np.float)
    
    pnoise = np.array([[posnoiseconst],
                       [posnoiseconst],
                       [posnoiseconst]],dtype = np.float)
    vnoise = np.array([[velnoiseconst],
                       [velnoiseconst],
                       [velnoiseconst]],dtype = np.float)
    
    pnoise = - np.dot(dcm,pnoise)
    vnoise = - np.dot(dcm,vnoise)

    NEDpos = np.array([[corState[0][0]],
                       [corState[1][0]],
                       [corState[2][0]]],dtype = np.float)

    NEDvel = np.array([[corState[3][0]],
                       [corState[4][0]],
                       [corState[5][0]]],dtype = np.float)
    
    NEDpos = NEDpos + NEDvel*deltaT + pnoise
    
    step1 = np.dot(dcm,a)
    step2 = (step1-gravity)*9.81*deltaT

    NEDvel = NEDvel + step2 + vnoise

    estState = np.array([[NEDpos[0][0]],
                         [NEDpos[1][0]],
                         [NEDpos[2][0]],
                         [NEDvel[0][0]],
                         [NEDvel[1][0]],
                         [NEDvel[2][0]]],dtype = np.float)

def estimatingCovarianceMatrix():

    global corrP,F,Qt,estP

    Ftr = F.transpose()

    step1 = np.dot(F,corrP)
    step2 = np.dot(step1,Ftr)
    estP = step2 + Qt

def kalmangain():

    global H,estP,R,kG

    Htr = H.transpose()

    step1 = np.dot(H,estP)
    step2 = np.dot(step1,Htr)
    step3 = step2 + R
    step4 = np.linalg.inv(step3)
    step5 = np.dot(estP,Htr)
    kG = np.dot(step5,step4)
    
def correctionState():
    
    global estState,kG,Vt,corState

    step1 = np.dot(kG,Vt)
    corState = estState - step1

def correctionP():

    global estP,kG,H,corrP

    step1 = np.dot(kG,H)
    step2 = np.dot(step1,estP)
    corrP = estP - step2


def EKF():

    global t,ax,ay,az,dcm,gravity,estState,corState,estP,corrP,F,R,Rgga,Rvtg,Qt,H,Hgga,Hvtg,kG,Z,Vt,ggaFix,vtgFix,mNEDX,mNEDY,mNEDZ,mNVELX,mNVELY,estimating,vVFix,HvV,RvV,vVelocity

    if ggaFix:
        
        R = Rgga
        H = Hgga
        Z = np.array([[mNEDX],
                      [mNEDY],
                      [mNEDZ]],dtype = np.float)
        estimating = np.array([[estState[0][0]],
                               [estState[1][0]],
                               [estState[2][0]]],dtype = np.float)
        
        Vt = estimating - Z
        kalmangain()
        correctionState()
        correctionP()

        ggaFix = False
        vVFix = True

    elif vtgFix:

        R = Rvtg
        H = Hvtg
        Z = np.array([[mNVELX],
                      [mNVELY]],dtype = np.float)
        estimating = np.array([[estState[3][0]],
                               [estState[4][0]]],dtype = np.float)
        
        Vt = estimating - Z
        kalmangain()
        correctionState()
        correctionP()

        vtgFix = False

    elif vVFix and vVelocity<10 and ggaFix == False:
        R = RvV
        H = HvV
        Z = np.array([[vVelocity]],dtype = np.float)
        estimating = np.array([estState[5][0]],dtype = np.float)
        Vt = estimating-Z
        kalmangain()
        correctionState()
        correctionP()

        vVFix = False

    else:

        corState = estState
        corrP = estP


    estimatingState()
    estimatingCovarianceMatrix()

################# COORDINATE TRANSFORMATION ####################
#WGS 84 ellipsoid model
Rea = 6378137 #The semi major axis (meters)
Reb = 6356752 #The semi-minor axis (meters)
e = 0.08181919 #The first eccentricity
ee = e*e
en = 1-ee
eSqrt = 0.99330562
Rea_eSqrt = Rea*eSqrt
Me = None #Meridian Radius
Ne = None #Vertical Radius
#############################
ECEF = np.array([[0],
                 [0],
                 [0]],dtype = np.float)
Xecef = None
Yecef = None
Zecef = None
referanceNED = False
referanceLat = None
referanceLon = None
referanceECEF = np.array([[0],
                          [0],
                          [0]],dtype = np.float)
referanceECEFX = None
referanceECEFY = None
referanceECEFZ = None
NED = np.array([[0],
                [0],
                [0]],dtype = np.float)
Xned = None
Yned = None
Zned = None
Xvel = None
Yvel = None
Zvel = None
ecef_ned = np.array([[0,0,0],
                     [0,0,0],
                     [0,0,0]],dtype = np.float)
ned_ecef = np.array([[0,0,0],
                     [0,0,0],
                     [0,0,0]],dtype = np.float)

clongitude = 0
clatitude = 0
cheight = 0
h0 = 0
h1 = 0
pTime = 0
cTime = 0
aralik = 0
def coordinateTransformation(timer):

    global t,ax,ay,az,ggaFix,vtgFix,corState,pitch,roll,yaw,aralik,clongitude,clatitude,cheight,mNEDX,mNEDY,mNEDZ,mNVELX,mNVELY,latitude,longitude,height,geoidalseperation,hemisLon,hemisLat,groundspeed,gpsyaw,Rea,Reb,ee,en,eSqrt,Rea_eSqrt,Me,Ne,ECEF,Xecef,Yecef,Zecef,NED,Xned,Yned,Zned,referanceNED,referanceLat,referanceLon,referanceECEF,referanceECEFX,referanceECEFY,referanceECEFZ,ned_ecef,ecef_ned,Xvel,Yvel,Zvel,h0,h1,pTime,cTime,vVelocity
    gnssFix = "No_GNSS!!!!"
    isStart = "No_Started"
    correct = "No_Correct"
    IMU_ecompensation()
    AHRS()
    if t >= 50:

        if ggaFix:
            if hemisLat == "S":
                latitude = -latitude

            if hemisLon == "W":
                longitude = -longitude
         
            lat = radians(latitude)
            lon = radians(longitude)

            step1 = 1- ee*(sin(lat)**2)

            Me = Rea_eSqrt/(step1**1.5)
            Ne = Rea/(step1**0.5)

            Xecef = (Ne+height)*cos(lat)*cos(lon)
            Yecef = (Ne+height)*cos(lat)*sin(lon)
            Zecef = (Ne*eSqrt+height)*sin(lat)

            ECEF = np.array([[Xecef],
                             [Yecef],
                             [Zecef]],dtype = np.float)

            if referanceNED == False:

                referanceLat = lat
                referanceLon = lon
                referanceECEFX = Xecef
                referanceECEFY = Yecef
                referanceECEFZ = Zecef

                ecef_ned = np.array([[(-sin(lat)*cos(lon)),(-sin(lat)*sin(lon)),(cos(lat))],
                                     [(-sin(lon)),(cos(lon)),0],
                                     [(-cos(lat)*cos(lon)),(-cos(lat)*sin(lon)),(-sin(lat))]],dtype = np.float)

                ned_ecef = ecef_ned.transpose()

                referanceECEF = np.array([[referanceECEFX],
                                          [referanceECEFY],
                                          [referanceECEFZ]],dtype = np.float)
        
                referanceNED = True
            step2 = ECEF - referanceECEF
            NED = np.dot(ecef_ned,step2)
            mNEDX = NED[0][0]
            mNEDY = NED[1][0]
            mNEDZ = NED[2][0]
            gnssFix = "GNSS_FIX!!!"
            correct = "Position!!"
            
            h1 = mNEDZ
            cTime = t

            if pTime == 0:
                h1 = h0
                pTime = cTime

...

This file has been truncated, please download it to see its full contents.

from machine import Timer
import utime
import ustruct
import sys
import uctypes
from ulab import numpy as np
from ulab import scipy as sc
import math


##### REGISTER MAP #####
ACCEL_CONFIG = 0x1C
GYRO_CONFIG = 0x1B

SELF_TEST_X_ACCEL = 0x0D
SELF_TEST_Y_ACCEL = 0x0E
SELF_TEST_Z_ACCEL = 0x0F

SELF_TEST_X_GYRO = 0x00
SELF_TEST_Y_GYRO = 0x01
SELF_TEST_Z_GYRO = 0X02

ACCEL_XOUT_H = 0x3B
ACCEL_XOUT_L = 0x3C
ACCEL_YOUT_H = 0x3D
ACCEL_YOUT_L = 0x3E
ACCEL_ZOUT_H = 0x3F
ACCEL_ZOUT_L = 0x40

GYRO_XOUT_H = 0x43
GYRO_XOUT_L = 0x44
GYRO_YOUT_H = 0x45
GYRO_YOUT_L = 0x46
GYRO_ZOUT_H = 0x47
GYRO_ZOUT_L = 0x48

PWR_MGMT_1 = 0x6B
PWR_MGMT_2 = 0x6C

ACCEL_WHO_AM_I = 0x75
GYRO_WHO_AM_I = 0x75

########################

GYRO_cs = machine.Pin(21, machine.Pin.OUT)
ACCEL_cs = machine.Pin(17, machine.Pin.OUT)
timer = Timer()

# Initialize SPI
spi = machine.SPI(0,
                  baudrate=1000000,
                  polarity=1,
                  phase=1,
                  bits=8,
                  firstbit=machine.SPI.MSB,
                  sck=machine.Pin(18),
                  mosi=machine.Pin(19),
                  miso=machine.Pin(20))



def reg_write(spi, cs, reg, data):
    """
    Write 1 byte to the specified register.
    """
    
    # Construct message (set ~W bit low, MB bit low)
    msg = bytearray()
    msg.append(0x00 | reg)
    msg.append(data)
    
    # Send out SPI message
    cs.value(0)
    spi.write(msg)
    cs.value(1)



def reg_read(spi, cs, reg, nbytes=1):
    """
    Read byte(s) from specified register. If nbytes > 1, read from consecutive
    registers.
    """
    
    # Determine if multiple byte (MB) bit should be set
    if nbytes < 1:
        return bytearray()
    elif nbytes == 1:
        mb = 0
    else:
        mb = 1
    
    # Construct message (set ~W bit high)
    msg = bytearray()
    msg.append(0x80 | (mb << 6) | reg)
    
    # Send out SPI message and read
    cs.value(0)
    spi.write(msg)
    data = spi.read(nbytes)
    cs.value(1)
    
    return data


# from stackoverflow J.F. Sebastian
def _twos_comp(val, bits=16):
    """compute the 2's complement of int val with bits"""
    if (val & (1 << (bits - 1))) != 0: # if sign bit is set
        val = val - (1 << bits)          # compute negative value
    return val


def ACCEL_set_2g():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG,0x00)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)


def ACCEL_set_4g():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG,0x08)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)


def ACCEL_set_8g():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG,0x10)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)


def ACCEL_set_16g():
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)
    reg_write(spi,ACCEL_cs,ACCEL_CONFIG,0x18)
    reg = reg_read(spi,ACCEL_cs,ACCEL_CONFIG,1)


def GYRO_set_250dps():
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)
    reg_write(spi,GYRO_cs,GYRO_CONFIG,0x00)
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)
    

def GYRO_set_500dps():
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)
    reg_write(spi,GYRO_cs,GYRO_CONFIG,0x10)
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)
    
def GYRO_set_1000dps():
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)
    reg_write(spi,GYRO_cs,GYRO_CONFIG,0x08)
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)


def GYRO_set_2000dps():
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)
    reg_write(spi,GYRO_cs,GYRO_CONFIG,0x18)
    reg = reg_read(spi,GYRO_cs,GYRO_CONFIG,1)



def ACCEL_data():
    
    #################### X axis ############################
    dataX = reg_read(spi, ACCEL_cs, ACCEL_XOUT_L, 1)
    dataX = dataX + reg_read(spi, ACCEL_cs, ACCEL_XOUT_H, 1)
    ax = int.from_bytes(dataX,'little')
    ax = _twos_comp(ax)
    
    #################### Y axis ############################
    dataY = reg_read(spi, ACCEL_cs, ACCEL_YOUT_L, 1)
    dataY = dataY + reg_read(spi, ACCEL_cs, ACCEL_YOUT_H, 1)
    ay = int.from_bytes(dataY,'little')
    ay = _twos_comp(ay)   
    
    #################### Z axis ############################
    dataZ = reg_read(spi, ACCEL_cs, ACCEL_ZOUT_L, 1)
    dataZ = dataZ + reg_read(spi, ACCEL_cs, ACCEL_ZOUT_H, 1)
    az = int.from_bytes(dataZ,'little')
    az = _twos_comp(az)
    
    return ax,ay,az


def GYRO_data():
    
    #################### X axis ############################
    dataX = reg_read(spi, GYRO_cs, GYRO_XOUT_L, 1)
    dataX = dataX + reg_read(spi, GYRO_cs, GYRO_XOUT_H, 1)
    wx = int.from_bytes(dataX,'little')
    wx = _twos_comp(wx)
    
    #################### Y axis ############################
    dataY = reg_read(spi, GYRO_cs, GYRO_YOUT_L, 1)
    dataY = dataY + reg_read(spi, GYRO_cs, GYRO_YOUT_H, 1)
    wy = int.from_bytes(dataY,'little')
    wy = _twos_comp(wy)
    
    #################### Z axis ############################
    dataZ = reg_read(spi, GYRO_cs, GYRO_ZOUT_L, 1)
    dataZ = dataZ + reg_read(spi, GYRO_cs, GYRO_ZOUT_H, 1)
    wz = int.from_bytes(dataZ,'little')
    wz = _twos_comp(wz)
    
    return -wx,-wy,-wz


def run():
    ax,ay,az = ACCEL_data()
    wx,wy,wz = GYRO_data()
    print((ax/8192),",",(ay/8192),",",(az/8192),",",((wx/32768)*250),",",((wy/32768)*250),",",((wz/32768)*250))
    


pitch = 0
roll = 0
yaw = 0

estQuaternion = np.array([[0],
                          [1],
                          [2],
                          [3]],dtype=np.float)

corrQuaternion = np.array([[0],
                           [0],
                           [0],
                           [0]],dtype=np.float)

estP = np.array([[1,0,0,0],
                 [0,1,0,0],
                 [0,0,1,0],
                 [0,0,0,1]],dtype=np.float)*50

corrP = np.array([[1,0,0,0],
                  [0,1,0,0],
                  [0,0,1,0],
                  [0,0,0,1]],dtype=np.float)

R = np.array([[0.00095,0,0],
              [0,0.00095,0],
              [0,0,0.00095]],dtype=np.float)

Qt = np.array([[0,0,0,0],
               [0,0,0,0],
               [0,0,0,0],
               [0,0,0,0]],dtype=np.float)

Ew = np.array([[0.0087,0,0],
               [0,0.0087,0],
               [0,0,0.0087]],dtype=np.float)

hqt = np.array([[0],
                [0],
                [0]],dtype=np.float)
Hqt = np.array([[0,0,0,0],
                [0,0,0,0],
                [0,0,0,0]],dtype=np.float)

F = np.array([[0,0,0,0],
              [0,0,0,0],
              [0,0,0,0],
              [0,0,0,0]],dtype=np.float)

KalmanGain = np.array([[0,0,0],
                       [0,0,0],
                       [0,0,0],
                       [0,0,0]],dtype=np.float)

Vt = np.array([[0],
               [0],
               [0]],dtype=np.float)

Zt = np.array([[0],
               [0],
               [0]],dtype=np.float)

axb = 0 #g
ayb = 0 #g
azb = 0 #g
axc = 0 #g
ayc = 0 #g
azc = 0 #g
wxb = 0 #deg/s
wyb = 0 #deg/s
wzb = 0 #deg/s
wxc = 0 #deg/s
wyc = 0 #deg/s
wzc = 0 #deg/s
topax = 0
topay = 0
topaz = 0
topwx = 0
topwy = 0
topwz = 0
deltaT = 0.01 #second
timeStep = 1 #unit
time = 0 #second
frequency = 100 #hertz

def initialize(ax,ay,az):#kullanld
    
    global estQuaternion
    
    estQuaternion = np.array([[((az+1)/2)**0.5],
                              [-(ay/((2*(az+1))**0.5))],
                              [(ax/((2*(az+1))**0.5))],
                              [0]],dtype=np.float)
    


def estQua():#kullanld
    global estQuaternion,corrQuaternion,wxc,wyc,wzc,deltaT
    
    ident = np.eye(4)
    omega = np.array([[0,-wxc,-wyc,-wzc],
                      [wxc,0,wzc,-wyc],
                      [wyc,-wzc,0,wxc],
                      [wzc,wyc,-wxc,0]],dtype=np.float)*(deltaT/2)
    step1 = ident + omega
    estQuaternion = np.dot(step1,estQuaternion)
    
def estCovarianceMatrix():#kullanld
    global F,corrP,Qt,estP
    
    Ftr = F.transpose()
    step1 = np.dot(corrP,Ftr)
    step2 = np.dot(F,step1)
    estP = step2 + Qt


def transitionMatrix():#kullanld
    global wxc,wyc,wzc,deltaT,F
    
    ident = np.eye(4)
    
    omega = np.array([[0,-wxc,-wyc,-wzc],
                      [wxc,0,wzc,-wyc],
                      [wyc,-wzc,0,wxc],
                      [wzc,wyc,-wxc,0]],dtype=np.float)*(deltaT/2)
    
    F =  ident + omega
    

def processNoiseCovarianceMatrix():#kullanld
    
    global estQuaternion,deltaT,Qt,Ew
    qw = estQuaternion[0][0]
    qx = estQuaternion[1][0]
    qy = estQuaternion[2][0]
    qz = estQuaternion[3][0]
    
    Wt = np.array([[-qx,-qy,-qz],
                   [qw,-qz,qy],
                   [qz,qw,-qx],
                   [-qy,qx,qw]],dtype=np.float)*(deltaT/2)

    Wtr = Wt.transpose()
    
    step1 = np.dot(Ew,Wtr)
    Qt = np.dot(Wt,step1)


def measModel():#kullanld
    global hqt,estQuaternion,Hqt
    qw = estQuaternion[0][0]
    qx = estQuaternion[1][0]
    qy = estQuaternion[2][0]
    qz = estQuaternion[3][0]
    hqt = np.array([[qx*qz-qw*qy],
                    [qw*qx+qy*qz],
                    [0.5-qx**2-qy**2]],dtype = np.float)*2
    
    Hqt = np.array([[-qy,qz,-qw,qx],
                    [qx,qw,qz,qy],
                    [0,-2*qx,2*qy,0]],dtype = np.float)*2
    
def kalmangain():#kullanld
    global Hqt,estP,R,KalmanGain
    
    Htr = Hqt.transpose()
    step1 = np.dot(estP,Htr)
    step2 = np.dot(Hqt,step1)
    S = step2+R
    step3 = np.linalg.inv(S)
    step4 = np.dot(Htr,step3)
    KalmanGain = np.dot(estP,step4)
    
def correctedQuaternion():#kullanld
    global estQuaternion,KalmanGain,Vt,corrQuaternion
    
    step1 = np.dot(KalmanGain,Vt)
    corrQuaternion = estQuaternion+step1

def correctedCovarianceMatrix():#kullanld
    global KalmanGain,corrP,estP,Hqt
    
    ident = np.eye(4) 
    step1 = np.dot(KalmanGain,Hqt)
    step2 = ident - step1
    corrP = np.dot(step2,estP)
    

def AHRS(timer):
    
    global pitch,roll,yaw,estQuaternion,corrQuaternion,estP,corrP,R,Qt,Ew,hqt,Hqt,F,KalmanGain,Vt,Zt,deltaT,timeStep,time,topax,topay,topaz,topwx,topwy,topwz,axb,ayb,azb,wxb,wyb,wzb,wxc,wyc,wzc,axc,ayc,azc
    
    ax,ay,az = ACCEL_data()
    ax = ax/8192
    ay = ay/8192
    az = az/8192
    wx,wy,wz = GYRO_data()
    wx = (wx/32768)*250
    wy = (wy/32768)*250
    wz = (wz/32768)*250
    
    if timeStep <= 100:
        print("Don't Touch")
        topax += ax
        topay += ay
        topaz += az
        topwx += wx
        topwy += wy
        topwz += wz
    
    elif timeStep == 101:
        axb = topax/100
        ayb = topay/100
        azb = (topaz/100)-1
        wxb = topwx/100
        wyb = topwy/100
        wzb = topwz/100
        wxc = wx - wxb
        wyc = wy - wyb
        wzc = wz - wzb
        axc = ax - axb
        ayc = ay - ayb
        azc = az - azb
        initialize(axc,ayc,azc)
    
    elif timeStep > 101:
        
        wxc = math.radians(wx - wxb)
        wyc = math.radians(wy - wyb)
        wzc = math.radians(wz - wzb)
        axc = ax - axb
        ayc = ay - ayb
        azc = az - azb
        norm = 1/((axc**2 + ayc**2 + azc**2)**0.5)
        axcn = axc*norm
        aycn = ayc*norm
        azcn = azc*norm
        
        measModel()
        Zt = np.array([[axcn],
                       [aycn],
                       [azcn]],dtype = np.float)
        
        Vt = Zt-hqt
        kalmangain()
        correctedQuaternion()
        correctedCovarianceMatrix()
        #####################
        cqw = corrQuaternion[0][0]
        cqx = corrQuaternion[1][0]
        cqy = corrQuaternion[2][0]
        cqz = corrQuaternion[3][0]
        norm = cqw*cqw + cqx*cqx + cqy*cqy + cqz*cqz
        cqwn = corrQuaternion[0][0]/norm
        cqxn = corrQuaternion[1][0]/norm
        cqyn = corrQuaternion[2][0]/norm
        cqzn = corrQuaternion[3][0]/norm
        if timeStep % 20 == 0:
            roll = math.atan2((2*(cqwn*cqxn+cqyn*cqzn)),(1 - cqxn**2 - cqyn**2))
            pitch = math.asin(2*(cqwn*cqyn - cqxn*cqzn))
            roll = math.degrees(roll)
            pitch = math.degrees(pitch)
            print(roll,pitch)
        #####################
        transitionMatrix()
        processNoiseCovarianceMatrix()
        estQua()
        estCovarianceMatrix()
    
    timeStep += 1
    #time += 10

ACCEL_set_4g()
utime.sleep(1)
GYRO_set_250dps()
utime.sleep(1)
timer.init(freq=100, mode=Timer.PERIODIC, callback=AHRS)
ACCEL_cs.value(1)
GYRO_cs.value(1)