#!/usr/bin/python# -*- coding: utf-8 -*-import matplotlib.pyplot as pltimport numpy as npfrom scipy.fft import fftimport mathfrom matplotlib.ticker import FuncFormatter
def parse_imu_file(file_path): imu_data = [] timestamps = [] ga = [] with open(file_path, 'r') as file: for line in file: data = line.strip().split(' ') timestamp = float(data[0]) accel_x = float(data[1]) accel_y = float(data[2]) accel_z = float(data[3]) gravity_x = float(data[4]) gravity_y = float(data[5]) gravity_z = float(data[6])
imu_data.append((timestamp, accel_x, accel_y, accel_z, gravity_x, gravity_y, gravity_z)) timestamps.append(timestamp) ga.append([gravity_x, gravity_y, gravity_z,accel_x, accel_y, accel_z]) return imu_data,timestamps,ga
def allan(data, fs): """ https://blog.csdn.net/m0_37576542/article/details/127902701 @summary: 计算allan方差、随机游走噪声 @param data: imu原始数据 @param fs: imu输出频率 @return:
""" # 开始计算allan方差---------------------------------------------------------------------------------------------- data = np.asarray(data, dtype=float) data[:, 0:3] = data[:, 0:3] * 180 / math.pi print(data.shape) data_int = np.cumsum(data, axis=0) / fs # 1/freq=dt, p.78 (4.6) N = len(data_int) taunum = math.floor(np.log((N - 1) / 3) / np.log(2)) # how many kind of tau print(N,taunum) allan_a = np.empty((int(taunum) + 1, 6)) for k in range(0, 6): for i in range(int(taunum) + 1): n = 2 ** i # data number in one box is qrow with 2 square to reduce the counter # use matrix [] added and col.27 mean to replace sumation in p.78 (4.9) temp2 = data_int[1 + 2 * n:N - 0 * n:n, k] temp1 = data_int[1 + 1 * n:N - 1 * n:n, k] temp0 = data_int[1 + 0 * n:N - 2 * n:n, k] allan_a[i, k] = np.sqrt(np.mean((temp2 - 2 * temp1 + temp0) ** 2) / (2 * (n / fs) ** 2)) # p78 (4.9)
t_axis = [2 ** i / fs for i in range(0, int(taunum) + 1)] # 输出陀螺仪Allan方差曲线 # Allan.fitAllanPic(t_axis, allan_a[:, 0:3] * 3600, "Allan Standard Deviation", # "cluster time(s)", "Allan standard (deg/hr)", "gryo.png") # # 输出加速计Allan方差曲线 # Allan.fitAllanPic(t_axis, allan_a[:, 3:6] * 3600, "Allan Standard Deviation", # "cluster time(s)", "Allan standard (m/s/sqt(h))", "acc.png") # 开始计算随机游走噪声-------------------------------------------------------------------------------------------- Ng_Ncku, Kg_Ncku, Bsg_Ncku = gryoRandomWalk(allan_a, t_axis) Na_Ncku, Ka_Ncku, Bsa_Ncku = accRandomWalk(allan_a, t_axis) print("Angle random walk (deg/sqrt(h)): [x:%s][y:%s][z:%s]" % tuple(Ng_Ncku * 3600)) print("rate random walk (deg/sqrt(h^3)): [x:%s][y:%s][z:%s]" % tuple(Kg_Ncku * 3600)) print("Gyro bias instabilities (deg/h): [x:%s][y:%s][z:%s]" % tuple(Bsg_Ncku / 0.664 * 3600)) print("Velocity random walk (m/s/sqrt(h)): [x:%s][y:%s][z:%s]" % tuple(Na_Ncku * 3600)) print("Acceleration random walk (m/s/sqrt(h^3)): [x:%s][y:%s][z:%s]" % tuple(Ka_Ncku * 3600)) print("Acceleration bias instabilities (m/s/h)): [x:%s][y:%s][z:%s]" % tuple(Bsa_Ncku / 0.664 * 3600)) return t_axis,allan_a
def gryoRandomWalk(allan, tAxis): """ @summary: 计算陀螺仪角度随机游走噪声、速度随机游走噪声、零偏不稳定性 @param allan: allan方差数据矩阵 @param tAxis: @return: """ # fit line for angle random walk T_K = 1 * 3600 # vertical line to find the corresponding coefficient p1 = 3 p2 = 10 A1 = -0.5 pmid = int(math.floor((p2 - p1) / 2) + p1 - 1) B = np.log10(allan[pmid, 0:3]) - A1 * np.log10(tAxis[pmid]) y2 = A1 * np.log10(T_K) + B
Ng_Ncku = 10 ** y2 y3 = A1 * np.log10(tAxis[0]) + B # plt.plot([tAxis[1], tAxis[pmid], T_K], 10**y3*3600) # plt.plot([tAxis[1], tAxis[pmid], T_K], allan[pmid, 0:3] * 3600) # plt.plot([tAxis[1], tAxis[pmid], T_K], Ng_Ncku * 3600)
# fit line for gyro rate random walk T_K = 3 * 3600 p1 = 11 p2 = 12 A1 = 0.5 T_start = tAxis[1] pmid = int(math.floor((p2 - p1) / 2) + p1 - 1) print(pmid) B = np.log10(allan[pmid, 0:3]) - A1 * np.log10(tAxis[pmid]) y2 = A1 * np.log10(T_K) + B Kg_Ncku = 10 ** y2 y3 = A1 * np.log10(T_start) + B # plt.plot([tAxis[1], tAxis[pmid], T_K], 10 ** y3 * 3600) # plt.plot([tAxis[1], tAxis[pmid], T_K], allan[pmid, 0:3] * 3600) # plt.plot([tAxis[1], tAxis[pmid], T_K], Kg_Ncku * 3600)
# fit line for bias instabilities Bsg_Ncku = allan[:, 0:3].min(0) return Ng_Ncku, Kg_Ncku, Bsg_Ncku
def accRandomWalk(allan, tAxis): """ @summary: 计算加速度计速度随机游走噪声、加速度随机游走噪声 @param allan: allan方差数据矩阵 @param tAxis: @return: """ # fit line for velocity random walk T_K = 1 * 3600 # vertical line to find the corresponding coefficient p1 = 4 p2 = 8 A1 = -0.5 pmid = int(math.floor((p2 - p1) / 2) + p1 - 1) B = np.log10(allan[pmid, 3:6]) - A1 * np.log10(tAxis[pmid]) y2 = A1 * np.log10(T_K) + B Na_Ncku = 10 ** y2 y3 = A1 * np.log10(tAxis[0]) + B
# fit line for acceleration random walk T_K = 3 * 3600 # % vertical line to find the corresponding coefficient p1 = 11 # % the starting point to fit theorectical slop p2 = 12 # % the ending point to fit theorectical slop A1 = 0.5 pmid = int(math.floor((p2 - p1) / 2) + p1 - 1) B = np.log10(allan[pmid, 3:6]) - A1 * np.log10(tAxis[pmid]) y2 = A1 * np.log10(T_K) + B Ka_Ncku = 10 ** y2 # fit line for bias instabilities Bsa_Ncku = allan[:, 3:6].min(0) return Na_Ncku, Ka_Ncku, Bsa_Ncku
def power_of_ten_formatter(x, pos): # 将坐标值转换为以10的幂次方表示 exponent = int(x) if exponent >= 0: return f"$10^{exponent}$" else: return f"$10^{{-{abs(exponent)}}}$"
def plot_imu_data(imu_data,intervals,ga): timestamps = [data[0] for data in imu_data] accel_x = [data[1
] for data in imu_data] accel_y = [data[2] for data in imu_data] accel_z = [data[3] for data in imu_data] gravity_x = [data[4] for data in imu_data] gravity_y = [data[5] for data in imu_data] gravity_z = [data[6] for data in imu_data]
interval = [inter for inter in intervals] interval.insert(0,10000)
sample_rate = np.mean(np.diff(timestamps))/100 t_axis,allan_a = allan(ga, sample_rate) ttt = np.arange(len(timestamps)) # 频率个数 plt.figure(figsize=(12, 6)) plt.subplot(2, 3, 1) plt.plot(ttt, accel_x, label='accel_x', color='red') plt.plot(ttt, accel_y, label='accel_y', color='green') plt.plot(ttt, accel_z, label='accel_z', color='blue') plt.legend() plt.xlabel('Timestamp') plt.ylabel('Accel')
plt.subplot(2, 3, 2) plt.plot(ttt, gravity_x, label='gravity_x', color='red') plt.plot(ttt, gravity_y, label='gravity_y', color='green') plt.plot(ttt, gravity_z, label='gravity_z', color='blue') plt.legend() plt.xlabel('Timestamp') plt.ylabel('Gravity')
plt.subplot(2, 3, 3) plt.plot(ttt, interval, label='interval', color='red') plt.legend() plt.xlabel('Timestamp') plt.ylabel('interval')
plt.subplot(2, 3, 4) plt.plot(t_axis, allan_a[:, 0]*3600, marker="o", label='gravity_x', color='red') plt.plot(t_axis, allan_a[:, 1]*3600, marker="o", label='gravity_y', color='green') plt.plot(t_axis, allan_a[:, 2]*3600, marker="o", label='gravity_z', color='blue') plt.gca().xaxis.set_major_formatter(FuncFormatter(power_of_ten_formatter)) plt.gca().yaxis.set_major_formatter(FuncFormatter(power_of_ten_formatter)) plt.legend() plt.xlabel('Timestamp') plt.ylabel('gravity allan')
plt.subplot(2, 3, 5) plt.plot(t_axis, allan_a[:, 3]*3600, marker="o", label='accel_x', color='red') plt.plot(t_axis, allan_a[:, 4]*3600, marker="o", label='accel_y', color='green') plt.plot(t_axis, allan_a[:, 5]*3600, marker="o", label='accel_z', color='blue') plt.gca().xaxis.set_major_formatter(FuncFormatter(power_of_ten_formatter)) plt.gca().yaxis.set_major_formatter(FuncFormatter(power_of_ten_formatter)) plt.legend() plt.xlabel('Timestamp') plt.ylabel('accel allan')
#https: fft_data_gravity_z = fft(gravity_z) psd_gravity_z = np.abs(fft_data_gravity_z)**2 normalized_power_spectrum = psd_gravity_z / np.max(psd_gravity_z) # 选择高频范围 high_freq_range = (50, 100) # 示例范围
# 计算高频噪声指标 high_freq_psd = normalized_power_spectrum[high_freq_range[0]:high_freq_range[1]]
total_noise_power = np.sum(high_freq_psd) average_noise_power = np.mean(high_freq_psd) peak_noise = np.max(high_freq_psd)
print("Total Noise Power:", total_noise_power) print("Average Noise Power:", average_noise_power) print("Peak Noise:", peak_noise) plt.subplot(2, 3, 6) plt.plot(high_freq_psd, label='high_freq_psdGZ', color='red',marker=".") plt.legend() plt.xlabel('Timestamp') plt.ylabel('high_freq_psd')
plt.tight_layout() plt.show()
def calculate_imu_interval(timestamps): intervals = [] for i in range(len(timestamps)-1): interval = timestamps[i+1] - timestamps[i] intervals.append(interval)
return intervals
# 示例用法file_path = 'imu.txt'imu_data,timestamps,ga = parse_imu_file(file_path)
# 计算imu时间间隔intervals = calculate_imu_interval(timestamps)
# 绘制imu数据变化图plot_imu_data(imu_data,intervals,ga)
# # 打印解析后的数据# for data in imu_data:# print(data)
