What's wrong with my methods or IMU data when drawing graphs restoring traces from IMU data in carla？

[Weiweixc]

I fetch the IMU data by this methods：
def _IMU_callback(weak_self, sensor_data):
self = weak_self()
real_location = self._parent.get_location()
initial_rotation = self._parent.get_transform().rotation

    vol = self._parent.get_velocity()
    x = real_location.x
    y = real_location.y
    vx = vol.x
    vy = vol.y
    vz = vol.z
    if not self:
        return
    limits = (-99.9, 99.9)
    self.accelerometer = [
        max(limits[0], min(limits[1], sensor_data.accelerometer.x)),
        max(limits[0], min(limits[1], sensor_data.accelerometer.y)),
        max(limits[0], min(limits[1], sensor_data.accelerometer.z))]
    self.gyroscope = [
        sensor_data.gyroscope.x,
        sensor_data.gyroscope.y,
        sensor_data.gyroscope.z]
    self.compass = math.degrees(sensor_data.compass)

    print("ax:" ,sensor_data.accelerometer.x)
    print("ay:" ,sensor_data.accelerometer.y)

    # vel = [[vx], [vy], [vz]]
    vel = [[0], [0], [0]]
    # R = IMUSensor.euler_to_rotation_matrix(initial_rotation.yaw, initial_rotation.pitch, initial_rotation.roll)
    R = IMUSensor.euler_to_rotation_matrix(0, 0, 0)
    pose = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
    for i in range(3):
        for j in range(3):
            pose[i][j] = R[i][j]
    timestamp = sensor_data.frame
    print("timestamp:" ,timestamp)
    if timestamp > 30:
        IMU_temp_data = IMUData(timestamp, [self.accelerometer[0], self.accelerometer[1], self.accelerometer[2]], [self.gyroscope[0], self.gyroscope[1], self.gyroscope[2]])
        Odom_temp_data = OdomData(timestamp, pose, vel)
        self.route_est.ReadData(IMU_temp_data, Odom_temp_data, 1)
        self.route_est.UpdatePose()
        temp_data = self.route_est.PublishPose()

IMU data processing functions are as follows:

route estimation

import time
from cmath import pi
import math

from sympy.algebras.quaternion import Quaternion

from sympy import Matrix, false, symbols, cos, sin, trigsimp, true
from sympy import Quaternion

class Activity:
def init(self):
self.G_=Matrix([0, 0, 0])
# angular velocity bias:
self.angular_vel_bias_=Matrix([[0.0], [0.0], [0.0]])
# linear acceleration bias:
self.linear_acc_bias_=Matrix([0.0, 0.0, 0.0])
# size of IMU data queue is not defined

    self.current_IMU_data=IMUData(0, [0,0,0], [0,0,0])
    self.previous_IMU_data=IMUData(0, [0,0,0], [0,0,0])
    self.current_Odom_data=0
    self.previous_Odom_data=0
    
    self.initialized_=False
    self.start_function=False

    self.pose=Matrix([[1,0,0,0], [0,1,0,0], [0,0,1,0],[0,0,0,1]])
    self.last_pose=Matrix([[1,0,0,0], [0,1,0,0], [0,0,1,0],[0,0,0,1]])
    self.vel=Matrix([[0],[0],[0]])
    self.accu_meters=0
    
    self.frame_index=0
    
    self.real_time_speed=0
    self.accu_angular=Matrix([[0], [0], [0]]) #Vector3d
    self.R_prev=Matrix([[1,0,0], [0,1,0], [0,0,1]])
    self.R_curr=Matrix([[1,0,0], [0,1,0], [0,0,1]])

    
def Get_Unbiased_Angular_Vel(self, angular_vel: Matrix):
    return angular_vel - self.angular_vel_bias_

def Get_Unbiased_Linear_Acc(self, linear_acc, R)  :
    # Vector3d linear_acc
    return R * (linear_acc - self.linear_acc_bias_)

def ReadData(self, readed_IMUData, readed_OdomData,speed):
    # fetch IMU measurements into buffer:
    self.previous_IMU_data=self.current_IMU_data
    self.current_IMU_data=readed_IMUData
    self.initial_Odom_data=readed_OdomData
    
    self.real_time_speed=speed
    
    # print(self.current_IMU_data.linear_acceleration)
    
def UpdatePose(self):
    
    if not self.initialized_:
        # use the latest measurement for initialization:
        self.previous_IMU_data = self.current_IMU_data
        odom_data=self.initial_Odom_data
        self.pose = odom_data.pose
        self.vel = odom_data.vel
        
        # imu_data=self.activity_IMU_buff.data_get()
        self.initialized_ = True
        
    else:
        # TODO: implement your estimation here
        # get deltas:
        print("t1",self.current_IMU_data.timestamp)
        print("t2",self.previous_IMU_data.timestamp)
        # time.sleep(1)
        delta_t=(self.current_IMU_data.timestamp-self.previous_IMU_data.timestamp)/100
        delta_t1=(self.current_IMU_data.timestamp - self.previous_IMU_data.timestamp)/100
        
        #self.accu_meters+=self.real_time_speed*delta_t #accumulated meters
        
        angular_delta=self.GetAngularDelta(self.previous_IMU_data, self.current_IMU_data, delta_t1)
        self.accu_angular+=angular_delta
        # print("accu_angular: ", (self.accu_angular/(2*pi))*360)
        
        # update orientation:
        
        self.UpdateOrientation(angular_delta)
        # get velocity delta:
        velocity_delta = self.GetVelocityDelta(self.previous_IMU_data, self.current_IMU_data, delta_t)
        print("deltv: " ,velocity_delta)
        #time.sleep(100)
        velocity_delta[2]=0
        
        # update position:
        self.UpdatePosition(delta_t, velocity_delta)

def PublishPose(self):
    # c. set position:
    return [self.pose.col(3)[0,0],self.pose.col(3)[1,0],self.pose.col(3)[2,0]]

def GetAngularDelta(self, IMU_data_first, IMU_data_next, delta_t):
    # TODO: this could be a helper routine for your own implementation
    angular_vel_curr =self.Get_Unbiased_Angular_Vel(IMU_data_first.angular_velocity)
    angular_vel_prev =self.Get_Unbiased_Angular_Vel(IMU_data_next.angular_velocity)
    print("vol_angular:" ,angular_vel_prev)
    # time.sleep(1)
    angular_delta = 0.5 * delta_t * (angular_vel_curr + angular_vel_prev)
    print("delt_angular:", angular_delta)
    angular_delta[1] = 0
    angular_delta[0] = 0
    
    # angular_delta = delta_t * angular_vel_prev
    return angular_delta
    # update orientation:

def UpdateOrientation(self, angular_delta):
    # magnitude:
    angular_delta_mag = math.sqrt(angular_delta[0,0]**2+angular_delta[1,0]**2+angular_delta[2,0]**2)
    # direction: Eigen::Vector3d 
    # angular_delta_dir = angular_delta.normalized()
    angular_delta_dir = angular_delta/angular_delta_mag
    
    # build delta q:
    angular_delta_cos = math.cos(angular_delta_mag / 2.0)
    angular_delta_sin = math.sin(angular_delta_mag / 2.0)
    
    # Quaterniond 
    dq = Quaternion(angular_delta_cos,
                    angular_delta_sin * angular_delta_dir[0],
                    angular_delta_sin * angular_delta_dir[1],
                    angular_delta_sin * angular_delta_dir[2])
    
    # q=Quaternion(pose.row(0)[0:3]);
    R=Matrix([self.pose.row(0)[0:3], self.pose.row(1)[0:3], self.pose.row(2)[0:3]])
    # print(R)
    q  = trigsimp(Quaternion.from_rotation_matrix(R))
    # q=Quaternion(pose.block<3, 3>(0, 0));
    
    # update:
    q = q * dq
    
    self.R_prev=R
    
    # write back:
    R=q.normalize().to_rotation_matrix()
    new_pose=Matrix([R.row(0), R.row(1), R.row(2), [0,0,0]])
    new_pose=new_pose.col_insert(3,self.pose.col(3))
    self.pose=new_pose
    
    R=Matrix([self.pose.row(0)[0:3], self.pose.row(1)[0:3], self.pose.row(2)[0:3]])
    
    self.R_curr=R

    # return [R_prev, R_curr] 
    # return [Matrix.eye(3), Matrix.eye(3)] 

# // get velocity delta:
def GetVelocityDelta(self, First_IMU_data, Next_IMU_data, delta_t):
    linear_acc_curr = self.Get_Unbiased_Linear_Acc(First_IMU_data.linear_acceleration, self.R_curr)
    # print("linear_acc_curr: ", linear_acc_curr)
    linear_acc_prev = self.Get_Unbiased_Linear_Acc(Next_IMU_data.linear_acceleration, self.R_prev)
    velocity_delta = 0.5 * delta_t * (linear_acc_curr + linear_acc_prev)

    return velocity_delta

# // update position:
def UpdatePosition(self, delta_t, velocity_delta):
    
    t=delta_t * self.vel + 0.5 * delta_t * velocity_delta
    
    self.pose[0,3]+=t[0,0]
    self.pose[1,3]+=t[1,0]
    self.pose[2,3]+=t[2,0]
    self.vel+=velocity_delta

    if self.real_time_speed==0:
        self.vel=Matrix([[0],[0],[0]])

class IMUData:
def init(self, timestamp:int, linear_acceleration_array:list, angular_velocity_array:list):
self.timestamp=timestamp
linear_acceleration_array_row0=linear_acceleration_array[0]
linear_acceleration_array_row1=linear_acceleration_array[1]
linear_acceleration_array_row2=linear_acceleration_array[2]

    # angular_velocity_array_row0=2*math.pi*angular_velocity_array[0]/360
    # angular_velocity_array_row1=2*math.pi*angular_velocity_array[1]/360
    # angular_velocity_array_row2=2*math.pi*angular_velocity_array[2]/360
    angular_velocity_array_row0=angular_velocity_array[0]
    angular_velocity_array_row1=angular_velocity_array[1]
    angular_velocity_array_row2=angular_velocity_array[2]
    self.linear_acceleration=Matrix([[linear_acceleration_array_row0], [linear_acceleration_array_row1], [0]]) #Vector3d
    self.angular_velocity=Matrix([[0], [0], [angular_velocity_array_row2]]) #Vector3d

class OdomData:
def init(self,timestamp:int, pose:list, vel:list):
self.timestamp=timestamp
self.pose=Matrix(pose) #Matrix4dEigen
self.vel=Matrix(vel) #Vector3d

[Weiweixc]

I have solved this problem by change mode to sync

[o-antill]

Hi there i has a question about your code after reading through it to try and solve my own problem! Why did you take the pose of the odometer and what happens when none is passed in place of the odometer data (As in if you didn't want to use an odometer ?)