一种扩展全对称多胞形集员Kalman混合滤波方法技术

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

An extended symmetric multi cell set membership Kalman hybrid filtering method

An extended fully symmetric polycell set membership Kalman hybrid filtering method is proposed. The optimal filtering calculation of system state variables is carried out for the nonlinear system model. The Taylor series polynomial expansion is implemented to approximate the nonlinear system function and obtain the system linearization equivalent model. The high-order term error of Taylor series linearization is processed by using full symmetric polycell approximation, and the full symmetric polycell set membership filtering is carried out. The method improves the optimal estimation accuracy of the state variable parameters of the nonlinear system and the calculation stability of the system. Compared with the traditional extended Kalman filtering algorithm, the SLAM system simulation experiment shows that the method has better calculation advantages and computational efficiency.

【技术实现步骤摘要】
一种扩展全对称多胞形集员Kalman混合滤波方法
本专利技术涉及航空系统信息处理科学中导航制导与控制的
，尤其涉及一种扩展全对称多胞形集员Kalman混合滤波方法，可应用于自主移动机器人即时定位与地图构建系统(SimultaneousLocalizationAndMapping,SLAM)问题中，实现SLAM系统误差模型状态参数的最优滤波计算。
技术介绍
估计问题分为两大类型：其中一类是基于随机噪声假设方法，如Kalman滤波和扩展Kalman滤波算法，这类方法要求噪声统计特性已知或者部分特性已知；另一类是基于噪声统计特性未知但其定界已知(UnknownButBounded，UBB)情形，目前研究最多的就是集员滤波(Set-MembershipEstimation，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，···；步骤四：开展全对称多胞形的预测计算，计算全对称多胞形集员Kalman混合滤波器增益矩阵；步骤五：开展系统随机状态变量的全对称多胞形观测更新计算，获得系统随机状态变量的全对称多胞形的中心值更新、生成算子矩阵更新、全对称多胞形误差方差矩阵更新及高斯噪声误差方差矩阵更新计算，从而完成全对称多胞形集员Kalman混合滤波迭代计算过程。所述SLAM系统的离散化的随机空间模型为：其中，是第k步的nx维系统状态变量，是第k步的nz维系统观测变量，和分别表示系统高斯过程噪声和观测噪声，且v＝{vk}k∈N和w＝{wn}n∈N都是零均值独立或者联合独立于系统状态变量初值x0，系统状态变量xk表现为马尔科夫链过程，(xk,yk)是一个具有独立噪声的隐马科夫链过程，f(·)和g(·)分别表示系统模型的非线性动态过程函数和观测函数；非线性的系统状态变量是由全对称多胞形和高斯随机噪声组成，表达为：x＝c+z+g，z∈(<0,R>)，c表示全对称多胞形的中心，z表示Taylor级数线性化操作获得的高阶余项误差，R表示全对称多胞形的形状矩阵算子，g表示随机状态变量的高斯误差项，Q表示高斯噪声方差矩阵，那么系统随机状态变量可表达为集合系统初始状态变量：x0＝c0+z0+g0，z0∈(<0,R0>)，对离散的非线性SLAM系统的模型方程实施Taylor级数多项式扩展，可以获得：其中，表示非线性函数的偏微分运算符，Δx＝x-xk，Eg,kvg,k项表示系统状态变量满足高斯分布的系统随机噪声，Eg,k表示系统高斯噪声矩阵，且高斯噪声vg,k满足Fg,kwg,k项表示观测向量满足高斯分布的观测随机噪声，Fg,k表示观测高斯噪声矩阵，且高斯噪声wg,k满足整理上式获得LPV结构的等价线性化系统模型方程：其中，噪声变量服从全对称多胞形分布特征，状态转移矩阵Ak为：Ak＝A0,k+Δx，不确定量δ在一个nδ维的超球面中；Ez,k和vz,k分别表示全对称多胞形描述的系统不确定噪声矩阵及其全对称多胞形噪声；观测转移矩阵Ck满足Ck＝C0,k+Δy，不确定量ε在一个nε维超球面上；Fz,k和wz,k分别表示全对称多胞形描述的观测向量不确定噪声矩阵及其全对称多胞形观测噪声。利用Luenberger观测器结构，引入观测增益矩阵Gk，观测方程可获得：从而可以获得随机变量中的各项更新表达式为：ck+1＝(Ak-GkCk)ck+Gkyk，zk+1＝(Ak-GkCk)zk+(Ez,kvz,k-GkFz,kwz,k)，gk+1＝(Ak-GkCk)gk+(Eg,kvg,k-GkFg,kwg,k)；那么随机变量更新为：其中，Rk+1＝[(Ak-GkCk)(↓qRk)，Ez,k,-GkFz,k]，从而第k+1步的系统随机变量表示为所述步骤三中已知第k步的系统状态变量：xk＝ck+zk+gk，且其满足全对称多胞形方差为：对全对称多胞形算子矩阵Rk实施降维计算：↓q表示降维算子；若算子矩阵Rk有p列向量，按列递减分解为Rk＝[r1,k,…,rj,k,…,rp,k]，||rj,k||2≥||rj+1,k||2；若p≤q，那么↓qRk＝Rk，否则↓qRk＝[R＞,k,b(R＜,k)]，其中R＞,k＝[r1,k,…,rq-n,k]，R＜,k＝[rq-n+1,k,…,rp,k]；进而全对称多胞形方差矩阵简化为：计算等价线性化系统模型方程中的全对称多胞形误差方差和高斯噪声方差矩阵：Qv,k＝(1-η)Qvz,k+ηQvg,kQw,k＝(1-η)Qwz,k+ηQwg,k；其中，η∈(0,1]为尺度因子。所述降维算子↓q中q表示矩阵↓qR的最大行数，满足若生成算子矩阵R可表示为R＝[r1,r2,…rj,…,rp]，且按照||rj||2≥||r本文档来自技高网...

【技术保护点】
1.一种扩展全对称多胞形集员Kalman混合滤波方法，其特征在于，其步骤如下：步骤一：建立SLAM系统非线性的离散化随机空间模型，包括状态方程和观测方程，初始为状态变量；步骤二：基于Taylor级数多项式性质对SLAM系统非线性的模型方程实施线性化等价变换，获得等价SLAM系统线性化的等价线性化系统模型方程。步骤三：已知第k步系统随机状态变量的全对称多胞形参数、系统噪声方差以及观测噪声方差及其协方差，对第k步的系统随机状态变量的全对称多胞形的生成算子矩阵实施降维计算，确定全对称多胞形的方差矩阵；k＝1，2，···；步骤四：开展全对称多胞形的预测计算，计算全对称多胞形集员Kalman混合滤波器增益矩阵；步骤五：开展系统随机状态变量的全对称多胞形观测更新计算，获得系统随机状态变量的全对称多胞形的中心值更新、生成算子矩阵更新、全对称多胞形误差方差矩阵更新及高斯噪声误差方差矩阵更新计算，从而完成全对称多胞形集员Kalman混合滤波迭代计算过程。

【技术特征摘要】
1.一种扩展全对称多胞形集员Kalman混合滤波方法，其特征在于，其步骤如下：步骤一：建立SLAM系统非线性的离散化随机空间模型，包括状态方程和观测方程，初始为状态变量；步骤二：基于Taylor级数多项式性质对SLAM系统非线性的模型方程实施线性化等价变换，获得等价SLAM系统线性化的等价线性化系统模型方程。步骤三：已知第k步系统随机状态变量的全对称多胞形参数、系统噪声方差以及观测噪声方差及其协方差，对第k步的系统随机状态变量的全对称多胞形的生成算子矩阵实施降维计算，确定全对称多胞形的方差矩阵；k＝1，2，···；步骤四：开展全对称多胞形的预测计算，计算全对称多胞形集员Kalman混合滤波器增益矩阵；步骤五：开展系统随机状态变量的全对称多胞形观测更新计算，获得系统随机状态变量的全对称多胞形的中心值更新、生成算子矩阵更新、全对称多胞形误差方差矩阵更新及高斯噪声误差方差矩阵更新计算，从而完成全对称多胞形集员Kalman混合滤波迭代计算过程。2.根据权利要求1所述的扩展全对称多胞形集员Kalman混合滤波方法，其特征在于，所述SLAM系统的离散化的随机空间模型为：其中，是第k步的nx维系统状态变量，是第k步的nz维系统观测变量，和分别表示系统高斯过程噪声和观测噪声，且v＝{vk}k∈N和w＝{wn}n∈N都是零均值独立或者联合独立于系统状态变量初值x0，系统状态变量xk表现为马尔科夫链过程，(xk,yk)是一个具有独立噪声的隐马科夫链过程，f(·)和g(·)分别表示系统模型的非线性动态过程函数和观测函数；非线性的系统状态变量是由全对称多胞形和高斯随机噪声组成，表达为：x＝c+z+g，z∈(<0,R＞)，c表示全对称多胞形的中心，z表示Taylor级数线性化操作获得的高阶余项误差，R表示全对称多胞形的形状矩阵算子，g表示随机状态变量的高斯误差项，Q表示高斯噪声方差矩阵，那么系统随机状态变量可表达为集合系统初始状态变量：x0＝c0+z0+g0，z0∈(＜0,R0>)，3.根据权利要求1所述的扩展全对称多胞形集员Kalman混合滤波方法，其特征在于，对离散的非线性SLAM系统的模型方程实施Taylor级数多项式扩展，可以获得：其中，表示非线性函数的偏微分运算符，Δx＝x-xk，Eg,kvg,k项表示系统状态变量满足高斯分布的系统随机噪声，Eg,k表示系统高斯噪声矩阵，且高斯噪声vg,k满足Fg,kwg,k项表示观测向量满足高斯分布的观测随机噪声，Fg,k表示观测高斯噪声矩阵，且高斯噪声wg,k满足整理上式获得LPV结构的等价线性化系统模型方程：其中，噪声变量服从全对称多胞形分布特征，状态转移矩阵Ak为：Ak＝A0,k+Δx，不确定量δ在一个nδ维的超球面中；Ez,k和vz,k分别表示全对称多胞形描述的系统不确定噪声矩阵及其全对称多胞形噪声；观测转移矩阵Ck满足Ck＝C0,k+Δy，不确定量ε在一个nε维超球面上；Fz,k和wz,k分别表示全对称多胞形描述的观测向量不确定噪声矩阵及其全...

【专利技术属性】
技术研发人员：丁国强，娄泰山，张焕龙，张铎，王晓雷，方洁，
申请(专利权)人：郑州轻工业学院，
类型：发明
国别省市：河南,41