[发明专利]一种扩展全对称多胞形集员Kalman混合滤波方法

说明书

本发明提出了一种扩展全对称多胞形集员Kalman混合滤波方法，针对非线性系统模型开展系统状态变量的最优滤波计算，实施了Taylor级数多项式扩展来逼近非线性系统函数，获得系统线性化等价模型；注重对线性化操作的高阶截断误差处理，把Taylor级数线性化的高阶项误差利用全对称多胞形逼近计算，开展全对称多胞形集员滤波计算；对于系统状态变量的高斯噪声仍然利用传统的Kalman滤波计算，实现全对称多胞形和Kalman混合滤波计算。本发明改善了非线性系统状态变量参数最优估计精度和系统计算稳定性，经由SLAM系统仿真实验，与传统扩展Kalman滤波算法对比，本发明具有较好的计算优势与计算效能。

技术领域

本发明涉及航空系统信息处理科学中导航制导与控制的技术领域，尤其涉及一种扩展全对称多胞形集员Kalman混合滤波方法，可应用于自主移动机器人即时定位与地图构建系统(Simultaneous Localization And Mapping,SLAM)问题中，实现SLAM系统误差模型状态参数的最优滤波计算。

背景技术

估计问题分为两大类型：其中一类是基于随机噪声假设方法，如Kalman滤波和扩展Kalman滤波算法，这类方法要求噪声统计特性已知或者部分特性已知；另一类是基于噪声统计特性未知但其定界已知(Unknown But Bounded，UBB)情形，目前研究最多的就是集员滤波(Set-Membership Estimation，SME)理论与算法，这类滤波算法仅要求系统噪声有界，且不需要确切知道有关噪声统计特性的先验知识。对于非线性滤波理论算法问题，它广泛存在于众多科学和实际工程领域中，需要根据非线性系统状态方程和观测方程，按照选定的估计准则开展非线性系统状态变量的最优估计计算，而Kalman滤波理论仅适用于线性系统，由此提出基于Taylor级数扩展的Kalman滤波算法，其核心就是应用非线性系统模型的线性化等价模型方程获得滤波器的协方差矩阵和Kalman增益矩阵，但是采用一阶扩展线性化的EKF算法计算精度很差，由此人们提出了UT无迹变换理论来逼近非线性系统状态变量的后验概率密度，从而构造出了UKF算法。UKF算法要求已知系统精确化模型，并且要求系统状态变量的统计特性精确已知，这在工程中很难获得。与UKF算法类似的还有CDKF算法，CKF以及GHKF算法等。而集员滤波理论则提供了一种处理这类问题的有效办法，它把系统噪声甚至系统模型的不确定性描述为未知分布但有界的可加性噪声，与传统滤波算法相比，仅要求系统噪声有界，而无需知道有关噪声的精确统计特性，因此适用面广且鲁棒性强。目前，集员滤波算法已经被广泛应用到状态估计、参数辨识和预测控制等领域。

集员滤波理论根据系统可行集合采用的逼近形状可分为椭球集员算法、盒子集员算法、超多面体集员算法及新近发展的全对称多胞形集员算法。传统的集员滤波算法一般是处理线性系统的，但是它和经典Kalman滤波理论一样，也需要处理实际应用中的非线性系统。

发明内容

针对现有滤波方法计算复杂度较高的技术问题，本发明提出一种扩展全对称多胞形集员Kalman混合滤波方法，对线性化操作的高阶截断误差处理，把Taylor级数线性化的高阶项误差利用全对称多胞形集员滤波逼近计算，对于系统状态变量的高斯噪声仍然利用传统的Kalman滤波计算，实现了SLAM系统误差模型状态参数最优滤波，提高了计算效率。

为了达到上述目的，本发明的技术方案是这样实现的：一种扩展全对称多胞形集员Kalman混合滤波方法，其步骤如下：

步骤一：建立SLAM系统非线性的离散化随机空间模型，包括状态方程和观测方程，初始为状态变量；

步骤二：基于Taylor级数多项式性质对SLAM系统非线性的模型方程实施线性化等价变换，获得等价SLAM系统线性化的等价线性化系统模型方程。

步骤三：已知第k步系统随机状态变量的全对称多胞形参数、系统噪声方差以及观测噪声方差及其协方差，对第k步的系统随机状态变量的全对称多胞形的生成算子矩阵实施降维计算，确定全对称多胞形的方差矩阵；k＝1，2，···；