#!/usr/bin/python
# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt
import numpy as np
from scipy.fft import fft
import math
from 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)