一、参考资料

深入浅出理解卡尔曼滤波

二、Allan方差分析方法

1. 引言

传统的误差指标往往是采用均值误差（反映整个误差序列有无宏观偏置）、标准差（反映整个误差序列的波动情况），以及均方根（RMS，可以认为是宏观偏置与波动情况的综合）。如下图所示，它们都是反映误差序列的整体情况的指标，其中含有短时间快速变化和长时间缓变的各个成份，无法细分出不同时间尺度上的误差波动情况。
在这里插入图片描述

Allan方差是一个相对而言直观形象、朴实无华的概念，它当初是在实践中针对高精度时钟稳定性分析的需求而提出的。Allan方差适用于分析惯导（和时钟）看重长期稳定性的传感器，Allan方差曲线对中长期时间尺度上的误差特性有很强的表现力。Allan方差相比于普通的方差具有更好的描述长时间误差的优势，因此，Allan方差被用来当作IMU噪声标定的标准方法。

2. Allan方差的概念

Allan方差，又称为阿伦方差或阿兰方差，Allan方差法是20世纪60年代由美国国家标准局的David Allan提出的基于时域的分析方法。

设计Allan方差分析方法的目的，是为了实现这样一种分析：对惯性传感器误差的关注，主要体现在不确定性和稳定性，具体来说，是不同时间尺度的稳定性。例如，战术武器领域关心的是多少秒到多少分钟时间尺度的稳定性，潜水艇领域关心的是天、轴周、月时间尺度的稳定性。那么有没有什么方法，将不同时间尺度上的不确定性，分门别类的表现出来，Allan教授针对这个问题，提出了Allan方差分析方法。

Allan方差是时频分析和惯性导航领域常用的一种误差分析方法，它有效地刻画了待研究的误差时间序列在不同时间尺度上的波动水平（不稳定性），并可根据不同时间尺度上的Allan方差值所构成的曲线的形状特征来辨识其中包含的随机过程模型。Allan方差分析方法对中长期的随机波动具有很强的表现力，它完全可以作为一个通用的时间序列分析工具来推广到其它应用领域。

在我们对惯性导航器件（陀螺和加速度计）进行误差分析时，常采用Allan方差分析方法。这是当初从时频领域（高精度时钟的频率稳定性）借鉴来的一种时间序列分析方法，非常适合于对中长期波动（不稳定性）进行定量描述和分析，因此正是我们惯性器件误差分析所需要的。

3. Allan方差计算过程

Allan方差是将误差序列在某个指定的时间尺度上的波动情况进行了精确提取，其具体计算步骤如下：
在这里插入图片描述

Step 1 分块

按窗口长度 $\tau=\left(\left.n-1\right.\right)t_0$ 将序列 y 分成 $N_c$ 块(clusters)，每块包n个数据点，块间无重叠。

Step 2 块平均

分别计算各块内n个数据点的均值，记为： $\begin{aligned}\overline{y}_k=\frac1n\sum_{i\operatorname{=}k}^{k+n-1}y_i\end{aligned}$ 。

Step 3 计算Allan方差

相邻块的平均值求差。
$\begin{aligned}\sigma^2(\tau)=\frac{1}{2(N_c-1)}\sum_{k=1}^{N_c-1}(\bar{y}_{k+1}-\bar{y}_k)^2\end{aligned}$

Step 4 统计RMS值

根据Step 3求得的差值，统计RMS值。

（可选）Step 5 绘制Allan方差曲线

改变块长度，重复1~3，并画出Allan标准差随块长度变化的双对数曲线，称为Allan方差曲线。从图中可知，每个时间块，对应一个Allan方差，将所有的不同块时间和对应的Allan方差连起来，即绘制成Allan标准曲线。那么，Allan方差分析就是分析Allan标准曲线。

4. Allan方差图读取误差系数

Allan方差法可用于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}$ 的总和。
在这里插入图片描述

5. Allan方差分析

通过块内求平均，去除短时间的不确定性；通过相邻块间求差，去除长时间的不确定性。

如果我们只对某一时间尺度上的误差（即误差波动）感兴趣的话，那么比这个时间尺度更小的细节变化（短时间快速跳动）和比这个时间尺度更大的宏观变化（长时间缓慢漂移）就都不关心了，希望在我们的误差指标中都被消除掉。而Allan方差是这样做到的：

通过分块确定所要考察的时间块长度；
利用块内求平均的办法把短于块长度的那些快速变化成份（细节）都抹掉；
再利用相邻块求差的办法把长于两块长度的那些缓慢变化成份（宏观）都抹掉；
最后对差值序列统计其方值（这是处理任何随机样本的标准操作），这样统计出来的就是介于1倍块长度和2倍块长度这样一个很窄的时间尺度范围内的误差波动情况。

6. Allan方差曲线

如果我们对误差序列的各个时间尺度上的成份都感兴趣的话，可以将块长度由短到长，“扫描”一遍，得出一组Allan方差值，然后画个“Allan方差 vs. 块长度”的曲线，这样就能全面反映被研究的误差序列的特性了。具体的，实际上是“Allan方差的开方（Allan Deviation） vs. 块长度”的双对数曲线，以便在更大的范围上有更强的表现力。以下是一张经典的高精度陀螺的Allan方差示意图。
在这里插入图片描述

从Allan方差曲线上，我们可以根据曲线的形状特征来识别出不同类型的随机误差（即随机过程模型）并提取其参数。例如：斜率为-1/2的直线段代表白噪声。具体可参见IEEE制定的一个关于陀螺测试的技术标准中的附录C内容[1]。这个附录中还给出了一个从Allan方差曲线中分析陀螺的多种随机误差的案例（如上图）。但我想提醒大家，实际的Allan方差曲线往往只能表现出少数两三个主要误差类型，因为其它误差都被这几个主要误差给淹没了。而由于我们在工程上关心的恰恰也就是主要误差源，因此我们并不受此困扰。更多的使用Allan方差分析过程中的注意事项可参考西工大严恭敏老师的博文《Allan方差分析的使用要点》，写得非常精准务实。

7. Allan方差的应用

Allan方差分析方法并不局限于对惯性器件和时钟误差的分析，也可推广到其它误差序列分析；而且它也不局限于是对误差序列进行分析，也可应用于任何时间序列的分析（例如建筑形变、地质演化等）。

8. Allan方差与ROS

下面是读取bag包，并获取Allen方差的图片，会生成对应的Allen方差图。这里主要参考了allan_variance_ros 对应的C++文件。

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)