Calculating position from IMU data in cases where GPS is not available

Question

First post here and I'm jumping in to python with both feet.

My project is to attempt to calculate the position of a underwater robot using only IMU sensors and a speed table.

I am very new to programming and I'm sure I'll get a lot of great feedback on the attached code, but the step I'm currently stuck on is creating a feedback loop between:

UTC[2] (status of the GPS A=available V=not available),

LATD/LOND (GPS position in decimal degrees), and

IMU_LAT/IMU_LON (IMU position in decimal degrees)

The idea would be that if UTC[2] was "A" the logic would equally average IMU_LAT/IMU_LON and LATD/LOND but if UTC[2] was "V" it would only calculate the position based on the last position recorded and IMU_north/IMU_east (offsets based on heading and acceleration values.

import time, inspect
import IMU
import serial
import datetime
import os
import math
import logging

log = open(time.strftime("%Y%m%d-%H%M%S")+'_GSPData.csv','w')
#f.write("UTC TIME,NAVSTATUS,LAT,LON,HDG,SPD,X,Y,Z")

RAD_TO_DEG = 57.29578
M_PI = 3.14159265358979323846
G_GAIN = 0.070  # [deg/s/LSB]  If you change the dps for gyro, you need to update this value accordingly
AA =  0.40      # Complementary filter constant

magXmin =  0
magYmin =  0
magZmin =  0
magXmax =  0
magYmax =  0
magZmax =  0

gyroXangle = 0.0
gyroYangle = 0.0
gyroZangle = 0.0
CFangleX = 0.0
CFangleY = 0.0

IMU.detectIMU()     #Detect if BerryIMUv1 or BerryIMUv2 is connected.
IMU.initIMU()       #Initialise the accelerometer, gyroscope and compass

a = datetime.datetime.now()

ser = serial.Serial('/dev/serial0', 9600)

def truncate(n, decimals=0):
    multiplier = 10 ** decimals
    return int(n * multiplier) / multiplier

log.write("UTC,NAVSTAT,LAT,LON,HDG,SPD,xm/s,ym/s,zm/s")
log.write("\n")

try:
    
    while True:
    #Read the GPS, accelerometer, gyroscope and magnetometer values
        NMEA = ser.readline()
        NMEA_str_data = NMEA.decode('utf-8')
        NMEA_data_arr=NMEA_str_data.split()
        UTC = NMEA_str_data.split(',')
        GYRx = IMU.readGYRx()
        GYRy = IMU.readGYRy()
        GYRz = IMU.readGYRz()
        ACCx = IMU.readACCx()
        ACCy = IMU.readACCy()
        ACCz = IMU.readACCz()
#output the values of the accelerometer in m/s
        yG = ((ACCx * 0.244)/1000)*9.80665
        xG = ((ACCy * 0.244)/1000)*9.80665
        zG = ((ACCz * 0.244)/1000)*9.80665
        MAGx = IMU.readMAGx()
        MAGy = IMU.readMAGy()
        MAGz = IMU.readMAGz()
    
      
#Apply compass calibration
        MAGx -= (magXmin + magXmax) /2
        MAGy -= (magYmin + magYmax) /2
        MAGz -= (magZmin + magZmax) /2
    
##Calculate loop Period(LP). How long between Gyro Reads
        b = datetime.datetime.now() - a
        a = datetime.datetime.now()
        LP = b.microseconds/(1000000*1.0)
        outputString = "Loop Time %5.2f " % ( LP )

    
#Convert Gyro raw to degrees per second
        rate_gyr_x =  GYRx * G_GAIN
        rate_gyr_y =  GYRy * G_GAIN
        rate_gyr_z =  GYRz * G_GAIN


#Calculate the angles from the gyro.
        gyroXangle+=rate_gyr_x*LP
        gyroYangle+=rate_gyr_y*LP
        gyroZangle+=rate_gyr_z*LP


        #Convert Accelerometer values to degrees
        AccXangle =  (math.atan2(ACCy,ACCz)*RAD_TO_DEG)
        AccYangle =  (math.atan2(ACCz,ACCx)+M_PI)*RAD_TO_DEG

        #convert the values to -180 and +180
        if AccYangle > 90:
            AccYangle -= 270.0
        else:
            AccYangle += 90.0

        #Complementary filter used to combine the accelerometer and gyro values.
        CFangleX=AA*(CFangleX+rate_gyr_x*LP) +(1 - AA) * AccXangle
        CFangleY=AA*(CFangleY+rate_gyr_y*LP) +(1 - AA) * AccYangle

        #Calculate heading
        heading = 180 * math.atan2(MAGy,MAGx)/M_PI

        #Only have our heading between 0 and 360
        if heading < 0:
            heading += 360
                          
    ####################################################################
    ###################Tilt compensated heading#########################
    ####################################################################
        #Normalize accelerometer raw values.
        accXnorm = ACCx/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
        accYnorm = ACCy/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
        accZnorm = ACCz/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
        Zms_norm = zG-9.80665
        Yms_norm = yG
        Xms_norm = xG
        #Calculate course
        Course = (180*math.atan2(Xms_norm,Yms_norm)/M_PI)
        
        #Only have our course between 0 and 360
        if Course < 0:
            Course +=360
        #Calculate pitch and roll
        pitch = math.asin(accXnorm)
        roll = -math.asin(accYnorm/math.cos(pitch))

        #Calculate the new tilt compensated values
        magXcomp = MAGx*math.cos(pitch)+MAGz*math.sin(pitch)

    #The compass and accelerometer are orientated differently on the LSM9DS0 and LSM9DS1 and the Z axis on the compass
    #is also reversed. This needs to be taken into consideration when performing the calculations
        if(IMU.LSM9DS0):
            magYcomp = MAGx*math.sin(roll)*math.sin(pitch)+MAGy*math.cos(roll)-MAGz*math.sin(roll)*math.cos(pitch)   #LSM9DS0
        else:
            magYcomp = MAGx*math.sin(roll)*math.sin(pitch)+MAGy*math.cos(roll)+MAGz*math.sin(roll)*math.cos(pitch)   #LSM9DS1

    #Calculate tilt compensated heading
        tiltCompensatedHeading = 180 * math.atan2(magYcomp,magXcomp)/M_PI

        if tiltCompensatedHeading < 0:
            tiltCompensatedHeading += 360
        #convert IMU readings to northings and eastings
        IMU_north= (math.cos(tiltCompensatedHeading))*(Yms_norm+Xms_norm)
        IMU_east= (math.sin(tiltCompensatedHeading))*(Yms_norm+Xms_norm)
 
         #convert IMU_north to D.D
        IMU_north_D= IMU_north/110723.41272
         #Convert IMU_east to d.d
        IMU_east_D= IMU_east/103616.02936
        
############################ END ##################################

       #"%am/s": no rounding   "%bm/s": unsupported "%cm/s": unsupported
        #"%dm/s": whole numbers "%em/s": scientific notation "%fm/s": six digits
        #"%gm/s": five digits
        
        if NMEA_str_data.startswith('$GNRMC'):
            
            if UTC[2] =="V":
                #print("GPS unavaliable","heading",round(tiltCompensatedHeading,2),",course",round(Course,2),xG,yG,zG,"IMU_LAT","IMU_LON")
                print("UTC",truncate(float(UTC[1]),0),",IMU",",LAT",",LON",",heading",round(tiltCompensatedHeading,2),",course",round(Course,2),truncate(IMU_north,4),truncate(IMU_east,4))
                #log the output GPS invalid
                log.write(UTC[1]+','+UTC[2]+','+""+','""+','+str(round(tiltCompensatedHeading,2))+','+UTC[7]+','+str(IMU_north)+','+str(IMU_east))
            else:   
                #convert UTC from DDMM.MMM to DD.DDDD
                if UTC[4] =="N":
                    LATD= (truncate(float(UTC[3]),-2)/100)+((float(UTC[3])-(truncate(float(UTC[3]),-2)))/60)
                else:
                    LATD= -(truncate(float(UTC[3]),-2)/100)+((float(UTC[3])-(truncate(float(UTC[3]),-2)))/60)
                if UTC[6] =="E":
                    LOND= (truncate(float(UTC[5]),-2)/100)+((float(UTC[5])-(truncate(float(UTC[5]),-2)))/60)            
                else:
                    LOND= -(truncate(float(UTC[5]),-2)/100)+((float(UTC[5])-(truncate(float(UTC[5]),-2)))/60)
                #calculate IMU_LAT
                IMU_LAT= LATD+IMU_north_D
                #Calculate IMU_LON
                IMU_LON= LOND+IMU_east_D    
                #write the output                 
                print("UTC",truncate(float(UTC[1]),0),",GPS",",LAT",truncate(LATD,5),truncate(IMU_LAT,5),",LON",truncate(LOND,5),truncate(IMU_LON,5),",heading",round(tiltCompensatedHeading,2),",course",round(Course,2),UTC[8],",speed",truncate(float(UTC[7]),2))
                #log the output GPS valid
                log.write(UTC[1]+','+UTC[2]+','+str(LATD)+','+str(LOND)+','+str(round(tiltCompensatedHeading,2))+','+UTC[7]+','+str(IMU_north)+','+str(IMU_east))
            log.write("\n")
            #slow program down a bit, makes the output more readable
            time.sleep(0.5)
    
        #print(" aX = %fG   aY =%fG   aZ =%fG  " % ( ACCx, ACCy, ACCz))
        #slow program down a bit, makes the output more readable
        #time.sleep(0.5)
except (KeyboardInterrupt, SystemExit): #when you press ctrl+c
    print ("Done.\nExiting.")
    log.close()

Like I said I'm new and I'm sure you pros are going to tell me its really sloppy but I will gladly accept any constructive criticism.

Thanks, Troy