0. 简介
在研究晶体振荡器和原子钟的稳定性时,人们发现这些系统的相位噪声中不仅有白噪声,而且有闪烁噪声。使用标准差分析这类噪声时发现结果是无法收敛的。为了解决这个问题,David Allan于1966年提出了Allan方差分析,该方法不仅可以准确识别噪声类型,还能精确确定噪声的特性参数,其最大优点在于对各类噪声的幂律谱项都是收敛的。对于IMU标定而言,标定可以分为确定性误差和随机误差,确定性误差包括:交轴耦合误差(Axis-misalignment),比例因子误差(Scale Factor),零偏(Bias)。随机误差则主要是高斯白噪声和bias随机游走。Allen方差主要用于标定随机误差。
1. Allan方差使用
Allen方差的计算方法如下:
-
保持IMU静止, 采集N个数据点。将采样数据划分为包含不同数量采样点的子集 : $T(n)= nT_s,n=1,2…N/2 ,T(n)$ 为该子集的平均时间。
-
将N个点定长分组,常用的交叠式Allen方差如下(n=3):
- 对于每个子集,计算均值后,计算相邻两组的方差。这样就会得到$n$个在每一平均时间$T(n)$对应的方差$σ(n)$。根据这$n$个点作图,可得到Allan标准差$σ$随平均时间$T$变化的双对数曲线。Allan方差有效反映了相邻两个采样段内平均频率差的起伏。
从上面的Allen方差图中可以得到5种随机误差
- 量化噪声:误差系数为 $Q$,Allan方差双对数曲线上斜率为-1的线段延长线与t=1的交点的纵坐标读数为 $\frac{\sqrt{3}}{T}Q$;
- 角度随机游走:其误差系数 $N$,Allan方差双对数曲线上斜率为$-1/2$的线段延长线与$t=1$交点的纵坐标读数即为$\frac{N}{\sqrt{T}}$;
- 零偏不稳定性:其误差系数 $B$,Allan方差双对数曲线上斜率为0的线段延长线与t=1交点的纵坐标读数为$\sqrt{\frac{2\ln2}{\pi}}B$ ,一般常取底部平坦区的最小值或取$t=10^1$或 $t=10^2$ 处的值;
- 角速率随机游走:其误差系数 $K$,斜率为1/2的线段延长线与$t = 1$交点的纵坐标读数为 $\frac{K}{\sqrt{T/3}}$
- 角速率斜坡:其误差系数 $R$,斜率为1的线段延长线与$t=1$交点的纵坐标读数为$\frac{RT}{\sqrt{2}}$;
假设各种误差源统计独立,那总的艾伦方差为各种误差源之和,即将量化噪声的平方$σ_Q$、角度随机游走的平方$σ_{RAW}$、零偏不稳定性的平方$σ_{bias}$、角速率随机游走的平方$σ_{RRW}$、角速率斜坡的平方$σ_{RR}$的总和。
2. Allan方差与ROS
下面是读取bag包并获取Allen方差的图片,并会生成对应的allen方差图。这里主要参考了https://github.com/ori-drs/allan_variance_ros对应的C++文件,为了方便这里提供了Python的代码以及注释。这个代码需要事先安装sudo pip install allantools
。
#!/usr/bin/env python
import rospy
import sys
import allantools
import rosbag
import numpy as np
import csv
import rospkg
import os
import matplotlib.pyplot as plt # only for plotting, not required for calculations
import math
def getRandomWalkSegment(tau,sigma):
m = -0.5 # slope of random walk
"""""""""""""""""""""""""""""""""
" Find point where slope = -0.5 "
"""""""""""""""""""""""""""""""""
randomWalk = None
i = 1
idx = 1
mindiff = 999
logTau = -999
while (logTau<0):
logTau = math.log(tau[i],10)
logSigma = math.log(sigma[i],10)
prevLogTau = math.log(tau[i-1],10)
prevLogSigma = math.log(sigma[i-1],10)
slope = (logSigma-prevLogSigma)/(logTau-prevLogTau)# 随机漫步的斜率
diff = abs(slope-m)# 当前斜率与目标斜率的差值
if (diff<mindiff):
mindiff = diff# 更新最小差值
idx = i
i = i + 1
""""""""""""""""""""""""""""""
" Project line to tau = 10^0 "
""""""""""""""""""""""""""""""
x1 = math.log(tau[idx],10)# 当前点的横坐标
y1 = math.log(sigma[idx],10)# 当前点的纵坐标
x2 = 0
y2 = m*(x2-x1)+y1
return (pow(10,x1),pow(10,y1),pow(10,x2),pow(10,y2))
def getBiasInstabilityPoint(tau,sigma):
i = 1
while (i<tau.size):
if (tau[i]>1) and ((sigma[i]-sigma[i-1])>0): # only check for tau > 10^0
break
i = i + 1
return (tau[i],sigma[i])
def main(args):
rospy.init_node('allan_variance_node')
t0 = rospy.get_time()
""""""""""""""
" Parameters "
""""""""""""""
bagfile = rospy.get_param('~bagfile_path','~/data/static.bag')# 输入的bag文件路径
topic = rospy.get_param('~imu_topic_name','/imu')# 输入的imu topic名称
axis = rospy.get_param('~axis',0)# 输入的轴,0为x轴,1为y轴,2为z轴
sampleRate = rospy.get_param('~sample_rate',100)# 输入的采样频率
isDeltaType = rospy.get_param('~delta_measurement',False)# 是否是delta measurement
numTau = rospy.get_param('~number_of_lags',1000)# 输入的tau数目
resultsPath = rospy.get_param('~results_directory_path',None)# 输出的结果路径
""""""""""""""""""""""""""
" Results Directory Path "
""""""""""""""""""""""""""
if resultsPath is None:
paths = rospkg.get_ros_paths()
path = paths[1] # path to workspace's devel
idx = path.find("ws/")# 找到路径
workspacePath = path[0:(idx+3)]# 取到workspace的路径
resultsPath = workspacePath + 'av_results/'# 结果输出路径
if not os.path.isdir(resultsPath):
os.mkdir(resultsPath)
print("\nResults will be save in the following directory: \n\n\t %s\n"%resultsPath)
""""""""""""""""""
" Form Tau Array "
""""""""""""""""""
taus = [None]*numTau# 初始化tau数组
cnt = 0;
for i in np.linspace(-2.0, 5.0, num=numTau): # lags will span from 10^-2 to 10^5, log spaced
taus[cnt] = pow(10,i)# 将tau数组赋值,维度在10^-2 到 10^5
cnt = cnt + 1
"""""""""""""""""
" Parse Bagfile "
"""""""""""""""""
bag = rosbag.Bag(bagfile)
N = bag.get_message_count(topic) # 在bag文件中找到该topic的消息数量
data = np.zeros( (6,N) ) # 初始化数据矩阵,维度为6*N
if isDeltaType:
scale = sampleRate
else:
scale = 1.0
cnt = 0
for topic, msg, t in bag.read_messages(topics=[topic]):# 遍历bag文件中的消息
data[0,cnt] = msg.linear_acceleration.x * scale
data[1,cnt] = msg.linear_acceleration.y * scale
data[2,cnt] = msg.linear_acceleration.z * scale
data[3,cnt] = msg.angular_velocity.x * scale
data[4,cnt] = msg.angular_velocity.y * scale
data[5,cnt] = msg.angular_velocity.z * scale
cnt = cnt + 1
bag.close()
print ("[%0.2f seconds] Bagfile parsed\n"%(rospy.get_time()-t0))
""""""""""""""""""
" Allan Variance "
""""""""""""""""""
if axis is 0:
currentAxis = 1 # 循环通过所有轴1-6
else:
currentAxis = axis # 只需循环一次,然后中断
while (currentAxis <= 6):
# taus_used 对应了是否可以使用taus的数组,adev是allan deviation degree的缩写,为allan偏差;adev_err是allan偏差的误差;adev_norm是allan偏差的标准化;
(taus_used, adev, adev_err, adev_n) = allantools.oadev(data[currentAxis-1], data_type='freq', rate=float(sampleRate), taus=np.array(taus) )# 计算allan variance
randomWalkSegment = getRandomWalkSegment(taus_used,adev)# 计算random walk segment
biasInstabilityPoint = getBiasInstabilityPoint(taus_used,adev)# 计算bias instability point
randomWalk = randomWalkSegment[3]# 获取random walk segment的纵坐标
biasInstability = biasInstabilityPoint[1]# 获取bias instability point的纵坐标
"""""""""""""""
" Save as CSV "
"""""""""""""""
if (currentAxis==1):
fname = 'allan_accel_x'
title = 'Allan Deviation: Accelerometer X'
elif (currentAxis==2):
fname = 'allan_accel_y'
title = 'Allan Deviation: Accelerometer Y'
elif (currentAxis==3):
fname = 'allan_accel_z'
title = 'Allan Deviation: Accelerometer Z'
elif (currentAxis==4):
fname = 'allan_gyro_x'
title = 'Allan Deviation: Gyroscope X'
elif (currentAxis==5):
fname = 'allan_gyro_y'
title = 'Allan Deviation: Gyroscope Y'
elif (currentAxis==6):
fname = 'allan_gyro_z'
title = 'Allan Deviation: Gyroscope Z'
print ("[%0.2f seconds] Finished calculating allan variance - writing results to %s"%(rospy.get_time()-t0,fname))
f = open(resultsPath + fname + '.csv', 'wt')
try:
writer = csv.writer(f)
writer.writerow( ('Random Walk', 'Bias Instability') )
writer.writerow( (randomWalk, biasInstability) )
writer.writerow( ('Tau', 'AllanDev', 'AllanDevError', 'AllanDevN') )
for i in range(taus_used.size):
writer.writerow( (taus_used[i],adev[i],adev_err[i],adev_n[i]) )
finally:
f.close()
"""""""""""""""
" Plot Result "
"""""""""""""""
plt.figure(figsize=(12,8))
ax = plt.gca()
ax.set_yscale('log')
ax.set_xscale('log')
plt.plot(taus_used,adev)
plt.plot([randomWalkSegment[0],randomWalkSegment[2]],
[randomWalkSegment[1],randomWalkSegment[3]],'k--')
plt.plot(1,randomWalk,'rx',markeredgewidth=2.5,markersize=14.0)
plt.plot(biasInstabilityPoint[0],biasInstabilityPoint[1],'ro')
plt.grid(True, which="both")
plt.title(title)
plt.xlabel('Tau (s)')
plt.ylabel('ADEV')
for item in ([ax.title, ax.xaxis.label, ax.yaxis.label] +
ax.get_xticklabels() + ax.get_yticklabels()):
item.set_fontsize(20)
plt.show(block=False)
plt.savefig(resultsPath + fname)
currentAxis = currentAxis + 1 + axis*6 # increment currentAxis also break if axis is not =0
inp=input("Press Enter key to close figures and end program\n")
if __name__ == '__main__':
main(sys.argv)
在拿到Allan方差后我们可以拿到csv并可以使用在Kalibr当中。当然可以使用这篇文章中的方法。
3. 参考链接
https://blog.csdn.net/lei1105034103/article/details/89159459
https://blog.csdn.net/weixin_38258767/article/details/111514038
评论(0)
您还未登录,请登录后发表或查看评论