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

https://blog.csdn.net/lun55423/article/details/123614260