基于抗差估计的稳健滤波方法 【技术领域】
本发明涉及的是一种数据处理技术领域的方法，具体是一种基于抗差估计的稳健滤波方法。
背景技术
惯性导航系统(Inertial Navigation System，INS)是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统，它利用惯性传感器(陀螺仪和加速度计)测量运动载体在惯性空间中的角运动和线运动，根据载体运动微分方程组实时、精确地解算出运动载体的位置、速度和姿态角。由于惯性导航系统是一个时间积分系统，并且在实际应用时，惯性传感器的测量存在误差，这将导致惯性导航系统的导航参数误差，尤其是位置误差随时间迅速积累。所以需要利用外部信息进行辅助，实现组合导航，使其有效地减小误差随时间积累的问题。
航位推算(Dead Reckoning，DR)是一种常用的自主定位技术，它利用姿态，航向和里程信息来推算运动载体相对于起始点的位置。航位推算可以使用里程计(Odometer，OD)作为里程传感器，从惯性导航系统中获得姿态和航向信息，进行定位解算，而且随时间累积的定位误差较小，可作为INS(惯性导航系统)的参考信息。于是建立以惯性导航系统为主，航位推算为辅的组合导航系统。
但是，INS/DR(惯性导航系统/航位推算)组合导航系统在实际应用中，由于惯性传感器工作环境中存在干扰，会产生误差明显偏大的野值。另外，INS/DR组合导航系统模型建立过程中，误差总是存在的。非线性系统的线性化，多变的干扰，系统模型中参数的变化，噪声的统计特性很难准确地获得，会给系统带来误差。
经对现有文献检索发现，文献“A new approach to linear filtering and predictionproblems(一种新的线性滤波和预测方法)”(Transactions of the ASME-Journal of BasicEngineering，82(Series D)：35-45.)中提出了标准的卡尔曼滤波，其在系统不存在干扰且模型准确的情况下，可以得到良好的结果，但是该技术对于系统的干扰以及模型的不确定不具有稳健性，这些误差将会导致卡尔曼滤波估计误差的增大甚至造成滤波器发散。
【发明内容】
本发明的目的在于克服现有技术存在的上述不足，提供一种基于抗差估计的稳健滤波方法，通过引入采用等价权函数的抗差估计方法对惯性传感器采集的数据进行初始化，并利用基于Krein空间的稳健卡尔曼滤波，在系统模型存在误差以及传感器存在干扰的情况下，实现了对于野值的剔除，并增强了滤波的稳健性，获得了满意的组合导航精度，从而保证了组合导航的可靠性。
本发明是通过以下技术方案实现的，包括以下步骤：
步骤一，利用惯性传感器得到运动载体的测量数据。
所述的惯性传感器包括：陀螺仪和加速度计。
所述的测量数据包括：角速度和加速度。
步骤二，对测量数据进行抗差估计，得到测量数据的抗差估计值。
所述的抗差估计是：选用IGGIII等价权函数，利用基于最小二乘的为初值的抗差估计，具体公式为：
x^R=(ATP‾A)-1ATP‾L,]]>
其中：为抗差估值，A为观测设计矩阵，P为IGGIII等价权矩阵，L为惯性传感器得到的测量数据。
步骤三，对测量数据的抗差估计值分别进行INS解算和DR解算，得到运动载体的INS导航信息和DR导航信息。
所述的INS解算是INS姿态更新方法和导航更新方法。
所述的DR解算是定位解算数字更新方法。
所述的INS导航信息是运动载体在INS解算下得到的姿态信息、速度信息和位置信息。
所述的DR导航信息是运动载体在DR解算下得到的位置信息。
步骤四，对运动载体的INS导航信息和DR导航信息进行稳健卡尔曼滤波，得到运动载体的INS/DR导航误差信息。
所述的稳健卡尔曼滤波是基于Krein空间估计的稳健卡尔曼滤波，具体公式为：
X^k+1|k+1=FkX^k|k+Lf,k+1Zk+1-Hk+1FkX^k|k-Kk+1FkX^k|k,]]>
Lf,k+1=Pk+1|kHk+1Kk+1TRe,k+1-1,]]>
Re,k+1=Hk+1Kk+1Pk+1|kHk+1Kk+1T+R~k+100-I,]]>
其中：为k+1时刻系统误差状态估计值，Fk为系统的状态转移矩阵，为k时刻系统误差状态估计值，Lf，k+1为卡尔曼滤波增益，Zk+1是INS导航信息或者是DR导航信息，Hk+1为观测矩阵，Kk+1为系统模型给定矩阵，Pk+1|k为误差协方差矩阵的一步预测，为量测噪声协方差矩阵。
所述的INS/DR导航误差信息包括：运动载体的姿态误差信息、速度误差信息和位置误差信息。
与现有技术相比，本发明具有如下有益效果：将抗差估计引入组合导航方法中，使组合导航系统具有较好的抗差性能；稳健地卡尔曼滤波削弱了系统模型不确定性对滤波结果的影响，提高了系统的稳健性，导航参数的精度提高了1～2个数量级。本发明在信息融合和组合导航系统领域有广泛的应用前景。
【附图说明】
图1是实施例中INS解算得到的运动载体的位置导航信息；
其中：(a)是INS解算得到的运动载体的维度信息；(b)是INS解算得到的运动载体的经度信息；(c)是INS解算得到的运动载体的高度信息。
图2是实施例中DR解算得到的运动载体的位置导航信息；
其中：(a)是DR解算得到的运动载体的维度信息；(b)是DR解算得到的运动载体的经度信息；(c)是DR解算得到的运动载体的高度信息。
图3是分别采用INS解算和实施例方法得到的运动载体的位置导航信息；
其中：(a)是分别采用INS解算和实施例方法得到的运动载体的维度信息；(b)是分别采用INS解算和实施例方法得到的运动载体的经度信息；(c)是分别采用INS解算和实施例方法得到的运动载体的高度信息。
【具体实施方式】
以下结合附图对本发明的方法进一步描述：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
实施例
本实施例包括以下步骤：
步骤一，采用陀螺仪获得运动载体的角速度，同时采用加速度仪获得运动载体的加速度。
步骤二，对运动载体的角速度和加速度分别进行抗差估计，得到运动载体角速度的抗差估计值和运动载体加速度的抗差估计值。
所述的抗差估计是：选用IGGIII等价权函数，利用基于最小二乘的为初值的抗差估计，具体公式为：
x^R=(ATP‾A)-1ATP‾L]]>(公式一)
其中：为抗差估值，A为观测设计矩阵，P为IGGIII等价权矩阵，L为运动载体的角速度或者是运动载体的加速度。
本实施例采用基于最小二乘估值为初值的抗差估计，比力矢量fb或角速率矢量ωibb的真实值均记为x，于是相应的残差方程为：
V=Ax^-L]]>(公式二)
其中：为最小二乘估值；V为残差向量；L为运动载体的角速度或者是运动载体的加速度；A为观测设计矩阵。
本实施例选用IGGIII等价权函数，为
p‾i=pi|v~i|≤k0pik0|v~i|(k1-|v~i|k1-k0)2k0<|v~i|≤k10|v~i|>k1]]>(公式三)
其中：pi为观测值的先验权；为标准化残差；为标准化残差的绝对值；k0和k1为常量，其中：k0的取值范围是1到1.5，k1的取值范围是2.5到8。
本实施例利用(公式二)得到残差向量，进行归一化处理得到利用(公式三)得到等价权矩阵P，进而根据(公式一)进行抗差估计。
步骤三，对运动载体角速度的抗差估计值和运动载体加速度的抗差估计值分别进行INS解算和DR解算，得到运动载体的INS导航信息和DR导航信息。
所述的INS解算是INS姿态更新方法和导航更新方法。
所述的DR解算是定位解算数字更新方法。
所述的INS导航信息是运动载体在INS解算下得到的姿态信息、速度信息和位置信息。
所述的DR导航信息是运动载体在DR解算下得到的位置信息。
本实施例选用陀螺常值漂移和随机漂移分别为0.02(°)/h和0.01(°)/h，加速度计常值偏置和随机偏置均为0.0001g，里程计刻度系数误差为0.2％，惯性器件数据输出频率为400Hz，行驶时间为1200s。
本实施例INS解算得到的运动载体的维度信息如图1(a)所示；INS解算得到的运动载体的经度信息如图1(b)所示；INS解算得到的运动载体的高度信息如图1(c)所示；DR解算得到的运动载体的维度信息如图2(a)所示；DR解算得到的运动载体的经度信息如图2(b)所示；DR解算得到的运动载体的高度信息如图2(c)所示。
步骤四，对运动载体的INS导航信息和DR导航信息进行稳健卡尔曼滤波，得到运动载体的INS/DR导航误差信息，即：运动载体的姿态误差信息、速度误差信息和位置误差信息。
所述的稳健卡尔曼滤波是基于Krein空间估计的稳健卡尔曼滤波，具体公式为：
X^k+1|k+1=FkX^k|k+Lf,k+1Zk+1-Hk+1FkX^k|k-Kk+1FkX^k|k]]>(公式四)
Lf,k+1=Pk+1|kHk+1Kk+1TRe,k+1-1]]>(公式五)
Re,k+1=Hk+1Kk+1Pk+1|kHk+1Kk+1T+R~k+100-I]]>(公式六)
其中：为k+1时刻系统误差状态估计值，Fk为系统的状态转移矩阵，为k时刻系统误差状态估计值，Lf，k+1为卡尔曼滤波增益，Zk+1是INS导航信息或者是DR导航信息，Hk+1为观测矩阵，Kk+1为系统模型给定矩阵，Zk+1|k为误差协方差矩阵的一步预测，为量测噪声协方差矩阵。
本实施例建立的模型为：
Xk+1＝FkXk+CkΔkKkXk+Gkwk
(公式七)
Zk＝HkXk+vk
其中：Xk为k时刻系统误差状态，Zk为k时刻INS导航信息或者是DR导航信息，Gk为k时刻系统噪声矩阵，Hk为k时刻系统观测矩阵，wk和vk分别为系统噪声和量测噪声向量，Gk为系统k时刻真实的状态转移矩阵，Δk是不确定矩阵，满足‖Δk‖≤1。
Gk和Kk是都是给定的矩阵，本实施例分别取为：
Ck=IO,]]>Kk＝[εI O]
其中：本实施例取ε＝0.04。
分别采用本实施例方法和标准的卡尔曼滤波方法得到的运动载体的绝对位置误差均值对比数据如表1所示。从表1中可以看出，本实施例方法在一定程度上削弱了系统模型不确定性对滤波结果的影响，提高了系统的稳健性。
表1
分别采用INS解算和实施例方法得到的运动载体的维度信息如图3(a)所示；分别采用INS解算和实施例方法得到的运动载体的经度信息如图3(b)所示；分别采用INS解算和实施例方法得到的运动载体的高度信息如图3(c)所示。从这三幅图中可以看出，INS导航得到的纬度、经度和高度随时间都不断累积增大，1200秒后，误差已经达到几十米，因而在实际应用中，INS导航一段时间后已经无法采用，而本实施例方法导航参数的精度提高了1～2个数量级，其在信息融合和组合导航系统领域有广泛的应用前景。