自适应扩展卡尔曼滤波噪声模型建立方法与流程

本发明涉及自适应扩展卡尔曼滤波技术领域，具体地涉及一种行走航迹推算的自适应扩展卡尔曼滤波噪声模型建立方法。

背景技术

行走航迹推算是导航系统的重要组成部分之一，在没有卫星信号或者信号弱的区域，如室内、丛林等区域可借助惯性导航准确的定位行人相对位置。现有的行走航迹推算算法通常使用加速度计、陀螺仪与磁力计等传感器进行姿态判定从而完成航迹推算，在姿态判定的过程中，扩展卡尔曼滤波算法是传感器数据融合判定姿态的关键所在，主要过程是使用陀螺仪数据进行姿态预测，再使用加速度与磁力计数据以一固定的置信度对预测姿态进行一定的修正。因此姿态判定的准确性直接影响了航迹推算的精确度。现有用于传感器姿态判定的扩展卡尔曼滤波算法通常使用固定的观测噪声，这一点在实际复杂多变的外界环境中是不合理的，给某个传感器以固定的观测噪声在某些环境中可以完美使用，但是在传感器数据受到外界较大影响的情况下，会极大地影响航迹推算精度。

技术实现要素：

为了解决上述存在的技术问题，本发明的目的是提出一种自适应扩展卡尔曼滤波噪声模型建立方法，提取外界环境影响与传感器数据之间的关系，并通过神经网络寻找最优模型参数，使外界环境对行走航迹推算的影响达到最小。

本发明的技术方案是：

一种自适应扩展卡尔曼滤波噪声模型建立方法，包括以下步骤：

s01：采集传感器数据，记录每次扩展卡尔曼滤波计算中观测值与预测值之间的差值与滤波计算后姿态中航向角的变化值，并计算观测值与预测值之间的差值与滤波计算后姿态中航向角的变化值之间的相关系数；

s02：根据观测值与预测值之间的差值与滤波计算后姿态中航向角的变化值之间的相关系数建立观测噪声模型；

s03：将噪声模型所需的参数作为神经网络的输入，将目标值作为神经网络对应的输出，进行神经网络模型训练，得到观测噪声模型的最优参数。

优选的技术方案中，所述观测噪声模型为：

stdevmag＝k×e+b

其中e为观测值与预测值之间的差值，k、b为参数。

优选的技术方案中，所述神经网络模型包括2层，第一层为log-sigmoid层，包含了s¹个神经元，第二层为线性层。

优选的技术方案中，所述步骤s03中目标值为期望，公式为：

t＝k1×stdθ+k2×eθ

其中stdθ为前进方向的标准差，eθ为来回路径的偏差夹角，k1与k2为轨迹方向一致性与方向准确性的权重。

与现有技术相比，本发明的优点是：

1、本发明通过传感器数据之间的相关性动态的调整观测数据置信度，减少外界环境导致的干扰数据对姿态判定的影响，从而提高行走航迹推算的准确性，达到最好的跟踪定位效果。

2、使用神经网络对传感器数据的噪声模型参数进行训练，找到最符合实际运用期望的噪声模型参数，从而使自适应扩展卡尔曼滤波能够达到较好的状态估计效果。在保证导航方向准确的同时有效的减少了外界环境对传感器干扰所导致的定位导航误差，与固定观测噪声的行走航迹推算效果相比，在保证方向准确性的同时抗干扰性提高了约20％。

附图说明

下面结合附图及实施例对本发明作进一步描述：

图1是本发明自适应扩展卡尔曼滤波噪声模型建立方法的流程图；

图2是本发明神经网络结构示意图；

图3是本发明神经网络参数调节示意图；

图4是本发明训练结束的性能曲面；

图5是本发明平方和误差与迭代次数曲线；

图6是本发明固定观测噪声与自适应观测噪声轨迹对比图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。

实施例：

如图1所示，一种行走航迹推算的自适应扩展卡尔曼滤波噪声模型建立方法，包含以下几个步骤：

第一步：采集用于训练神经网络的直线行走数据，使用陀螺仪数据计算的预测姿态，使用加速度计和陀螺仪观测的姿态。记录行走过程中每一次扩展卡尔曼滤波中观测值与预测值之间的差值与滤波计算后姿态中航向角的变化值，并将数据进行相关性分析，计算观测值与预测值之间的差值与滤波计算后姿态中航向角的变化值之间的相关系数，由于采集的数据为直线行走数据，因此导致航向角产生较大变化的因素即为易受环境影响的因素，也就是需要动态调整观测噪声的因素。

第二步：根据观测值与预测值之间的差值与滤波计算后姿态中航向角的变化值之间的相关性确立观测噪声模型，建立线性或非线性噪声模型，本例中采用线性模型。

建立线性模型如下：

stdevmag＝k×e+b

其中e为磁力计数据与预测估计值的差值，k与b为需要训练的系数。即，要寻找最合适的k与b，使得直线行走轨迹更加准确。

第三步：给定噪声模型以不同的参数，将模型带入pdr扩展卡尔曼滤波中进行计算，得出模拟轨迹，记录轨迹方向准确性与方向一致性，根据实际运用需求给予方向准确性与一致性不同的权重并相加，得到目标值，本例中方向准确性权重较高，一致性其次，即需要得到最优的噪声参数，使目标值最小。

期望输出如下：

t＝k1×stdθ+k2×eθ

上式中stdθ为前进的方向的标准差，eθ为来回路径的偏差夹角。k1与k2为轨迹方向一致性与方向准确性的权重。

第四步：将噪声模型所需的参数作为神经网络的输入，将目标值作为神经网络对应的输出，进行神经网络模型训练，直至模型误差小于阈值时停止。

采取较为通用的两层神经网络模型来进行网络结构的设计，两层神经网络网络结构图，如图2表示，如图2所示，该两层神经网络包含了2个输入与1个输出，输入为系数k与b，输出为期望t。神经网络的第一层为log-sigmoid层，其中包含了s¹个神经元，通常取s¹为2～8个，第二层为线性层，也是输出层。出于最小化网络系统参数的考虑，从8个隐藏神经元开始依次2个递减进行试验，在满足误差足够小的情况下选取最小的系统模型。

如图3所示，采用bp算法对上述的神经网络模型进行训练，对于两层神经网络而言，算法第一步是通过网络将输入向前传播：

p＝[kibi]^t

a⁰＝p

a^m+1＝f^m+1(w^m+1a^m+b^m+1),m＝0,1

a＝a²

其中p为输入向量，aⁱ为第i层的输出，a为网络输出。

第二步是通过网络将敏感性反向传播：

其中sⁱ为第i层反向传播的敏感度。

第一层为log-sigmoid层，所以

第二层为线性层，

最后一步即为更新权值与偏置值

w^m(k+1)＝w^m(k)-αs^m(a^m-1)^t,m＝2,1

b^m(k+1)＝b^m(k)-αs^m

其中wⁱ与bⁱ为第i层的权值矩阵与偏置值矩阵。

取k值从0到10，步长0.5，取b值从0到5，步长0.5，进行pdr计算，其对应输出取t＝0.2×stdθ+1×eθ，记录并循环依次带入bp算法中进行计算，循环若干次直至平方和误差小于阈值，即为训练成功。要注意的是需要多次随机选取w与b的初始值，以免落入局部最优解，三层神经网络模型bp训练方法与二层相类似。

训练效果如下：

在第一层神经元为8的情况下，训练出的性能曲面如下图4所示：

在图4中，黑色越深表示值越小，也就是误差最小，该点所对应的k与b值即需要的能使模型误差最小的系数。很明显可见在k＝1.4，b＝2.7的时候取得最小值，此时来回路径每一步方向的标准差为18.600与20.74，相较于固定置信度的pdr算法标准差分别降低了14.1％与28.5％，可见其对外界环境的抗干扰性有了较好的提升。

图5为随迭代次数平方和误差下降的曲线图，可见在0.01的学习速率下要迭代1*10^4次后误差小于阈值并模型基本收敛，达到预期效果。经试验，若提高学习速率会出现一定程度的震荡。

当模型训练完毕，即要寻找最小目标值所对应的参数值，此参数值即为需要的噪声模型的最优参数，使用神经网络训练得到的最优参数进行轨迹追踪与原始固定噪声轨迹图对比如图6所示，很明显可以看出自适应观测噪声pdr轨迹跟踪抗干扰性有了一定的提高。表1列举了改进后的自适应扩展卡尔曼滤波行走航迹推算算法相较于原始算法在三种路径下的定位误差与抗干扰性能，从表可以看出改进算法具有较大的优势。

表1改进算法性能对比表

应当理解的是，本发明的上述具体实施方式仅仅用于示例性说明或解释本发明的原理，而不构成对本发明的限制。因此，在不偏离本发明的精神和范围的情况下所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。此外，本发明所附权利要求旨在涵盖落入所附权利要求范围和边界、或者这种范围和边界的等同形式内的全部变化和修改例。